From 3cddb4edab317758dc8a8cac94b90794641c7488 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Hofst=C3=A4tter?= Date: Wed, 4 Aug 2021 17:02:51 +0200 Subject: [PATCH 001/455] Fix returning invalid namespace if sub_namespace is empty (#1658) * Create valid effective namespace when sub-namespace is empty Fix #1656. Signed-off-by: Markus Hofstaetter * Add regression test for effective namespace and empty sub-namespace Adds regression test for #1656. Signed-off-by: Markus Hofstaetter --- rclcpp/src/rclcpp/node.cpp | 12 +++++++++++- rclcpp/test/rclcpp/test_node.cpp | 13 +++++++++++++ 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index c9ed1e3ac2..46f1af12df 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -61,6 +61,12 @@ extend_sub_namespace(const std::string & existing_sub_namespace, const std::stri extension.c_str(), "a sub-namespace should not have a leading /", 0); + } else if (existing_sub_namespace.empty() && extension.empty()) { + throw rclcpp::exceptions::NameValidationError( + "sub_namespace", + extension.c_str(), + "sub-nodes should not extend nodes by an empty sub-namespace", + 0); } std::string new_sub_namespace; @@ -86,7 +92,11 @@ create_effective_namespace(const std::string & node_namespace, const std::string // and do not need trimming of `/` and other things, as they were validated // in other functions already. - if (node_namespace.back() == '/') { + // A node may not have a sub_namespace if it is no sub_node. In this case, + // just return the original namespace + if (sub_namespace.empty()) { + return node_namespace; + } else if (node_namespace.back() == '/') { // this is the special case where node_namespace is just `/` return node_namespace + sub_namespace; } else { diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index 70b207b03e..e034af16b2 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -102,6 +102,7 @@ TEST_F(TestNode, get_name_and_namespace) { auto node = std::make_shared("my_node", "/ns"); EXPECT_STREQ("my_node", node->get_name()); EXPECT_STREQ("/ns", node->get_namespace()); + EXPECT_STREQ("/ns", node->get_effective_namespace().c_str()); EXPECT_STREQ("/ns/my_node", node->get_fully_qualified_name()); } { @@ -116,30 +117,35 @@ TEST_F(TestNode, get_name_and_namespace) { auto node = std::make_shared("my_node", "ns"); EXPECT_STREQ("my_node", node->get_name()); EXPECT_STREQ("/ns", node->get_namespace()); + EXPECT_STREQ("/ns", node->get_effective_namespace().c_str()); EXPECT_STREQ("/ns/my_node", node->get_fully_qualified_name()); } { auto node = std::make_shared("my_node"); EXPECT_STREQ("my_node", node->get_name()); EXPECT_STREQ("/", node->get_namespace()); + EXPECT_STREQ("/", node->get_effective_namespace().c_str()); EXPECT_STREQ("/my_node", node->get_fully_qualified_name()); } { auto node = std::make_shared("my_node", ""); EXPECT_STREQ("my_node", node->get_name()); EXPECT_STREQ("/", node->get_namespace()); + EXPECT_STREQ("/", node->get_effective_namespace().c_str()); EXPECT_STREQ("/my_node", node->get_fully_qualified_name()); } { auto node = std::make_shared("my_node", "/my/ns"); EXPECT_STREQ("my_node", node->get_name()); EXPECT_STREQ("/my/ns", node->get_namespace()); + EXPECT_STREQ("/my/ns", node->get_effective_namespace().c_str()); EXPECT_STREQ("/my/ns/my_node", node->get_fully_qualified_name()); } { auto node = std::make_shared("my_node", "my/ns"); EXPECT_STREQ("my_node", node->get_name()); EXPECT_STREQ("/my/ns", node->get_namespace()); + EXPECT_STREQ("/my/ns", node->get_effective_namespace().c_str()); EXPECT_STREQ("/my/ns/my_node", node->get_fully_qualified_name()); } { @@ -278,6 +284,13 @@ TEST_F(TestNode, subnode_construction_and_destruction) { auto subnode = node->create_sub_node("~sub_ns"); }, rclcpp::exceptions::InvalidNamespaceError); } + { + ASSERT_THROW( + { + auto node = std::make_shared("my_node", "/ns"); + auto subnode = node->create_sub_node(""); + }, rclcpp::exceptions::NameValidationError); + } } TEST_F(TestNode, get_logger) { From 6c0a46bcc800757f432829b958700e269c6f8282 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 4 Aug 2021 16:08:47 -0700 Subject: [PATCH 002/455] Use rosidl_get_typesupport_target() (#1729) Signed-off-by: Shane Loretz --- rclcpp/test/rclcpp/CMakeLists.txt | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 35d9b4ea4e..d78a1adef5 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -12,6 +12,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs LIBRARY_NAME ${PROJECT_NAME} SKIP_INSTALL ) +# Need the target name to depend on generated interface libraries +rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}_test_msgs" "rosidl_typesupport_cpp") ament_add_gtest( test_allocator_common @@ -374,8 +376,10 @@ if(TARGET test_publisher_with_type_adapter) "rosidl_typesupport_cpp" "test_msgs" ) - rosidl_target_interfaces(test_publisher_with_type_adapter ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp") - target_link_libraries(test_publisher_with_type_adapter ${PROJECT_NAME} mimick) + target_link_libraries(test_publisher_with_type_adapter + ${PROJECT_NAME} + mimick + ${cpp_typesupport_target}) endif() ament_add_gtest(test_subscription_with_type_adapter test_subscription_with_type_adapter.cpp @@ -390,8 +394,10 @@ if(TARGET test_subscription_with_type_adapter) "rosidl_typesupport_cpp" "test_msgs" ) - rosidl_target_interfaces(test_subscription_with_type_adapter ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp") - target_link_libraries(test_subscription_with_type_adapter ${PROJECT_NAME} mimick) + target_link_libraries(test_subscription_with_type_adapter + ${PROJECT_NAME} + mimick + ${cpp_typesupport_target}) endif() ament_add_gtest(test_publisher_subscription_count_api test_publisher_subscription_count_api.cpp) @@ -680,8 +686,9 @@ if(TARGET test_subscription_topic_statistics) "rosidl_typesupport_cpp" "statistics_msgs" "test_msgs") - rosidl_target_interfaces(test_subscription_topic_statistics ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp") - target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME}) + target_link_libraries(test_subscription_topic_statistics + ${PROJECT_NAME} + ${cpp_typesupport_target}) endif() ament_add_gtest(test_subscription_options test_subscription_options.cpp) From 3b1144f1e0c775d8e6cee240618a0bc0647ec9db Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 6 Aug 2021 12:08:50 -0700 Subject: [PATCH 003/455] Use FindPython3 and make python3 dependency explicit (#1745) * Use FindPython3 and make python3 dependency explicit Signed-off-by: Shane Loretz * Need CMake 3.12 for FindPython3 Signed-off-by: Shane Loretz --- rclcpp/CMakeLists.txt | 10 ++++++---- rclcpp/package.xml | 1 + 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index c1cb8bf3e2..76876b4b2d 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.12) project(rclcpp) @@ -109,6 +109,8 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/waitable.cpp ) +find_package(Python3 REQUIRED COMPONENTS Interpreter) + # "watch" template for changes configure_file( "resource/logging.hpp.em" @@ -122,7 +124,7 @@ set(python_code_logging string(REPLACE ";" "$" python_code_logging "${python_code_logging}") add_custom_command(OUTPUT include/rclcpp/logging.hpp COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp" - COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code_logging}" + COMMAND Python3::Interpreter ARGS -c "${python_code_logging}" DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/logging.hpp.em.watch" COMMENT "Expanding logging.hpp.em" VERBATIM @@ -146,7 +148,7 @@ foreach(interface_file ${interface_files}) string(REPLACE ";" "$" python_${interface_name}_traits "${python_${interface_name}_traits}") add_custom_command(OUTPUT include/rclcpp/node_interfaces/${interface_name}_traits.hpp COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp/node_interfaces" - COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_${interface_name}_traits}" + COMMAND Python3::Interpreter ARGS -c "${python_${interface_name}_traits}" DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/${interface_name}_traits.hpp.em.watch" COMMENT "Expanding interface_traits.hpp.em into ${interface_name}_traits.hpp" VERBATIM @@ -166,7 +168,7 @@ foreach(interface_file ${interface_files}) string(REPLACE ";" "$" python_get_${interface_name} "${python_get_${interface_name}}") add_custom_command(OUTPUT include/rclcpp/node_interfaces/get_${interface_name}.hpp COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp/node_interfaces" - COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_get_${interface_name}}" + COMMAND Python3::Interpreter ARGS -c "${python_get_${interface_name}}" DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/get_${interface_name}.hpp.em.watch" COMMENT "Expanding get_interface.hpp.em into get_${interface_file}.hpp" VERBATIM diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 56726b9490..ae5228e6df 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -11,6 +11,7 @@ Dirk Thomas ament_cmake_ros + python3 ament_index_cpp builtin_interfaces From b918bd4c25c2ec59bf6ef58eff04612902eb9330 Mon Sep 17 00:00:00 2001 From: mauropasse Date: Mon, 9 Aug 2021 14:25:04 +0100 Subject: [PATCH 004/455] Reset timer trigger time before execute. (#1739) This PR fixes an issue with StaticSingleThreadedExecutor not reseting the timers, so they were always ready and executing callbacks continuosly. https://github.com/ros2/rclcpp/pull/1692 Signed-off-by: Mauro Passerino --- .../src/rclcpp/executors/static_single_threaded_executor.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index bc27bdf464..8f165812af 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include "rcpputils/scope_exit.hpp" @@ -237,7 +238,9 @@ StaticSingleThreadedExecutor::execute_ready_executables(bool spin_once) for (size_t i = 0; i < wait_set_.size_of_timers; ++i) { if (i < entities_collector_->get_number_of_timers()) { if (wait_set_.timers[i] && entities_collector_->get_timer(i)->is_ready()) { - execute_timer(entities_collector_->get_timer(i)); + auto timer = entities_collector_->get_timer(i); + timer->call(); + execute_timer(std::move(timer)); if (spin_once) { return true; } From 893b9b4f8269f4dbee3fc11b821961adc1822860 Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Thu, 12 Aug 2021 15:48:35 -0400 Subject: [PATCH 005/455] Add tracing instrumentation for executor and message taking (#1738) * Add tracing instrumentation for executor and message taking Signed-off-by: Christophe Bedard * Move publisher handle argument to rcl_publish tracepoint Signed-off-by: Christophe Bedard * Keep publisher handle arg for rclcpp_publish tracepoint but use nullptr Signed-off-by: Christophe Bedard --- rclcpp/include/rclcpp/publisher.hpp | 5 +---- rclcpp/src/rclcpp/executor.cpp | 10 ++++++++++ rclcpp/src/rclcpp/subscription_base.cpp | 1 + 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 346a87f3f1..bebedda845 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -438,10 +438,7 @@ class Publisher : public PublisherBase void do_inter_process_publish(const ROSMessageType & msg) { - TRACEPOINT( - rclcpp_publish, - static_cast(publisher_handle_.get()), - static_cast(&msg)); + TRACEPOINT(rclcpp_publish, nullptr, static_cast(&msg)); auto status = rcl_publish(publisher_handle_.get(), &msg, nullptr); if (RCL_RET_PUBLISHER_INVALID == status) { diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 8256f28790..a5d4050e70 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -33,6 +33,8 @@ #include "rcutils/logging_macros.h" +#include "tracetools/tracetools.h" + using namespace std::chrono_literals; using rclcpp::exceptions::throw_from_rcl_error; @@ -515,9 +517,15 @@ Executor::execute_any_executable(AnyExecutable & any_exec) return; } if (any_exec.timer) { + TRACEPOINT( + rclcpp_executor_execute, + static_cast(any_exec.timer->get_timer_handle().get())); execute_timer(any_exec.timer); } if (any_exec.subscription) { + TRACEPOINT( + rclcpp_executor_execute, + static_cast(any_exec.subscription->get_subscription_handle().get())); execute_subscription(any_exec.subscription); } if (any_exec.service) { @@ -678,6 +686,7 @@ Executor::execute_client( void Executor::wait_for_work(std::chrono::nanoseconds timeout) { + TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count()); { std::lock_guard guard(mutex_); @@ -827,6 +836,7 @@ Executor::get_next_ready_executable_from_map( const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { + TRACEPOINT(rclcpp_executor_get_next_ready); bool success = false; std::lock_guard guard{mutex_}; // Check the timers to see if there are any that are ready diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index ba76e5f9bc..472034f362 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -143,6 +143,7 @@ SubscriptionBase::take_type_erased(void * message_out, rclcpp::MessageInfo & mes &message_info_out.get_rmw_message_info(), nullptr // rmw_subscription_allocation_t is unused here ); + TRACEPOINT(rclcpp_take, static_cast(message_out)); if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { return false; } else if (RCL_RET_OK != ret) { From d7764b4322d83162c94e0c68d42158905f582ef2 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Wed, 9 Jun 2021 15:08:40 +0100 Subject: [PATCH 006/455] Fix bug: Add node while spinning Signed-off-by: Mauro Passerino --- .../static_executor_entities_collector.hpp | 8 ++-- .../static_executor_entities_collector.cpp | 20 ++++++---- .../static_single_threaded_executor.cpp | 6 +-- rclcpp/test/benchmark/benchmark_executor.cpp | 3 +- ...est_static_executor_entities_collector.cpp | 37 +++++++------------ 5 files changed, 35 insertions(+), 39 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp index 695b35a226..2c4613c845 100644 --- a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp @@ -57,15 +57,13 @@ class StaticExecutorEntitiesCollector final /** * \param p_wait_set A reference to the wait set to be used in the executor * \param memory_strategy Shared pointer to the memory strategy to set. - * \param executor_guard_condition executor's guard condition * \throws std::runtime_error if memory strategy is null */ RCLCPP_PUBLIC void init( rcl_wait_set_t * p_wait_set, - rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy, - rcl_guard_condition_t * executor_guard_condition); + rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy); /// Finalize StaticExecutorEntitiesCollector to clear resources RCLCPP_PUBLIC @@ -338,6 +336,10 @@ class StaticExecutorEntitiesCollector final /// List of weak nodes registered in the static executor std::list weak_nodes_; + // Mutex to protect vector of new nodes. + std::mutex new_nodes_mutex_; + std::vector new_nodes_; + /// Wait set for managing entities that the rmw layer waits on. rcl_wait_set_t * p_wait_set_ = nullptr; diff --git a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp index c0b7fb0e03..b6ceb5b331 100644 --- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp @@ -61,8 +61,7 @@ StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector() void StaticExecutorEntitiesCollector::init( rcl_wait_set_t * p_wait_set, - rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy, - rcl_guard_condition_t * executor_guard_condition) + rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy) { // Empty initialize executable list exec_list_ = rclcpp::experimental::ExecutableList(); @@ -74,9 +73,6 @@ StaticExecutorEntitiesCollector::init( } memory_strategy_ = memory_strategy; - // Add executor's guard condition - memory_strategy_->add_guard_condition(executor_guard_condition); - // Get memory strategy and executable list. Prepare wait_set_ std::shared_ptr shared_ptr; execute(shared_ptr); @@ -108,6 +104,14 @@ StaticExecutorEntitiesCollector::execute(std::shared_ptr & data) fill_executable_list(); // Resize the wait_set_ based on memory_strategy handles (rcl_wait_set_resize) prepare_wait_set(); + // Add new nodes guard conditions to map + std::lock_guard guard{new_nodes_mutex_}; + for (const auto & weak_node : new_nodes_) { + if (auto node_ptr = weak_node.lock()) { + weak_nodes_to_guard_conditions_[node_ptr] = node_ptr->get_notify_guard_condition(); + } + } + new_nodes_.clear(); } void @@ -277,7 +281,8 @@ StaticExecutorEntitiesCollector::add_to_wait_set(rcl_wait_set_t * wait_set) size_t StaticExecutorEntitiesCollector::get_number_of_ready_guard_conditions() { - return weak_nodes_to_guard_conditions_.size(); + std::lock_guard guard{new_nodes_mutex_}; + return weak_nodes_to_guard_conditions_.size() + new_nodes_.size(); } bool @@ -329,7 +334,8 @@ StaticExecutorEntitiesCollector::add_callback_group( throw std::runtime_error("Callback group was already added to executor."); } if (is_new_node) { - weak_nodes_to_guard_conditions_[node_ptr] = node_ptr->get_notify_guard_condition(); + std::lock_guard guard{new_nodes_mutex_}; + new_nodes_.push_back(node_ptr); return true; } return false; diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 8f165812af..8350521fe3 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -48,7 +48,7 @@ StaticSingleThreadedExecutor::spin() // Set memory_strategy_ and exec_list_ based on weak_nodes_ // Prepare wait_set_ based on memory_strategy_ - entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_); + entities_collector_->init(&wait_set_, memory_strategy_); while (rclcpp::ok(this->context_) && spinning.load()) { // Refresh wait set and wait for work @@ -82,7 +82,7 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati { // Make sure the entities collector has been initialized if (!entities_collector_->is_init()) { - entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_); + entities_collector_->init(&wait_set_, memory_strategy_); } auto start = std::chrono::steady_clock::now(); @@ -119,7 +119,7 @@ StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout) { // Make sure the entities collector has been initialized if (!entities_collector_->is_init()) { - entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_); + entities_collector_->init(&wait_set_, memory_strategy_); } if (rclcpp::ok(context_) && spinning.load()) { diff --git a/rclcpp/test/benchmark/benchmark_executor.cpp b/rclcpp/test/benchmark/benchmark_executor.cpp index 85815b3b50..b0fbab4a36 100644 --- a/rclcpp/test/benchmark/benchmark_executor.cpp +++ b/rclcpp/test/benchmark/benchmark_executor.cpp @@ -376,9 +376,8 @@ BENCHMARK_F( 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); + entities_collector_->init(&wait_set, memory_strategy); 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 51278af9f9..dc81b5611e 100644 --- a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp @@ -143,12 +143,11 @@ TEST_F(TestStaticExecutorEntitiesCollector, init_bad_arguments) { 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(); // Check memory strategy is nullptr rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy = nullptr; EXPECT_THROW( - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition), + entities_collector_->init(&wait_set, memory_strategy), std::runtime_error); } @@ -169,9 +168,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) { 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); + entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); EXPECT_EQ( expected_number_of_entities->subscriptions, @@ -185,7 +183,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) { entities_collector_->get_number_of_waitables()); EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface())); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy); 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()); @@ -216,10 +214,9 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) { 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(); // Expect weak_node pointers to be cleaned up and used - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); @@ -288,9 +285,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { 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); + entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); EXPECT_EQ( @@ -310,7 +306,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { entities_collector_->get_number_of_waitables()); entities_collector_->remove_node(node->get_node_base_interface()); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy); 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()); @@ -367,9 +363,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_clear_ 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); + entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); { @@ -398,9 +393,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_resize 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); + entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); { @@ -436,9 +430,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_rcl_wait_failed) { 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); + entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); { @@ -485,9 +478,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_add_handles_to_wait 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); + entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); { @@ -517,9 +509,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_to_wait_set_nullptr) { 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); + entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); RCLCPP_EXPECT_THROW_EQ( @@ -545,7 +536,6 @@ TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group) 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(); rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); @@ -555,7 +545,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group) cb_group.reset(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u); @@ -623,9 +613,8 @@ TEST_F( 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); + entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); cb_group->get_associated_with_executor_atomic().exchange(false); From 4fcd05db726d87cfefc3c8588aafbdca3f159c62 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Thu, 19 Aug 2021 18:59:57 +0100 Subject: [PATCH 007/455] Change log level for lifecycle_publisher (#1715) * Change log level for lifecycle_publisher De-activating a lifecycle publisher while the function that was invoking `publish` is still running floods the log of useless warning messages. This requires to add a boolean check around the publish call, thus making useless the choice of a lifecycle publisher Signed-off-by: Alberto Soragna * change lifecycle publisher to log warning only once per transition Signed-off-by: Alberto Soragna * rework lifecycle publisher log mechanism to use an helper function Signed-off-by: Alberto Soragna * change doxygen format to use implicit brief Signed-off-by: Alberto Soragna Co-authored-by: Alberto Soragna --- .../rclcpp_lifecycle/lifecycle_publisher.hpp | 37 ++++++++++++++----- 1 file changed, 27 insertions(+), 10 deletions(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp index fdf9ba17c9..d1b5f80975 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -65,6 +65,7 @@ class LifecyclePublisher : public LifecyclePublisherInterface, const rclcpp::PublisherOptionsWithAllocator & options) : rclcpp::Publisher(node_base, topic, qos, options), enabled_(false), + should_log_(true), logger_(rclcpp::get_logger("LifecyclePublisher")) { } @@ -81,11 +82,7 @@ class LifecyclePublisher : public LifecyclePublisherInterface, publish(std::unique_ptr msg) { if (!enabled_) { - RCLCPP_WARN( - logger_, - "Trying to publish message on the topic '%s', but the publisher is not activated", - this->get_topic_name()); - + log_publisher_not_enabled(); return; } rclcpp::Publisher::publish(std::move(msg)); @@ -101,11 +98,7 @@ class LifecyclePublisher : public LifecyclePublisherInterface, publish(const MessageT & msg) { if (!enabled_) { - RCLCPP_WARN( - logger_, - "Trying to publish message on the topic '%s', but the publisher is not activated", - this->get_topic_name()); - + log_publisher_not_enabled(); return; } rclcpp::Publisher::publish(msg); @@ -121,6 +114,7 @@ class LifecyclePublisher : public LifecyclePublisherInterface, on_deactivate() { enabled_ = false; + should_log_ = true; } virtual bool @@ -130,7 +124,30 @@ class LifecyclePublisher : public LifecyclePublisherInterface, } private: + /// LifecyclePublisher log helper function + /** + * Helper function that logs a message saying that publisher can't publish + * because it's not enabled. + */ + void log_publisher_not_enabled() + { + // Nothing to do if we are not meant to log + if (!should_log_) { + return; + } + + // Log the message + RCLCPP_WARN( + logger_, + "Trying to publish message on the topic '%s', but the publisher is not activated", + this->get_topic_name()); + + // We stop logging until the flag gets enabled again + should_log_ = false; + } + bool enabled_ = false; + bool should_log_ = true; rclcpp::Logger logger_; }; From 679fb2ba334971d9769b44258df9095025567559 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 23 Aug 2021 09:57:18 -0300 Subject: [PATCH 008/455] Update client API to be able to remove pending requests (#1734) * Revert "Revert "Updating client API to be able to remove pending requests (#1728)" (#1733)" This reverts commit d5f3d35fbe3127e30d4ee9d40f57c42584eeb830. Signed-off-by: Ivan Santiago Paunovic * Address peer review comments Signed-off-by: Ivan Santiago Paunovic * Fix tests in rclcpp_components, rclcpp_lifecycle Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/client.hpp | 418 ++++++++++++++++-- rclcpp/test/rclcpp/test_client.cpp | 16 +- .../test/test_component_manager_api.cpp | 155 ++++--- .../test/test_lifecycle_service_client.cpp | 21 +- 4 files changed, 481 insertions(+), 129 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index bd2d326012..1fc761f491 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -17,12 +17,15 @@ #include #include -#include +#include #include +#include // NOLINT, cpplint doesn't think this is a cpp std header #include #include #include #include +#include // NOLINT +#include #include "rcl/client.h" #include "rcl/error_handling.h" @@ -45,6 +48,68 @@ namespace rclcpp { +namespace detail +{ +template +struct FutureAndRequestId +{ + FutureT future; + int64_t request_id; + + FutureAndRequestId(FutureT impl, int64_t req_id) + : future(std::move(impl)), request_id(req_id) + {} + + /// Allow implicit conversions to `std::future` by reference. + operator FutureT &() {return this->future;} + + /// Deprecated, use the `future` member variable instead. + /** + * Allow implicit conversions to `std::future` by value. + * \deprecated + */ + [[deprecated("FutureAndRequestId: use .future instead of an implicit conversion")]] + operator FutureT() {return this->future;} + + // delegate future like methods in the std::future impl_ + + /// See std::future::get(). + auto get() {return this->future.get();} + /// See std::future::valid(). + bool valid() const noexcept {return this->future.valid();} + /// See std::future::wait(). + void wait() const {return this->future.wait();} + /// See std::future::wait_for(). + template + std::future_status wait_for( + const std::chrono::duration & timeout_duration) const + { + return this->future.wait_for(timeout_duration); + } + /// See std::future::wait_until(). + template + std::future_status wait_until( + const std::chrono::time_point & timeout_time) const + { + return this->future.wait_until(timeout_time); + } + + // Rule of five, we could use the rule of zero here, but better be explicit as some of the + // methods are deleted. + + /// Move constructor. + FutureAndRequestId(FutureAndRequestId && other) noexcept = default; + /// Deleted copy constructor, each instance is a unique owner of the future. + FutureAndRequestId(const FutureAndRequestId & other) = delete; + /// Move assignment. + FutureAndRequestId & operator=(FutureAndRequestId && other) noexcept = default; + /// Deleted copy assignment, each instance is a unique owner of the future. + FutureAndRequestId & operator=(const FutureAndRequestId & other) = delete; + /// Destructor. + ~FutureAndRequestId() = default; +}; +} // namespace detail + namespace node_interfaces { class NodeBaseInterface; @@ -178,6 +243,9 @@ template class Client : public ClientBase { public: + using Request = typename ServiceT::Request; + using Response = typename ServiceT::Response; + using SharedRequest = typename ServiceT::Request::SharedPtr; using SharedResponse = typename ServiceT::Response::SharedPtr; @@ -187,6 +255,7 @@ class Client : public ClientBase using SharedPromise = std::shared_ptr; using SharedPromiseWithRequest = std::shared_ptr; + using Future = std::future; using SharedFuture = std::shared_future; using SharedFutureWithRequest = std::shared_future>; @@ -195,6 +264,64 @@ class Client : public ClientBase RCLCPP_SMART_PTR_DEFINITIONS(Client) + /// A convenient Client::Future and request id pair. + /** + * Public members: + * - future: a std::future. + * - request_id: the request id associated with the future. + * + * All the other methods are equivalent to the ones std::future provides. + */ + struct FutureAndRequestId + : detail::FutureAndRequestId> + { + using detail::FutureAndRequestId>::FutureAndRequestId; + + /// Deprecated, use `.future.share()` instead. + /** + * Allow implicit conversions to `std::shared_future` by value. + * \deprecated + */ + [[deprecated( + "FutureAndRequestId: use .future.share() instead of an implicit conversion")]] + operator SharedFuture() {return this->future.share();} + + // delegate future like methods in the std::future impl_ + + /// See std::future::share(). + SharedFuture share() noexcept {return this->future.share();} + }; + + /// A convenient Client::SharedFuture and request id pair. + /** + * Public members: + * - future: a std::shared_future. + * - request_id: the request id associated with the future. + * + * All the other methods are equivalent to the ones std::shared_future provides. + */ + struct SharedFutureAndRequestId + : detail::FutureAndRequestId> + { + using detail::FutureAndRequestId>::FutureAndRequestId; + }; + + /// A convenient Client::SharedFutureWithRequest and request id pair. + /** + * Public members: + * - future: a std::shared_future. + * - request_id: the request id associated with the future. + * + * All the other methods are equivalent to the ones std::shared_future provides. + */ + struct SharedFutureWithRequestAndRequestId + : detail::FutureAndRequestId>> + { + using detail::FutureAndRequestId< + std::shared_future> + >::FutureAndRequestId; + }; + /// Default constructor. /** * The constructor for a Client is almost never called directly. @@ -292,34 +419,89 @@ class Client : public ClientBase std::shared_ptr request_header, std::shared_ptr response) override { - std::unique_lock lock(pending_requests_mutex_); - auto typed_response = std::static_pointer_cast(response); - int64_t sequence_number = request_header->sequence_number; - // TODO(esteve) this should throw instead since it is not expected to happen in the first place - if (this->pending_requests_.count(sequence_number) == 0) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Received invalid sequence number. Ignoring..."); + std::optional + optional_pending_request = this->get_and_erase_pending_request(request_header->sequence_number); + if (!optional_pending_request) { return; } - auto tuple = this->pending_requests_[sequence_number]; - auto call_promise = std::get<0>(tuple); - auto callback = std::get<1>(tuple); - auto future = std::get<2>(tuple); - this->pending_requests_.erase(sequence_number); - // Unlock here to allow the service to be called recursively from one of its callbacks. - lock.unlock(); - - call_promise->set_value(typed_response); - callback(future); + auto & value = *optional_pending_request; + auto typed_response = std::static_pointer_cast( + std::move(response)); + if (std::holds_alternative(value)) { + auto & promise = std::get(value); + promise.set_value(std::move(typed_response)); + } else if (std::holds_alternative(value)) { + auto & inner = std::get(value); + const auto & callback = std::get(inner); + auto & promise = std::get(inner); + auto & future = std::get(inner); + promise.set_value(std::move(typed_response)); + callback(std::move(future)); + } else if (std::holds_alternative(value)) { + auto & inner = std::get(value); + const auto & callback = std::get(inner); + auto & promise = std::get(inner); + auto & future = std::get(inner); + auto & request = std::get(inner); + promise.set_value(std::make_pair(std::move(request), std::move(typed_response))); + callback(std::move(future)); + } } - SharedFuture + /// Send a request to the service server. + /** + * This method returns a `FutureAndRequestId` instance + * that can be passed to Executor::spin_until_future_complete() to + * wait until it has been completed. + * + * If the future never completes, + * e.g. the call to Executor::spin_until_future_complete() times out, + * Client::remove_pending_request() must be called to clean the client internal state. + * Not doing so will make the `Client` instance to use more memory each time a response is not + * received from the service server. + * + * ```cpp + * auto future = client->async_send_request(my_request); + * if ( + * rclcpp::FutureReturnCode::TIMEOUT == + * executor->spin_until_future_complete(future, timeout)) + * { + * client->remove_pending_request(future); + * // handle timeout + * } else { + * handle_response(future.get()); + * } + * ``` + * + * \param[in] request request to be send. + * \return a FutureAndRequestId instance. + */ + FutureAndRequestId async_send_request(SharedRequest request) { - return async_send_request(request, [](SharedFuture) {}); + Promise promise; + auto future = promise.get_future(); + auto req_id = async_send_request_impl( + *request, + std::move(promise)); + return FutureAndRequestId(std::move(future), req_id); } + /// Send a request to the service server and schedule a callback in the executor. + /** + * Similar to the previous overload, but a callback will automatically be called when a response is received. + * + * If the callback is never called, because we never got a reply for the service server, remove_pending_request() + * has to be called with the returned request id or prune_pending_requests(). + * Not doing so will make the `Client` instance use more memory each time a response is not + * received from the service server. + * In this case, it's convenient to setup a timer to cleanup the pending requests. + * See for example the `examples_rclcpp_async_client` package in https://github.com/ros2/examples. + * + * \param[in] request request to be send. + * \param[in] cb callback that will be called when we get a response for this request. + * \return the request id representing the request just sent. + */ template< typename CallbackT, typename std::enable_if< @@ -329,23 +511,28 @@ class Client : public ClientBase >::value >::type * = nullptr > - SharedFuture + SharedFutureAndRequestId async_send_request(SharedRequest request, CallbackT && cb) { - std::lock_guard lock(pending_requests_mutex_); - int64_t sequence_number; - rcl_ret_t ret = rcl_send_request(get_client_handle().get(), request.get(), &sequence_number); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request"); - } - - SharedPromise call_promise = std::make_shared(); - SharedFuture f(call_promise->get_future()); - pending_requests_[sequence_number] = - std::make_tuple(call_promise, std::forward(cb), f); - return f; + Promise promise; + auto shared_future = promise.get_future().share(); + auto req_id = async_send_request_impl( + *request, + std::make_tuple( + CallbackType{std::forward(cb)}, + shared_future, + std::move(promise))); + return SharedFutureAndRequestId{std::move(shared_future), req_id}; } + /// Send a request to the service server and schedule a callback in the executor. + /** + * Similar to the previous method, but you can get both the request and response in the callback. + * + * \param[in] request request to be send. + * \param[in] cb callback that will be called when we get a response for this request. + * \return the request id representing the request just sent. + */ template< typename CallbackT, typename std::enable_if< @@ -355,28 +542,165 @@ class Client : public ClientBase >::value >::type * = nullptr > - SharedFutureWithRequest + SharedFutureWithRequestAndRequestId async_send_request(SharedRequest request, CallbackT && cb) { - SharedPromiseWithRequest promise = std::make_shared(); - SharedFutureWithRequest future_with_request(promise->get_future()); + PromiseWithRequest promise; + auto shared_future = promise.get_future().share(); + auto req_id = async_send_request_impl( + *request, + std::make_tuple( + CallbackWithRequestType{std::forward(cb)}, + request, + shared_future, + std::move(promise))); + return SharedFutureWithRequestAndRequestId{std::move(shared_future), req_id}; + } + + /// Cleanup a pending request. + /** + * This notifies the client that we have waited long enough for a response from the server + * to come, we have given up and we are not waiting for a response anymore. + * + * Not calling this will make the client start using more memory for each request + * that never got a reply from the server. + * + * \param[in] request_id request id returned by async_send_request(). + * \return true when a pending request was removed, false if not (e.g. a response was received). + */ + bool + remove_pending_request(int64_t request_id) + { + std::lock_guard guard(pending_requests_mutex_); + return pending_requests_.erase(request_id) != 0u; + } + + /// Cleanup a pending request. + /** + * Convenient overload, same as: + * + * `Client::remove_pending_request(this, future.request_id)`. + */ + bool + remove_pending_request(const FutureAndRequestId & future) + { + return this->remove_pending_request(future.request_id); + } - auto wrapping_cb = [future_with_request, promise, request, - cb = std::forward(cb)](SharedFuture future) { - auto response = future.get(); - promise->set_value(std::make_pair(request, response)); - cb(future_with_request); - }; + /// Cleanup a pending request. + /** + * Convenient overload, same as: + * + * `Client::remove_pending_request(this, future.request_id)`. + */ + bool + remove_pending_request(const SharedFutureAndRequestId & future) + { + return this->remove_pending_request(future.request_id); + } - async_send_request(request, wrapping_cb); + /// Cleanup a pending request. + /** + * Convenient overload, same as: + * + * `Client::remove_pending_request(this, future.request_id)`. + */ + bool + remove_pending_request(const SharedFutureWithRequestAndRequestId & future) + { + return this->remove_pending_request(future.request_id); + } - return future_with_request; + /// Clean all pending requests. + /** + * \return number of pending requests that were removed. + */ + size_t + prune_pending_requests() + { + std::lock_guard guard(pending_requests_mutex_); + auto ret = pending_requests_.size(); + pending_requests_.clear(); + return ret; + } + + /// Clean all pending requests older than a time_point. + /** + * \param[in] time_point Requests that were sent before this point are going to be removed. + * \param[inout] pruned_requests Removed requests id will be pushed to the vector + * if a pointer is provided. + * \return number of pending requests that were removed. + */ + template> + size_t + prune_requests_older_than( + std::chrono::time_point time_point, + std::vector * pruned_requests = nullptr) + { + std::lock_guard guard(pending_requests_mutex_); + auto old_size = pending_requests_.size(); + for (auto it = pending_requests_.begin(), last = pending_requests_.end(); it != last; ) { + if (it->second.first < time_point) { + pruned_requests->push_back(it->first); + it = pending_requests_.erase(it); + } else { + ++it; + } + } + return old_size - pending_requests_.size(); + } + +protected: + using CallbackTypeValueVariant = std::tuple; + using CallbackWithRequestTypeValueVariant = std::tuple< + CallbackWithRequestType, SharedRequest, SharedFutureWithRequest, PromiseWithRequest>; + + using CallbackInfoVariant = std::variant< + std::promise, + CallbackTypeValueVariant, + CallbackWithRequestTypeValueVariant>; + + int64_t + async_send_request_impl(const Request & request, CallbackInfoVariant value) + { + int64_t sequence_number; + rcl_ret_t ret = rcl_send_request(get_client_handle().get(), &request, &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request"); + } + { + std::lock_guard lock(pending_requests_mutex_); + pending_requests_.try_emplace( + sequence_number, + std::make_pair(std::chrono::system_clock::now(), std::move(value))); + } + return sequence_number; + } + + std::optional + get_and_erase_pending_request(int64_t request_number) + { + std::unique_lock lock(pending_requests_mutex_); + auto it = this->pending_requests_.find(request_number); + if (it == this->pending_requests_.end()) { + RCUTILS_LOG_DEBUG_NAMED( + "rclcpp", + "Received invalid sequence number. Ignoring..."); + return std::nullopt; + } + auto value = std::move(it->second.second); + this->pending_requests_.erase(request_number); + return value; } -private: RCLCPP_DISABLE_COPY(Client) - std::map> pending_requests_; + std::unordered_map< + int64_t, + std::pair< + std::chrono::time_point, + CallbackInfoVariant>> + pending_requests_; std::mutex pending_requests_mutex_; }; diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp index ea64340f03..5e2adb5e7d 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -216,7 +216,7 @@ class TestClientWithServer : public ::testing::Test received_response = true; }; - auto future = client->async_send_request(request, std::move(callback)); + auto req_id = client->async_send_request(request, std::move(callback)); auto start = std::chrono::steady_clock::now(); while (!received_response && @@ -228,6 +228,9 @@ class TestClientWithServer : public ::testing::Test if (!received_response) { return ::testing::AssertionFailure() << "Waiting for response timed out"; } + if (client->remove_pending_request(req_id)) { + return ::testing::AssertionFailure() << "Should not be able to remove a finished request"; + } return request_result; } @@ -256,7 +259,7 @@ TEST_F(TestClientWithServer, async_send_request_callback_with_request) { EXPECT_NE(nullptr, request_response_pair.second); received_response = true; }; - auto future = client->async_send_request(request, std::move(callback)); + auto req_id = client->async_send_request(request, std::move(callback)); auto start = std::chrono::steady_clock::now(); while (!received_response && @@ -265,6 +268,15 @@ TEST_F(TestClientWithServer, async_send_request_callback_with_request) { rclcpp::spin_some(node); } EXPECT_TRUE(received_response); + EXPECT_FALSE(client->remove_pending_request(req_id)); +} + +TEST_F(TestClientWithServer, test_client_remove_pending_request) { + auto client = node->create_client("no_service_server_available_here"); + auto request = std::make_shared(); + auto future = client->async_send_request(request); + + EXPECT_TRUE(client->remove_pending_request(future)); } TEST_F(TestClientWithServer, async_send_request_rcl_send_request_error) { diff --git a/rclcpp_components/test/test_component_manager_api.cpp b/rclcpp_components/test/test_component_manager_api.cpp index 73d4bc3687..84b11fe7e9 100644 --- a/rclcpp_components/test/test_component_manager_api.cpp +++ b/rclcpp_components/test/test_component_manager_api.cpp @@ -57,13 +57,14 @@ TEST_F(TestComponentManager, components_api) request->package_name = "rclcpp_components"; request->plugin_name = "test_rclcpp_components::TestComponentFoo"; - auto result = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); - EXPECT_EQ(result.get()->success, true); - EXPECT_EQ(result.get()->error_message, ""); - EXPECT_EQ(result.get()->full_node_name, "/test_component_foo"); - EXPECT_EQ(result.get()->unique_id, 1u); + EXPECT_EQ(result->success, true); + EXPECT_EQ(result->error_message, ""); + EXPECT_EQ(result->full_node_name, "/test_component_foo"); + EXPECT_EQ(result->unique_id, 1u); } { @@ -71,13 +72,14 @@ TEST_F(TestComponentManager, components_api) request->package_name = "rclcpp_components"; request->plugin_name = "test_rclcpp_components::TestComponentBar"; - auto result = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); - EXPECT_EQ(result.get()->success, true); - EXPECT_EQ(result.get()->error_message, ""); - EXPECT_EQ(result.get()->full_node_name, "/test_component_bar"); - EXPECT_EQ(result.get()->unique_id, 2u); + EXPECT_EQ(result->success, true); + EXPECT_EQ(result->error_message, ""); + EXPECT_EQ(result->full_node_name, "/test_component_bar"); + EXPECT_EQ(result->unique_id, 2u); } // Test remapping the node name @@ -87,13 +89,14 @@ TEST_F(TestComponentManager, components_api) request->plugin_name = "test_rclcpp_components::TestComponentFoo"; request->node_name = "test_component_baz"; - auto result = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); - EXPECT_EQ(result.get()->success, true); - EXPECT_EQ(result.get()->error_message, ""); - EXPECT_EQ(result.get()->full_node_name, "/test_component_baz"); - EXPECT_EQ(result.get()->unique_id, 3u); + EXPECT_EQ(result->success, true); + EXPECT_EQ(result->error_message, ""); + EXPECT_EQ(result->full_node_name, "/test_component_baz"); + EXPECT_EQ(result->unique_id, 3u); } // Test remapping the node namespace @@ -104,13 +107,14 @@ TEST_F(TestComponentManager, components_api) request->node_namespace = "/ns"; request->node_name = "test_component_bing"; - auto result = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); - EXPECT_EQ(result.get()->success, true); - EXPECT_EQ(result.get()->error_message, ""); - EXPECT_EQ(result.get()->full_node_name, "/ns/test_component_bing"); - EXPECT_EQ(result.get()->unique_id, 4u); + EXPECT_EQ(result->success, true); + EXPECT_EQ(result->error_message, ""); + EXPECT_EQ(result->full_node_name, "/ns/test_component_bing"); + EXPECT_EQ(result->unique_id, 4u); } { @@ -119,13 +123,14 @@ TEST_F(TestComponentManager, components_api) request->package_name = "rclcpp_components"; request->plugin_name = "test_rclcpp_components::TestComponent"; - auto result = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); - EXPECT_EQ(result.get()->success, false); - EXPECT_EQ(result.get()->error_message, "Failed to find class with the requested plugin name."); - EXPECT_EQ(result.get()->full_node_name, ""); - EXPECT_EQ(result.get()->unique_id, 0u); + auto result = future.get(); + EXPECT_EQ(result->success, false); + EXPECT_EQ(result->error_message, "Failed to find class with the requested plugin name."); + EXPECT_EQ(result->full_node_name, ""); + EXPECT_EQ(result->unique_id, 0u); } { @@ -134,13 +139,14 @@ TEST_F(TestComponentManager, components_api) request->package_name = "rclcpp_components_foo"; request->plugin_name = "test_rclcpp_components::TestComponentFoo"; - auto result = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); - EXPECT_EQ(result.get()->success, false); - EXPECT_EQ(result.get()->error_message, "Could not find requested resource in ament index"); - EXPECT_EQ(result.get()->full_node_name, ""); - EXPECT_EQ(result.get()->unique_id, 0u); + auto result = future.get(); + EXPECT_EQ(result->success, false); + EXPECT_EQ(result->error_message, "Could not find requested resource in ament index"); + EXPECT_EQ(result->full_node_name, ""); + EXPECT_EQ(result->unique_id, 0u); } { @@ -151,13 +157,14 @@ TEST_F(TestComponentManager, components_api) request->node_name = "test_component_remap"; request->remap_rules.push_back("alice:=bob"); - auto result = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); - EXPECT_EQ(result.get()->success, true); - EXPECT_EQ(result.get()->error_message, ""); - EXPECT_EQ(result.get()->full_node_name, "/test_component_remap"); - EXPECT_EQ(result.get()->unique_id, 5u); + EXPECT_EQ(result->success, true); + EXPECT_EQ(result->error_message, ""); + EXPECT_EQ(result->full_node_name, "/test_component_remap"); + EXPECT_EQ(result->unique_id, 5u); } { @@ -170,14 +177,15 @@ TEST_F(TestComponentManager, components_api) rclcpp::ParameterValue(true)); request->extra_arguments.push_back(use_intraprocess_comms.to_parameter_msg()); - auto result = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); - EXPECT_EQ(result.get()->success, true); - EXPECT_EQ(result.get()->error_message, ""); - std::cout << result.get()->full_node_name << std::endl; - EXPECT_EQ(result.get()->full_node_name, "/test_component_intra_process"); - EXPECT_EQ(result.get()->unique_id, 6u); + EXPECT_EQ(result->success, true); + EXPECT_EQ(result->error_message, ""); + std::cout << result->full_node_name << std::endl; + EXPECT_EQ(result->full_node_name, "/test_component_intra_process"); + EXPECT_EQ(result->unique_id, 6u); } { @@ -191,15 +199,16 @@ TEST_F(TestComponentManager, components_api) rclcpp::ParameterValue("hello")); request->extra_arguments.push_back(use_intraprocess_comms.to_parameter_msg()); - auto result = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); - EXPECT_EQ(result.get()->success, false); + EXPECT_EQ(result->success, false); EXPECT_EQ( - result.get()->error_message, + result->error_message, "Extra component argument 'use_intra_process_comms' must be a boolean"); - EXPECT_EQ(result.get()->full_node_name, ""); - EXPECT_EQ(result.get()->unique_id, 0u); + EXPECT_EQ(result->full_node_name, ""); + EXPECT_EQ(result->unique_id, 0u); } auto node_names = node->get_node_names(); @@ -223,11 +232,12 @@ TEST_F(TestComponentManager, components_api) { auto request = std::make_shared(); - auto result = client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + auto future = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); - auto result_node_names = result.get()->full_node_names; - auto result_unique_ids = result.get()->unique_ids; + auto result = future.get(); + auto result_node_names = result->full_node_names; + auto result_unique_ids = result->unique_ids; EXPECT_EQ(result_node_names.size(), 6u); EXPECT_EQ(result_node_names[0], "/test_component_foo"); @@ -258,22 +268,24 @@ TEST_F(TestComponentManager, components_api) auto request = std::make_shared(); request->unique_id = 1; - auto result = client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + auto future = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); - EXPECT_EQ(result.get()->success, true); - EXPECT_EQ(result.get()->error_message, ""); + EXPECT_EQ(result->success, true); + EXPECT_EQ(result->error_message, ""); } { auto request = std::make_shared(); request->unique_id = 1; - auto result = client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + auto future = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); - EXPECT_EQ(result.get()->success, false); - EXPECT_EQ(result.get()->error_message, "No node found with unique_id: 1"); + EXPECT_EQ(result->success, false); + EXPECT_EQ(result->error_message, "No node found with unique_id: 1"); } } @@ -287,11 +299,12 @@ TEST_F(TestComponentManager, components_api) { auto request = std::make_shared(); - auto result = client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + auto future = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); - auto result_node_names = result.get()->full_node_names; - auto result_unique_ids = result.get()->unique_ids; + auto result = future.get(); + auto result_node_names = result->full_node_names; + auto result_unique_ids = result->unique_ids; EXPECT_EQ(result_node_names.size(), 5u); EXPECT_EQ(result_node_names[0], "/test_component_bar"); diff --git a/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp index d7188ca350..6dd2c0ec17 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp @@ -98,8 +98,9 @@ class LifecycleServiceClient : public rclcpp::Node return unknown_state; } - if (future_result.get()) { - return future_result.get()->current_state; + auto result = future_result.get(); + if (result) { + return result->current_state; } else { return unknown_state; } @@ -140,9 +141,9 @@ class LifecycleServiceClient : public rclcpp::Node if (future_status != std::future_status::ready) { return std::vector(); } - - if (future_result.get()) { - return future_result.get()->available_states; + auto result = future_result.get(); + if (result) { + return result->available_states; } return std::vector(); @@ -164,8 +165,9 @@ class LifecycleServiceClient : public rclcpp::Node return std::vector(); } - if (future_result.get()) { - return future_result.get()->available_transitions; + auto result = future_result.get(); + if (result) { + return result->available_transitions; } return std::vector(); @@ -187,8 +189,9 @@ class LifecycleServiceClient : public rclcpp::Node return std::vector(); } - if (future_result.get()) { - return future_result.get()->available_transitions; + auto result = future_result.get(); + if (result) { + return result->available_transitions; } return std::vector(); From 2dd09ae274ad5073cf875d98ccc2c97aa26f2d41 Mon Sep 17 00:00:00 2001 From: Ahmed Sobhy <68698147+asobhy-qnx@users.noreply.github.com> Date: Mon, 23 Aug 2021 10:19:05 -0400 Subject: [PATCH 009/455] remove can_be_nullptr assignment check for QNX case (#1752) Signed-off-by: Ahmed Sobhy --- rclcpp/include/rclcpp/any_service_callback.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp/include/rclcpp/any_service_callback.hpp index 56c07d4c66..10aef7d0ee 100644 --- a/rclcpp/include/rclcpp/any_service_callback.hpp +++ b/rclcpp/include/rclcpp/any_service_callback.hpp @@ -40,9 +40,14 @@ struct can_be_nullptr : std::false_type {}; // but we see a warning that they can never be null when using it. // We also test if `T &` can be assigned to `nullptr` to avoid the issue. template +#ifdef __QNXNTO__ +struct can_be_nullptr() == nullptr)>>: std::true_type {}; +#else struct can_be_nullptr() == nullptr), decltype(std::declval() = nullptr)>> : std::true_type {}; +#endif } // namespace detail // Forward declare From fd08f0dbe75abfe04b9f09160396b736f1677c01 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 23 Aug 2021 17:45:06 +0000 Subject: [PATCH 010/455] Changelogs Signed-off-by: Ivan Santiago Paunovic --- rclcpp/CHANGELOG.rst | 14 ++++++++++++++ rclcpp_action/CHANGELOG.rst | 5 +++++ rclcpp_components/CHANGELOG.rst | 5 +++++ rclcpp_lifecycle/CHANGELOG.rst | 8 ++++++++ 4 files changed, 32 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 350624dc87..bd49bd09c7 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove can_be_nullptr assignment check for QNX case. (`#1752 `_) +* Update client API to be able to remove pending requests. (`#1734 `_) +* Fix: Allow to add a node while spinning in the StaticSingleThreadedExecutor. (`#1690 `_) +* Add tracing instrumentation for executor and message taking. (`#1738 `_) +* Fix: Reset timer trigger time before execute in StaticSingleThreadedExecutor. (`#1739 `_) +* Use FindPython3 and make python3 dependency explicit. (`#1745 `_) +* Use rosidl_get_typesupport_target(). (`#1729 `_) +* Fix returning invalid namespace if sub_namespace is empty. (`#1658 `_) +* Add free function to wait for a subscription message. (`#1705 `_) +* Use rcpputils/scope_exit.hpp and remove rclcpp/scope_exit.hpp. (`#1727 `_) +* Contributors: Ahmed Sobhy, Christophe Bedard, Ivan Santiago Paunovic, Karsten Knese, M. Hofstätter, Mauro Passerino, Shane Loretz, mauropasse + 12.0.0 (2021-07-26) ------------------- * Remove unsafe get_callback_groups API. diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 76b806d276..999960ef70 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use rcpputils/scope_exit.hpp and remove rclcpp/scope_exit.hpp. (`#1727 `_) +* Contributors: Christophe Bedard + 12.0.0 (2021-07-26) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 3fd10b6d81..895ce49b50 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update client API to be able to remove pending requests. (`#1734 `_) +* Contributors: Ivan Santiago Paunovic + 12.0.0 (2021-07-26) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 7a46abb7b5..35be70563b 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,14 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update client API to be able to remove pending requests. (`#1734 `_) +* Change log level for lifecycle_publisher. (`#1715 `_) +* Fix: RCLCPP_PUBLIC -> RCLCPP_LIFECYCLE_PUBLIC (`#1732 `_) +* Use rcpputils/scope_exit.hpp and remove rclcpp/scope_exit.hpp (`#1727 `_) +* Contributors: Alberto Soragna, Christophe Bedard, Ivan Santiago Paunovic, Shane Loretz + 12.0.0 (2021-07-26) ------------------- * Remove unsafe get_callback_groups API. From d0cd6bb0a46f5b2841e9f282c9eecfa8cffe1ffa Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 23 Aug 2021 17:46:42 +0000 Subject: [PATCH 011/455] 13.0.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index bd49bd09c7..ac6812e882 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +13.0.0 (2021-08-23) +------------------- * Remove can_be_nullptr assignment check for QNX case. (`#1752 `_) * Update client API to be able to remove pending requests. (`#1734 `_) * Fix: Allow to add a node while spinning in the StaticSingleThreadedExecutor. (`#1690 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index ae5228e6df..99783768e9 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 12.0.0 + 13.0.0 The ROS client library in C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 999960ef70..8dd1ea0398 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +13.0.0 (2021-08-23) +------------------- * Use rcpputils/scope_exit.hpp and remove rclcpp/scope_exit.hpp. (`#1727 `_) * Contributors: Christophe Bedard diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 21a210296c..2ca049b77d 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 12.0.0 + 13.0.0 Adds action APIs for C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 895ce49b50..ee0af41ec4 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +13.0.0 (2021-08-23) +------------------- * Update client API to be able to remove pending requests. (`#1734 `_) * Contributors: Ivan Santiago Paunovic diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 03613b8e0c..1dfdc05e55 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 12.0.0 + 13.0.0 Package containing tools for dynamically loadable components Ivan Paunovic Mabel Zhang diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 35be70563b..954b09fec4 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +13.0.0 (2021-08-23) +------------------- * Update client API to be able to remove pending requests. (`#1734 `_) * Change log level for lifecycle_publisher. (`#1715 `_) * Fix: RCLCPP_PUBLIC -> RCLCPP_LIFECYCLE_PUBLIC (`#1732 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 9eea76121d..8c5107ce2d 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 12.0.0 + 13.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Mabel Zhang From ecb81ef2c35a3e605b7c501ace4ef1baf4755bf9 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Thu, 26 Aug 2021 18:33:03 -0400 Subject: [PATCH 012/455] Deprecate the `void shared_ptr` subscription callback signatures (#1713) * Deprecated `shared_ptr` sub callbacks Addresses #1619. Signed-off-by: Abrar Rahman Protyasha * Resolve deprecated subscription callbacks in tests Specifically, `void shared_ptr` subscription callbacks have been migrated to `void shared_ptr` subscription callbacks. This change has been performed only on the test files that do not actually house unit tests for the `AnySubscriptionCallback` class. For unit tests that actually target the deprecated `set` functions, the deprecation warnings have to be avoided. This patch will be introduced in a separate commit. Signed-off-by: Abrar Rahman Protyasha * Suppress deprecation warnings in unit tests This commit specifically introduces suppression of the deprecation warnings produced while compiling unit tests for the `AnySubscriptionCallback` class. The macro mechanics to conditionally include the `deprecated` attribute is not ideal, but the diagnostic pragma solution (`# pragma GCC diagnostic ignored`) did not work for these unit tests, possibly because of the way gtest is initializing the necessary `InstanceContext` objects. A `TODO` directive has been left to figure out a better way to address this warning suppression. Signed-off-by: Abrar Rahman Protyasha * Fix shared ptr callback in wait_for_message Moving away from deprecated signatures. Signed-off-by: Abrar Rahman Protyasha * `rclcpp_action`: Fix deprecated subscr. callbacks Signed-off-by: Abrar Rahman Protyasha * `rclcpp_lifecycle`: Fix deprecated sub callbacks Signed-off-by: Abrar Rahman Protyasha --- .../rclcpp/any_subscription_callback.hpp | 18 +++++++------- rclcpp/include/rclcpp/wait_for_message.hpp | 2 +- .../rclcpp/test_any_subscription_callback.cpp | 2 ++ rclcpp/test/rclcpp/test_generic_pubsub.cpp | 2 +- .../test_publisher_with_type_adapter.cpp | 2 +- rclcpp/test/rclcpp/test_subscription.cpp | 2 +- rclcpp_action/test/test_server.cpp | 24 +++++++++---------- rclcpp_lifecycle/test/test_lifecycle_node.cpp | 2 +- 8 files changed, 29 insertions(+), 25 deletions(-) diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index bb45d6a07d..62e116af56 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -406,10 +406,10 @@ class AnySubscriptionCallback /// Function for shared_ptr to non-const MessageT, which is deprecated. template - // TODO(wjwwood): enable this deprecation after Galactic - // [[deprecated( - // "use 'void (std::shared_ptr)' instead" - // )]] +#if !defined(RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS) + // suppress deprecation warnings in `test_any_subscription_callback.cpp` + [[deprecated("use 'void(std::shared_ptr)' instead")]] +#endif void set_deprecated(std::function)> callback) { @@ -418,10 +418,12 @@ class AnySubscriptionCallback /// Function for shared_ptr to non-const MessageT with MessageInfo, which is deprecated. template - // TODO(wjwwood): enable this deprecation after Galactic - // [[deprecated( - // "use 'void (std::shared_ptr, const rclcpp::MessageInfo &)' instead" - // )]] +#if !defined(RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS) + // suppress deprecation warnings in `test_any_subscription_callback.cpp` + [[deprecated( + "use 'void(std::shared_ptr, const rclcpp::MessageInfo &)' instead" + )]] +#endif void set_deprecated(std::function, const rclcpp::MessageInfo &)> callback) { diff --git a/rclcpp/include/rclcpp/wait_for_message.hpp b/rclcpp/include/rclcpp/wait_for_message.hpp index f5f3adc9a4..20c9df4db6 100644 --- a/rclcpp/include/rclcpp/wait_for_message.hpp +++ b/rclcpp/include/rclcpp/wait_for_message.hpp @@ -90,7 +90,7 @@ bool wait_for_message( const std::string & topic, std::chrono::duration time_to_wait = std::chrono::duration(-1)) { - auto sub = node->create_subscription(topic, 1, [](const std::shared_ptr) {}); + auto sub = node->create_subscription(topic, 1, [](const std::shared_ptr) {}); return wait_for_message( out, sub, node->get_node_options().context(), time_to_wait); } diff --git a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp index 8e44eb71bf..32abdf2934 100644 --- a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp +++ b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp @@ -19,6 +19,8 @@ #include #include +// TODO(aprotyas): Figure out better way to suppress deprecation warnings. +#define RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS 1 #include "rclcpp/any_subscription_callback.hpp" #include "test_msgs/msg/empty.hpp" #include "test_msgs/msg/empty.h" diff --git a/rclcpp/test/rclcpp/test_generic_pubsub.cpp b/rclcpp/test/rclcpp/test_generic_pubsub.cpp index 71a60a77fb..ff34ec52b3 100644 --- a/rclcpp/test/rclcpp/test_generic_pubsub.cpp +++ b/rclcpp/test/rclcpp/test_generic_pubsub.cpp @@ -187,7 +187,7 @@ TEST_F(RclcppGenericNodeFixture, generic_publisher_uses_qos) auto publisher = node_->create_generic_publisher(topic_name, topic_type, qos); auto subscription = node_->create_subscription( topic_name, qos, - [](std::shared_ptr/* message */) {}); + [](std::shared_ptr/* message */) {}); auto connected = [publisher, subscription]() -> bool { return publisher->get_subscription_count() && subscription->get_publisher_count(); }; diff --git a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp index 2f5d540568..f5ab43cb7c 100644 --- a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp @@ -191,7 +191,7 @@ TEST_F( auto callback = [message_data, &is_received]( - const rclcpp::msg::String::SharedPtr msg, + const rclcpp::msg::String::ConstSharedPtr msg, const rclcpp::MessageInfo & message_info ) -> void { diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index 2895865c1f..5812985272 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -510,7 +510,7 @@ INSTANTIATE_TEST_SUITE_P( TEST_F(TestSubscription, get_network_flow_endpoints_errors) { initialize(); const rclcpp::QoS subscription_qos(1); - auto subscription_callback = [](test_msgs::msg::Empty::SharedPtr msg) { + auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr msg) { (void)msg; }; auto subscription = node->create_subscription( diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 6f8d874359..7030243c77 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -485,10 +485,10 @@ TEST_F(TestServer, publish_status_accepted) (void)as; // Subscribe to status messages - std::vector received_msgs; + std::vector received_msgs; auto subscriber = node->create_subscription( "fibonacci/_action/status", 10, - [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + [&received_msgs](action_msgs::msg::GoalStatusArray::ConstSharedPtr list) { received_msgs.push_back(list); }); @@ -548,10 +548,10 @@ TEST_F(TestServer, publish_status_canceling) (void)as; // Subscribe to status messages - std::vector received_msgs; + std::vector received_msgs; auto subscriber = node->create_subscription( "fibonacci/_action/status", 10, - [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + [&received_msgs](action_msgs::msg::GoalStatusArray::ConstSharedPtr list) { received_msgs.push_back(list); }); @@ -605,10 +605,10 @@ TEST_F(TestServer, publish_status_canceled) (void)as; // Subscribe to status messages - std::vector received_msgs; + std::vector received_msgs; auto subscriber = node->create_subscription( "fibonacci/_action/status", 10, - [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + [&received_msgs](action_msgs::msg::GoalStatusArray::ConstSharedPtr list) { received_msgs.push_back(list); }); @@ -664,10 +664,10 @@ TEST_F(TestServer, publish_status_succeeded) (void)as; // Subscribe to status messages - std::vector received_msgs; + std::vector received_msgs; auto subscriber = node->create_subscription( "fibonacci/_action/status", 10, - [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + [&received_msgs](action_msgs::msg::GoalStatusArray::ConstSharedPtr list) { received_msgs.push_back(list); }); @@ -721,10 +721,10 @@ TEST_F(TestServer, publish_status_aborted) (void)as; // Subscribe to status messages - std::vector received_msgs; + std::vector received_msgs; auto subscriber = node->create_subscription( "fibonacci/_action/status", 10, - [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + [&received_msgs](action_msgs::msg::GoalStatusArray::ConstSharedPtr list) { received_msgs.push_back(list); }); @@ -779,9 +779,9 @@ TEST_F(TestServer, publish_feedback) // Subscribe to feedback messages using FeedbackT = Fibonacci::Impl::FeedbackMessage; - std::vector received_msgs; + std::vector received_msgs; auto subscriber = node->create_subscription( - "fibonacci/_action/feedback", 10, [&received_msgs](FeedbackT::SharedPtr msg) + "fibonacci/_action/feedback", 10, [&received_msgs](FeedbackT::ConstSharedPtr msg) { received_msgs.push_back(msg); }); diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 7ee883a5f0..e1863a4d39 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -416,7 +416,7 @@ TEST_F(TestDefaultStateMachine, bad_mood) { TEST_F(TestDefaultStateMachine, lifecycle_subscriber) { auto test_node = std::make_shared>("testnode"); - auto cb = [](const std::shared_ptr msg) {(void) msg;}; + auto cb = [](const std::shared_ptr msg) {(void) msg;}; auto lifecycle_sub = test_node->create_subscription("~/empty", 10, cb); From 665e37784a6f1772e0efd2685c10fb0c17fdac96 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 3 Sep 2021 08:49:57 -0400 Subject: [PATCH 013/455] Make sure to check for empty extension before accessing front. (#1764) The documentation for std::string::front() says that calling it on an empty string is undefined behavior. It actually seems to work on all platforms except Windows Debug, where it throws an error. Make sure to check for empty first. We also notice that there is no reason to check the existing_sub_namespace for empty; the rest of the code handles that fine. So remove that check. Finally, we add an anonymous namespace so that these functions do not pollute the global namespace. Signed-off-by: Chris Lalancette --- rclcpp/src/rclcpp/node.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 46f1af12df..5d1f9a3077 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -47,6 +47,9 @@ using rclcpp::Node; using rclcpp::NodeOptions; using rclcpp::exceptions::throw_from_rcl_error; +namespace +{ + RCLCPP_LOCAL std::string extend_sub_namespace(const std::string & existing_sub_namespace, const std::string & extension) @@ -54,18 +57,18 @@ extend_sub_namespace(const std::string & existing_sub_namespace, const std::stri // Assumption is that the existing_sub_namespace does not need checking // because it would be checked already when it was set with this function. - // check if the new sub-namespace extension is absolute - if (extension.front() == '/') { + if (extension.empty()) { throw rclcpp::exceptions::NameValidationError( "sub_namespace", extension.c_str(), - "a sub-namespace should not have a leading /", + "sub-nodes should not extend nodes by an empty sub-namespace", 0); - } else if (existing_sub_namespace.empty() && extension.empty()) { + } else if (extension.front() == '/') { + // check if the new sub-namespace extension is absolute throw rclcpp::exceptions::NameValidationError( "sub_namespace", extension.c_str(), - "sub-nodes should not extend nodes by an empty sub-namespace", + "a sub-namespace should not have a leading /", 0); } @@ -76,7 +79,7 @@ extend_sub_namespace(const std::string & existing_sub_namespace, const std::stri new_sub_namespace = existing_sub_namespace + "/" + extension; } - // remove any trailing `/` so that new extensions do no result in `//` + // remove any trailing `/` so that new extensions do not result in `//` if (new_sub_namespace.back() == '/') { new_sub_namespace = new_sub_namespace.substr(0, new_sub_namespace.size() - 1); } @@ -104,6 +107,8 @@ create_effective_namespace(const std::string & node_namespace, const std::string } } +} // namespace + Node::Node( const std::string & node_name, const NodeOptions & options) From 001f0fb620f544c78b0645919ee55f99d175f000 Mon Sep 17 00:00:00 2001 From: Yong-Hao Zou Date: Mon, 13 Sep 2021 21:25:06 +0800 Subject: [PATCH 014/455] Replace recursion with do-while (#1765) to avoid potential stack-overflow. Signed-off-by: zouyonghao --- rclcpp/src/rclcpp/context.cpp | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 580689d694..b86fe8eafc 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -402,16 +402,15 @@ bool Context::sleep_for(const std::chrono::nanoseconds & nanoseconds) { std::chrono::nanoseconds time_left = nanoseconds; - { - std::unique_lock lock(interrupt_mutex_); - auto start = std::chrono::steady_clock::now(); - // this will release the lock while waiting - interrupt_condition_variable_.wait_for(lock, nanoseconds); - time_left -= std::chrono::steady_clock::now() - start; - } - if (time_left > std::chrono::nanoseconds::zero() && this->is_valid()) { - return sleep_for(time_left); - } + do { + { + std::unique_lock lock(interrupt_mutex_); + auto start = std::chrono::steady_clock::now(); + // this will release the lock while waiting + interrupt_condition_variable_.wait_for(lock, nanoseconds); + time_left -= std::chrono::steady_clock::now() - start; + } + } while (time_left > std::chrono::nanoseconds::zero() && this->is_valid()); // Return true if the timeout elapsed successfully, otherwise false. return this->is_valid(); } From 2801553d61c5a30a0327d5cbc8d28bcd74e9703d Mon Sep 17 00:00:00 2001 From: livanov93 Date: Tue, 14 Sep 2021 02:36:11 +0200 Subject: [PATCH 015/455] Improve documentation for auto declaration from overrides (#1751) * Improve documentation for auto declaration from overrides. Signed-off-by: voodoo * Fix spelling. Signed-off-by: voodoo * Extend description. Signed-off-by: voodoo --- rclcpp/include/rclcpp/node_options.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rclcpp/include/rclcpp/node_options.hpp b/rclcpp/include/rclcpp/node_options.hpp index 73a2c701af..d9735357dd 100644 --- a/rclcpp/include/rclcpp/node_options.hpp +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -349,6 +349,9 @@ class NodeOptions * global arguments (e.g. parameter overrides from a YAML file), which are * not explicitly declared will not appear on the node at all, even if * `allow_undeclared_parameters` is true. + * Parameter declaration from overrides is done in the node's base constructor, + * so the user must take care to check if the parameter is already (e.g. + * automatically) declared before declaring it themselves. * Already declared parameters will not be re-declared, and parameters * declared in this way will use the default constructed ParameterDescriptor. */ From f3a51877758f3bd5a8300b1cfb6d3ad85cf7257e Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Thu, 23 Sep 2021 13:45:58 -0700 Subject: [PATCH 016/455] NodeGraph exposure with test cases (#1484) * add NodeGraph::get_node_names_and_namespaces() Signed-off-by: Tomoya.Fujita * add test for NodeGraph::get_node_names_and_namespaces Signed-off-by: Tomoya.Fujita * add NodeGraph::get_client_names_and_types_by_node() Signed-off-by: Tomoya.Fujita * add test for NodeGraph::get_client_names_and_types_by_node Signed-off-by: Tomoya.Fujita * add NodeGraph::get_publisher/subscriber_names_and_types_by_node() Signed-off-by: Tomoya.Fujita * add test for NodeGraph::get_publisher/subscriber_names_and_types_by_node Signed-off-by: Tomoya.Fujita * fix comments. Signed-off-by: Tomoya.Fujita * use make_scope_exit and update comments. Signed-off-by: Tomoya Fujita * comment fix. Signed-off-by: Tomoya Fujita * minor fixes. Signed-off-by: Tomoya Fujita * add comments for enclaves. Signed-off-by: Tomoya Fujita * add URL about ROS 2 Security Enclaves. Signed-off-by: Tomoya Fujita * Deprecated `shared_ptr` sub callbacks Signed-off-by: Tomoya Fujita --- .../rclcpp/node_interfaces/node_graph.hpp | 25 ++ .../node_interfaces/node_graph_interface.hpp | 84 ++++++- .../src/rclcpp/node_interfaces/node_graph.cpp | 187 +++++++++++++- .../node_interfaces/test_node_graph.cpp | 232 +++++++++++++++++- 4 files changed, 521 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp index 4036b1b754..794038d386 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -70,10 +71,34 @@ class NodeGraph : public NodeGraphInterface const std::string & node_name, const std::string & namespace_) const override; + RCLCPP_PUBLIC + std::map> + get_client_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const override; + + RCLCPP_PUBLIC + std::map> + get_publisher_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_, + bool no_demangle = false) const override; + + RCLCPP_PUBLIC + std::map> + get_subscriber_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_, + bool no_demangle = false) const override; + RCLCPP_PUBLIC std::vector get_node_names() const override; + RCLCPP_PUBLIC + std::vector> + get_node_names_with_enclaves() const override; + RCLCPP_PUBLIC std::vector> get_node_names_and_namespaces() const override; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp index 2234c91826..243c22cf5e 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -146,7 +147,9 @@ class NodeGraphInterface /** * A topic is considered to exist when at least one publisher or subscriber * exists for it, whether they be local or remote to this process. - * The returned names are the actual names used and do not have remap rules applied. + * The returned names are the actual names of the topics, either announced by another nodes or by this one. + * Attempting to create publishers or subscribers using names returned by this function may not + * result in the desired topic name being used depending on the remap rules in use. * * \param[in] no_demangle if true, topic names and types are not demangled */ @@ -160,7 +163,9 @@ class NodeGraphInterface * A service is considered to exist when at least one service server or * service client exists for it, whether they be local or remote to this * process. - * The returned names are the actual names used and do not have remap rules applied. + * The returned names are the actual names of the services, either announced by another nodes or by this one. + * Attempting to create clients or services using names returned by this function may not result in + * the desired service name being used depending on the remap rules in use. */ RCLCPP_PUBLIC virtual @@ -170,7 +175,9 @@ class NodeGraphInterface /// Return a map of existing service names to list of service types for a specific node. /** * This function only considers services - not clients. - * The returned names are the actual names used and do not have remap rules applied. + * The returned names are the actual names after remap rules applied. + * Attempting to create service clients using names returned by this function may not + * result in the desired service name being used depending on the remap rules in use. * * \param[in] node_name name of the node * \param[in] namespace_ namespace of the node @@ -182,18 +189,85 @@ class NodeGraphInterface const std::string & node_name, const std::string & namespace_) const = 0; + /// Return a map of existing service names and types with a specific node. + /** + * This function only considers clients - not service servers. + * The returned names are the actual names after remap rules applied. + * Attempting to create service servers using names returned by this function may not + * result in the desired service name being used depending on the remap rules in use. + * + * \param[in] node_name name of the node + * \param[in] namespace_ namespace of the node + */ + RCLCPP_PUBLIC + virtual + std::map> + get_client_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const = 0; + + /// Return a map of existing topic names to list of topic types for a specific node. + /** + * This function only considers publishers - not subscribers. + * The returned names are the actual names after remap rules applied. + * Attempting to create publishers or subscribers using names returned by this function may not + * result in the desired topic name being used depending on the remap rules in use. + * + * \param[in] node_name name of the node + * \param[in] namespace_ namespace of the node + * \param[in] no_demangle if true, topic names and types are not demangled + */ + RCLCPP_PUBLIC + virtual + std::map> + get_publisher_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_, + bool no_demangle = false) const = 0; + + /// Return a map of existing topic names to list of topic types for a specific node. + /** + * This function only considers subscribers - not publishers. + * The returned names are the actual names after remap rules applied. + * Attempting to create publishers or subscribers using names returned by this function may not + * result in the desired topic name being used depending on the remap rules in use. + * + * \param[in] node_name name of the node + * \param[in] namespace_ namespace of the node + * \param[in] no_demangle if true, topic names and types are not demangled + */ + RCLCPP_PUBLIC + virtual + std::map> + get_subscriber_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_, + bool no_demangle = false) const = 0; + /// Return a vector of existing node names (string). /* - * The returned names are the actual names used and do not have remap rules applied. + * The returned names are the actual names after remap rules applied. */ RCLCPP_PUBLIC virtual std::vector get_node_names() const = 0; + /// Return a vector of existing node names, namespaces and enclaves (tuple of string). + /* + * The returned names are the actual names after remap rules applied. + * The enclaves contain the runtime security artifacts, those can be + * used to establish secured network. + * See https://design.ros2.org/articles/ros2_security_enclaves.html + */ + RCLCPP_PUBLIC + virtual + std::vector> + get_node_names_with_enclaves() const = 0; + /// Return a vector of existing node names and namespaces (pair of string). /* - * The returned names are the actual names used and do not have remap rules applied. + * The returned names are the actual names after remap rules applied. */ RCLCPP_PUBLIC virtual diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index 415b6277ec..c993a4ed6f 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -27,6 +28,7 @@ #include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/graph_listener.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rcpputils/scope_exit.hpp" using rclcpp::node_interfaces::NodeGraph; using rclcpp::exceptions::throw_from_rcl_error; @@ -178,6 +180,115 @@ NodeGraph::get_service_names_and_types_by_node( return services_and_types; } +std::map> +NodeGraph::get_client_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const +{ + rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types(); + auto service_names_and_types_finalizer = rcpputils::make_scope_exit( + [&service_names_and_types]() { + if (rcl_names_and_types_fini(&service_names_and_types) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), "could not destroy service names and types"); + } + }); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_get_client_names_and_types_by_node( + node_base_->get_rcl_node_handle(), + &allocator, + node_name.c_str(), + namespace_.c_str(), + &service_names_and_types); + if (ret != RCL_RET_OK) { + throw_from_rcl_error(ret, "failed to get service names and types by node"); + } + + std::map> services_and_types; + for (size_t i = 0; i < service_names_and_types.names.size; ++i) { + std::string service_name = service_names_and_types.names.data[i]; + for (size_t j = 0; j < service_names_and_types.types[i].size; ++j) { + services_and_types[service_name].emplace_back(service_names_and_types.types[i].data[j]); + } + } + + return services_and_types; +} + +std::map> +NodeGraph::get_publisher_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_, + bool no_demangle) const +{ + rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types(); + auto topic_names_and_types_finalizer = rcpputils::make_scope_exit( + [&topic_names_and_types]() { + if (rcl_names_and_types_fini(&topic_names_and_types) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), "could not destroy topic names and types"); + } + }); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_get_publisher_names_and_types_by_node( + node_base_->get_rcl_node_handle(), + &allocator, + no_demangle, + node_name.c_str(), + namespace_.c_str(), + &topic_names_and_types); + if (ret != RCL_RET_OK) { + throw_from_rcl_error(ret, "failed to get topic names and types by node"); + } + + std::map> topics_and_types; + for (size_t i = 0; i < topic_names_and_types.names.size; ++i) { + std::string topic_name = topic_names_and_types.names.data[i]; + for (size_t j = 0; j < topic_names_and_types.types[i].size; ++j) { + topics_and_types[topic_name].emplace_back(topic_names_and_types.types[i].data[j]); + } + } + + return topics_and_types; +} + +std::map> +NodeGraph::get_subscriber_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_, + bool no_demangle) const +{ + rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types(); + auto topic_names_and_types_finalizer = rcpputils::make_scope_exit( + [&topic_names_and_types]() { + if (rcl_names_and_types_fini(&topic_names_and_types) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), "could not destroy topic names and types"); + } + }); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_get_subscriber_names_and_types_by_node( + node_base_->get_rcl_node_handle(), + &allocator, + no_demangle, + node_name.c_str(), + namespace_.c_str(), + &topic_names_and_types); + if (ret != RCL_RET_OK) { + throw_from_rcl_error(ret, "failed to get topic names and types by node"); + } + + std::map> topics_and_types; + for (size_t i = 0; i < topic_names_and_types.names.size; ++i) { + std::string topic_name = topic_names_and_types.names.data[i]; + for (size_t j = 0; j < topic_names_and_types.types[i].size; ++j) { + topics_and_types[topic_name].emplace_back(topic_names_and_types.types[i].data[j]); + } + } + + return topics_and_types; +} + std::vector NodeGraph::get_node_names() const { @@ -206,6 +317,81 @@ NodeGraph::get_node_names() const return nodes; } +std::vector> +NodeGraph::get_node_names_with_enclaves() const +{ + rcutils_string_array_t node_names_c = + rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_namespaces_c = + rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_enclaves_c = + rcutils_get_zero_initialized_string_array(); + + auto allocator = rcl_get_default_allocator(); + auto ret = rcl_get_node_names_with_enclaves( + node_base_->get_rcl_node_handle(), + allocator, + &node_names_c, + &node_namespaces_c, + &node_enclaves_c); + if (ret != RCL_RET_OK) { + auto error_msg = + std::string("failed to get node names with enclaves: ") + rcl_get_error_string().str; + rcl_reset_error(); + if (rcutils_string_array_fini(&node_names_c) != RCUTILS_RET_OK) { + error_msg += std::string(", failed also to cleanup node names, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + if (rcutils_string_array_fini(&node_namespaces_c) != RCUTILS_RET_OK) { + error_msg += std::string(", failed also to cleanup node namespaces, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + if (rcutils_string_array_fini(&node_enclaves_c) != RCUTILS_RET_OK) { + error_msg += std::string(", failed also to cleanup node enclaves, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + throw std::runtime_error(error_msg); + } + + std::vector> node_tuples; + for (size_t i = 0; i < node_names_c.size; ++i) { + if (node_names_c.data[i] && node_namespaces_c.data[i] && node_enclaves_c.data[i]) { + node_tuples.emplace_back( + std::make_tuple(node_names_c.data[i], node_namespaces_c.data[i], node_enclaves_c.data[i])); + } + } + + std::string error("failed to finalize array"); + rcl_ret_t ret_names = rcutils_string_array_fini(&node_names_c); + if (ret_names != RCUTILS_RET_OK) { + error += std::string(", could not destroy node names, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + rcl_ret_t ret_ns = rcutils_string_array_fini(&node_namespaces_c); + if (ret_ns != RCUTILS_RET_OK) { + error += std::string(", could not destroy node namespaces, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + + rcl_ret_t ret_ecv = rcutils_string_array_fini(&node_enclaves_c); + if (ret_ecv != RCUTILS_RET_OK) { + error += std::string(", could not destroy node enclaves, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + + if (ret_names != RCUTILS_RET_OK || ret_ns != RCUTILS_RET_OK || ret_ecv != RCUTILS_RET_OK) { + throw std::runtime_error(error); + } + + return node_tuples; +} + std::vector> NodeGraph::get_node_names_and_namespaces() const { @@ -237,7 +423,6 @@ NodeGraph::get_node_names_and_namespaces() const throw std::runtime_error(error_msg); } - std::vector> node_names; node_names.reserve(node_names_c.size); for (size_t i = 0; i < node_names_c.size; ++i) { diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp index 6a44d7abf7..03f654a928 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -122,6 +122,10 @@ TEST_F(TestNodeGraph, construct_from_node) auto names_and_namespaces = node_graph()->get_node_names_and_namespaces(); EXPECT_EQ(1u, names_and_namespaces.size()); + auto names_namespaces_and_enclaves = + node_graph()->get_node_names_with_enclaves(); + EXPECT_EQ(1u, names_namespaces_and_enclaves.size()); + EXPECT_EQ(0u, node_graph()->count_publishers("not_a_topic")); EXPECT_EQ(0u, node_graph()->count_subscribers("not_a_topic")); EXPECT_NE(nullptr, node_graph()->get_graph_guard_condition()); @@ -211,7 +215,7 @@ TEST_F(TestNodeGraph, get_service_names_and_types_by_node) node_graph()->get_service_names_and_types_by_node(node2_name, absolute_namespace); auto start = std::chrono::steady_clock::now(); - while (std::chrono::steady_clock::now() - start < std::chrono::seconds(1)) { + while (std::chrono::steady_clock::now() - start < std::chrono::seconds(3)) { services_of_node1 = node_graph()->get_service_names_and_types_by_node(node_name, absolute_namespace); services_of_node2 = @@ -225,6 +229,40 @@ TEST_F(TestNodeGraph, get_service_names_and_types_by_node) EXPECT_FALSE(services_of_node2.find("/ns/node1_service") != services_of_node2.end()); } +TEST_F(TestNodeGraph, get_client_names_and_types_by_node) +{ + auto client = node()->create_client("node1_service"); + + const std::string node2_name = "node2"; + auto node2 = std::make_shared(node2_name, node_namespace); + + // rcl_get_client_names_and_types_by_node() expects the node to exist, otherwise it fails + EXPECT_THROW( + node_graph()->get_client_names_and_types_by_node("not_a_node", "not_absolute_namespace"), + rclcpp::exceptions::RCLError); + + // Check that node1_service exists for node1 but not node2. This shouldn't exercise graph + // discovery as node_graph belongs to node1 anyway. This is just to test the API itself. + auto services_of_node1 = + node_graph()->get_client_names_and_types_by_node(node_name, absolute_namespace); + auto services_of_node2 = + node_graph()->get_client_names_and_types_by_node(node2_name, absolute_namespace); + + auto start = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start < std::chrono::seconds(3)) { + services_of_node1 = + node_graph()->get_client_names_and_types_by_node(node_name, absolute_namespace); + services_of_node2 = + node_graph()->get_client_names_and_types_by_node(node2_name, absolute_namespace); + if (services_of_node1.find("/ns/node1_service") != services_of_node1.end()) { + break; + } + } + + EXPECT_TRUE(services_of_node1.find("/ns/node1_service") != services_of_node1.end()); + EXPECT_FALSE(services_of_node2.find("/ns/node1_service") != services_of_node2.end()); +} + TEST_F(TestNodeGraph, get_service_names_and_types_by_node_rcl_errors) { auto callback = []( @@ -244,6 +282,21 @@ TEST_F(TestNodeGraph, get_service_names_and_types_by_node_rcl_errors) " service names and types, leaking memory: error not set")); } + +TEST_F(TestNodeGraph, get_client_names_and_types_by_node_rcl_errors) +{ + auto client = node()->create_client("node1_service"); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_client_names_and_types_by_node, RCL_RET_ERROR); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_client_names_and_types_by_node(node_name, node_namespace), + std::runtime_error( + "failed to get service names and types by node: error not set")); +} + TEST_F(TestNodeGraph, get_service_names_and_types_by_node_names_and_types_fini_error) { auto callback = []( @@ -259,12 +312,163 @@ TEST_F(TestNodeGraph, get_service_names_and_types_by_node_names_and_types_fini_e rclcpp::exceptions::RCLError); } +TEST_F(TestNodeGraph, get_client_names_and_types_by_node_names_and_types_fini_error) +{ + auto client = node()->create_client("node1_service"); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + + EXPECT_NO_THROW( + node_graph()->get_client_names_and_types_by_node(node_name, absolute_namespace)); +} + +TEST_F(TestNodeGraph, get_publisher_names_and_types_by_node) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("node1_topic", publisher_qos); + + const std::string node2_name = "node2"; + auto node2 = std::make_shared(node2_name, node_namespace); + + // rcl_get_publisher_names_and_types_by_node() expects the node to exist, otherwise it fails + EXPECT_THROW( + node_graph()->get_publisher_names_and_types_by_node("not_a_node", "not_absolute_namespace"), + rclcpp::exceptions::RCLError); + + // Check that node1_topic exists for node1 but not node2. This shouldn't exercise graph + // discovery as node_graph belongs to node1 anyway. This is just to test the API itself. + auto topics_of_node1 = + node_graph()->get_publisher_names_and_types_by_node(node_name, absolute_namespace); + auto topics_of_node2 = + node_graph()->get_publisher_names_and_types_by_node(node2_name, absolute_namespace); + + auto start = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start < std::chrono::seconds(3)) { + topics_of_node1 = + node_graph()->get_publisher_names_and_types_by_node(node_name, absolute_namespace); + topics_of_node2 = + node_graph()->get_publisher_names_and_types_by_node(node2_name, absolute_namespace); + if (topics_of_node1.find("/ns/node1_topic") != topics_of_node1.end()) { + break; + } + } + + EXPECT_TRUE(topics_of_node1.find("/ns/node1_topic") != topics_of_node1.end()); + EXPECT_FALSE(topics_of_node2.find("/ns/node1_topic") != topics_of_node2.end()); +} + +TEST_F(TestNodeGraph, get_subscriber_names_and_types_by_node) +{ + const rclcpp::QoS subscriber_qos(10); + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + auto subscription = + node()->create_subscription( + "node1_topic", subscriber_qos, std::move(callback)); + + const std::string node2_name = "node2"; + auto node2 = std::make_shared(node2_name, node_namespace); + + // rcl_get_subscriber_names_and_types_by_node() expects the node to exist, otherwise it fails + EXPECT_THROW( + node_graph()->get_subscriber_names_and_types_by_node("not_a_node", "not_absolute_namespace"), + rclcpp::exceptions::RCLError); + + // Check that node1_topic exists for node1 but not node2. This shouldn't exercise graph + // discovery as node_graph belongs to node1 anyway. This is just to test the API itself. + auto topics_of_node1 = + node_graph()->get_subscriber_names_and_types_by_node(node_name, absolute_namespace); + auto topics_of_node2 = + node_graph()->get_subscriber_names_and_types_by_node(node2_name, absolute_namespace); + + auto start = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start < std::chrono::seconds(3)) { + topics_of_node1 = + node_graph()->get_subscriber_names_and_types_by_node(node_name, absolute_namespace); + topics_of_node2 = + node_graph()->get_subscriber_names_and_types_by_node(node2_name, absolute_namespace); + if (topics_of_node1.find("/ns/node1_topic") != topics_of_node1.end()) { + break; + } + } + + EXPECT_TRUE(topics_of_node1.find("/ns/node1_topic") != topics_of_node1.end()); + EXPECT_FALSE(topics_of_node2.find("/ns/node1_topic") != topics_of_node2.end()); +} + +TEST_F(TestNodeGraph, get_publisher_names_and_types_by_node_rcl_errors) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_publisher_names_and_types_by_node, RCL_RET_ERROR); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_publisher_names_and_types_by_node(node_name, node_namespace), + std::runtime_error( + "failed to get topic names and types by node: error not set")); +} + + +TEST_F(TestNodeGraph, get_subscriber_names_and_types_by_node_rcl_errors) +{ + const rclcpp::QoS subscriber_qos(10); + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + auto subscription = + node()->create_subscription( + "topic", subscriber_qos, std::move(callback)); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_subscriber_names_and_types_by_node, RCL_RET_ERROR); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_subscriber_names_and_types_by_node(node_name, node_namespace), + std::runtime_error( + "failed to get topic names and types by node: error not set")); +} + +TEST_F(TestNodeGraph, get_publisher_names_and_types_by_node_names_and_types_fini_error) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + + EXPECT_NO_THROW( + node_graph()->get_publisher_names_and_types_by_node(node_name, absolute_namespace)); +} + +TEST_F(TestNodeGraph, get_subscriber_names_and_types_by_node_names_and_types_fini_error) +{ + const rclcpp::QoS subscriber_qos(10); + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + auto subscription = + node()->create_subscription( + "topic", subscriber_qos, std::move(callback)); + + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + + EXPECT_NO_THROW( + node_graph()->get_subscriber_names_and_types_by_node(node_name, absolute_namespace)); +} + TEST_F(TestNodeGraph, get_node_names_and_namespaces) { auto names_and_namespaces = node_graph()->get_node_names_and_namespaces(); EXPECT_EQ(1u, names_and_namespaces.size()); } +TEST_F(TestNodeGraph, get_node_names_with_enclaves) +{ + auto names_namespaces_and_enclaves = + node_graph()->get_node_names_with_enclaves(); + EXPECT_EQ(1u, names_namespaces_and_enclaves.size()); +} + TEST_F(TestNodeGraph, get_node_names_and_namespaces_rcl_errors) { auto mock = mocking_utils::patch_and_return( @@ -278,6 +482,20 @@ TEST_F(TestNodeGraph, get_node_names_and_namespaces_rcl_errors) " error not set, failed also to cleanup node namespaces, leaking memory: error not set")); } +TEST_F(TestNodeGraph, get_node_names_with_enclaves_rcl_errors) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_node_names_with_enclaves, RCL_RET_ERROR); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcutils_string_array_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_node_names_with_enclaves(), + std::runtime_error( + "failed to get node names with enclaves: error not set, failed also to cleanup node names, " + "leaking memory: error not set, failed also to cleanup node namespaces, leaking memory: " + "error not set, failed also to cleanup node enclaves, leaking memory: error not set")); +} + TEST_F(TestNodeGraph, get_node_names_and_namespaces_fini_errors) { auto mock_names_fini = mocking_utils::patch_and_return( @@ -287,6 +505,18 @@ TEST_F(TestNodeGraph, get_node_names_and_namespaces_fini_errors) std::runtime_error("could not destroy node names, could not destroy node namespaces")); } +TEST_F(TestNodeGraph, get_node_names_with_enclaves_fini_errors) +{ + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcutils_string_array_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_node_names_with_enclaves(), + std::runtime_error( + "failed to finalize array, could not destroy node names, leaking memory: error not set" + ", could not destroy node namespaces, leaking memory: error not set" + ", could not destroy node enclaves, leaking memory: error not set")); +} + TEST_F(TestNodeGraph, count_publishers_rcl_error) { auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_publishers, RCL_RET_ERROR); From 81df5843f321b08db621c17502ff94193ffd3657 Mon Sep 17 00:00:00 2001 From: Emerson Knapp <537409+emersonknapp@users.noreply.github.com> Date: Fri, 24 Sep 2021 12:15:23 -0700 Subject: [PATCH 017/455] Add Clock::sleep_until method (#1748) * Add Clock::sleep_until method Handle all 3 clock types, and edge cases for shutdown and enabling/disabling ROS time Signed-off-by: Emerson Knapp --- rclcpp/include/rclcpp/clock.hpp | 19 +++ rclcpp/src/rclcpp/clock.cpp | 97 ++++++++++++++- rclcpp/test/rclcpp/test_time.cpp | 153 ++++++++++++++++++++++++ rclcpp/test/rclcpp/test_time_source.cpp | 33 +++++ 4 files changed, 300 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 9d2ef45b4d..7c74d7acfc 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -78,6 +78,25 @@ class Clock Time now(); + /** + * Sleep until a specified Time, according to clock type. + * + * Notes for RCL_ROS_TIME clock type: + * - Can sleep forever if ros time is active and received clock never reaches `until` + * - If ROS time enabled state changes during the sleep, this method will immediately return + * false. There is not a consistent choice of sleeping time when the time source changes, + * so this is up to the caller to call again if needed. + * + * \param until absolute time according to current clock type to sleep until. + * \return true immediately if `until` is in the past + * \return true when the time `until` is reached + * \return false if time cannot be reached reliably, for example from shutdown or a change + * of time source. + */ + RCLCPP_PUBLIC + bool + sleep_until(Time until); + /** * Returns the clock of the type `RCL_ROS_TIME` is active. * diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index e9207e6f31..051560ddeb 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -14,10 +14,13 @@ #include "rclcpp/clock.hpp" +#include #include #include +#include "rclcpp/contexts/default_context.hpp" #include "rclcpp/exceptions.hpp" +#include "rclcpp/utilities.hpp" #include "rcutils/logging_macros.h" @@ -47,6 +50,8 @@ class Clock::Impl rcl_clock_t rcl_clock_; rcl_allocator_t allocator_; std::mutex clock_mutex_; + std::condition_variable cv_; + rclcpp::OnShutdownCallbackHandle shutdown_cb_; }; JumpHandler::JumpHandler( @@ -59,9 +64,18 @@ JumpHandler::JumpHandler( {} Clock::Clock(rcl_clock_type_t clock_type) -: impl_(new Clock::Impl(clock_type)) {} +: impl_(new Clock::Impl(clock_type)) +{ + impl_->shutdown_cb_ = rclcpp::contexts::get_global_default_context()->add_on_shutdown_callback( + [this]() { + impl_->cv_.notify_all(); + }); +} -Clock::~Clock() {} +Clock::~Clock() +{ + rclcpp::contexts::get_global_default_context()->remove_on_shutdown_callback(impl_->shutdown_cb_); +} Time Clock::now() @@ -76,6 +90,85 @@ Clock::now() return now; } +bool +Clock::sleep_until(Time until) +{ + const auto this_clock_type = get_clock_type(); + if (until.get_clock_type() != this_clock_type) { + RCUTILS_LOG_ERROR( + "sleep_until Time clock type %d does not match this clock's type %d.", + until.get_clock_type(), this_clock_type); + return false; + } + bool time_source_changed = false; + + if (this_clock_type == RCL_STEADY_TIME) { + auto steady_time = std::chrono::steady_clock::time_point( + std::chrono::nanoseconds(until.nanoseconds())); + + // loop over spurious wakeups but notice shutdown + std::unique_lock lock(impl_->clock_mutex_); + while (now() < until && rclcpp::ok()) { + impl_->cv_.wait_until(lock, steady_time); + } + } else if (this_clock_type == RCL_SYSTEM_TIME) { + auto system_time = std::chrono::time_point< + std::chrono::system_clock, std::chrono::nanoseconds>( + std::chrono::nanoseconds(until.nanoseconds())); + + // loop over spurious wakeups but notice shutdown + std::unique_lock lock(impl_->clock_mutex_); + while (now() < until && rclcpp::ok()) { + impl_->cv_.wait_until(lock, system_time); + } + } else if (this_clock_type == RCL_ROS_TIME) { + // Install jump handler for any amount of time change, for two purposes: + // - if ROS time is active, check if time reached on each new clock sample + // - Trigger via on_clock_change to detect if time source changes, to invalidate sleep + rcl_jump_threshold_t threshold; + threshold.on_clock_change = true; + threshold.min_backward.nanoseconds = 0; + threshold.min_forward.nanoseconds = 0; + auto clock_handler = create_jump_callback( + []() {}, + [this](const rcl_time_jump_t &) {impl_->cv_.notify_all();}, + threshold); + + try { + if (!ros_time_is_active()) { + auto system_time = std::chrono::time_point< + std::chrono::system_clock, std::chrono::nanoseconds>( + std::chrono::nanoseconds(until.nanoseconds())); + + // loop over spurious wakeups but notice shutdown or time source change + std::unique_lock lock(impl_->clock_mutex_); + while (now() < until && rclcpp::ok() && !ros_time_is_active()) { + impl_->cv_.wait_until(lock, system_time); + } + time_source_changed = ros_time_is_active(); + } else { + // RCL_ROS_TIME with ros_time_is_active. + // Just wait without "until" because installed + // jump callbacks wake the cv on every new sample. + std::unique_lock lock(impl_->clock_mutex_); + while (now() < until && rclcpp::ok() && ros_time_is_active()) { + impl_->cv_.wait(lock); + } + time_source_changed = !ros_time_is_active(); + } + } catch (...) { + RCUTILS_LOG_ERROR("Unexpected exception from ros_time_is_active()"); + return false; + } + } + + if (!rclcpp::ok() || time_source_changed) { + return false; + } + + return now() >= until; +} + bool Clock::ros_time_is_active() { diff --git a/rclcpp/test/rclcpp/test_time.cpp b/rclcpp/test/rclcpp/test_time.cpp index f7889e96f6..b7b8f5f6c1 100644 --- a/rclcpp/test/rclcpp/test_time.cpp +++ b/rclcpp/test/rclcpp/test_time.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include "rcl/error_handling.h" @@ -24,6 +25,7 @@ #include "rclcpp/clock.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp/time.hpp" +#include "rclcpp/time_source.hpp" #include "rclcpp/utilities.hpp" #include "../utils/rclcpp_gtest_macros.hpp" @@ -447,3 +449,154 @@ TEST_F(TestTime, test_overflow_underflow_throws) { test_time = rclcpp::Duration::from_nanoseconds(INT64_MIN) + rclcpp::Time(-1), std::underflow_error("addition leads to int64_t underflow")); } + +class TestClockSleep : public ::testing::Test +{ +protected: + void SetUp() + { + // Shutdown in case there was a dangling global context from other test fixtures + rclcpp::shutdown(); + rclcpp::init(0, nullptr); + node = std::make_shared("clock_sleep_node"); + param_client = std::make_shared(node); + ASSERT_TRUE(param_client->wait_for_service(5s)); + } + + void TearDown() + { + node.reset(); + rclcpp::shutdown(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::SyncParametersClient::SharedPtr param_client; +}; + +TEST_F(TestClockSleep, bad_clock_type) { + rclcpp::Clock clock(RCL_SYSTEM_TIME); + rclcpp::Time steady_until(12345, 0, RCL_STEADY_TIME); + ASSERT_FALSE(clock.sleep_until(steady_until)); + + rclcpp::Time ros_until(54321, 0, RCL_ROS_TIME); + ASSERT_FALSE(clock.sleep_until(ros_until)); +} + +TEST_F(TestClockSleep, sleep_until_basic_system) { + static const auto MILLION = 1000L * 1000L; + const auto milliseconds = 300; + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto delay = rclcpp::Duration(0, milliseconds * MILLION); + auto sleep_until = clock.now() + delay; + + auto start = std::chrono::system_clock::now(); + ASSERT_TRUE(clock.sleep_until(sleep_until)); + auto end = std::chrono::system_clock::now(); + + EXPECT_GE(clock.now(), sleep_until); + EXPECT_GE(end - start, std::chrono::milliseconds(milliseconds)); +} + +TEST_F(TestClockSleep, sleep_until_basic_steady) { + static const auto MILLION = 1000L * 1000L; + const auto milliseconds = 300; + rclcpp::Clock clock(RCL_STEADY_TIME); + auto delay = rclcpp::Duration(0, milliseconds * MILLION); + auto sleep_until = clock.now() + delay; + + auto steady_start = std::chrono::steady_clock::now(); + ASSERT_TRUE(clock.sleep_until(sleep_until)); + auto steady_end = std::chrono::steady_clock::now(); + + EXPECT_GE(clock.now(), sleep_until); + EXPECT_GE(steady_end - steady_start, std::chrono::milliseconds(milliseconds)); +} + +TEST_F(TestClockSleep, sleep_until_steady_past_returns_immediately) { + rclcpp::Clock clock(RCL_STEADY_TIME); + auto until = clock.now() - rclcpp::Duration(1000, 0); + // This should return immediately, other possible behavior might be sleep forever and timeout + ASSERT_TRUE(clock.sleep_until(until)); +} + +TEST_F(TestClockSleep, sleep_until_system_past_returns_immediately) { + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto until = clock.now() - rclcpp::Duration(1000, 0); + // This should return immediately, other possible behavior might be sleep forever and timeout + ASSERT_TRUE(clock.sleep_until(until)); +} + +TEST_F(TestClockSleep, sleep_until_ros_time_enable_interrupt) { + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source; + time_source.attachNode(node); + time_source.attachClock(clock); + + // 5 second timeout, but it should be interrupted right away + const auto until = clock->now() + rclcpp::Duration(5, 0); + + // Try sleeping with ROS time off, then turn it on to interrupt + bool sleep_succeeded = true; + auto sleep_thread = std::thread( + [clock, until, &sleep_succeeded]() { + sleep_succeeded = clock->sleep_until(until); + }); + // yield execution long enough to let the sleep thread get to waiting on the condition variable + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + auto set_parameters_results = param_client->set_parameters( + {rclcpp::Parameter("use_sim_time", true)}); + for (auto & result : set_parameters_results) { + ASSERT_TRUE(result.successful); + } + sleep_thread.join(); + EXPECT_FALSE(sleep_succeeded); +} + +TEST_F(TestClockSleep, sleep_until_ros_time_disable_interrupt) { + param_client->set_parameters({rclcpp::Parameter("use_sim_time", true)}); + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source; + time_source.attachNode(node); + time_source.attachClock(clock); + + // /clock shouldn't be publishing, shouldn't be possible to reach timeout + const auto until = clock->now() + rclcpp::Duration(600, 0); + + // Try sleeping with ROS time off, then turn it on to interrupt + bool sleep_succeeded = true; + auto sleep_thread = std::thread( + [clock, until, &sleep_succeeded]() { + sleep_succeeded = clock->sleep_until(until); + }); + // yield execution long enough to let the sleep thread get to waiting on the condition variable + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + auto set_parameters_results = param_client->set_parameters( + {rclcpp::Parameter("use_sim_time", false)}); + for (auto & result : set_parameters_results) { + ASSERT_TRUE(result.successful); + } + sleep_thread.join(); + EXPECT_FALSE(sleep_succeeded); +} + +TEST_F(TestClockSleep, sleep_until_shutdown_interrupt) { + param_client->set_parameters({rclcpp::Parameter("use_sim_time", true)}); + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source; + time_source.attachNode(node); + time_source.attachClock(clock); + + // the timeout doesn't matter here - no /clock is being published, so it should never wake + const auto until = clock->now() + rclcpp::Duration(600, 0); + + bool sleep_succeeded = true; + auto sleep_thread = std::thread( + [clock, until, &sleep_succeeded]() { + sleep_succeeded = clock->sleep_until(until); + }); + // yield execution long enough to let the sleep thread get to waiting on the condition variable + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + rclcpp::shutdown(); + sleep_thread.join(); + EXPECT_FALSE(sleep_succeeded); +} diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index 3b64e9e8b1..3a79cffe0c 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -742,3 +742,36 @@ TEST_F(TestTimeSource, check_sim_time_updated_in_callback_if_use_clock_thread) { // Node should have get out of timer callback ASSERT_FALSE(clock_thread_testing_node.GetIsCallbackFrozen()); } + +TEST_F(TestTimeSource, clock_sleep_until_with_ros_time_basic) { + SimClockPublisherNode pub_node; + pub_node.SpinNode(); + + node->set_parameter({"use_sim_time", true}); + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source; + time_source.attachNode(node); + time_source.attachClock(clock); + + // Wait until time source has definitely received a first ROS time from the pub node + { + rcl_jump_threshold_t threshold; + threshold.on_clock_change = false; + threshold.min_backward.nanoseconds = 0; + threshold.min_forward.nanoseconds = 0; + + std::condition_variable cv; + std::mutex mutex; + auto handler = clock->create_jump_callback( + []() {}, + [&cv](const rcl_time_jump_t &) {cv.notify_all();}, + threshold); + std::unique_lock lock(mutex); + cv.wait(lock); + } + + auto now = clock->now(); + // Any amount of time will do, just need to make sure that we awake and return true + auto until = now + rclcpp::Duration(0, 500); + EXPECT_TRUE(clock->sleep_until(until)); +} From fa3a6fa597c5d40b2fce46e5fb95f541e62076e7 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 24 Sep 2021 15:22:17 -0700 Subject: [PATCH 018/455] fixup some small things that cppcheck noticed in my editor (#1783) * fixup some small things that cppcheck noticed in my editor Signed-off-by: William Woodall * fix implementation too Signed-off-by: William Woodall --- rclcpp/include/rclcpp/parameter_event_handler.hpp | 10 +++++----- rclcpp/src/rclcpp/parameter_event_handler.cpp | 8 ++++---- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter_event_handler.hpp b/rclcpp/include/rclcpp/parameter_event_handler.hpp index c7b781f0ec..084cf4afbd 100644 --- a/rclcpp/include/rclcpp/parameter_event_handler.hpp +++ b/rclcpp/include/rclcpp/parameter_event_handler.hpp @@ -169,8 +169,8 @@ class ParameterEventHandler NodeT node, const rclcpp::QoS & qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events))) + : node_base_(rclcpp::node_interfaces::get_node_base_interface(node)) { - node_base_ = rclcpp::node_interfaces::get_node_base_interface(node); auto node_topics = rclcpp::node_interfaces::get_node_topics_interface(node); event_subscription_ = rclcpp::create_subscription( @@ -249,8 +249,8 @@ class ParameterEventHandler get_parameter_from_event( const rcl_interfaces::msg::ParameterEvent & event, rclcpp::Parameter & parameter, - const std::string parameter_name, - const std::string node_name = ""); + const std::string & parameter_name, + const std::string & node_name = ""); /// Get an rclcpp::Parameter from parameter event /** @@ -269,8 +269,8 @@ class ParameterEventHandler static rclcpp::Parameter get_parameter_from_event( const rcl_interfaces::msg::ParameterEvent & event, - const std::string parameter_name, - const std::string node_name = ""); + const std::string & parameter_name, + const std::string & node_name = ""); /// Get all rclcpp::Parameter values from a parameter event /** diff --git a/rclcpp/src/rclcpp/parameter_event_handler.cpp b/rclcpp/src/rclcpp/parameter_event_handler.cpp index 232a32f887..e67a5994ad 100644 --- a/rclcpp/src/rclcpp/parameter_event_handler.cpp +++ b/rclcpp/src/rclcpp/parameter_event_handler.cpp @@ -101,8 +101,8 @@ bool ParameterEventHandler::get_parameter_from_event( const rcl_interfaces::msg::ParameterEvent & event, rclcpp::Parameter & parameter, - const std::string parameter_name, - const std::string node_name) + const std::string & parameter_name, + const std::string & node_name) { if (event.node != node_name) { return false; @@ -128,8 +128,8 @@ ParameterEventHandler::get_parameter_from_event( rclcpp::Parameter ParameterEventHandler::get_parameter_from_event( const rcl_interfaces::msg::ParameterEvent & event, - const std::string parameter_name, - const std::string node_name) + const std::string & parameter_name, + const std::string & node_name) { rclcpp::Parameter p; if (!get_parameter_from_event(event, p, parameter_name, node_name)) { From 9e445bdb910503cd16f3ccf57382e01169738cc3 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 29 Sep 2021 11:30:51 -0300 Subject: [PATCH 019/455] Update forward declarations of `rcl_lifecycle` types (#1788) * Include rcl_lifecycle and drop forward declarations Signed-off-by: Michel Hidalgo --- rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp | 4 +--- rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp | 4 +--- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp index f09e390395..a0ac997ff3 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp @@ -17,13 +17,11 @@ #include +#include "rcl_lifecycle/data_types.h" #include "rclcpp_lifecycle/visibility_control.h" #include "rcutils/allocator.h" -// forward declare rcl_state_t -typedef struct rcl_lifecycle_state_t rcl_lifecycle_state_t; - namespace rclcpp_lifecycle { diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp index e15ccce7e2..874be69aa6 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp @@ -17,14 +17,12 @@ #include +#include "rcl_lifecycle/data_types.h" #include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/visibility_control.h" #include "rcutils/allocator.h" -// forward declare rcl_transition_t -typedef struct rcl_lifecycle_transition_t rcl_lifecycle_transition_t; - namespace rclcpp_lifecycle { From d5ec2580805c3abd8b6fecaf3d09040e00e9f15e Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Wed, 29 Sep 2021 12:24:19 -0700 Subject: [PATCH 020/455] typo fix. (#1790) Signed-off-by: Tomoya Fujita --- rclcpp/include/rclcpp/subscription_base.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 7653292b5e..72ddc23be2 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -107,7 +107,7 @@ class SubscriptionBase : public std::enable_shared_from_this /// Get the actual QoS settings, after the defaults have been determined. /** * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT - * can only be resolved after the creation of the publisher, and it + * can only be resolved after the creation of the subscription, and it * depends on the underlying rmw implementation. * If the underlying setting in use can't be represented in ROS terms, * it will be set to RMW_QOS_POLICY_*_UNKNOWN. From d04319a4381909c871b062e73b5975689126fbb6 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Tue, 5 Oct 2021 12:14:35 -0300 Subject: [PATCH 021/455] Revert "Add Clock::sleep_until method (#1748)" (#1793) This reverts commit 81df5843f321b08db621c17502ff94193ffd3657. Signed-off-by: Jorge Perez --- rclcpp/include/rclcpp/clock.hpp | 19 --- rclcpp/src/rclcpp/clock.cpp | 97 +-------------- rclcpp/test/rclcpp/test_time.cpp | 153 ------------------------ rclcpp/test/rclcpp/test_time_source.cpp | 33 ----- 4 files changed, 2 insertions(+), 300 deletions(-) diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 7c74d7acfc..9d2ef45b4d 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -78,25 +78,6 @@ class Clock Time now(); - /** - * Sleep until a specified Time, according to clock type. - * - * Notes for RCL_ROS_TIME clock type: - * - Can sleep forever if ros time is active and received clock never reaches `until` - * - If ROS time enabled state changes during the sleep, this method will immediately return - * false. There is not a consistent choice of sleeping time when the time source changes, - * so this is up to the caller to call again if needed. - * - * \param until absolute time according to current clock type to sleep until. - * \return true immediately if `until` is in the past - * \return true when the time `until` is reached - * \return false if time cannot be reached reliably, for example from shutdown or a change - * of time source. - */ - RCLCPP_PUBLIC - bool - sleep_until(Time until); - /** * Returns the clock of the type `RCL_ROS_TIME` is active. * diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index 051560ddeb..e9207e6f31 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -14,13 +14,10 @@ #include "rclcpp/clock.hpp" -#include #include #include -#include "rclcpp/contexts/default_context.hpp" #include "rclcpp/exceptions.hpp" -#include "rclcpp/utilities.hpp" #include "rcutils/logging_macros.h" @@ -50,8 +47,6 @@ class Clock::Impl rcl_clock_t rcl_clock_; rcl_allocator_t allocator_; std::mutex clock_mutex_; - std::condition_variable cv_; - rclcpp::OnShutdownCallbackHandle shutdown_cb_; }; JumpHandler::JumpHandler( @@ -64,18 +59,9 @@ JumpHandler::JumpHandler( {} Clock::Clock(rcl_clock_type_t clock_type) -: impl_(new Clock::Impl(clock_type)) -{ - impl_->shutdown_cb_ = rclcpp::contexts::get_global_default_context()->add_on_shutdown_callback( - [this]() { - impl_->cv_.notify_all(); - }); -} +: impl_(new Clock::Impl(clock_type)) {} -Clock::~Clock() -{ - rclcpp::contexts::get_global_default_context()->remove_on_shutdown_callback(impl_->shutdown_cb_); -} +Clock::~Clock() {} Time Clock::now() @@ -90,85 +76,6 @@ Clock::now() return now; } -bool -Clock::sleep_until(Time until) -{ - const auto this_clock_type = get_clock_type(); - if (until.get_clock_type() != this_clock_type) { - RCUTILS_LOG_ERROR( - "sleep_until Time clock type %d does not match this clock's type %d.", - until.get_clock_type(), this_clock_type); - return false; - } - bool time_source_changed = false; - - if (this_clock_type == RCL_STEADY_TIME) { - auto steady_time = std::chrono::steady_clock::time_point( - std::chrono::nanoseconds(until.nanoseconds())); - - // loop over spurious wakeups but notice shutdown - std::unique_lock lock(impl_->clock_mutex_); - while (now() < until && rclcpp::ok()) { - impl_->cv_.wait_until(lock, steady_time); - } - } else if (this_clock_type == RCL_SYSTEM_TIME) { - auto system_time = std::chrono::time_point< - std::chrono::system_clock, std::chrono::nanoseconds>( - std::chrono::nanoseconds(until.nanoseconds())); - - // loop over spurious wakeups but notice shutdown - std::unique_lock lock(impl_->clock_mutex_); - while (now() < until && rclcpp::ok()) { - impl_->cv_.wait_until(lock, system_time); - } - } else if (this_clock_type == RCL_ROS_TIME) { - // Install jump handler for any amount of time change, for two purposes: - // - if ROS time is active, check if time reached on each new clock sample - // - Trigger via on_clock_change to detect if time source changes, to invalidate sleep - rcl_jump_threshold_t threshold; - threshold.on_clock_change = true; - threshold.min_backward.nanoseconds = 0; - threshold.min_forward.nanoseconds = 0; - auto clock_handler = create_jump_callback( - []() {}, - [this](const rcl_time_jump_t &) {impl_->cv_.notify_all();}, - threshold); - - try { - if (!ros_time_is_active()) { - auto system_time = std::chrono::time_point< - std::chrono::system_clock, std::chrono::nanoseconds>( - std::chrono::nanoseconds(until.nanoseconds())); - - // loop over spurious wakeups but notice shutdown or time source change - std::unique_lock lock(impl_->clock_mutex_); - while (now() < until && rclcpp::ok() && !ros_time_is_active()) { - impl_->cv_.wait_until(lock, system_time); - } - time_source_changed = ros_time_is_active(); - } else { - // RCL_ROS_TIME with ros_time_is_active. - // Just wait without "until" because installed - // jump callbacks wake the cv on every new sample. - std::unique_lock lock(impl_->clock_mutex_); - while (now() < until && rclcpp::ok() && ros_time_is_active()) { - impl_->cv_.wait(lock); - } - time_source_changed = !ros_time_is_active(); - } - } catch (...) { - RCUTILS_LOG_ERROR("Unexpected exception from ros_time_is_active()"); - return false; - } - } - - if (!rclcpp::ok() || time_source_changed) { - return false; - } - - return now() >= until; -} - bool Clock::ros_time_is_active() { diff --git a/rclcpp/test/rclcpp/test_time.cpp b/rclcpp/test/rclcpp/test_time.cpp index b7b8f5f6c1..f7889e96f6 100644 --- a/rclcpp/test/rclcpp/test_time.cpp +++ b/rclcpp/test/rclcpp/test_time.cpp @@ -17,7 +17,6 @@ #include #include #include -#include #include #include "rcl/error_handling.h" @@ -25,7 +24,6 @@ #include "rclcpp/clock.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp/time.hpp" -#include "rclcpp/time_source.hpp" #include "rclcpp/utilities.hpp" #include "../utils/rclcpp_gtest_macros.hpp" @@ -449,154 +447,3 @@ TEST_F(TestTime, test_overflow_underflow_throws) { test_time = rclcpp::Duration::from_nanoseconds(INT64_MIN) + rclcpp::Time(-1), std::underflow_error("addition leads to int64_t underflow")); } - -class TestClockSleep : public ::testing::Test -{ -protected: - void SetUp() - { - // Shutdown in case there was a dangling global context from other test fixtures - rclcpp::shutdown(); - rclcpp::init(0, nullptr); - node = std::make_shared("clock_sleep_node"); - param_client = std::make_shared(node); - ASSERT_TRUE(param_client->wait_for_service(5s)); - } - - void TearDown() - { - node.reset(); - rclcpp::shutdown(); - } - - rclcpp::Node::SharedPtr node; - rclcpp::SyncParametersClient::SharedPtr param_client; -}; - -TEST_F(TestClockSleep, bad_clock_type) { - rclcpp::Clock clock(RCL_SYSTEM_TIME); - rclcpp::Time steady_until(12345, 0, RCL_STEADY_TIME); - ASSERT_FALSE(clock.sleep_until(steady_until)); - - rclcpp::Time ros_until(54321, 0, RCL_ROS_TIME); - ASSERT_FALSE(clock.sleep_until(ros_until)); -} - -TEST_F(TestClockSleep, sleep_until_basic_system) { - static const auto MILLION = 1000L * 1000L; - const auto milliseconds = 300; - rclcpp::Clock clock(RCL_SYSTEM_TIME); - auto delay = rclcpp::Duration(0, milliseconds * MILLION); - auto sleep_until = clock.now() + delay; - - auto start = std::chrono::system_clock::now(); - ASSERT_TRUE(clock.sleep_until(sleep_until)); - auto end = std::chrono::system_clock::now(); - - EXPECT_GE(clock.now(), sleep_until); - EXPECT_GE(end - start, std::chrono::milliseconds(milliseconds)); -} - -TEST_F(TestClockSleep, sleep_until_basic_steady) { - static const auto MILLION = 1000L * 1000L; - const auto milliseconds = 300; - rclcpp::Clock clock(RCL_STEADY_TIME); - auto delay = rclcpp::Duration(0, milliseconds * MILLION); - auto sleep_until = clock.now() + delay; - - auto steady_start = std::chrono::steady_clock::now(); - ASSERT_TRUE(clock.sleep_until(sleep_until)); - auto steady_end = std::chrono::steady_clock::now(); - - EXPECT_GE(clock.now(), sleep_until); - EXPECT_GE(steady_end - steady_start, std::chrono::milliseconds(milliseconds)); -} - -TEST_F(TestClockSleep, sleep_until_steady_past_returns_immediately) { - rclcpp::Clock clock(RCL_STEADY_TIME); - auto until = clock.now() - rclcpp::Duration(1000, 0); - // This should return immediately, other possible behavior might be sleep forever and timeout - ASSERT_TRUE(clock.sleep_until(until)); -} - -TEST_F(TestClockSleep, sleep_until_system_past_returns_immediately) { - rclcpp::Clock clock(RCL_SYSTEM_TIME); - auto until = clock.now() - rclcpp::Duration(1000, 0); - // This should return immediately, other possible behavior might be sleep forever and timeout - ASSERT_TRUE(clock.sleep_until(until)); -} - -TEST_F(TestClockSleep, sleep_until_ros_time_enable_interrupt) { - auto clock = std::make_shared(RCL_ROS_TIME); - rclcpp::TimeSource time_source; - time_source.attachNode(node); - time_source.attachClock(clock); - - // 5 second timeout, but it should be interrupted right away - const auto until = clock->now() + rclcpp::Duration(5, 0); - - // Try sleeping with ROS time off, then turn it on to interrupt - bool sleep_succeeded = true; - auto sleep_thread = std::thread( - [clock, until, &sleep_succeeded]() { - sleep_succeeded = clock->sleep_until(until); - }); - // yield execution long enough to let the sleep thread get to waiting on the condition variable - std::this_thread::sleep_for(std::chrono::milliseconds(200)); - auto set_parameters_results = param_client->set_parameters( - {rclcpp::Parameter("use_sim_time", true)}); - for (auto & result : set_parameters_results) { - ASSERT_TRUE(result.successful); - } - sleep_thread.join(); - EXPECT_FALSE(sleep_succeeded); -} - -TEST_F(TestClockSleep, sleep_until_ros_time_disable_interrupt) { - param_client->set_parameters({rclcpp::Parameter("use_sim_time", true)}); - auto clock = std::make_shared(RCL_ROS_TIME); - rclcpp::TimeSource time_source; - time_source.attachNode(node); - time_source.attachClock(clock); - - // /clock shouldn't be publishing, shouldn't be possible to reach timeout - const auto until = clock->now() + rclcpp::Duration(600, 0); - - // Try sleeping with ROS time off, then turn it on to interrupt - bool sleep_succeeded = true; - auto sleep_thread = std::thread( - [clock, until, &sleep_succeeded]() { - sleep_succeeded = clock->sleep_until(until); - }); - // yield execution long enough to let the sleep thread get to waiting on the condition variable - std::this_thread::sleep_for(std::chrono::milliseconds(200)); - auto set_parameters_results = param_client->set_parameters( - {rclcpp::Parameter("use_sim_time", false)}); - for (auto & result : set_parameters_results) { - ASSERT_TRUE(result.successful); - } - sleep_thread.join(); - EXPECT_FALSE(sleep_succeeded); -} - -TEST_F(TestClockSleep, sleep_until_shutdown_interrupt) { - param_client->set_parameters({rclcpp::Parameter("use_sim_time", true)}); - auto clock = std::make_shared(RCL_ROS_TIME); - rclcpp::TimeSource time_source; - time_source.attachNode(node); - time_source.attachClock(clock); - - // the timeout doesn't matter here - no /clock is being published, so it should never wake - const auto until = clock->now() + rclcpp::Duration(600, 0); - - bool sleep_succeeded = true; - auto sleep_thread = std::thread( - [clock, until, &sleep_succeeded]() { - sleep_succeeded = clock->sleep_until(until); - }); - // yield execution long enough to let the sleep thread get to waiting on the condition variable - std::this_thread::sleep_for(std::chrono::milliseconds(200)); - rclcpp::shutdown(); - sleep_thread.join(); - EXPECT_FALSE(sleep_succeeded); -} diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index 3a79cffe0c..3b64e9e8b1 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -742,36 +742,3 @@ TEST_F(TestTimeSource, check_sim_time_updated_in_callback_if_use_clock_thread) { // Node should have get out of timer callback ASSERT_FALSE(clock_thread_testing_node.GetIsCallbackFrozen()); } - -TEST_F(TestTimeSource, clock_sleep_until_with_ros_time_basic) { - SimClockPublisherNode pub_node; - pub_node.SpinNode(); - - node->set_parameter({"use_sim_time", true}); - auto clock = std::make_shared(RCL_ROS_TIME); - rclcpp::TimeSource time_source; - time_source.attachNode(node); - time_source.attachClock(clock); - - // Wait until time source has definitely received a first ROS time from the pub node - { - rcl_jump_threshold_t threshold; - threshold.on_clock_change = false; - threshold.min_backward.nanoseconds = 0; - threshold.min_forward.nanoseconds = 0; - - std::condition_variable cv; - std::mutex mutex; - auto handler = clock->create_jump_callback( - []() {}, - [&cv](const rcl_time_jump_t &) {cv.notify_all();}, - threshold); - std::unique_lock lock(mutex); - cv.wait(lock); - } - - auto now = clock->now(); - // Any amount of time will do, just need to make sure that we awake and return true - auto until = now + rclcpp::Duration(0, 500); - EXPECT_TRUE(clock->sleep_until(until)); -} From 301957515a0448de22def36559087b3338c76700 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Tue, 12 Oct 2021 18:50:00 -0700 Subject: [PATCH 022/455] add node_waitables_ to copy constructor. (#1799) * add node_waitables_ to copy constructor. Signed-off-by: Tomoya Fujita * add node_time_source_ to copy constructor. Signed-off-by: Tomoya Fujita * add construction_and_destruction_sub_node for action server. Signed-off-by: Tomoya Fujita Co-authored-by: Abrar Rahman Protyasha --- rclcpp/src/rclcpp/node.cpp | 2 ++ rclcpp_action/test/test_server.cpp | 21 +++++++++++++++++++++ 2 files changed, 23 insertions(+) diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 5d1f9a3077..140fa5f993 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -237,6 +237,8 @@ Node::Node( node_services_(other.node_services_), node_clock_(other.node_clock_), node_parameters_(other.node_parameters_), + node_time_source_(other.node_time_source_), + node_waitables_(other.node_waitables_), node_options_(other.node_options_), sub_namespace_(extend_sub_namespace(other.get_sub_namespace(), sub_namespace)), effective_namespace_(create_effective_namespace(other.get_namespace(), sub_namespace_)) diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 7030243c77..4d33c4db6d 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -205,6 +205,27 @@ TEST_F(TestServer, construction_and_destruction_wait_set_error) }, rclcpp::exceptions::RCLError); } +TEST_F(TestServer, construction_and_destruction_sub_node) +{ + auto parent_node = std::make_shared("construct_node", "/rclcpp_action/construct"); + auto sub_node = parent_node->create_sub_node("construct_sub_node"); + + ASSERT_NO_THROW( + { + using GoalHandle = rclcpp_action::ServerGoalHandle; + auto as = rclcpp_action::create_server( + sub_node, "fibonacci", + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::REJECT; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}); + (void)as; + }); +} + TEST_F(TestServer, handle_goal_called) { auto node = std::make_shared("handle_goal_node", "/rclcpp_action/handle_goal"); From d107a844eae6f4d6a86515f0b3e469802ab1e785 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 13 Oct 2021 10:05:12 -0300 Subject: [PATCH 023/455] Handle sigterm, in the same way we handle sigint (#1771) Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/context.hpp | 6 +- rclcpp/include/rclcpp/init_options.hpp | 2 +- rclcpp/include/rclcpp/utilities.hpp | 33 +++- rclcpp/src/rclcpp/init_options.cpp | 4 +- rclcpp/src/rclcpp/signal_handler.cpp | 238 ++++++++++++++----------- rclcpp/src/rclcpp/signal_handler.hpp | 85 +++++---- rclcpp/src/rclcpp/utilities.cpp | 19 +- 7 files changed, 241 insertions(+), 146 deletions(-) diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 5d71ffe06c..7d7472c88b 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -75,7 +75,7 @@ class Context : public std::enable_shared_from_this * Every context which is constructed is added to a global vector of contexts, * which is used by the signal handler to conditionally shutdown each context * on SIGINT. - * See the shutdown_on_sigint option in the InitOptions class. + * See the shutdown_on_signal option in the InitOptions class. */ RCLCPP_PUBLIC Context(); @@ -93,7 +93,7 @@ class Context : public std::enable_shared_from_this * Note that this function does not setup any signal handlers, so if you want * it to be shutdown by the signal handler, then you need to either install * them manually with rclcpp::install_signal_handlers() or use rclcpp::init(). - * In addition to installing the signal handlers, the shutdown_on_sigint + * In addition to installing the signal handlers, the shutdown_on_signal * of the InitOptions needs to be `true` for this context to be shutdown by * the signal handler, otherwise it will be passed over. * @@ -268,7 +268,7 @@ class Context : public std::enable_shared_from_this * * - this context is shutdown() * - this context is destructed (resulting in shutdown) - * - this context has shutdown_on_sigint=true and SIGINT occurs (resulting in shutdown) + * - this context has shutdown_on_signal=true and SIGINT/SIGTERM occurs (resulting in shutdown) * - interrupt_all_sleep_for() is called * * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for. diff --git a/rclcpp/include/rclcpp/init_options.hpp b/rclcpp/include/rclcpp/init_options.hpp index 6f87904f25..ab1346db32 100644 --- a/rclcpp/include/rclcpp/init_options.hpp +++ b/rclcpp/include/rclcpp/init_options.hpp @@ -29,7 +29,7 @@ class InitOptions { public: /// If true, the context will be shutdown on SIGINT by the signal handler (if it was installed). - bool shutdown_on_sigint = true; + bool shutdown_on_signal = true; /// Constructor /** diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 5011a12c6a..e9d8408bb3 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -42,6 +42,20 @@ std::string to_string(T value) namespace rclcpp { + +/// Option to indicate which signal handlers rclcpp should install. +enum class SignalHandlerOptions +{ + /// Install both sigint and sigterm, this is the default behavior. + All, + /// Install only a sigint handler. + SigInt, + /// Install only a sigterm handler. + SigTerm, + /// Do not install any signal handler. + None, +}; + /// Initialize communications via the rmw implementation and set up a global signal handler. /** * Initializes the global context which is accessible via the function @@ -50,10 +64,16 @@ namespace rclcpp * rclcpp::install_signal_handlers(). * * \sa rclcpp::Context::init() for more details on arguments and possible exceptions + * + * \param signal_handler_options option to indicate which signal handlers should be installed. */ RCLCPP_PUBLIC void -init(int argc, char const * const argv[], const InitOptions & init_options = InitOptions()); +init( + int argc, + char const * const argv[], + const InitOptions & init_options = InitOptions(), + SignalHandlerOptions signal_handler_options = SignalHandlerOptions::All); /// Install the global signal handler for rclcpp. /** @@ -67,17 +87,26 @@ init(int argc, char const * const argv[], const InitOptions & init_options = Ini * * This function is thread-safe. * + * \param signal_handler_options option to indicate which signal handlers should be installed. * \return true if signal handler was installed by this function, false if already installed. */ RCLCPP_PUBLIC bool -install_signal_handlers(); +install_signal_handlers(SignalHandlerOptions signal_handler_options = SignalHandlerOptions::All); /// Return true if the signal handlers are installed, otherwise false. RCLCPP_PUBLIC bool signal_handlers_installed(); +/// Get the current signal handler options. +/** + * If no signal handler is installed, SignalHandlerOptions::None is returned. + */ +RCLCPP_PUBLIC +SignalHandlerOptions +get_current_signal_handler_options(); + /// Uninstall the global signal handler for rclcpp. /** * This function does not necessarily need to be called, but can be used to diff --git a/rclcpp/src/rclcpp/init_options.cpp b/rclcpp/src/rclcpp/init_options.cpp index 49b408b8b4..5b8452c09a 100644 --- a/rclcpp/src/rclcpp/init_options.cpp +++ b/rclcpp/src/rclcpp/init_options.cpp @@ -43,7 +43,7 @@ InitOptions::InitOptions(const rcl_init_options_t & init_options) InitOptions::InitOptions(const InitOptions & other) : InitOptions(*other.get_rcl_init_options()) { - shutdown_on_sigint = other.shutdown_on_sigint; + shutdown_on_signal = other.shutdown_on_signal; initialize_logging_ = other.initialize_logging_; } @@ -70,7 +70,7 @@ InitOptions::operator=(const InitOptions & other) if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to copy rcl init options"); } - this->shutdown_on_sigint = other.shutdown_on_sigint; + this->shutdown_on_signal = other.shutdown_on_signal; this->initialize_logging_ = other.initialize_logging_; } return *this; diff --git a/rclcpp/src/rclcpp/signal_handler.cpp b/rclcpp/src/rclcpp/signal_handler.cpp index eda8585002..55ef988786 100644 --- a/rclcpp/src/rclcpp/signal_handler.cpp +++ b/rclcpp/src/rclcpp/signal_handler.cpp @@ -30,80 +30,131 @@ #endif #include "rclcpp/logging.hpp" +#include "rclcpp/utilities.hpp" #include "rcutils/strerror.h" #include "rmw/impl/cpp/demangle.hpp" using rclcpp::SignalHandler; +using rclcpp::SignalHandlerOptions; -// initialize static storage in SignalHandler class -SignalHandler::signal_handler_type SignalHandler::old_signal_handler_; -std::atomic_bool SignalHandler::signal_received_ = ATOMIC_VAR_INIT(false); -std::atomic_bool SignalHandler::wait_for_signal_is_setup_ = ATOMIC_VAR_INIT(false); -#if defined(_WIN32) -HANDLE SignalHandler::signal_handler_sem_; -#elif defined(__APPLE__) -dispatch_semaphore_t SignalHandler::signal_handler_sem_; -#else // posix -sem_t SignalHandler::signal_handler_sem_; +SignalHandler::signal_handler_type +SignalHandler::set_signal_handler( + int signal_value, + const SignalHandler::signal_handler_type & signal_handler) +{ + bool signal_handler_install_failed; + SignalHandler::signal_handler_type old_signal_handler; +#if defined(RCLCPP_HAS_SIGACTION) + ssize_t ret = sigaction(signal_value, &signal_handler, &old_signal_handler); + signal_handler_install_failed = (ret == -1); +#else + old_signal_handler = std::signal(signal_value, signal_handler); + signal_handler_install_failed = (old_signal_handler == SIG_ERR); #endif + if (signal_handler_install_failed) { + char error_string[1024]; + rcutils_strerror(error_string, sizeof(error_string)); + auto msg = + "Failed to set signal handler (" + std::to_string(errno) + "): " + error_string; + throw std::runtime_error(msg); + } + return old_signal_handler; +} -// The logger must be initialized before the local static variable signal_handler, -// from the method get_global_signal_handler(), so that it is destructed after -// it, because the destructor of SignalHandler uses this logger object. -static rclcpp::Logger g_logger = rclcpp::get_logger("rclcpp"); +// Unfortunately macros (or duplicated code) are needed here, +// as the signal handler must be a function pointer. +#if defined(RCLCPP_HAS_SIGACTION) +void +SignalHandler::signal_handler( + int signum, siginfo_t * siginfo, void * context) +{ + RCLCPP_INFO(SignalHandler::get_logger(), "signal_handler(signum=%d)", signum); + auto & instance = SignalHandler::get_global_signal_handler(); + + auto old_signal_handler = instance.get_old_signal_handler(signum); + if (old_signal_handler.sa_flags & SA_SIGINFO) { + if (old_signal_handler.sa_sigaction != NULL) { + old_signal_handler.sa_sigaction(signum, siginfo, context); + } + } else { + if ( + old_signal_handler.sa_handler != NULL && /* Is set */ + old_signal_handler.sa_handler != SIG_DFL && /* Is not default*/ + old_signal_handler.sa_handler != SIG_IGN) /* Is not ignored */ + { + old_signal_handler.sa_handler(signum); + } + } + instance.signal_handler_common(); +} +#else +void +SignalHandler::signal_handler(int signum) +{ + RCLCPP_INFO(SignalHandler::get_logger(), "signal_handler(signum=%d)", signum); + auto & instance = SignalHandler::get_global_signal_handler(); + auto old_signal_handler = instance.get_old_signal_handler(signum); + if ( + SIG_ERR != old_signal_handler && SIG_IGN != old_signal_handler && + SIG_DFL != old_signal_handler) + { + old_signal_handler(signum); + } + instance.signal_handler_common(); +} +#endif rclcpp::Logger & SignalHandler::get_logger() { - return g_logger; + return SignalHandler::get_global_signal_handler().logger_; } SignalHandler & SignalHandler::get_global_signal_handler() { - // This is initialized after the g_logger static global, ensuring - // SignalHandler::get_logger() may be called from the destructor of - // SignalHandler, according to this: - // - // Variables declared at block scope with the specifier static have static - // storage duration but are initialized the first time control passes - // through their declaration (unless their initialization is zero- or - // constant-initialization, which can be performed before the block is - // first entered). On all further calls, the declaration is skipped. - // - // -- https://en.cppreference.com/w/cpp/language/storage_duration#Static_local_variables - // - // Which is guaranteed to occur after static initialization for global (see: - // https://en.cppreference.com/w/cpp/language/initialization#Static_initialization), - // which is when g_logger will be initialized. - // And destruction will occur in the reverse order. static SignalHandler signal_handler; return signal_handler; } bool -SignalHandler::install() +SignalHandler::install(SignalHandlerOptions signal_handler_options) { std::lock_guard lock(install_mutex_); bool already_installed = installed_.exchange(true); if (already_installed) { return false; } + if (signal_handler_options == SignalHandlerOptions::None) { + return true; + } + signal_handlers_options_ = signal_handler_options; try { setup_wait_for_signal(); signal_received_.store(false); - SignalHandler::signal_handler_type signal_handler_argument; + SignalHandler::signal_handler_type handler_argument; #if defined(RCLCPP_HAS_SIGACTION) - memset(&signal_handler_argument, 0, sizeof(signal_handler_argument)); - sigemptyset(&signal_handler_argument.sa_mask); - signal_handler_argument.sa_sigaction = signal_handler; - signal_handler_argument.sa_flags = SA_SIGINFO; + memset(&handler_argument, 0, sizeof(handler_argument)); + sigemptyset(&handler_argument.sa_mask); + handler_argument.sa_sigaction = &this->signal_handler; + handler_argument.sa_flags = SA_SIGINFO; #else - signal_handler_argument = signal_handler; + handler_argument = &this->signal_handler; #endif + if ( + signal_handler_options == SignalHandlerOptions::SigInt || + signal_handler_options == SignalHandlerOptions::All) + { + old_sigint_handler_ = set_signal_handler(SIGINT, handler_argument); + } - old_signal_handler_ = SignalHandler::set_signal_handler(SIGINT, signal_handler_argument); + if ( + signal_handler_options == SignalHandlerOptions::SigTerm || + signal_handler_options == SignalHandlerOptions::All) + { + old_sigterm_handler_ = set_signal_handler(SIGTERM, handler_argument); + } signal_handler_thread_ = std::thread(&SignalHandler::deferred_signal_handler, this); } catch (...) { @@ -125,7 +176,19 @@ SignalHandler::uninstall() try { // TODO(wjwwood): what happens if someone overrides our signal handler then calls uninstall? // I think we need to assert that we're the current signal handler, and mitigate if not. - set_signal_handler(SIGINT, old_signal_handler_); + if ( + SignalHandlerOptions::SigInt == signal_handlers_options_ || + SignalHandlerOptions::All == signal_handlers_options_) + { + set_signal_handler(SIGINT, old_sigint_handler_); + } + if ( + SignalHandlerOptions::SigTerm == signal_handlers_options_ || + SignalHandlerOptions::All == signal_handlers_options_) + { + set_signal_handler(SIGTERM, old_sigterm_handler_); + } + signal_handlers_options_ = SignalHandlerOptions::None; RCLCPP_DEBUG(get_logger(), "SignalHandler::uninstall(): notifying deferred signal handler"); notify_signal_handler(); signal_handler_thread_.join(); @@ -151,98 +214,57 @@ SignalHandler::~SignalHandler() } catch (const std::exception & exc) { RCLCPP_ERROR( get_logger(), - "caught %s exception when uninstalling the sigint handler in rclcpp::~SignalHandler: %s", + "caught %s exception when uninstalling signal handlers in rclcpp::~SignalHandler: %s", rmw::impl::cpp::demangle(exc).c_str(), exc.what()); } catch (...) { RCLCPP_ERROR( get_logger(), - "caught unknown exception when uninstalling the sigint handler in rclcpp::~SignalHandler"); + "caught unknown exception when uninstalling signal handlers in rclcpp::~SignalHandler"); } } SignalHandler::signal_handler_type -SignalHandler::set_signal_handler( - int signal_value, - const SignalHandler::signal_handler_type & signal_handler) +SignalHandler::get_old_signal_handler(int signum) { - bool signal_handler_install_failed; - SignalHandler::signal_handler_type old_signal_handler; + if (SIGINT == signum) { + return old_sigint_handler_; + } else if (SIGTERM == signum) { + return old_sigterm_handler_; + } #if defined(RCLCPP_HAS_SIGACTION) - ssize_t ret = sigaction(signal_value, &signal_handler, &old_signal_handler); - signal_handler_install_failed = (ret == -1); + SignalHandler::signal_handler_type ret; + memset(&ret, 0, sizeof(ret)); + sigemptyset(&ret.sa_mask); + ret.sa_handler = SIG_DFL; + return ret; #else - old_signal_handler = std::signal(signal_value, signal_handler); - signal_handler_install_failed = (old_signal_handler == SIG_ERR); + return SIG_DFL; #endif - if (signal_handler_install_failed) { - char error_string[1024]; - rcutils_strerror(error_string, sizeof(error_string)); - auto msg = - "Failed to set SIGINT signal handler (" + std::to_string(errno) + "): " + error_string; - throw std::runtime_error(msg); - } - - return old_signal_handler; } void SignalHandler::signal_handler_common() { - signal_received_.store(true); + auto & instance = SignalHandler::get_global_signal_handler(); + instance.signal_received_.store(true); RCLCPP_DEBUG( get_logger(), - "signal_handler(): SIGINT received, notifying deferred signal handler"); - notify_signal_handler(); + "signal_handler(): notifying deferred signal handler"); + instance.notify_signal_handler(); } -#if defined(RCLCPP_HAS_SIGACTION) -void -SignalHandler::signal_handler(int signal_value, siginfo_t * siginfo, void * context) -{ - RCLCPP_INFO(get_logger(), "signal_handler(signal_value=%d)", signal_value); - - if (old_signal_handler_.sa_flags & SA_SIGINFO) { - if (old_signal_handler_.sa_sigaction != NULL) { - old_signal_handler_.sa_sigaction(signal_value, siginfo, context); - } - } else { - if ( - old_signal_handler_.sa_handler != NULL && // Is set - old_signal_handler_.sa_handler != SIG_DFL && // Is not default - old_signal_handler_.sa_handler != SIG_IGN) // Is not ignored - { - old_signal_handler_.sa_handler(signal_value); - } - } - - signal_handler_common(); -} -#else -void -SignalHandler::signal_handler(int signal_value) -{ - RCLCPP_INFO(get_logger(), "signal_handler(signal_value=%d)", signal_value); - - if (old_signal_handler_) { - old_signal_handler_(signal_value); - } - - signal_handler_common(); -} -#endif - void SignalHandler::deferred_signal_handler() { while (true) { if (signal_received_.exchange(false)) { - RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): SIGINT received, shutting down"); + RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): shutting down"); for (auto context_ptr : rclcpp::get_contexts()) { - if (context_ptr->get_init_options().shutdown_on_sigint) { + if (context_ptr->get_init_options().shutdown_on_signal) { RCLCPP_DEBUG( get_logger(), "deferred_signal_handler(): " - "shutting down rclcpp::Context @ %p, because it had shutdown_on_sigint == true", + "shutting down rclcpp::Context @ %p, because it had shutdown_on_signal == true", static_cast(context_ptr.get())); context_ptr->shutdown("signal handler"); } @@ -252,9 +274,11 @@ SignalHandler::deferred_signal_handler() RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): signal handling uninstalled"); break; } - RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): waiting for SIGINT or uninstall"); + RCLCPP_DEBUG( + get_logger(), "deferred_signal_handler(): waiting for SIGINT/SIGTERM or uninstall"); wait_for_signal(); - RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): woken up due to SIGINT or uninstall"); + RCLCPP_DEBUG( + get_logger(), "deferred_signal_handler(): woken up due to SIGINT/SIGTERM or uninstall"); } } @@ -356,3 +380,9 @@ SignalHandler::notify_signal_handler() noexcept } #endif } + +rclcpp::SignalHandlerOptions +SignalHandler::get_current_signal_handler_options() +{ + return signal_handlers_options_; +} diff --git a/rclcpp/src/rclcpp/signal_handler.hpp b/rclcpp/src/rclcpp/signal_handler.hpp index 81cb6e180c..0d3c399ccb 100644 --- a/rclcpp/src/rclcpp/signal_handler.hpp +++ b/rclcpp/src/rclcpp/signal_handler.hpp @@ -21,6 +21,7 @@ #include #include "rclcpp/logging.hpp" +#include "rclcpp/utilities.hpp" // includes for semaphore notification code #if defined(_WIN32) @@ -39,14 +40,14 @@ namespace rclcpp { -/// Responsible for manaaging the SIGINT signal handling. +/// Responsible for managing the SIGINT/SIGTERM signal handling. /** * This class is responsible for: * - * - installing the signal handler for SIGINT - * - uninstalling the signal handler for SIGINT - * - creating a thread to execute "on sigint" work outside of the signal handler - * - safely notifying the dedicated signal handling thread when receiving SIGINT + * - installing the signal handler for SIGINT/SIGTERM + * - uninstalling the signal handler for SIGINT/SIGTERM + * - creating a thread to execute "on signal" work outside of the signal handler + * - safely notifying the dedicated signal handling thread when receiving SIGINT/SIGTERM * - implementation of all of the signal handling work, like shutting down contexts * * \internal @@ -64,15 +65,18 @@ class SignalHandler final rclcpp::Logger & get_logger(); - /// Install the signal handler for SIGINT and start the dedicated signal handling thread. + /// Install the signal handler for SIGINT/SIGTERM and start the dedicated signal handling thread. /** - * Also stores the current signal handler to be called on SIGINT and to + * Also stores the current signal handler to be called on signal and to * restore when uninstalling this signal handler. + * + * \param signal_handler_options option to indicate which signal handlers should be installed. */ bool - install(); + install(SignalHandlerOptions signal_handler_options = SignalHandlerOptions::All); - /// Uninstall the signal handler for SIGINT and join the dedicated singal handling thread. + /// Uninstall the signal handler for SIGINT/SIGTERM and join the dedicated singal handling + /// thread. /** * Also restores the previous signal handler. */ @@ -83,26 +87,34 @@ class SignalHandler final bool is_installed(); -private: - SignalHandler() = default; - - ~SignalHandler(); + /// Get the current signal handler options. + /** + * If no signal handler is installed, SignalHandlerOptions::None is returned. + */ + rclcpp::SignalHandlerOptions + get_current_signal_handler_options(); +private: + /// Signal handler type, platform dependent. #if defined(RCLCPP_HAS_SIGACTION) using signal_handler_type = struct sigaction; #else using signal_handler_type = void (*)(int); #endif - // POSIX signal handler structure storage for the existing signal handler. - static SignalHandler::signal_handler_type old_signal_handler_; - /// Set the signal handler function. - static - SignalHandler::signal_handler_type - set_signal_handler(int signal_value, const SignalHandler::signal_handler_type & signal_handler); + + SignalHandler() = default; + + ~SignalHandler(); + + SignalHandler(const SignalHandler &) = delete; + SignalHandler(SignalHandler &&) = delete; + SignalHandler & + operator=(const SignalHandler &) = delete; + SignalHandler && + operator=(SignalHandler &&) = delete; /// Common signal handler code between sigaction and non-sigaction versions. - static void signal_handler_common(); @@ -127,7 +139,6 @@ class SignalHandler final * This must be called before wait_for_signal() or notify_signal_handler(). * This is not thread-safe. */ - static void setup_wait_for_signal(); @@ -137,7 +148,6 @@ class SignalHandler final * * This is not thread-safe. */ - static void teardown_wait_for_signal() noexcept; @@ -147,7 +157,6 @@ class SignalHandler final * * This is not thread-safe. */ - static void wait_for_signal(); @@ -158,29 +167,45 @@ class SignalHandler final * * This is thread-safe. */ - static void notify_signal_handler() noexcept; + static + signal_handler_type + set_signal_handler( + int signal_value, + const signal_handler_type & signal_handler); + + signal_handler_type + get_old_signal_handler(int signum); + + rclcpp::SignalHandlerOptions signal_handlers_options_ = rclcpp::SignalHandlerOptions::None; + + signal_handler_type old_sigint_handler_; + signal_handler_type old_sigterm_handler_; + + // logger instance + rclcpp::Logger logger_ = rclcpp::get_logger("rclcpp"); + // Whether or not a signal has been received. - static std::atomic_bool signal_received_; + std::atomic_bool signal_received_ = false; // A thread to which singal handling tasks are deferred. std::thread signal_handler_thread_; // A mutex used to synchronize the install() and uninstall() methods. std::mutex install_mutex_; // Whether or not the signal handler has been installed. - std::atomic_bool installed_{false}; + std::atomic_bool installed_ = false; // Whether or not the semaphore for wait_for_signal is setup. - static std::atomic_bool wait_for_signal_is_setup_; + std::atomic_bool wait_for_signal_is_setup_; // Storage for the wait_for_signal semaphore. #if defined(_WIN32) - static HANDLE signal_handler_sem_; + HANDLE signal_handler_sem_; #elif defined(__APPLE__) - static dispatch_semaphore_t signal_handler_sem_; + dispatch_semaphore_t signal_handler_sem_; #else // posix - static sem_t signal_handler_sem_; + sem_t signal_handler_sem_; #endif }; diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index c3e3915d79..dcc79c96b8 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -31,18 +31,22 @@ namespace rclcpp { void -init(int argc, char const * const argv[], const InitOptions & init_options) +init( + int argc, + char const * const argv[], + const InitOptions & init_options, + SignalHandlerOptions signal_handler_options) { using rclcpp::contexts::get_global_default_context; get_global_default_context()->init(argc, argv, init_options); // Install the signal handlers. - install_signal_handlers(); + install_signal_handlers(signal_handler_options); } bool -install_signal_handlers() +install_signal_handlers(SignalHandlerOptions signal_handler_options) { - return SignalHandler::get_global_signal_handler().install(); + return SignalHandler::get_global_signal_handler().install(signal_handler_options); } bool @@ -51,6 +55,13 @@ signal_handlers_installed() return SignalHandler::get_global_signal_handler().is_installed(); } +SignalHandlerOptions +get_current_signal_handler_options() +{ + return SignalHandler::get_global_signal_handler().get_current_signal_handler_options(); +} + + bool uninstall_signal_handlers() { From 942b74c8bdc9b9089c05d7b3961f5aaaa2b1eca2 Mon Sep 17 00:00:00 2001 From: Geoffrey Biggs Date: Wed, 13 Oct 2021 23:36:19 +0900 Subject: [PATCH 024/455] Fix dangerous std::bind capture in ParameterEventHandler implementation (#1770) Signed-off-by: Geoffrey Biggs --- .../rclcpp/parameter_event_handler.hpp | 51 +++++++++++-------- rclcpp/src/rclcpp/parameter_event_handler.cpp | 26 +++++----- .../rclcpp/test_parameter_event_handler.cpp | 6 +-- 3 files changed, 46 insertions(+), 37 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter_event_handler.hpp b/rclcpp/include/rclcpp/parameter_event_handler.hpp index 084cf4afbd..04de6c1862 100644 --- a/rclcpp/include/rclcpp/parameter_event_handler.hpp +++ b/rclcpp/include/rclcpp/parameter_event_handler.hpp @@ -173,9 +173,13 @@ class ParameterEventHandler { auto node_topics = rclcpp::node_interfaces::get_node_topics_interface(node); + callbacks_ = std::make_shared(); + event_subscription_ = rclcpp::create_subscription( node_topics, "/parameter_events", qos, - std::bind(&ParameterEventHandler::event_callback, this, std::placeholders::_1)); + [callbacks = callbacks_](const rcl_interfaces::msg::ParameterEvent & event) { + callbacks->event_callback(event); + }); } using ParameterEventCallbackType = @@ -285,17 +289,6 @@ class ParameterEventHandler using CallbacksContainerType = std::list; protected: - /// Callback for parameter events subscriptions. - RCLCPP_PUBLIC - void - event_callback(const rcl_interfaces::msg::ParameterEvent & event); - - // Utility function for resolving node path. - std::string resolve_path(const std::string & path); - - // Node interface used for base functionality - std::shared_ptr node_base_; - // *INDENT-OFF* Uncrustify doesn't handle indented public/private labels // Hash function for string pair required in std::unordered_map // See: https://stackoverflow.com/questions/35985960/c-why-is-boosthash-combine-the-best-way-to-combine-hash-values @@ -319,18 +312,34 @@ class ParameterEventHandler }; // *INDENT-ON* - // Map container for registered parameters - std::unordered_map< - std::pair, - CallbacksContainerType, - StringPairHash - > parameter_callbacks_; + struct Callbacks + { + std::recursive_mutex mutex_; + + // Map container for registered parameters + std::unordered_map< + std::pair, + CallbacksContainerType, + StringPairHash + > parameter_callbacks_; - rclcpp::Subscription::SharedPtr event_subscription_; + std::list event_callbacks_; + + /// Callback for parameter events subscriptions. + RCLCPP_PUBLIC + void + event_callback(const rcl_interfaces::msg::ParameterEvent & event); + }; - std::list event_callbacks_; + std::shared_ptr callbacks_; - std::recursive_mutex mutex_; + // Utility function for resolving node path. + std::string resolve_path(const std::string & path); + + // Node interface used for base functionality + std::shared_ptr node_base_; + + rclcpp::Subscription::SharedPtr event_subscription_; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/parameter_event_handler.cpp b/rclcpp/src/rclcpp/parameter_event_handler.cpp index e67a5994ad..6ba5266809 100644 --- a/rclcpp/src/rclcpp/parameter_event_handler.cpp +++ b/rclcpp/src/rclcpp/parameter_event_handler.cpp @@ -29,10 +29,10 @@ ParameterEventCallbackHandle::SharedPtr ParameterEventHandler::add_parameter_event_callback( ParameterEventCallbackType callback) { - std::lock_guard lock(mutex_); + std::lock_guard lock(callbacks_->mutex_); auto handle = std::make_shared(); handle->callback = callback; - event_callbacks_.emplace_front(handle); + callbacks_->event_callbacks_.emplace_front(handle); return handle; } @@ -41,15 +41,15 @@ void ParameterEventHandler::remove_parameter_event_callback( ParameterEventCallbackHandle::SharedPtr callback_handle) { - std::lock_guard lock(mutex_); + std::lock_guard lock(callbacks_->mutex_); auto it = std::find_if( - event_callbacks_.begin(), - event_callbacks_.end(), + callbacks_->event_callbacks_.begin(), + callbacks_->event_callbacks_.end(), [callback_handle](const auto & weak_handle) { return callback_handle.get() == weak_handle.lock().get(); }); - if (it != event_callbacks_.end()) { - event_callbacks_.erase(it); + if (it != callbacks_->event_callbacks_.end()) { + callbacks_->event_callbacks_.erase(it); } else { throw std::runtime_error("Callback doesn't exist"); } @@ -61,7 +61,7 @@ ParameterEventHandler::add_parameter_callback( ParameterCallbackType callback, const std::string & node_name) { - std::lock_guard lock(mutex_); + std::lock_guard lock(callbacks_->mutex_); auto full_node_name = resolve_path(node_name); auto handle = std::make_shared(); @@ -69,7 +69,7 @@ ParameterEventHandler::add_parameter_callback( handle->parameter_name = parameter_name; handle->node_name = full_node_name; // the last callback registered is executed first. - parameter_callbacks_[{parameter_name, full_node_name}].emplace_front(handle); + callbacks_->parameter_callbacks_[{parameter_name, full_node_name}].emplace_front(handle); return handle; } @@ -78,9 +78,9 @@ void ParameterEventHandler::remove_parameter_callback( ParameterCallbackHandle::SharedPtr callback_handle) { - std::lock_guard lock(mutex_); + std::lock_guard lock(callbacks_->mutex_); auto handle = callback_handle.get(); - auto & container = parameter_callbacks_[{handle->parameter_name, handle->node_name}]; + auto & container = callbacks_->parameter_callbacks_[{handle->parameter_name, handle->node_name}]; auto it = std::find_if( container.begin(), container.end(), @@ -90,7 +90,7 @@ ParameterEventHandler::remove_parameter_callback( if (it != container.end()) { container.erase(it); if (container.empty()) { - parameter_callbacks_.erase({handle->parameter_name, handle->node_name}); + callbacks_->parameter_callbacks_.erase({handle->parameter_name, handle->node_name}); } } else { throw std::runtime_error("Callback doesn't exist"); @@ -158,7 +158,7 @@ ParameterEventHandler::get_parameters_from_event( } void -ParameterEventHandler::event_callback(const rcl_interfaces::msg::ParameterEvent & event) +ParameterEventHandler::Callbacks::event_callback(const rcl_interfaces::msg::ParameterEvent & event) { std::lock_guard lock(mutex_); diff --git a/rclcpp/test/rclcpp/test_parameter_event_handler.cpp b/rclcpp/test/rclcpp/test_parameter_event_handler.cpp index 097b0ee391..9bdd96eab4 100644 --- a/rclcpp/test/rclcpp/test_parameter_event_handler.cpp +++ b/rclcpp/test/rclcpp/test_parameter_event_handler.cpp @@ -29,17 +29,17 @@ class TestParameterEventHandler : public rclcpp::ParameterEventHandler void test_event(rcl_interfaces::msg::ParameterEvent::ConstSharedPtr event) { - event_callback(*event); + callbacks_->event_callback(*event); } size_t num_event_callbacks() { - return event_callbacks_.size(); + return callbacks_->event_callbacks_.size(); } size_t num_parameter_callbacks() { - return parameter_callbacks_.size(); + return callbacks_->parameter_callbacks_.size(); } }; From a569214273acb2015ca0085d6ccf77822083e8a6 Mon Sep 17 00:00:00 2001 From: Geoffrey Biggs Date: Sat, 16 Oct 2021 00:52:41 +0900 Subject: [PATCH 025/455] Fix dangerous std::bind capture in TimeSource implementation (#1768) * Convert internal state into shareable structs * Add documentation * PIMPL the class * Use a weak_ptr to avoid a potential dangling reference * Comply with the rule of 5 Signed-off-by: Geoffrey Biggs --- rclcpp/include/rclcpp/time_source.hpp | 105 +--- rclcpp/src/rclcpp/time_source.cpp | 776 +++++++++++++++--------- rclcpp/test/rclcpp/test_time_source.cpp | 4 +- 3 files changed, 532 insertions(+), 353 deletions(-) diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp index 0a40e60201..9d067d933c 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -74,6 +74,14 @@ class TimeSource const rclcpp::QoS & qos = rclcpp::ClockQoS(), bool use_clock_thread = true); + // The TimeSource is uncopyable + TimeSource(const TimeSource &) = delete; + TimeSource & operator=(const TimeSource &) = delete; + + // The TimeSource is moveable + TimeSource(TimeSource &&) = default; + TimeSource & operator=(TimeSource &&) = default; + /// Attach node to the time source. /** * \param node std::shared pointer to a initialized node @@ -116,89 +124,36 @@ class TimeSource RCLCPP_PUBLIC void attachClock(rclcpp::Clock::SharedPtr clock); - /// Detach a clock to the time source + /// Detach a clock from the time source RCLCPP_PUBLIC void detachClock(rclcpp::Clock::SharedPtr clock); + /// Get whether a separate clock thread is used or not + RCLCPP_PUBLIC + bool get_use_clock_thread(); + + /// Set whether to use a separate clock thread or not + RCLCPP_PUBLIC + void set_use_clock_thread(bool use_clock_thread); + + /// Check if the clock thread is joinable + RCLCPP_PUBLIC + bool clock_thread_is_joinable(); + /// TimeSource Destructor RCLCPP_PUBLIC ~TimeSource(); -protected: - // Dedicated thread for clock subscription. - bool use_clock_thread_; - std::thread clock_executor_thread_; - private: - // Preserve the node reference - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; - rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; - rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; - rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; - rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; - - // Store (and update on node attach) logger for logging. - Logger logger_; - - // QoS of the clock subscription. - rclcpp::QoS qos_; - - // The subscription for the clock callback - using MessageT = rosgraph_msgs::msg::Clock; - using Alloc = std::allocator; - using SubscriptionT = rclcpp::Subscription; - std::shared_ptr clock_subscription_{nullptr}; - std::mutex clock_sub_lock_; - rclcpp::CallbackGroup::SharedPtr clock_callback_group_; - rclcpp::executors::SingleThreadedExecutor::SharedPtr clock_executor_; - std::promise cancel_clock_executor_promise_; - - // The clock callback itself - void clock_cb(std::shared_ptr msg); - - // Create the subscription for the clock topic - void create_clock_sub(); - - // Destroy the subscription for the clock topic - void destroy_clock_sub(); - - // Parameter Event subscription - using ParamMessageT = rcl_interfaces::msg::ParameterEvent; - using ParamSubscriptionT = rclcpp::Subscription; - std::shared_ptr parameter_subscription_; - - // Callback for parameter updates - void on_parameter_event(std::shared_ptr event); - - // An enum to hold the parameter state - enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE}; - UseSimTimeParameterState parameter_state_; - - // An internal method to use in the clock callback that iterates and enables all clocks - void enable_ros_time(); - // An internal method to use in the clock callback that iterates and disables all clocks - void disable_ros_time(); - - // Internal helper functions used inside iterators - static void set_clock( - const builtin_interfaces::msg::Time::SharedPtr msg, - bool set_ros_time_enabled, - rclcpp::Clock::SharedPtr clock); - - // Local storage of validity of ROS time - // This is needed when new clocks are added. - bool ros_time_active_{false}; - // Last set message to be passed to newly registered clocks - std::shared_ptr last_msg_set_; - - // A lock to protect iterating the associated_clocks_ field. - std::mutex clock_list_lock_; - // A vector to store references to associated clocks. - std::vector associated_clocks_; - // A handler for the use_sim_time parameter callback. - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr sim_time_cb_handler_{nullptr}; + class ClocksState; + std::shared_ptr clocks_state_; + + class NodeState; + std::shared_ptr node_state_; + + // Preserve the arguments received by the constructor for reuse at runtime + bool constructed_use_clock_thread_; + rclcpp::QoS constructed_qos_; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 140b5531d2..61c5fdd785 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -33,29 +33,499 @@ namespace rclcpp { +class TimeSource::ClocksState : public std::enable_shared_from_this +{ +public: + ClocksState() + : logger_(rclcpp::get_logger("rclcpp")) + { + } + + // An internal method to use in the clock callback that iterates and enables all clocks + void enable_ros_time() + { + if (ros_time_active_) { + // already enabled no-op + return; + } + + // Local storage + ros_time_active_ = true; + + // Update all attached clocks to zero or last recorded time + std::lock_guard guard(clock_list_lock_); + auto time_msg = std::make_shared(); + if (last_msg_set_) { + time_msg = std::make_shared(last_msg_set_->clock); + } + for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { + set_clock(time_msg, true, *it); + } + } + + // An internal method to use in the clock callback that iterates and disables all clocks + void disable_ros_time() + { + if (!ros_time_active_) { + // already disabled no-op + return; + } + + // Local storage + ros_time_active_ = false; + + // Update all attached clocks + std::lock_guard guard(clock_list_lock_); + for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { + auto msg = std::make_shared(); + set_clock(msg, false, *it); + } + } + + // Check if ROS time is active + bool is_ros_time_active() const + { + return ros_time_active_; + } + + // Attach a clock + void attachClock(rclcpp::Clock::SharedPtr clock) + { + if (clock->get_clock_type() != RCL_ROS_TIME) { + throw std::invalid_argument("Cannot attach clock to a time source that's not a ROS clock"); + } + + std::lock_guard guard(clock_list_lock_); + associated_clocks_.push_back(clock); + // Set the clock to zero unless there's a recently received message + auto time_msg = std::make_shared(); + if (last_msg_set_) { + time_msg = std::make_shared(last_msg_set_->clock); + } + set_clock(time_msg, ros_time_active_, clock); + } + + // Detach a clock + void detachClock(rclcpp::Clock::SharedPtr clock) + { + std::lock_guard guard(clock_list_lock_); + auto result = std::find(associated_clocks_.begin(), associated_clocks_.end(), clock); + if (result != associated_clocks_.end()) { + associated_clocks_.erase(result); + } else { + RCLCPP_ERROR(logger_, "failed to remove clock"); + } + } + + // Internal helper function used inside iterators + static void set_clock( + const builtin_interfaces::msg::Time::SharedPtr msg, + bool set_ros_time_enabled, + rclcpp::Clock::SharedPtr clock) + { + std::lock_guard clock_guard(clock->get_clock_mutex()); + + // Do change + if (!set_ros_time_enabled && clock->ros_time_is_active()) { + auto ret = rcl_disable_ros_time_override(clock->get_clock_handle()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Failed to disable ros_time_override_status"); + } + } else if (set_ros_time_enabled && !clock->ros_time_is_active()) { + auto ret = rcl_enable_ros_time_override(clock->get_clock_handle()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Failed to enable ros_time_override_status"); + } + } + + auto ret = rcl_set_ros_time_override( + clock->get_clock_handle(), + rclcpp::Time(*msg).nanoseconds()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Failed to set ros_time_override_status"); + } + } + + // Internal helper function + void set_all_clocks( + const builtin_interfaces::msg::Time::SharedPtr msg, + bool set_ros_time_enabled) + { + std::lock_guard guard(clock_list_lock_); + for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { + set_clock(msg, set_ros_time_enabled, *it); + } + } + + // Cache the last clock message received + void cache_last_msg(std::shared_ptr msg) + { + last_msg_set_ = msg; + } + +private: + // Store (and update on node attach) logger for logging. + Logger logger_; + + // A lock to protect iterating the associated_clocks_ field. + std::mutex clock_list_lock_; + // A vector to store references to associated clocks. + std::vector associated_clocks_; + + // Local storage of validity of ROS time + // This is needed when new clocks are added. + bool ros_time_active_{false}; + // Last set message to be passed to newly registered clocks + std::shared_ptr last_msg_set_; +}; + +class TimeSource::NodeState : public std::enable_shared_from_this +{ +public: + NodeState(std::weak_ptr clocks_state, const rclcpp::QoS & qos, bool use_clock_thread) + : clocks_state_(std::move(clocks_state)), + use_clock_thread_(use_clock_thread), + logger_(rclcpp::get_logger("rclcpp")), + qos_(qos) + { + } + + ~NodeState() + { + if ( + node_base_ || node_topics_ || node_graph_ || node_services_ || + node_logging_ || node_clock_ || node_parameters_) + { + detachNode(); + } + } + + // Check if a clock thread will be used + bool get_use_clock_thread() + { + return use_clock_thread_; + } + + // Set whether a clock thread will be used + void set_use_clock_thread(bool use_clock_thread) + { + use_clock_thread_ = use_clock_thread; + } + + // Check if the clock thread is joinable + bool clock_thread_is_joinable() + { + return clock_executor_thread_.joinable(); + } + + // Attach a node to this time source + void attachNode( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface) + { + node_base_ = node_base_interface; + node_topics_ = node_topics_interface; + node_graph_ = node_graph_interface; + node_services_ = node_services_interface; + node_logging_ = node_logging_interface; + node_clock_ = node_clock_interface; + node_parameters_ = node_parameters_interface; + // TODO(tfoote): Update QOS + + logger_ = node_logging_->get_logger(); + + // Though this defaults to false, it can be overridden by initial parameter values for the + // node, which may be given by the user at the node's construction or even by command-line + // arguments. + rclcpp::ParameterValue use_sim_time_param; + const std::string use_sim_time_name = "use_sim_time"; + if (!node_parameters_->has_parameter(use_sim_time_name)) { + use_sim_time_param = node_parameters_->declare_parameter( + use_sim_time_name, + rclcpp::ParameterValue(false), + rcl_interfaces::msg::ParameterDescriptor()); + } else { + use_sim_time_param = node_parameters_->get_parameter(use_sim_time_name).get_parameter_value(); + } + if (use_sim_time_param.get_type() == rclcpp::PARAMETER_BOOL) { + if (use_sim_time_param.get()) { + parameter_state_ = SET_TRUE; + // There should be no way to call this attachNode when the clocks_state_ pointer is not + // valid because it means the TimeSource is being destroyed + if (auto clocks_state_ptr = clocks_state_.lock()) { + clocks_state_ptr->enable_ros_time(); + create_clock_sub(); + } + } + } else { + RCLCPP_ERROR( + logger_, "Invalid type '%s' for parameter 'use_sim_time', should be 'bool'", + rclcpp::to_string(use_sim_time_param.get_type()).c_str()); + } + sim_time_cb_handler_ = node_parameters_->add_on_set_parameters_callback( + [use_sim_time_name](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto & parameter : parameters) { + if ( + parameter.get_name() == use_sim_time_name && + parameter.get_type() != rclcpp::PARAMETER_BOOL) + { + result.successful = false; + result.reason = "'" + use_sim_time_name + "' must be a bool"; + break; + } + } + return result; + }); + + // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609 + parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event( + node_topics_, + [state = std::weak_ptr(this->shared_from_this())]( + std::shared_ptr event) { + if (auto state_ptr = state.lock()) { + state_ptr->on_parameter_event(event); + } + // Do nothing if the pointer could not be locked because it means the TimeSource is now + // without an attached node + }); + } + + // Detach the attached node + void detachNode() + { + // There should be no way to call detachNode when the clocks_state_ pointer is not valid + // because it means the TimeSource is being destroyed + if (auto clocks_state_ptr = clocks_state_.lock()) { + clocks_state_ptr->disable_ros_time(); + } + destroy_clock_sub(); + parameter_subscription_.reset(); + node_base_.reset(); + node_topics_.reset(); + node_graph_.reset(); + node_services_.reset(); + node_logging_.reset(); + node_clock_.reset(); + if (sim_time_cb_handler_ && node_parameters_) { + node_parameters_->remove_on_set_parameters_callback(sim_time_cb_handler_.get()); + } + sim_time_cb_handler_.reset(); + node_parameters_.reset(); + } + +private: + std::weak_ptr clocks_state_; + + // Dedicated thread for clock subscription. + bool use_clock_thread_; + std::thread clock_executor_thread_; + + // Preserve the node reference + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; + + // Store (and update on node attach) logger for logging. + Logger logger_; + + // QoS of the clock subscription. + rclcpp::QoS qos_; + + // The subscription for the clock callback + using MessageT = rosgraph_msgs::msg::Clock; + using Alloc = std::allocator; + using SubscriptionT = rclcpp::Subscription; + std::shared_ptr clock_subscription_{nullptr}; + std::mutex clock_sub_lock_; + rclcpp::CallbackGroup::SharedPtr clock_callback_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr clock_executor_; + std::promise cancel_clock_executor_promise_; + + // The clock callback itself + void clock_cb(std::shared_ptr msg) + { + auto clocks_state_ptr = clocks_state_.lock(); + if (!clocks_state_ptr) { + // The clock_state_ pointer is no longer valid, implying that the TimeSource object is being + // destroyed, so do nothing + return; + } + if (!clocks_state_ptr->is_ros_time_active() && SET_TRUE == this->parameter_state_) { + clocks_state_ptr->enable_ros_time(); + } + // Cache the last message in case a new clock is attached. + clocks_state_ptr->cache_last_msg(msg); + auto time_msg = std::make_shared(msg->clock); + + if (SET_TRUE == this->parameter_state_) { + clocks_state_ptr->set_all_clocks(time_msg, true); + } + } + + // Create the subscription for the clock topic + void create_clock_sub() + { + std::lock_guard guard(clock_sub_lock_); + if (clock_subscription_) { + // Subscription already created. + return; + } + + rclcpp::SubscriptionOptions options; + options.qos_overriding_options = rclcpp::QosOverridingOptions( + { + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Reliability, + }); + + if (use_clock_thread_) { + clock_callback_group_ = node_base_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false + ); + options.callback_group = clock_callback_group_; + rclcpp::ExecutorOptions exec_options; + exec_options.context = node_base_->get_context(); + clock_executor_ = + std::make_shared(exec_options); + if (!clock_executor_thread_.joinable()) { + cancel_clock_executor_promise_ = std::promise{}; + clock_executor_thread_ = std::thread( + [this]() { + auto future = cancel_clock_executor_promise_.get_future(); + clock_executor_->add_callback_group(clock_callback_group_, node_base_); + clock_executor_->spin_until_future_complete(future); + } + ); + } + } + + clock_subscription_ = rclcpp::create_subscription( + node_parameters_, + node_topics_, + "/clock", + rclcpp::QoS(KeepLast(1)).best_effort(), + [state = std::weak_ptr(this->shared_from_this())]( + std::shared_ptr msg) { + if (auto state_ptr = state.lock()) { + state_ptr->clock_cb(msg); + } + // Do nothing if the pointer could not be locked because it means the TimeSource is now + // without an attached node + }, + options + ); + } + + // Destroy the subscription for the clock topic + void destroy_clock_sub() + { + std::lock_guard guard(clock_sub_lock_); + if (clock_executor_thread_.joinable()) { + cancel_clock_executor_promise_.set_value(); + clock_executor_->cancel(); + clock_executor_thread_.join(); + clock_executor_->remove_callback_group(clock_callback_group_); + } + clock_subscription_.reset(); + } + + // Parameter Event subscription + using ParamMessageT = rcl_interfaces::msg::ParameterEvent; + using ParamSubscriptionT = rclcpp::Subscription; + std::shared_ptr parameter_subscription_; + + // Callback for parameter updates + void on_parameter_event(std::shared_ptr event) + { + auto clocks_state_ptr = clocks_state_.lock(); + if (!clocks_state_ptr) { + // The clock_state_ pointer is no longer valid, implying that the TimeSource object is being + // destroyed, so do nothing + return; + } + // Filter out events on 'use_sim_time' parameter instances in other nodes. + if (event->node != node_base_->get_fully_qualified_name()) { + return; + } + // Filter for only 'use_sim_time' being added or changed. + rclcpp::ParameterEventsFilter filter(event, {"use_sim_time"}, + {rclcpp::ParameterEventsFilter::EventType::NEW, + rclcpp::ParameterEventsFilter::EventType::CHANGED}); + for (auto & it : filter.get_events()) { + if (it.second->value.type != ParameterType::PARAMETER_BOOL) { + RCLCPP_ERROR(logger_, "use_sim_time parameter cannot be set to anything but a bool"); + continue; + } + if (it.second->value.bool_value) { + parameter_state_ = SET_TRUE; + clocks_state_ptr->enable_ros_time(); + create_clock_sub(); + } else { + parameter_state_ = SET_FALSE; + clocks_state_ptr->disable_ros_time(); + destroy_clock_sub(); + } + } + // Handle the case that use_sim_time was deleted. + rclcpp::ParameterEventsFilter deleted(event, {"use_sim_time"}, + {rclcpp::ParameterEventsFilter::EventType::DELETED}); + for (auto & it : deleted.get_events()) { + (void) it; // if there is a match it's already matched, don't bother reading it. + // If the parameter is deleted mark it as unset but dont' change state. + parameter_state_ = UNSET; + } + } + + // An enum to hold the parameter state + enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE}; + UseSimTimeParameterState parameter_state_; + + // A handler for the use_sim_time parameter callback. + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr sim_time_cb_handler_{nullptr}; +}; + TimeSource::TimeSource( std::shared_ptr node, const rclcpp::QoS & qos, bool use_clock_thread) -: use_clock_thread_(use_clock_thread), - logger_(rclcpp::get_logger("rclcpp")), - qos_(qos) +: constructed_use_clock_thread_(use_clock_thread), + constructed_qos_(qos) { - this->attachNode(node); + clocks_state_ = std::make_shared(); + node_state_ = std::make_shared(clocks_state_->weak_from_this(), qos, use_clock_thread); + attachNode(node); } TimeSource::TimeSource( const rclcpp::QoS & qos, bool use_clock_thread) -: use_clock_thread_(use_clock_thread), - logger_(rclcpp::get_logger("rclcpp")), - qos_(qos) +: constructed_use_clock_thread_(use_clock_thread), + constructed_qos_(qos) { + clocks_state_ = std::make_shared(); + node_state_ = std::make_shared(clocks_state_->weak_from_this(), qos, use_clock_thread); } void TimeSource::attachNode(rclcpp::Node::SharedPtr node) { - use_clock_thread_ = node->get_node_options().use_clock_thread(); + node_state_->set_use_clock_thread(node->get_node_options().use_clock_thread()); attachNode( node->get_node_base_interface(), node->get_node_topics_interface(), @@ -75,298 +545,52 @@ void TimeSource::attachNode( rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface) { - node_base_ = node_base_interface; - node_topics_ = node_topics_interface; - node_graph_ = node_graph_interface; - node_services_ = node_services_interface; - node_logging_ = node_logging_interface; - node_clock_ = node_clock_interface; - node_parameters_ = node_parameters_interface; - // TODO(tfoote): Update QOS - - logger_ = node_logging_->get_logger(); - - // Though this defaults to false, it can be overridden by initial parameter values for the node, - // which may be given by the user at the node's construction or even by command-line arguments. - rclcpp::ParameterValue use_sim_time_param; - const std::string use_sim_time_name = "use_sim_time"; - if (!node_parameters_->has_parameter(use_sim_time_name)) { - use_sim_time_param = node_parameters_->declare_parameter( - use_sim_time_name, - rclcpp::ParameterValue(false), - rcl_interfaces::msg::ParameterDescriptor()); - } else { - use_sim_time_param = node_parameters_->get_parameter(use_sim_time_name).get_parameter_value(); - } - if (use_sim_time_param.get_type() == rclcpp::PARAMETER_BOOL) { - if (use_sim_time_param.get()) { - parameter_state_ = SET_TRUE; - enable_ros_time(); - create_clock_sub(); - } - } else { - RCLCPP_ERROR( - logger_, "Invalid type '%s' for parameter 'use_sim_time', should be 'bool'", - rclcpp::to_string(use_sim_time_param.get_type()).c_str()); - } - sim_time_cb_handler_ = node_parameters_->add_on_set_parameters_callback( - [use_sim_time_name](const std::vector & parameters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - for (const auto & parameter : parameters) { - if ( - parameter.get_name() == use_sim_time_name && - parameter.get_type() != rclcpp::PARAMETER_BOOL) - { - result.successful = false; - result.reason = "'" + use_sim_time_name + "' must be a bool"; - break; - } - } - return result; - }); - - // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609 - parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event( - node_topics_, - std::bind(&TimeSource::on_parameter_event, this, std::placeholders::_1)); + node_state_->attachNode( + std::move(node_base_interface), + std::move(node_topics_interface), + std::move(node_graph_interface), + std::move(node_services_interface), + std::move(node_logging_interface), + std::move(node_clock_interface), + std::move(node_parameters_interface)); } void TimeSource::detachNode() { - this->ros_time_active_ = false; - destroy_clock_sub(); - parameter_subscription_.reset(); - node_base_.reset(); - node_topics_.reset(); - node_graph_.reset(); - node_services_.reset(); - node_logging_.reset(); - node_clock_.reset(); - if (sim_time_cb_handler_ && node_parameters_) { - node_parameters_->remove_on_set_parameters_callback(sim_time_cb_handler_.get()); - } - sim_time_cb_handler_.reset(); - node_parameters_.reset(); - disable_ros_time(); + node_state_.reset(); + node_state_ = std::make_shared( + clocks_state_->weak_from_this(), + constructed_qos_, + constructed_use_clock_thread_); } void TimeSource::attachClock(std::shared_ptr clock) { - if (clock->get_clock_type() != RCL_ROS_TIME) { - throw std::invalid_argument("Cannot attach clock to a time source that's not a ROS clock"); - } - - std::lock_guard guard(clock_list_lock_); - associated_clocks_.push_back(clock); - // Set the clock to zero unless there's a recently received message - auto time_msg = std::make_shared(); - if (last_msg_set_) { - time_msg = std::make_shared(last_msg_set_->clock); - } - set_clock(time_msg, ros_time_active_, clock); + clocks_state_->attachClock(std::move(clock)); } void TimeSource::detachClock(std::shared_ptr clock) { - std::lock_guard guard(clock_list_lock_); - auto result = std::find(associated_clocks_.begin(), associated_clocks_.end(), clock); - if (result != associated_clocks_.end()) { - associated_clocks_.erase(result); - } else { - RCLCPP_ERROR(logger_, "failed to remove clock"); - } + clocks_state_->detachClock(std::move(clock)); } -TimeSource::~TimeSource() +bool TimeSource::get_use_clock_thread() { - if ( - node_base_ || node_topics_ || node_graph_ || node_services_ || - node_logging_ || node_clock_ || node_parameters_) - { - this->detachNode(); - } + return node_state_->get_use_clock_thread(); } -void TimeSource::set_clock( - const builtin_interfaces::msg::Time::SharedPtr msg, bool set_ros_time_enabled, - std::shared_ptr clock) +void TimeSource::set_use_clock_thread(bool use_clock_thread) { - std::lock_guard clock_guard(clock->get_clock_mutex()); - - // Do change - if (!set_ros_time_enabled && clock->ros_time_is_active()) { - auto ret = rcl_disable_ros_time_override(clock->get_clock_handle()); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "Failed to disable ros_time_override_status"); - } - } else if (set_ros_time_enabled && !clock->ros_time_is_active()) { - auto ret = rcl_enable_ros_time_override(clock->get_clock_handle()); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "Failed to enable ros_time_override_status"); - } - } - - auto ret = rcl_set_ros_time_override(clock->get_clock_handle(), rclcpp::Time(*msg).nanoseconds()); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "Failed to set ros_time_override_status"); - } + node_state_->set_use_clock_thread(use_clock_thread); } -void TimeSource::clock_cb(std::shared_ptr msg) +bool TimeSource::clock_thread_is_joinable() { - if (!this->ros_time_active_ && SET_TRUE == this->parameter_state_) { - enable_ros_time(); - } - // Cache the last message in case a new clock is attached. - last_msg_set_ = msg; - auto time_msg = std::make_shared(msg->clock); - - if (SET_TRUE == this->parameter_state_) { - std::lock_guard guard(clock_list_lock_); - for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { - set_clock(time_msg, true, *it); - } - } + return node_state_->clock_thread_is_joinable(); } -void TimeSource::create_clock_sub() -{ - std::lock_guard guard(clock_sub_lock_); - if (clock_subscription_) { - // Subscription already created. - return; - } - - rclcpp::SubscriptionOptions options; - options.qos_overriding_options = rclcpp::QosOverridingOptions( - { - rclcpp::QosPolicyKind::Depth, - rclcpp::QosPolicyKind::Durability, - rclcpp::QosPolicyKind::History, - rclcpp::QosPolicyKind::Reliability, - }); - - if (use_clock_thread_) { - clock_callback_group_ = node_base_->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive, - false - ); - options.callback_group = clock_callback_group_; - rclcpp::ExecutorOptions exec_options; - exec_options.context = node_base_->get_context(); - clock_executor_ = - std::make_shared(exec_options); - if (!clock_executor_thread_.joinable()) { - cancel_clock_executor_promise_ = std::promise{}; - clock_executor_thread_ = std::thread( - [this]() { - auto future = cancel_clock_executor_promise_.get_future(); - clock_executor_->add_callback_group(clock_callback_group_, node_base_); - clock_executor_->spin_until_future_complete(future); - } - ); - } - } - - clock_subscription_ = rclcpp::create_subscription( - node_parameters_, - node_topics_, - "/clock", - rclcpp::QoS(KeepLast(1)).best_effort(), - std::bind(&TimeSource::clock_cb, this, std::placeholders::_1), - options - ); -} - -void TimeSource::destroy_clock_sub() -{ - std::lock_guard guard(clock_sub_lock_); - if (clock_executor_thread_.joinable()) { - cancel_clock_executor_promise_.set_value(); - clock_executor_->cancel(); - clock_executor_thread_.join(); - clock_executor_->remove_callback_group(clock_callback_group_); - } - clock_subscription_.reset(); -} - -void TimeSource::on_parameter_event( - std::shared_ptr event) -{ - // Filter out events on 'use_sim_time' parameter instances in other nodes. - if (event->node != node_base_->get_fully_qualified_name()) { - return; - } - // Filter for only 'use_sim_time' being added or changed. - rclcpp::ParameterEventsFilter filter(event, {"use_sim_time"}, - {rclcpp::ParameterEventsFilter::EventType::NEW, - rclcpp::ParameterEventsFilter::EventType::CHANGED}); - for (auto & it : filter.get_events()) { - if (it.second->value.type != ParameterType::PARAMETER_BOOL) { - RCLCPP_ERROR(logger_, "use_sim_time parameter cannot be set to anything but a bool"); - continue; - } - if (it.second->value.bool_value) { - parameter_state_ = SET_TRUE; - enable_ros_time(); - create_clock_sub(); - } else { - parameter_state_ = SET_FALSE; - disable_ros_time(); - destroy_clock_sub(); - } - } - // Handle the case that use_sim_time was deleted. - rclcpp::ParameterEventsFilter deleted(event, {"use_sim_time"}, - {rclcpp::ParameterEventsFilter::EventType::DELETED}); - for (auto & it : deleted.get_events()) { - (void) it; // if there is a match it's already matched, don't bother reading it. - // If the parameter is deleted mark it as unset but dont' change state. - parameter_state_ = UNSET; - } -} - -void TimeSource::enable_ros_time() -{ - if (ros_time_active_) { - // already enabled no-op - return; - } - - // Local storage - ros_time_active_ = true; - - // Update all attached clocks to zero or last recorded time - std::lock_guard guard(clock_list_lock_); - auto time_msg = std::make_shared(); - if (last_msg_set_) { - time_msg = std::make_shared(last_msg_set_->clock); - } - for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { - set_clock(time_msg, true, *it); - } -} - -void TimeSource::disable_ros_time() +TimeSource::~TimeSource() { - if (!ros_time_active_) { - // already disabled no-op - return; - } - - // Local storage - ros_time_active_ = false; - - // Update all attached clocks - std::lock_guard guard(clock_list_lock_); - for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { - auto msg = std::make_shared(); - set_clock(msg, false, *it); - } } } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index 3b64e9e8b1..bbf5d6bc31 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -530,12 +530,12 @@ class ClockThreadTestingTimeSource : public rclcpp::TimeSource bool GetUseClockThreadOption() { - return this->use_clock_thread_; + return this->get_use_clock_thread(); } bool IsClockThreadJoinable() { - return this->clock_executor_thread_.joinable(); + return this->clock_thread_is_joinable(); } }; From 4a343a1f2361fc3a38551d8b9c1bce704ca338f1 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 18 Oct 2021 15:10:16 -0300 Subject: [PATCH 026/455] Changelogs Signed-off-by: Ivan Santiago Paunovic --- rclcpp/CHANGELOG.rst | 12 ++++++++++++ rclcpp_action/CHANGELOG.rst | 5 +++++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 6 ++++++ 4 files changed, 26 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index ac6812e882..21885b3a73 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix dangerous std::bind capture in TimeSource implementation. (`#1768 `_) +* Fix dangerous std::bind capture in ParameterEventHandler implementation. (`#1770 `_) +* Handle sigterm, in the same way sigint is being handled. (`#1771 `_) +* rclcpp::Node copy constructor: make copy of node_waitables\_ member. (`#1799 `_) +* Extend NodeGraph to match what rcl provides. (`#1484 `_) +* Context::sleep_for(): replace recursion with do-while to avoid potential stack-overflow. (`#1765 `_) +* extend_sub_namespace(): Verify string::empty() before calling string::front(). (`#1764 `_) +* Deprecate the `void shared_ptr` subscription callback signatures. (`#1713 `_) +* Contributors: Abrar Rahman Protyasha, Chris Lalancette, Emerson Knapp, Geoffrey Biggs, Ivan Santiago Paunovic, Jorge Perez, Tomoya Fujita, William Woodall, Yong-Hao Zou, livanov93 + 13.0.0 (2021-08-23) ------------------- * Remove can_be_nullptr assignment check for QNX case. (`#1752 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 8dd1ea0398..7f357c7c32 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Deprecate the `void shared_ptr` subscription callback signatures (`#1713 `_) +* Contributors: Abrar Rahman Protyasha, Tomoya Fujita + 13.0.0 (2021-08-23) ------------------- * Use rcpputils/scope_exit.hpp and remove rclcpp/scope_exit.hpp. (`#1727 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index ee0af41ec4..344d025903 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 13.0.0 (2021-08-23) ------------------- * Update client API to be able to remove pending requests. (`#1734 `_) diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 954b09fec4..fb0e6aecfe 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,12 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update forward declarations of `rcl_lifecycle` types (`#1788 `_) +* Deprecate the `void shared_ptr` subscription callback signatures (`#1713 `_) +* Contributors: Abrar Rahman Protyasha, Michel Hidalgo + 13.0.0 (2021-08-23) ------------------- * Update client API to be able to remove pending requests. (`#1734 `_) From 82950f1141f144f267b95c9a9540563c16842445 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 18 Oct 2021 19:26:36 +0000 Subject: [PATCH 027/455] 13.1.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 21885b3a73..220c16e898 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +13.1.0 (2021-10-18) +------------------- * Fix dangerous std::bind capture in TimeSource implementation. (`#1768 `_) * Fix dangerous std::bind capture in ParameterEventHandler implementation. (`#1770 `_) * Handle sigterm, in the same way sigint is being handled. (`#1771 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 99783768e9..e4356c6684 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 13.0.0 + 13.1.0 The ROS client library in C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 7f357c7c32..94f2fc7842 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +13.1.0 (2021-10-18) +------------------- * Deprecate the `void shared_ptr` subscription callback signatures (`#1713 `_) * Contributors: Abrar Rahman Protyasha, Tomoya Fujita diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 2ca049b77d..de5cc6941e 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 13.0.0 + 13.1.0 Adds action APIs for C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 344d025903..7951a187a5 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +13.1.0 (2021-10-18) +------------------- 13.0.0 (2021-08-23) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 1dfdc05e55..a2ee5664a3 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 13.0.0 + 13.1.0 Package containing tools for dynamically loadable components Ivan Paunovic Mabel Zhang diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index fb0e6aecfe..0498541596 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +13.1.0 (2021-10-18) +------------------- * Update forward declarations of `rcl_lifecycle` types (`#1788 `_) * Deprecate the `void shared_ptr` subscription callback signatures (`#1713 `_) * Contributors: Abrar Rahman Protyasha, Michel Hidalgo diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 8c5107ce2d..5e9a7c0fe6 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 13.0.0 + 13.1.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Mabel Zhang From 0fd866d201fd63e21a84e4c78857217fd884eecc Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Tue, 19 Oct 2021 17:49:54 +0200 Subject: [PATCH 028/455] Fix unused QoS profile for clock subscription and make ClockQoS the default (#1801) Signed-off-by: Nikolai Morin --- rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp | 2 +- rclcpp/src/rclcpp/time_source.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp b/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp index 1c9afebe1f..1594d5f8e5 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp @@ -48,7 +48,7 @@ class NodeTimeSource : public NodeTimeSourceInterface rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, - const rclcpp::QoS & qos = rclcpp::RosoutQoS(), + const rclcpp::QoS & qos = rclcpp::ClockQoS(), bool use_clock_thread = true ); diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 61c5fdd785..7e1226e1b2 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -420,7 +420,7 @@ class TimeSource::NodeState : public std::enable_shared_from_this node_parameters_, node_topics_, "/clock", - rclcpp::QoS(KeepLast(1)).best_effort(), + qos_, [state = std::weak_ptr(this->shared_from_this())]( std::shared_ptr msg) { if (auto state_ptr = state.lock()) { From 2d7bd9f4cb0a19091b848b664972da2ce23e5cee Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 20 Oct 2021 08:32:43 -0700 Subject: [PATCH 029/455] Wait for publisher and subscription to match (#1777) Fix #1775 Connext takes significantly longer for discovery to happened compared to the other RMWs. So, waiting an arbitrary amount of time for a message to be received is brittle. By instead waiting for the publisher and subscription to match, the tests become more robust. Signed-off-by: Jacob Perron --- .../test_subscription_with_type_adapter.cpp | 42 ++++++++++++++++++- 1 file changed, 41 insertions(+), 1 deletion(-) diff --git a/rclcpp/test/rclcpp/test_subscription_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_subscription_with_type_adapter.cpp index 54c11bf1b1..7668feadb0 100644 --- a/rclcpp/test/rclcpp/test_subscription_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_subscription_with_type_adapter.cpp @@ -77,7 +77,9 @@ class CLASSNAME (test_intra_process_within_one_node, RMW_IMPLEMENTATION) : publi public: static void SetUpTestCase() { - rclcpp::init(0, nullptr); + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } } static void TearDownTestCase() @@ -128,6 +130,20 @@ void wait_for_message_to_be_received( executor.spin_once(g_sleep_per_loop); } } + +bool wait_for_match( + const std::shared_ptr sub, + const std::shared_ptr pub) +{ + int i = 0; + bool matched = false; + while (!matched && i < g_max_loops) { + matched = sub->get_publisher_count() > 0 && pub->get_subscription_count() > 0; + std::this_thread::sleep_for(g_sleep_per_loop); + } + return matched; +} + /* * Testing publisher creation signatures with a type adapter. */ @@ -175,6 +191,7 @@ TEST_F( ASSERT_STREQ(message_data.c_str(), msg.c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -191,6 +208,7 @@ TEST_F( ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -206,6 +224,7 @@ TEST_F( ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -222,6 +241,7 @@ TEST_F( ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -237,6 +257,7 @@ TEST_F( ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -253,6 +274,7 @@ TEST_F( ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -268,6 +290,7 @@ TEST_F( ASSERT_STREQ(message_data.c_str(), msg.data.c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -284,6 +307,7 @@ TEST_F( ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -299,6 +323,7 @@ TEST_F( ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -315,6 +340,7 @@ TEST_F( ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -330,6 +356,7 @@ TEST_F( ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -346,6 +373,7 @@ TEST_F( ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -381,6 +409,7 @@ TEST_F( ASSERT_STREQ(message_data.c_str(), msg.c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -398,6 +427,7 @@ TEST_F( ASSERT_FALSE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -413,6 +443,7 @@ TEST_F( ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -430,6 +461,7 @@ TEST_F( ASSERT_FALSE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -445,6 +477,7 @@ TEST_F( ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -461,6 +494,7 @@ TEST_F( ASSERT_FALSE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -476,6 +510,7 @@ TEST_F( ASSERT_STREQ(message_data.c_str(), msg.data.c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -492,6 +527,7 @@ TEST_F( ASSERT_FALSE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -507,6 +543,7 @@ TEST_F( ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -523,6 +560,7 @@ TEST_F( ASSERT_FALSE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -538,6 +576,7 @@ TEST_F( ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); @@ -554,6 +593,7 @@ TEST_F( ASSERT_FALSE(message_info.get_rmw_message_info().from_intra_process); }; auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); pub->publish(msg); ASSERT_FALSE(is_received); wait_for_message_to_be_received(is_received, node); From 5ecc5b6c19cdd192c02e82d06fb2314ea3683018 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 22 Oct 2021 11:27:54 -0400 Subject: [PATCH 030/455] Suppress clang dead-store warnings in the benchmarks. (#1802) clang static analysis complains that there are dead stores in most of the benchmark tests, which is technically correct. We use an idiom like: for (auto _ : state) { } And never access _. Silence clang here by doing (void)_; all of the places this is seen. Signed-off-by: Chris Lalancette --- rclcpp/test/benchmark/benchmark_client.cpp | 6 ++++++ rclcpp/test/benchmark/benchmark_executor.cpp | 14 ++++++++++++++ rclcpp/test/benchmark/benchmark_init_shutdown.cpp | 2 ++ rclcpp/test/benchmark/benchmark_node.cpp | 2 ++ .../benchmark_node_parameters_interface.cpp | 11 +++++++++++ .../test/benchmark/benchmark_parameter_client.cpp | 10 ++++++++++ rclcpp/test/benchmark/benchmark_service.cpp | 4 ++++ .../test/benchmark/benchmark_action_client.cpp | 9 +++++++++ .../test/benchmark/benchmark_action_server.cpp | 8 ++++++++ .../test/benchmark/benchmark_components.cpp | 3 +++ .../test/benchmark/benchmark_lifecycle_client.cpp | 5 +++++ .../test/benchmark/benchmark_lifecycle_node.cpp | 7 +++++++ .../test/benchmark/benchmark_state.cpp | 2 ++ .../test/benchmark/benchmark_transition.cpp | 2 ++ 14 files changed, 85 insertions(+) diff --git a/rclcpp/test/benchmark/benchmark_client.cpp b/rclcpp/test/benchmark/benchmark_client.cpp index 05608623a0..26ee58b633 100644 --- a/rclcpp/test/benchmark/benchmark_client.cpp +++ b/rclcpp/test/benchmark/benchmark_client.cpp @@ -62,6 +62,7 @@ BENCHMARK_F(ClientPerformanceTest, construct_client_no_service)(benchmark::State reset_heap_counters(); for (auto _ : state) { + (void)_; auto client = node->create_client("not_an_existing_service"); benchmark::DoNotOptimize(client); benchmark::ClobberMemory(); @@ -79,6 +80,7 @@ BENCHMARK_F(ClientPerformanceTest, construct_client_empty_srv)(benchmark::State reset_heap_counters(); for (auto _ : state) { + (void)_; auto client = node->create_client(empty_service_name); benchmark::DoNotOptimize(client); benchmark::ClobberMemory(); @@ -96,6 +98,7 @@ BENCHMARK_F(ClientPerformanceTest, destroy_client_empty_srv)(benchmark::State & reset_heap_counters(); for (auto _ : state) { + (void)_; state.PauseTiming(); auto client = node->create_client(empty_service_name); state.ResumeTiming(); @@ -109,6 +112,7 @@ BENCHMARK_F(ClientPerformanceTest, destroy_client_empty_srv)(benchmark::State & BENCHMARK_F(ClientPerformanceTest, wait_for_service)(benchmark::State & state) { int count = 0; for (auto _ : state) { + (void)_; state.PauseTiming(); const std::string service_name = std::string("service_") + std::to_string(count++); // Create client before service so it has to 'discover' the service after construction @@ -132,6 +136,7 @@ BENCHMARK_F(ClientPerformanceTest, async_send_request_only)(benchmark::State & s reset_heap_counters(); for (auto _ : state) { + (void)_; auto future = client->async_send_request(shared_request); benchmark::DoNotOptimize(future); benchmark::ClobberMemory(); @@ -144,6 +149,7 @@ BENCHMARK_F(ClientPerformanceTest, async_send_request_and_response)(benchmark::S reset_heap_counters(); for (auto _ : state) { + (void)_; auto future = client->async_send_request(shared_request); rclcpp::spin_until_future_complete( node->get_node_base_interface(), future, std::chrono::seconds(1)); diff --git a/rclcpp/test/benchmark/benchmark_executor.cpp b/rclcpp/test/benchmark/benchmark_executor.cpp index b0fbab4a36..652007b589 100644 --- a/rclcpp/test/benchmark/benchmark_executor.cpp +++ b/rclcpp/test/benchmark/benchmark_executor.cpp @@ -78,6 +78,7 @@ BENCHMARK_F(PerformanceTestExecutor, single_thread_executor_spin_some)(benchmark reset_heap_counters(); for (auto _ : st) { + (void)_; st.PauseTiming(); for (unsigned int i = 0u; i < kNumberOfNodes; i++) { publishers[i]->publish(empty_msgs); @@ -104,6 +105,7 @@ BENCHMARK_F(PerformanceTestExecutor, multi_thread_executor_spin_some)(benchmark: reset_heap_counters(); for (auto _ : st) { + (void)_; st.PauseTiming(); for (unsigned int i = 0u; i < kNumberOfNodes; i++) { publishers[i]->publish(empty_msgs); @@ -142,6 +144,7 @@ BENCHMARK_F(PerformanceTestExecutorSimple, single_thread_executor_add_node)(benc { rclcpp::executors::SingleThreadedExecutor executor; for (auto _ : st) { + (void)_; executor.add_node(node); st.PauseTiming(); executor.remove_node(node); @@ -154,6 +157,7 @@ BENCHMARK_F( { rclcpp::executors::SingleThreadedExecutor executor; for (auto _ : st) { + (void)_; st.PauseTiming(); executor.add_node(node); st.ResumeTiming(); @@ -165,6 +169,7 @@ BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_add_node)(bench { rclcpp::executors::MultiThreadedExecutor executor; for (auto _ : st) { + (void)_; executor.add_node(node); st.PauseTiming(); executor.remove_node(node); @@ -176,6 +181,7 @@ BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_remove_node)(be { rclcpp::executors::MultiThreadedExecutor executor; for (auto _ : st) { + (void)_; st.PauseTiming(); executor.add_node(node); st.ResumeTiming(); @@ -189,6 +195,7 @@ BENCHMARK_F( { rclcpp::executors::StaticSingleThreadedExecutor executor; for (auto _ : st) { + (void)_; executor.add_node(node); st.PauseTiming(); executor.remove_node(node); @@ -202,6 +209,7 @@ BENCHMARK_F( { rclcpp::executors::StaticSingleThreadedExecutor executor; for (auto _ : st) { + (void)_; st.PauseTiming(); executor.add_node(node); st.ResumeTiming(); @@ -228,6 +236,7 @@ BENCHMARK_F( reset_heap_counters(); for (auto _ : st) { + (void)_; // static_single_thread_executor has a special design. We need to add/remove the node each // time you call spin st.PauseTiming(); @@ -265,6 +274,7 @@ BENCHMARK_F( reset_heap_counters(); for (auto _ : st) { + (void)_; ret = rclcpp::executors::spin_node_until_future_complete( executor, node, shared_future, 1s); if (ret != rclcpp::FutureReturnCode::SUCCESS) { @@ -294,6 +304,7 @@ BENCHMARK_F( reset_heap_counters(); for (auto _ : st) { + (void)_; ret = rclcpp::executors::spin_node_until_future_complete( executor, node, shared_future, 1s); if (ret != rclcpp::FutureReturnCode::SUCCESS) { @@ -317,6 +328,7 @@ BENCHMARK_F( reset_heap_counters(); for (auto _ : st) { + (void)_; auto ret = rclcpp::executors::spin_node_until_future_complete( executor, node, shared_future, 1s); if (ret != rclcpp::FutureReturnCode::SUCCESS) { @@ -342,6 +354,7 @@ BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark reset_heap_counters(); for (auto _ : st) { + (void)_; ret = rclcpp::spin_until_future_complete(node, shared_future, 1s); if (ret != rclcpp::FutureReturnCode::SUCCESS) { st.SkipWithError(rcutils_get_error_string().str); @@ -383,6 +396,7 @@ BENCHMARK_F( reset_heap_counters(); for (auto _ : st) { + (void)_; std::shared_ptr data = entities_collector_->take_data(); entities_collector_->execute(data); } diff --git a/rclcpp/test/benchmark/benchmark_init_shutdown.cpp b/rclcpp/test/benchmark/benchmark_init_shutdown.cpp index 6c3ae8b72e..c2a60f693a 100644 --- a/rclcpp/test/benchmark/benchmark_init_shutdown.cpp +++ b/rclcpp/test/benchmark/benchmark_init_shutdown.cpp @@ -26,6 +26,7 @@ BENCHMARK_F(PerformanceTest, rclcpp_init)(benchmark::State & state) reset_heap_counters(); for (auto _ : state) { + (void)_; rclcpp::init(0, nullptr); state.PauseTiming(); @@ -43,6 +44,7 @@ BENCHMARK_F(PerformanceTest, rclcpp_shutdown)(benchmark::State & state) reset_heap_counters(); for (auto _ : state) { + (void)_; state.PauseTiming(); rclcpp::init(0, nullptr); state.ResumeTiming(); diff --git a/rclcpp/test/benchmark/benchmark_node.cpp b/rclcpp/test/benchmark/benchmark_node.cpp index 2a1d80674c..a5d7f4c80f 100644 --- a/rclcpp/test/benchmark/benchmark_node.cpp +++ b/rclcpp/test/benchmark/benchmark_node.cpp @@ -44,6 +44,7 @@ BENCHMARK_F(NodePerformanceTest, create_node)(benchmark::State & state) reset_heap_counters(); for (auto _ : state) { + (void)_; // Using pointer to separate construction and destruction in timing auto node = std::make_shared("node"); #ifndef __clang_analyzer__ @@ -66,6 +67,7 @@ BENCHMARK_F(NodePerformanceTest, destroy_node)(benchmark::State & state) reset_heap_counters(); for (auto _ : state) { + (void)_; // Using pointer to separate construction and destruction in timing state.PauseTiming(); auto node = std::make_shared("node"); diff --git a/rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp b/rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp index be447b7190..04c7071d64 100644 --- a/rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp +++ b/rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp @@ -71,6 +71,7 @@ class NodeParametersInterfaceTest : public performance_test_fixture::Performance BENCHMARK_F(NodeParametersInterfaceTest, declare_undeclare)(benchmark::State & state) { for (auto _ : state) { + (void)_; node->declare_parameter(param3_name, rclcpp::ParameterValue{}, dynamically_typed_descriptor); node->undeclare_parameter(param3_name); } @@ -79,6 +80,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, declare_undeclare)(benchmark::State & s BENCHMARK_F(NodeParametersInterfaceTest, has_parameter_hit)(benchmark::State & state) { for (auto _ : state) { + (void)_; if (!node->has_parameter(param1_name)) { state.SkipWithError("Parameter was expected"); break; @@ -89,6 +91,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, has_parameter_hit)(benchmark::State & s BENCHMARK_F(NodeParametersInterfaceTest, has_parameter_miss)(benchmark::State & state) { for (auto _ : state) { + (void)_; if (node->has_parameter(param3_name)) { state.SkipWithError("Parameter was not expected"); break; @@ -112,6 +115,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_bool)(benchmark::State & reset_heap_counters(); for (auto _ : state) { + (void)_; node->set_parameters(param_values2); node->set_parameters(param_values1); } @@ -133,6 +137,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_atomically_bool)(benchma reset_heap_counters(); for (auto _ : state) { + (void)_; node->set_parameters_atomically(param_values2); node->set_parameters_atomically(param_values1); } @@ -164,6 +169,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_callback_bool)(benchmark reset_heap_counters(); for (auto _ : state) { + (void)_; node->set_parameters(param_values2); node->set_parameters(param_values1); } @@ -191,6 +197,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_string)(benchmark::State reset_heap_counters(); for (auto _ : state) { + (void)_; node->set_parameters(param_values2); node->set_parameters(param_values1); } @@ -212,6 +219,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_array)(benchmark::State reset_heap_counters(); for (auto _ : state) { + (void)_; node->set_parameters(param_values2); node->set_parameters(param_values1); } @@ -224,6 +232,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, get_parameter)(benchmark::State & state reset_heap_counters(); for (auto _ : state) { + (void)_; node->get_parameter(param1_name, param1_value); } } @@ -239,6 +248,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, list_parameters_hit)(benchmark::State & reset_heap_counters(); for (auto _ : state) { + (void)_; param_list = node->list_parameters(prefixes, 10); if (param_list.names.size() != 2) { state.SkipWithError("Expected node names"); @@ -258,6 +268,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, list_parameters_miss)(benchmark::State reset_heap_counters(); for (auto _ : state) { + (void)_; param_list = node->list_parameters(prefixes, 10); if (param_list.names.size() != 0) { state.SkipWithError("Expected no node names"); diff --git a/rclcpp/test/benchmark/benchmark_parameter_client.cpp b/rclcpp/test/benchmark/benchmark_parameter_client.cpp index c0bdcf2932..0d9a6fd9c9 100644 --- a/rclcpp/test/benchmark/benchmark_parameter_client.cpp +++ b/rclcpp/test/benchmark/benchmark_parameter_client.cpp @@ -136,6 +136,7 @@ static bool result_is_successful(rcl_interfaces::msg::SetParametersResult result BENCHMARK_F(ParameterClientTest, create_destroy_client)(benchmark::State & state) { for (auto _ : state) { + (void)_; params_client.reset(); params_client = std::make_shared(node, remote_node_name); if (!params_client->wait_for_service()) { @@ -148,6 +149,7 @@ BENCHMARK_F(ParameterClientTest, create_destroy_client)(benchmark::State & state BENCHMARK_F(ParameterClientTest, has_parameter_hit)(benchmark::State & state) { for (auto _ : state) { + (void)_; if (!params_client->has_parameter(param1_name)) { state.SkipWithError("Parameter was expected"); break; @@ -158,6 +160,7 @@ BENCHMARK_F(ParameterClientTest, has_parameter_hit)(benchmark::State & state) BENCHMARK_F(ParameterClientTest, has_parameter_miss)(benchmark::State & state) { for (auto _ : state) { + (void)_; if (params_client->has_parameter(param3_name)) { state.SkipWithError("Parameter was not expected"); break; @@ -179,6 +182,7 @@ BENCHMARK_F(ParameterClientTest, set_parameters_bool)(benchmark::State & state) }; for (auto _ : state) { + (void)_; std::vector results = params_client->set_parameters(param_values2); if (!std::all_of(results.begin(), results.end(), result_is_successful)) { @@ -208,6 +212,7 @@ BENCHMARK_F(ParameterClientTest, set_parameters_atomically_bool)(benchmark::Stat }; for (auto _ : state) { + (void)_; rcl_interfaces::msg::SetParametersResult result = params_client->set_parameters_atomically(param_values2); if (!result.successful) { @@ -237,6 +242,7 @@ BENCHMARK_F(ParameterClientTest, set_parameters_string)(benchmark::State & state }; for (auto _ : state) { + (void)_; std::vector results = params_client->set_parameters(param_values2); if (!std::all_of(results.begin(), results.end(), result_is_successful)) { @@ -266,6 +272,7 @@ BENCHMARK_F(ParameterClientTest, set_parameters_array)(benchmark::State & state) }; for (auto _ : state) { + (void)_; std::vector results = params_client->set_parameters(param_values2); if (!std::all_of(results.begin(), results.end(), result_is_successful)) { @@ -284,6 +291,7 @@ BENCHMARK_F(ParameterClientTest, set_parameters_array)(benchmark::State & state) BENCHMARK_F(ParameterClientTest, get_parameters)(benchmark::State & state) { for (auto _ : state) { + (void)_; std::vector results = params_client->get_parameters({param1_name}); if (results.size() != 1 || results[0].get_name() != param1_name) { state.SkipWithError("Got the wrong parameter(s)"); @@ -300,6 +308,7 @@ BENCHMARK_F(ParameterClientTest, list_parameters_hit)(benchmark::State & state) }; for (auto _ : state) { + (void)_; rcl_interfaces::msg::ListParametersResult param_list = params_client->list_parameters(prefixes, 10); if (param_list.names.size() != 2) { @@ -317,6 +326,7 @@ BENCHMARK_F(ParameterClientTest, list_parameters_miss)(benchmark::State & state) }; for (auto _ : state) { + (void)_; rcl_interfaces::msg::ListParametersResult param_list = params_client->list_parameters(prefixes, 10); if (param_list.names.size() != 0) { diff --git a/rclcpp/test/benchmark/benchmark_service.cpp b/rclcpp/test/benchmark/benchmark_service.cpp index f1d9aca68b..a42723da90 100644 --- a/rclcpp/test/benchmark/benchmark_service.cpp +++ b/rclcpp/test/benchmark/benchmark_service.cpp @@ -69,6 +69,7 @@ BENCHMARK_F(ServicePerformanceTest, construct_service_no_client)(benchmark::Stat reset_heap_counters(); for (auto _ : state) { + (void)_; auto service = node->create_service("not_a_service", callback); benchmark::DoNotOptimize(service); benchmark::ClobberMemory(); @@ -87,6 +88,7 @@ BENCHMARK_F(ServicePerformanceTest, construct_service_empty_srv)(benchmark::Stat reset_heap_counters(); for (auto _ : state) { + (void)_; auto service = node->create_service(empty_service_name, callback); benchmark::DoNotOptimize(service); benchmark::ClobberMemory(); @@ -105,6 +107,7 @@ BENCHMARK_F(ServicePerformanceTest, destroy_service_empty_srv)(benchmark::State reset_heap_counters(); for (auto _ : state) { + (void)_; state.PauseTiming(); auto service = node->create_service(empty_service_name, callback); state.ResumeTiming(); @@ -123,6 +126,7 @@ BENCHMARK_F(ServicePerformanceTest, async_send_response)(benchmark::State & stat reset_heap_counters(); for (auto _ : state) { + (void)_; state.PauseTiming(); // Clear executor queue rclcpp::spin_some(node->get_node_base_interface()); diff --git a/rclcpp_action/test/benchmark/benchmark_action_client.cpp b/rclcpp_action/test/benchmark/benchmark_action_client.cpp index 72d221883e..8b11935117 100644 --- a/rclcpp_action/test/benchmark/benchmark_action_client.cpp +++ b/rclcpp_action/test/benchmark/benchmark_action_client.cpp @@ -122,6 +122,7 @@ BENCHMARK_F(ActionClientPerformanceTest, construct_client_without_server)(benchm { constexpr char action_name[] = "no_corresponding_server"; for (auto _ : state) { + (void)_; auto client = rclcpp_action::create_client(node, action_name); // Only timing construction, so destruction needs to happen explicitly while timing is paused @@ -136,6 +137,7 @@ BENCHMARK_F(ActionClientPerformanceTest, construct_client_with_server)(benchmark SetUpServer(fibonacci_action_name); reset_heap_counters(); for (auto _ : state) { + (void)_; auto client = rclcpp_action::create_client(node, fibonacci_action_name); // Only timing construction, so destruction needs to happen explicitly while timing is paused @@ -148,6 +150,7 @@ BENCHMARK_F(ActionClientPerformanceTest, construct_client_with_server)(benchmark BENCHMARK_F(ActionClientPerformanceTest, destroy_client)(benchmark::State & state) { for (auto _ : state) { + (void)_; // This client does not have a corresponding server state.PauseTiming(); auto client = rclcpp_action::create_client(node, fibonacci_action_name); @@ -170,6 +173,7 @@ BENCHMARK_F(ActionClientPerformanceTest, async_send_goal_only)(benchmark::State reset_heap_counters(); for (auto _ : state) { + (void)_; auto future_goal_handle = client->async_send_goal(goal); } } @@ -188,6 +192,7 @@ BENCHMARK_F(ActionClientPerformanceTest, async_send_goal_rejected)(benchmark::St reset_heap_counters(); for (auto _ : state) { + (void)_; auto future_goal_handle = client->async_send_goal(goal); rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1)); if (!future_goal_handle.valid()) { @@ -215,6 +220,7 @@ BENCHMARK_F(ActionClientPerformanceTest, async_send_goal_get_accepted_response)( reset_heap_counters(); for (auto _ : state) { + (void)_; // This server's execution is deferred auto future_goal_handle = client->async_send_goal(goal); rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1)); @@ -246,6 +252,7 @@ BENCHMARK_F(ActionClientPerformanceTest, async_get_result)(benchmark::State & st reset_heap_counters(); for (auto _ : state) { + (void)_; // Send goal, accept and execute while timing is paused state.PauseTiming(); auto future_goal_handle = client->async_send_goal(goal); @@ -298,6 +305,7 @@ BENCHMARK_F(ActionClientPerformanceTest, async_cancel_goal)(benchmark::State & s reset_heap_counters(); for (auto _ : state) { + (void)_; state.PauseTiming(); auto future_goal_handle = client->async_send_goal(goal); @@ -333,6 +341,7 @@ BENCHMARK_F(ActionClientPerformanceTest, async_cancel_all_goals)(benchmark::Stat reset_heap_counters(); for (auto _ : state) { + (void)_; state.PauseTiming(); for (int i = 0; i < num_concurrently_inflight_goals; ++i) { auto future_goal_handle = client->async_send_goal(goal); diff --git a/rclcpp_action/test/benchmark/benchmark_action_server.cpp b/rclcpp_action/test/benchmark/benchmark_action_server.cpp index 3ac7fc5812..6817f86b14 100644 --- a/rclcpp_action/test/benchmark/benchmark_action_server.cpp +++ b/rclcpp_action/test/benchmark/benchmark_action_server.cpp @@ -67,6 +67,7 @@ BENCHMARK_F(ActionServerPerformanceTest, construct_server_without_client)(benchm { constexpr char action_name[] = "no_corresponding_client"; for (auto _ : state) { + (void)_; auto action_server = rclcpp_action::create_server( node, action_name, [](const GoalUUID &, std::shared_ptr) { @@ -88,6 +89,7 @@ BENCHMARK_F(ActionServerPerformanceTest, construct_server_without_client)(benchm BENCHMARK_F(ActionServerPerformanceTest, construct_server_with_client)(benchmark::State & state) { for (auto _ : state) { + (void)_; auto action_server = rclcpp_action::create_server( node, fibonacci_action_name, [](const GoalUUID &, std::shared_ptr) { @@ -109,6 +111,7 @@ BENCHMARK_F(ActionServerPerformanceTest, construct_server_with_client)(benchmark BENCHMARK_F(ActionServerPerformanceTest, destroy_server)(benchmark::State & state) { for (auto _ : state) { + (void)_; state.PauseTiming(); auto action_server = rclcpp_action::create_server( node, fibonacci_action_name, @@ -144,6 +147,7 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_accept_goal)(benchmark::S reset_heap_counters(); for (auto _ : state) { + (void)_; state.PauseTiming(); auto client_goal_handle_future = AsyncSendGoalOfOrder(1); state.ResumeTiming(); @@ -176,6 +180,7 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_cancel_goal)(benchmark::S reset_heap_counters(); for (auto _ : state) { + (void)_; state.PauseTiming(); auto client_goal_handle_future = AsyncSendGoalOfOrder(1); // This spin completes when the goal has been accepted, but not executed because server @@ -212,6 +217,7 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_execute_goal)(benchmark:: reset_heap_counters(); for (auto _ : state) { + (void)_; state.PauseTiming(); auto client_goal_handle_future = AsyncSendGoalOfOrder(1); @@ -257,6 +263,7 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_set_success)(benchmark::S reset_heap_counters(); for (auto _ : state) { + (void)_; state.PauseTiming(); auto client_goal_handle_future = AsyncSendGoalOfOrder(goal_order); @@ -301,6 +308,7 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_abort)(benchmark::State & reset_heap_counters(); for (auto _ : state) { + (void)_; state.PauseTiming(); auto client_goal_handle_future = AsyncSendGoalOfOrder(goal_order); diff --git a/rclcpp_components/test/benchmark/benchmark_components.cpp b/rclcpp_components/test/benchmark/benchmark_components.cpp index 6fbe6df107..9b9c8cf4c8 100644 --- a/rclcpp_components/test/benchmark/benchmark_components.cpp +++ b/rclcpp_components/test/benchmark/benchmark_components.cpp @@ -74,6 +74,7 @@ class ComponentTest : public benchmark::Fixture BENCHMARK_F(ComponentTest, get_component_resources)(benchmark::State & state) { for (auto _ : state) { + (void)_; std::vector resources = manager->get_component_resources("rclcpp_components"); if (resources.size() != 3) { @@ -93,6 +94,7 @@ BENCHMARK_F(ComponentTest, create_component_factory)(benchmark::State & state) } for (auto _ : state) { + (void)_; manager->create_component_factory(resources[0]).reset(); } } @@ -114,6 +116,7 @@ BENCHMARK_F(ComponentTest, create_node_instance)(benchmark::State & state) const rclcpp::NodeOptions options = rclcpp::NodeOptions().context(context); for (auto _ : state) { + (void)_; rclcpp_components::NodeInstanceWrapper node = factory->create_node_instance(options); benchmark::DoNotOptimize(node); benchmark::ClobberMemory(); diff --git a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp index 27b01df51a..c8d166ef66 100644 --- a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp +++ b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp @@ -227,6 +227,7 @@ class BenchmarkLifecycleClient : public performance_test_fixture::PerformanceTes BENCHMARK_F(BenchmarkLifecycleClient, get_state)(benchmark::State & state) { for (auto _ : state) { + (void)_; const auto lifecycle_state = lifecycle_client->get_state(); if (lifecycle_state.id != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { const std::string msg = @@ -248,6 +249,7 @@ BENCHMARK_F(BenchmarkLifecycleClient, change_state)(benchmark::State & state) { reset_heap_counters(); for (auto _ : state) { + (void)_; success = lifecycle_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); if (!success) { @@ -264,6 +266,7 @@ BENCHMARK_F(BenchmarkLifecycleClient, change_state)(benchmark::State & state) { BENCHMARK_F(BenchmarkLifecycleClient, get_available_states)(benchmark::State & state) { for (auto _ : state) { + (void)_; constexpr size_t expected_states = 11u; const auto states = lifecycle_client->get_available_states(); if (states.size() != expected_states) { @@ -279,6 +282,7 @@ BENCHMARK_F(BenchmarkLifecycleClient, get_available_states)(benchmark::State & s BENCHMARK_F(BenchmarkLifecycleClient, get_available_transitions)(benchmark::State & state) { for (auto _ : state) { + (void)_; constexpr size_t expected_transitions = 2u; const auto transitions = lifecycle_client->get_available_transitions(); if (transitions.size() != expected_transitions) { @@ -294,6 +298,7 @@ BENCHMARK_F(BenchmarkLifecycleClient, get_available_transitions)(benchmark::Stat BENCHMARK_F(BenchmarkLifecycleClient, get_transition_graph)(benchmark::State & state) { for (auto _ : state) { + (void)_; constexpr size_t expected_transitions = 25u; const auto transitions = lifecycle_client->get_transition_graph(); if (transitions.size() != expected_transitions) { diff --git a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp index bc53499554..0bf3c48ab9 100644 --- a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp @@ -49,6 +49,7 @@ BENCHMARK_F(BenchmarkLifecycleNodeConstruction, construct_lifecycle_node)( benchmark::State & state) { for (auto _ : state) { + (void)_; auto node = std::make_shared("node", "ns"); PERFORMANCE_TEST_FIXTURE_PAUSE_MEASUREMENTS( state, @@ -60,6 +61,7 @@ BENCHMARK_F(BenchmarkLifecycleNodeConstruction, construct_lifecycle_node)( BENCHMARK_F(BenchmarkLifecycleNodeConstruction, destroy_lifecycle_node)(benchmark::State & state) { for (auto _ : state) { + (void)_; std::shared_ptr node(nullptr); PERFORMANCE_TEST_FIXTURE_PAUSE_MEASUREMENTS( state, @@ -94,6 +96,7 @@ class BenchmarkLifecycleNode : public performance_test_fixture::PerformanceTest // This is a simple getter, but it crosses over into the rcl library. BENCHMARK_F(BenchmarkLifecycleNode, get_current_state)(benchmark::State & state) { for (auto _ : state) { + (void)_; const auto & lifecycle_state = node->get_current_state(); if (lifecycle_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { const std::string message = @@ -107,6 +110,7 @@ BENCHMARK_F(BenchmarkLifecycleNode, get_current_state)(benchmark::State & state) BENCHMARK_F(BenchmarkLifecycleNode, get_available_states)(benchmark::State & state) { for (auto _ : state) { + (void)_; constexpr size_t expected_states = 11u; const auto lifecycle_states = node->get_available_states(); if (lifecycle_states.size() != expected_states) { @@ -120,6 +124,7 @@ BENCHMARK_F(BenchmarkLifecycleNode, get_available_states)(benchmark::State & sta BENCHMARK_F(BenchmarkLifecycleNode, get_available_transitions)(benchmark::State & state) { for (auto _ : state) { + (void)_; constexpr size_t expected_transitions = 2u; const auto & transitions = node->get_available_transitions(); if (transitions.size() != expected_transitions) { @@ -133,6 +138,7 @@ BENCHMARK_F(BenchmarkLifecycleNode, get_available_transitions)(benchmark::State BENCHMARK_F(BenchmarkLifecycleNode, get_transition_graph)(benchmark::State & state) { for (auto _ : state) { + (void)_; constexpr size_t expected_transitions = 25u; const auto & transitions = node->get_transition_graph(); if (transitions.size() != expected_transitions) { @@ -155,6 +161,7 @@ BENCHMARK_F(BenchmarkLifecycleNode, transition_valid_state)(benchmark::State & s reset_heap_counters(); for (auto _ : state) { + (void)_; const auto & active = node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); if (active.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { diff --git a/rclcpp_lifecycle/test/benchmark/benchmark_state.cpp b/rclcpp_lifecycle/test/benchmark/benchmark_state.cpp index 2e26b08433..8a1003bfbc 100644 --- a/rclcpp_lifecycle/test/benchmark/benchmark_state.cpp +++ b/rclcpp_lifecycle/test/benchmark/benchmark_state.cpp @@ -22,6 +22,7 @@ using PerformanceTest = performance_test_fixture::PerformanceTest; BENCHMARK_F(PerformanceTest, construct_destruct_state)(benchmark::State & state) { for (auto _ : state) { + (void)_; rclcpp_lifecycle::State lifecycle_state(1, "state"); benchmark::DoNotOptimize(lifecycle_state); benchmark::ClobberMemory(); @@ -32,6 +33,7 @@ BENCHMARK_F(PerformanceTest, copy_destruct_state)(benchmark::State & state) { rclcpp_lifecycle::State lifecycle_state(1, "state"); for (auto _ : state) { + (void)_; rclcpp_lifecycle::State state_copy(lifecycle_state); benchmark::DoNotOptimize(state_copy); benchmark::ClobberMemory(); diff --git a/rclcpp_lifecycle/test/benchmark/benchmark_transition.cpp b/rclcpp_lifecycle/test/benchmark/benchmark_transition.cpp index 9403ce663e..6ac6761aed 100644 --- a/rclcpp_lifecycle/test/benchmark/benchmark_transition.cpp +++ b/rclcpp_lifecycle/test/benchmark/benchmark_transition.cpp @@ -22,6 +22,7 @@ using PerformanceTest = performance_test_fixture::PerformanceTest; BENCHMARK_F(PerformanceTest, construct_destruct_transition)(benchmark::State & state) { for (auto _ : state) { + (void)_; rclcpp_lifecycle::Transition transition(1, "transition"); benchmark::DoNotOptimize(transition); benchmark::ClobberMemory(); @@ -32,6 +33,7 @@ BENCHMARK_F(PerformanceTest, copy_destruct_transition)(benchmark::State & state) { rclcpp_lifecycle::Transition transition(1, "transition"); for (auto _ : state) { + (void)_; rclcpp_lifecycle::Transition transition_copy(transition); benchmark::DoNotOptimize(transition_copy); benchmark::ClobberMemory(); From c59793618a97132f5a5c93d8db2d5c7745c9cc0b Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Mon, 25 Oct 2021 14:27:12 -0400 Subject: [PATCH 031/455] Take message ownership from moved LoanedMessage (#1808) * Take message ownership from moved LoanedMessage Otherwise, since the rvalue reference passed into the move constructor of `LoanedMessage` was not set to `nullptr`, the underlying message would be double-free'd. Signed-off-by: Abrar Rahman Protyasha * Add unit test for LoanedMessage move construction Signed-off-by: Abrar Rahman Protyasha * Include header to satisfy cpplint `std::move` was being used without including said header, which made cpplint unhappy. Signed-off-by: Abrar Rahman Protyasha --- rclcpp/include/rclcpp/loaned_message.hpp | 4 +++- rclcpp/test/rclcpp/test_loaned_message.cpp | 17 +++++++++++++++++ 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/loaned_message.hpp b/rclcpp/include/rclcpp/loaned_message.hpp index 4dd396e033..6b30e271e0 100644 --- a/rclcpp/include/rclcpp/loaned_message.hpp +++ b/rclcpp/include/rclcpp/loaned_message.hpp @@ -117,7 +117,9 @@ class LoanedMessage : pub_(std::move(other.pub_)), message_(std::move(other.message_)), message_allocator_(std::move(other.message_allocator_)) - {} + { + other.message_ = nullptr; + } /// Destructor of the LoanedMessage class. /** diff --git a/rclcpp/test/rclcpp/test_loaned_message.cpp b/rclcpp/test/rclcpp/test_loaned_message.cpp index 01284a27c1..ae6852e8f2 100644 --- a/rclcpp/test/rclcpp/test_loaned_message.cpp +++ b/rclcpp/test/rclcpp/test_loaned_message.cpp @@ -14,6 +14,7 @@ #include #include +#include #include "rclcpp/loaned_message.hpp" #include "rclcpp/rclcpp.hpp" @@ -175,3 +176,19 @@ TEST_F(TestLoanedMessage, construct_with_loaned_message_publisher) { EXPECT_NO_THROW(loaned_message.reset()); } } + +TEST_F(TestLoanedMessage, move_loaned_message) { + auto node = std::make_shared("loaned_message_test_node"); + auto pub = node->create_publisher("loaned_message_test_topic", 1); + + auto loaned_msg_to_move = pub->borrow_loaned_message(); + // Force the move constructor to invoke + auto loaned_msg_moved_to = LoanedMessageT(std::move(loaned_msg_to_move)); + + ASSERT_TRUE(loaned_msg_moved_to.is_valid()); + ASSERT_FALSE(loaned_msg_to_move.is_valid()); + + loaned_msg_moved_to.get().float32_value = 42.0f; + ASSERT_EQ(42.0f, loaned_msg_moved_to.get().float32_value); + SUCCEED(); +} From b135e89c1eda70fc3edf65f6acdbd01e1226de75 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Sat, 30 Oct 2021 08:11:39 +0800 Subject: [PATCH 032/455] Add the interface for pre-shutdown callback (#1714) * Add the interface for pre-shutdown callback Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/context.hpp | 76 +++++++++++++++-- rclcpp/src/rclcpp/context.cpp | 114 +++++++++++++++++++++++--- rclcpp/test/rclcpp/test_utilities.cpp | 33 ++++++++ 3 files changed, 206 insertions(+), 17 deletions(-) diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 7d7472c88b..227fc3928c 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -48,17 +48,20 @@ class ContextAlreadyInitialized : public std::runtime_error /// Forward declare WeakContextsWrapper class WeakContextsWrapper; -class OnShutdownCallbackHandle +class ShutdownCallbackHandle { friend class Context; public: - using OnShutdownCallbackType = std::function; + using ShutdownCallbackType = std::function; private: - std::weak_ptr callback; + std::weak_ptr callback; }; +using OnShutdownCallbackHandle = ShutdownCallbackHandle; +using PreShutdownCallbackHandle = ShutdownCallbackHandle; + /// Context which encapsulates shared state between nodes and other similar entities. /** * A context also represents the lifecycle between init and shutdown of rclcpp. @@ -189,7 +192,7 @@ class Context : public std::enable_shared_from_this bool shutdown(const std::string & reason); - using OnShutdownCallback = OnShutdownCallbackHandle::OnShutdownCallbackType; + using OnShutdownCallback = OnShutdownCallbackHandle::ShutdownCallbackType; /// Add a on_shutdown callback to be called when shutdown is called for this context. /** @@ -197,7 +200,7 @@ class Context : public std::enable_shared_from_this * to last step in shutdown(). * * When shutdown occurs due to the signal handler, these callbacks are run - * asynchronoulsy in the dedicated singal handling thread. + * asynchronously in the dedicated singal handling thread. * * Also, shutdown() may be called from the destructor of this function. * Therefore, it is not safe to throw exceptions from these callbacks. @@ -221,7 +224,7 @@ class Context : public std::enable_shared_from_this * to last step in shutdown(). * * When shutdown occurs due to the signal handler, these callbacks are run - * asynchronously in the dedicated singal handling thread. + * asynchronously in the dedicated signal handling thread. * * Also, shutdown() may be called from the destructor of this function. * Therefore, it is not safe to throw exceptions from these callbacks. @@ -249,6 +252,33 @@ class Context : public std::enable_shared_from_this bool remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle); + using PreShutdownCallback = PreShutdownCallbackHandle::ShutdownCallbackType; + + /// Add a pre_shutdown callback to be called before shutdown is called for this context. + /** + * These callbacks will be called in the order they are added. + * + * When shutdown occurs due to the signal handler, these callbacks are run + * asynchronously in the dedicated signal handling thread. + * + * \param[in] callback the pre_shutdown callback to be registered + * \return the created callback handle + */ + RCLCPP_PUBLIC + virtual + PreShutdownCallbackHandle + add_pre_shutdown_callback(PreShutdownCallback callback); + + /// Remove an registered pre_shutdown callback. + /** + * \param[in] callback_handle the pre_shutdown callback handle to be removed. + * \return true if the callback is found and removed, otherwise false. + */ + RCLCPP_PUBLIC + virtual + bool + remove_pre_shutdown_callback(const PreShutdownCallbackHandle & callback_handle); + /// Return the shutdown callbacks. /** * Returned callbacks are a copy of the registered callbacks. @@ -257,6 +287,14 @@ class Context : public std::enable_shared_from_this std::vector get_on_shutdown_callbacks() const; + /// Return the pre-shutdown callbacks. + /** + * Returned callbacks are a copy of the registered callbacks. + */ + RCLCPP_PUBLIC + std::vector + get_pre_shutdown_callbacks() const; + /// Return the internal rcl context. RCLCPP_PUBLIC std::shared_ptr @@ -338,6 +376,9 @@ class Context : public std::enable_shared_from_this std::unordered_set> on_shutdown_callbacks_; mutable std::mutex on_shutdown_callbacks_mutex_; + std::unordered_set> pre_shutdown_callbacks_; + mutable std::mutex pre_shutdown_callbacks_mutex_; + /// Condition variable for timed sleep (see sleep_for). std::condition_variable interrupt_condition_variable_; /// Mutex for protecting the global condition variable. @@ -345,6 +386,29 @@ class Context : public std::enable_shared_from_this /// Keep shared ownership of global vector of weak contexts std::shared_ptr weak_contexts_; + + enum class ShutdownType + { + pre_shutdown, + on_shutdown + }; + + using ShutdownCallback = ShutdownCallbackHandle::ShutdownCallbackType; + + RCLCPP_LOCAL + ShutdownCallbackHandle + add_shutdown_callback( + ShutdownType shutdown_type, + ShutdownCallback callback); + + RCLCPP_LOCAL + bool + remove_shutdown_callback( + ShutdownType shutdown_type, + const ShutdownCallbackHandle & callback_handle); + + std::vector + get_shutdown_callback(ShutdownType shutdown_type) const; }; /// Return a copy of the list of context shared pointers. diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index b86fe8eafc..fff5753954 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include "rcl/init.h" @@ -307,6 +308,15 @@ Context::shutdown(const std::string & reason) // if it is not valid, then it cannot be shutdown return false; } + + // call each pre-shutdown callback + { + std::lock_guard lock{pre_shutdown_callbacks_mutex_}; + for (const auto & callback : pre_shutdown_callbacks_) { + (*callback)(); + } + } + // rcl shutdown rcl_ret_t ret = rcl_shutdown(rcl_context_.get()); if (RCL_RET_OK != ret) { @@ -355,36 +365,118 @@ Context::on_shutdown(OnShutdownCallback callback) rclcpp::OnShutdownCallbackHandle Context::add_on_shutdown_callback(OnShutdownCallback callback) { - auto callback_shared_ptr = std::make_shared(callback); - { - std::lock_guard lock(on_shutdown_callbacks_mutex_); - on_shutdown_callbacks_.emplace(callback_shared_ptr); + return add_shutdown_callback(ShutdownType::on_shutdown, callback); +} + +bool +Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle) +{ + return remove_shutdown_callback(ShutdownType::on_shutdown, callback_handle); +} + +rclcpp::PreShutdownCallbackHandle +Context::add_pre_shutdown_callback(PreShutdownCallback callback) +{ + return add_shutdown_callback(ShutdownType::pre_shutdown, callback); +} + +bool +Context::remove_pre_shutdown_callback( + const PreShutdownCallbackHandle & callback_handle) +{ + return remove_shutdown_callback(ShutdownType::pre_shutdown, callback_handle); +} + +rclcpp::ShutdownCallbackHandle +Context::add_shutdown_callback( + ShutdownType shutdown_type, + ShutdownCallback callback) +{ + auto callback_shared_ptr = + std::make_shared(callback); + + switch (shutdown_type) { + case ShutdownType::pre_shutdown: + { + std::lock_guard lock(pre_shutdown_callbacks_mutex_); + pre_shutdown_callbacks_.emplace(callback_shared_ptr); + } + break; + case ShutdownType::on_shutdown: + { + std::lock_guard lock(on_shutdown_callbacks_mutex_); + on_shutdown_callbacks_.emplace(callback_shared_ptr); + } + break; } - OnShutdownCallbackHandle callback_handle; + ShutdownCallbackHandle callback_handle; callback_handle.callback = callback_shared_ptr; return callback_handle; } bool -Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle) +Context::remove_shutdown_callback( + ShutdownType shutdown_type, + const ShutdownCallbackHandle & callback_handle) { - std::lock_guard lock(on_shutdown_callbacks_mutex_); + std::mutex * mutex_ptr = nullptr; + std::unordered_set< + std::shared_ptr> * callback_list_ptr; + + switch (shutdown_type) { + case ShutdownType::pre_shutdown: + mutex_ptr = &pre_shutdown_callbacks_mutex_; + callback_list_ptr = &pre_shutdown_callbacks_; + break; + case ShutdownType::on_shutdown: + mutex_ptr = &on_shutdown_callbacks_mutex_; + callback_list_ptr = &on_shutdown_callbacks_; + break; + } + + std::lock_guard lock(*mutex_ptr); auto callback_shared_ptr = callback_handle.callback.lock(); if (callback_shared_ptr == nullptr) { return false; } - return on_shutdown_callbacks_.erase(callback_shared_ptr) == 1; + return callback_list_ptr->erase(callback_shared_ptr) == 1; } std::vector Context::get_on_shutdown_callbacks() const { - std::vector callbacks; + return get_shutdown_callback(ShutdownType::on_shutdown); +} + +std::vector +Context::get_pre_shutdown_callbacks() const +{ + return get_shutdown_callback(ShutdownType::pre_shutdown); +} +std::vector +Context::get_shutdown_callback(ShutdownType shutdown_type) const +{ + std::mutex * mutex_ptr = nullptr; + const std::unordered_set< + std::shared_ptr> * callback_list_ptr; + + switch (shutdown_type) { + case ShutdownType::pre_shutdown: + mutex_ptr = &pre_shutdown_callbacks_mutex_; + callback_list_ptr = &pre_shutdown_callbacks_; + break; + case ShutdownType::on_shutdown: + mutex_ptr = &on_shutdown_callbacks_mutex_; + callback_list_ptr = &on_shutdown_callbacks_; + break; + } + + std::vector callbacks; { - std::lock_guard lock(on_shutdown_callbacks_mutex_); - for (auto & iter : on_shutdown_callbacks_) { + std::lock_guard lock(*mutex_ptr); + for (auto & iter : *callback_list_ptr) { callbacks.emplace_back(*iter); } } diff --git a/rclcpp/test/rclcpp/test_utilities.cpp b/rclcpp/test/rclcpp/test_utilities.cpp index c57fcb9fc3..c177efb7fc 100644 --- a/rclcpp/test/rclcpp/test_utilities.cpp +++ b/rclcpp/test/rclcpp/test_utilities.cpp @@ -111,10 +111,42 @@ TEST(TestUtilities, multi_init) { EXPECT_FALSE(rclcpp::ok(context2)); } +TEST(TestUtilities, test_pre_shutdown_callback_add_remove) { + auto context1 = std::make_shared(); + context1->init(0, nullptr); + + bool is_called1 = false; + bool is_called2 = false; + auto callback1 = [&is_called1]() {is_called1 = true;}; + auto callback2 = [&is_called2]() {is_called2 = true;}; + + EXPECT_EQ(0u, context1->get_pre_shutdown_callbacks().size()); + + rclcpp::PreShutdownCallbackHandle callback_handle1 = + context1->add_pre_shutdown_callback(callback1); + EXPECT_EQ(1u, context1->get_pre_shutdown_callbacks().size()); + + rclcpp::PreShutdownCallbackHandle callback_handle2 = + context1->add_pre_shutdown_callback(callback2); + EXPECT_EQ(2u, context1->get_pre_shutdown_callbacks().size()); + + rclcpp::PreShutdownCallbackHandle wrong_callback_handle; + EXPECT_FALSE(context1->remove_pre_shutdown_callback(wrong_callback_handle)); + + EXPECT_TRUE(context1->remove_pre_shutdown_callback(callback_handle1)); + EXPECT_EQ(1u, context1->get_pre_shutdown_callbacks().size()); + + rclcpp::shutdown(context1); + + EXPECT_FALSE(is_called1); + EXPECT_TRUE(is_called2); +} + TEST(TestUtilities, test_context_basic_access) { auto context1 = std::make_shared(); EXPECT_NE(nullptr, context1->get_init_options().get_rcl_init_options()); EXPECT_EQ(0u, context1->get_on_shutdown_callbacks().size()); + EXPECT_EQ(0u, context1->get_pre_shutdown_callbacks().size()); EXPECT_EQ(std::string{""}, context1->shutdown_reason()); } @@ -123,6 +155,7 @@ TEST(TestUtilities, test_context_basic_access_const_methods) { EXPECT_NE(nullptr, context1->get_init_options().get_rcl_init_options()); EXPECT_EQ(0u, context1->get_on_shutdown_callbacks().size()); + EXPECT_EQ(0u, context1->get_pre_shutdown_callbacks().size()); } MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==) From 94264320b4a2fde8999b895824d2c85cf0e4d2ac Mon Sep 17 00:00:00 2001 From: Grey Date: Tue, 2 Nov 2021 03:29:03 +0800 Subject: [PATCH 033/455] Fix lifetime of context so it remains alive while its dependent node handles are still in use (#1754) * Fix lifetime of context so it remains alive while its dependent node handles are still in use Signed-off-by: Michael X. Grey * Explicitly delete moving and assigning Signed-off-by: Michael X. Grey * Satisfy uncrustify Signed-off-by: Michael X. Grey * Fix more spacing complaints Signed-off-by: Michael X. Grey * Fixing style Signed-off-by: Michael X. Grey * Provide a safe move constructor to avoid double-allocation Signed-off-by: Michael X. Grey * Fix uncrustify Signed-off-by: Michael X. Grey --- .../src/rclcpp/node_interfaces/node_base.cpp | 103 +++++++++++++++--- rclcpp/test/rclcpp/test_node.cpp | 25 +++++ 2 files changed, 114 insertions(+), 14 deletions(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 5240c8c818..f6d851dc54 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include "rclcpp/node_interfaces/node_base.hpp" @@ -31,6 +32,93 @@ using rclcpp::exceptions::throw_from_rcl_error; using rclcpp::node_interfaces::NodeBase; +namespace +{ +/// A class to manage the lifetime of a node handle and its context +/** + * This class bundles together the lifetime of the rcl_node_t handle with the + * lifetime of the RCL context. This ensures that the context will remain alive + * for as long as the rcl_node_t handle is alive. + */ +class NodeHandleWithContext +{ +public: + /// Make an instance of a NodeHandleWithContext + /** + * The make function returns a std::shared_ptr which is actually + * an alias for a std::shared_ptr. There is no use for + * accessing a NodeHandleWithContext instance directly, because its only job + * is to couple together the lifetime of a context with the lifetime of a + * node handle that depends on it. + */ + static std::shared_ptr + make( + rclcpp::Context::SharedPtr context, + std::shared_ptr logging_mutex, + rcl_node_t * node_handle) + { + auto aliased_ptr = std::make_shared( + NodeHandleWithContext( + std::move(context), + std::move(logging_mutex), + node_handle)); + + return std::shared_ptr(std::move(aliased_ptr), node_handle); + } + + // This class should not be copied. It should only exist in the + // std::shared_ptr that it was originally provided in. + NodeHandleWithContext(const NodeHandleWithContext &) = delete; + NodeHandleWithContext & operator=(const NodeHandleWithContext &) = delete; + NodeHandleWithContext & operator=(NodeHandleWithContext &&) = delete; + + NodeHandleWithContext(NodeHandleWithContext && other) + : context_(std::move(other.context_)), + logging_mutex_(std::move(other.logging_mutex_)), + node_handle_(other.node_handle_) + { + other.node_handle_ = nullptr; + } + + ~NodeHandleWithContext() + { + if (!node_handle_) { + // If the node_handle_ is null, then this object was moved-from. We don't + // need to do any cleanup. + return; + } + + std::lock_guard guard(*logging_mutex_); + // TODO(ivanpauno): Instead of mutually excluding rcl_node_fini with the global logger mutex, + // rcl_logging_rosout_fini_publisher_for_node could be decoupled from there and be called + // here directly. + if (rcl_node_fini(node_handle_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rcl node handle: %s", rcl_get_error_string().str); + } + delete node_handle_; + } + +private: + // The constructor is private because this class is only meant to be aliased + // into a std::shared_ptr and should not be constructed any other + // way. + NodeHandleWithContext( + rclcpp::Context::SharedPtr context, + std::shared_ptr logging_mutex, + rcl_node_t * node_handle) + : context_(std::move(context)), + logging_mutex_(std::move(logging_mutex)), + node_handle_(node_handle) + {} + + rclcpp::Context::SharedPtr context_; + std::shared_ptr logging_mutex_; + rcl_node_t * node_handle_; +}; +} // anonymous namespace + NodeBase::NodeBase( const std::string & node_name, const std::string & namespace_, @@ -131,20 +219,7 @@ NodeBase::NodeBase( throw_from_rcl_error(ret, "failed to initialize rcl node"); } - node_handle_.reset( - rcl_node.release(), - [logging_mutex](rcl_node_t * node) -> void { - std::lock_guard guard(*logging_mutex); - // TODO(ivanpauno): Instead of mutually excluding rcl_node_fini with the global logger mutex, - // rcl_logging_rosout_fini_publisher_for_node could be decoupled from there and be called - // here directly. - if (rcl_node_fini(node) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Error in destruction of rcl node handle: %s", rcl_get_error_string().str); - } - delete node; - }); + node_handle_ = NodeHandleWithContext::make(context_, logging_mutex, rcl_node.release()); // Create the default callback group. using rclcpp::CallbackGroupType; diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index e034af16b2..d8c57c3605 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -97,6 +97,31 @@ TEST_F(TestNode, construction_and_destruction) { } } +/* + Testing lifecycles of subscriptions and publishers after node dies + */ +TEST_F(TestNode, pub_and_sub_lifecycles) { + using test_msgs::msg::Empty; + rclcpp::Publisher::SharedPtr pub; + rclcpp::Subscription::SharedPtr sub; + const auto callback = [](Empty::ConstSharedPtr) {}; + + { + // Create the node and context in a nested scope so that their + // std::shared_ptrs expire before we use pub and sub + auto context = std::make_shared(); + context->init(0, nullptr); + rclcpp::NodeOptions options; + options.context(context); + + const auto node = std::make_shared("my_node", "/ns", options); + pub = node->create_publisher("topic", 10); + sub = node->create_subscription("topic", 10, callback); + } + + pub->publish(Empty()); +} + TEST_F(TestNode, get_name_and_namespace) { { auto node = std::make_shared("my_node", "/ns"); From e2aeb1028b48a288bf4c0233c06b36a9e5250cf3 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 5 Nov 2021 08:24:30 -0700 Subject: [PATCH 034/455] Re-add Clock::sleep_until (#1814) * Revert "Revert "Add Clock::sleep_until method (#1748)" (#1793)" This reverts commit d04319a4381909c871b062e73b5975689126fbb6. Signed-off-by: Shane Loretz * Context, Shutdown Callback, Condition Var per call The `Clock` doesn't have enough information to know which Context should wake it on shutdown, so this adds a Context as an argument to sleep_until(). Since the context is per call, the shutdown callback is also registered per call and cannot be stored on impl_. The condition_variable is also unique per call to reduce spurious wakeups when multiple threads sleep on the same clock. Signed-off-by: Shane Loretz * Throw if until has wrong clock type Signed-off-by: Shane Loretz * unnecessary newline Signed-off-by: Shane Loretz * Shorten line Signed-off-by: Shane Loretz * Fix time jump thresholds and add ROS time test Use -1 and 1 thresholds because 0 and 0 is supposed to disable the callbacks Signed-off-by: Shane Loretz * Shorten line Signed-off-by: Shane Loretz * rclcpp::ok() -> context->is_valid() Signed-off-by: Shane Loretz * No pre-jump handler instead of noop handler Signed-off-by: Shane Loretz * If ros_time_is_active errors, let it throw Signed-off-by: Shane Loretz * Get time source change from callback to avoid race if ROS time toggled quickly Signed-off-by: Shane Loretz * Fix threshold and no pre-jump callback Signed-off-by: Shane Loretz * Use RCUTILS_MS_TO_NS macro Signed-off-by: Shane Loretz * Explicit cast for duration to system time Signed-off-by: Shane Loretz * Throws at the end of docblock Signed-off-by: Shane Loretz * Add tests for invalid and non-global contexts Signed-off-by: Shane Loretz --- rclcpp/include/rclcpp/clock.hpp | 25 +++ rclcpp/src/rclcpp/clock.cpp | 96 ++++++++++ rclcpp/test/rclcpp/test_time.cpp | 226 +++++++++++++++++++++++- rclcpp/test/rclcpp/test_time_source.cpp | 33 ++++ 4 files changed, 378 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 9d2ef45b4d..f70e5efeea 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -19,6 +19,7 @@ #include #include +#include "rclcpp/contexts/default_context.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/time.hpp" #include "rclcpp/visibility_control.hpp" @@ -78,6 +79,30 @@ class Clock Time now(); + /** + * Sleep until a specified Time, according to clock type. + * + * Notes for RCL_ROS_TIME clock type: + * - Can sleep forever if ros time is active and received clock never reaches `until` + * - If ROS time enabled state changes during the sleep, this method will immediately return + * false. There is not a consistent choice of sleeping time when the time source changes, + * so this is up to the caller to call again if needed. + * + * \param until absolute time according to current clock type to sleep until. + * \param context the rclcpp context the clock should use to check that ROS is still initialized. + * \return true immediately if `until` is in the past + * \return true when the time `until` is reached + * \return false if time cannot be reached reliably, for example from shutdown or a change + * of time source. + * \throws std::runtime_error if the context is invalid + * \throws std::runtime_error if `until` has a different clock type from this clock + */ + RCLCPP_PUBLIC + bool + sleep_until( + Time until, + Context::SharedPtr context = contexts::get_global_default_context()); + /** * Returns the clock of the type `RCL_ROS_TIME` is active. * diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index e9207e6f31..c9e283ebba 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -14,11 +14,14 @@ #include "rclcpp/clock.hpp" +#include #include #include #include "rclcpp/exceptions.hpp" +#include "rclcpp/utilities.hpp" +#include "rcpputils/scope_exit.hpp" #include "rcutils/logging_macros.h" namespace rclcpp @@ -76,6 +79,99 @@ Clock::now() return now; } +bool +Clock::sleep_until(Time until, Context::SharedPtr context) +{ + if (!context || !context->is_valid()) { + throw std::runtime_error("context cannot be slept with because it's invalid"); + } + const auto this_clock_type = get_clock_type(); + if (until.get_clock_type() != this_clock_type) { + throw std::runtime_error("until's clock type does not match this clock's type"); + } + bool time_source_changed = false; + + std::condition_variable cv; + + // Wake this thread if the context is shutdown + rclcpp::OnShutdownCallbackHandle shutdown_cb_handle = context->add_on_shutdown_callback( + [&cv]() { + cv.notify_one(); + }); + // No longer need the shutdown callback when this function exits + auto callback_remover = rcpputils::scope_exit( + [context, &shutdown_cb_handle]() { + context->remove_on_shutdown_callback(shutdown_cb_handle); + }); + + if (this_clock_type == RCL_STEADY_TIME) { + auto steady_time = std::chrono::steady_clock::time_point( + std::chrono::nanoseconds(until.nanoseconds())); + + // loop over spurious wakeups but notice shutdown + std::unique_lock lock(impl_->clock_mutex_); + while (now() < until && context->is_valid()) { + cv.wait_until(lock, steady_time); + } + } else if (this_clock_type == RCL_SYSTEM_TIME) { + auto system_time = std::chrono::system_clock::time_point( + // Cast because system clock resolution is too big for nanoseconds on some systems + std::chrono::duration_cast( + std::chrono::nanoseconds(until.nanoseconds()))); + + // loop over spurious wakeups but notice shutdown + std::unique_lock lock(impl_->clock_mutex_); + while (now() < until && context->is_valid()) { + cv.wait_until(lock, system_time); + } + } else if (this_clock_type == RCL_ROS_TIME) { + // Install jump handler for any amount of time change, for two purposes: + // - if ROS time is active, check if time reached on each new clock sample + // - Trigger via on_clock_change to detect if time source changes, to invalidate sleep + rcl_jump_threshold_t threshold; + threshold.on_clock_change = true; + // 0 is disable, so -1 and 1 are smallest possible time changes + threshold.min_backward.nanoseconds = -1; + threshold.min_forward.nanoseconds = 1; + auto clock_handler = create_jump_callback( + nullptr, + [&cv, &time_source_changed](const rcl_time_jump_t & jump) { + if (jump.clock_change != RCL_ROS_TIME_NO_CHANGE) { + time_source_changed = true; + } + cv.notify_one(); + }, + threshold); + + if (!ros_time_is_active()) { + auto system_time = std::chrono::system_clock::time_point( + // Cast because system clock resolution is too big for nanoseconds on some systems + std::chrono::duration_cast( + std::chrono::nanoseconds(until.nanoseconds()))); + + // loop over spurious wakeups but notice shutdown or time source change + std::unique_lock lock(impl_->clock_mutex_); + while (now() < until && context->is_valid() && !time_source_changed) { + cv.wait_until(lock, system_time); + } + } else { + // RCL_ROS_TIME with ros_time_is_active. + // Just wait without "until" because installed + // jump callbacks wake the cv on every new sample. + std::unique_lock lock(impl_->clock_mutex_); + while (now() < until && context->is_valid() && !time_source_changed) { + cv.wait(lock); + } + } + } + + if (!context->is_valid() || time_source_changed) { + return false; + } + + return now() >= until; +} + bool Clock::ros_time_is_active() { diff --git a/rclcpp/test/rclcpp/test_time.cpp b/rclcpp/test/rclcpp/test_time.cpp index f7889e96f6..26bfe034eb 100644 --- a/rclcpp/test/rclcpp/test_time.cpp +++ b/rclcpp/test/rclcpp/test_time.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include "rcl/error_handling.h" @@ -24,7 +25,9 @@ #include "rclcpp/clock.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp/time.hpp" +#include "rclcpp/time_source.hpp" #include "rclcpp/utilities.hpp" +#include "rcutils/time.h" #include "../utils/rclcpp_gtest_macros.hpp" @@ -91,8 +94,8 @@ TEST_F(TestTime, time_sources) { EXPECT_NE(0u, steady_now.nanosec); } -static const int64_t HALF_SEC_IN_NS = 500 * 1000 * 1000; -static const int64_t ONE_SEC_IN_NS = 1000 * 1000 * 1000; +static const int64_t HALF_SEC_IN_NS = RCUTILS_MS_TO_NS(500); +static const int64_t ONE_SEC_IN_NS = RCUTILS_MS_TO_NS(1000); static const int64_t ONE_AND_HALF_SEC_IN_NS = 3 * HALF_SEC_IN_NS; TEST_F(TestTime, conversions) { @@ -447,3 +450,222 @@ TEST_F(TestTime, test_overflow_underflow_throws) { test_time = rclcpp::Duration::from_nanoseconds(INT64_MIN) + rclcpp::Time(-1), std::underflow_error("addition leads to int64_t underflow")); } + +class TestClockSleep : public ::testing::Test +{ +protected: + void SetUp() + { + // Shutdown in case there was a dangling global context from other test fixtures + rclcpp::shutdown(); + rclcpp::init(0, nullptr); + node = std::make_shared("clock_sleep_node"); + param_client = std::make_shared(node); + ASSERT_TRUE(param_client->wait_for_service(5s)); + } + + void TearDown() + { + node.reset(); + rclcpp::shutdown(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::SyncParametersClient::SharedPtr param_client; +}; + +TEST_F(TestClockSleep, bad_clock_type) { + rclcpp::Clock clock(RCL_SYSTEM_TIME); + rclcpp::Time steady_until(12345, 0, RCL_STEADY_TIME); + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_until(steady_until), + std::runtime_error("until's clock type does not match this clock's type")); + + rclcpp::Time ros_until(54321, 0, RCL_ROS_TIME); + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_until(ros_until), + std::runtime_error("until's clock type does not match this clock's type")); +} + +TEST_F(TestClockSleep, invalid_context) { + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto until = clock.now(); + + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_until(until, nullptr), + std::runtime_error("context cannot be slept with because it's invalid")); + + auto uninitialized_context = std::make_shared(); + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_until(until, uninitialized_context), + std::runtime_error("context cannot be slept with because it's invalid")); + + auto shutdown_context = std::make_shared(); + shutdown_context->init(0, nullptr); + shutdown_context->shutdown("i am a teapot"); + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_until(until, shutdown_context), + std::runtime_error("context cannot be slept with because it's invalid")); +} + +TEST_F(TestClockSleep, non_global_context) { + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto until = clock.now() + rclcpp::Duration(0, 1); + + auto non_global_context = std::make_shared(); + non_global_context->init(0, nullptr); + ASSERT_TRUE(clock.sleep_until(until, non_global_context)); +} + +TEST_F(TestClockSleep, sleep_until_basic_system) { + const auto milliseconds = 300; + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto delay = rclcpp::Duration(0, RCUTILS_MS_TO_NS(milliseconds)); + auto sleep_until = clock.now() + delay; + + auto start = std::chrono::system_clock::now(); + ASSERT_TRUE(clock.sleep_until(sleep_until)); + auto end = std::chrono::system_clock::now(); + + EXPECT_GE(clock.now(), sleep_until); + EXPECT_GE(end - start, std::chrono::milliseconds(milliseconds)); +} + +TEST_F(TestClockSleep, sleep_until_basic_steady) { + const auto milliseconds = 300; + rclcpp::Clock clock(RCL_STEADY_TIME); + auto delay = rclcpp::Duration(0, RCUTILS_MS_TO_NS(milliseconds)); + auto sleep_until = clock.now() + delay; + + auto steady_start = std::chrono::steady_clock::now(); + ASSERT_TRUE(clock.sleep_until(sleep_until)); + auto steady_end = std::chrono::steady_clock::now(); + + EXPECT_GE(clock.now(), sleep_until); + EXPECT_GE(steady_end - steady_start, std::chrono::milliseconds(milliseconds)); +} + +TEST_F(TestClockSleep, sleep_until_steady_past_returns_immediately) { + rclcpp::Clock clock(RCL_STEADY_TIME); + auto until = clock.now() - rclcpp::Duration(1000, 0); + // This should return immediately, other possible behavior might be sleep forever and timeout + ASSERT_TRUE(clock.sleep_until(until)); +} + +TEST_F(TestClockSleep, sleep_until_system_past_returns_immediately) { + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto until = clock.now() - rclcpp::Duration(1000, 0); + // This should return immediately, other possible behavior might be sleep forever and timeout + ASSERT_TRUE(clock.sleep_until(until)); +} + +TEST_F(TestClockSleep, sleep_until_ros_time_enable_interrupt) { + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source; + time_source.attachNode(node); + time_source.attachClock(clock); + + // 5 second timeout, but it should be interrupted right away + const auto until = clock->now() + rclcpp::Duration(5, 0); + + // Try sleeping with ROS time off, then turn it on to interrupt + bool sleep_succeeded = true; + auto sleep_thread = std::thread( + [clock, until, &sleep_succeeded]() { + sleep_succeeded = clock->sleep_until(until); + }); + // yield execution long enough to let the sleep thread get to waiting on the condition variable + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + auto set_parameters_results = param_client->set_parameters( + {rclcpp::Parameter("use_sim_time", true)}); + for (auto & result : set_parameters_results) { + ASSERT_TRUE(result.successful); + } + sleep_thread.join(); + EXPECT_FALSE(sleep_succeeded); +} + +TEST_F(TestClockSleep, sleep_until_ros_time_disable_interrupt) { + param_client->set_parameters({rclcpp::Parameter("use_sim_time", true)}); + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source; + time_source.attachNode(node); + time_source.attachClock(clock); + + // /clock shouldn't be publishing, shouldn't be possible to reach timeout + const auto until = clock->now() + rclcpp::Duration(600, 0); + + // Try sleeping with ROS time off, then turn it on to interrupt + bool sleep_succeeded = true; + auto sleep_thread = std::thread( + [clock, until, &sleep_succeeded]() { + sleep_succeeded = clock->sleep_until(until); + }); + // yield execution long enough to let the sleep thread get to waiting on the condition variable + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + auto set_parameters_results = param_client->set_parameters( + {rclcpp::Parameter("use_sim_time", false)}); + for (auto & result : set_parameters_results) { + ASSERT_TRUE(result.successful); + } + sleep_thread.join(); + EXPECT_FALSE(sleep_succeeded); +} + +TEST_F(TestClockSleep, sleep_until_shutdown_interrupt) { + param_client->set_parameters({rclcpp::Parameter("use_sim_time", true)}); + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source; + time_source.attachNode(node); + time_source.attachClock(clock); + + // the timeout doesn't matter here - no /clock is being published, so it should never wake + const auto until = clock->now() + rclcpp::Duration(600, 0); + + bool sleep_succeeded = true; + auto sleep_thread = std::thread( + [clock, until, &sleep_succeeded]() { + sleep_succeeded = clock->sleep_until(until); + }); + // yield execution long enough to let the sleep thread get to waiting on the condition variable + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + rclcpp::shutdown(); + sleep_thread.join(); + EXPECT_FALSE(sleep_succeeded); +} + +TEST_F(TestClockSleep, sleep_until_basic_ros) { + rclcpp::Clock clock(RCL_ROS_TIME); + rcl_clock_t * rcl_clock = clock.get_clock_handle(); + + ASSERT_EQ(RCL_ROS_TIME, clock.get_clock_type()); + + // Not zero, because 0 means time not initialized + const rcl_time_point_value_t start_time = 1337; + const rcl_time_point_value_t end_time = start_time + 1; + + // Initialize time + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(rcl_clock)); + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(rcl_clock, start_time)); + + const auto until = rclcpp::Time(end_time, RCL_ROS_TIME); + + bool sleep_succeeded = false; + auto sleep_thread = std::thread( + [&clock, until, &sleep_succeeded]() { + sleep_succeeded = clock.sleep_until(until); + }); + + // yield execution long enough to let the sleep thread get to waiting on the condition variable + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + // False because still sleeping + EXPECT_FALSE(sleep_succeeded); + + // Jump time to the end + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(rcl_clock, end_time)); + ASSERT_EQ(until, clock.now()); + + sleep_thread.join(); + EXPECT_TRUE(sleep_succeeded); +} diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index bbf5d6bc31..4a572c7a70 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -742,3 +742,36 @@ TEST_F(TestTimeSource, check_sim_time_updated_in_callback_if_use_clock_thread) { // Node should have get out of timer callback ASSERT_FALSE(clock_thread_testing_node.GetIsCallbackFrozen()); } + +TEST_F(TestTimeSource, clock_sleep_until_with_ros_time_basic) { + SimClockPublisherNode pub_node; + pub_node.SpinNode(); + + node->set_parameter({"use_sim_time", true}); + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source; + time_source.attachNode(node); + time_source.attachClock(clock); + + // Wait until time source has definitely received a first ROS time from the pub node + { + rcl_jump_threshold_t threshold; + threshold.on_clock_change = false; + threshold.min_backward.nanoseconds = -1; + threshold.min_forward.nanoseconds = 1; + + std::condition_variable cv; + std::mutex mutex; + auto handler = clock->create_jump_callback( + nullptr, + [&cv](const rcl_time_jump_t &) {cv.notify_all();}, + threshold); + std::unique_lock lock(mutex); + cv.wait(lock); + } + + auto now = clock->now(); + // Any amount of time will do, just need to make sure that we awake and return true + auto until = now + rclcpp::Duration(0, 500); + EXPECT_TRUE(clock->sleep_until(until)); +} From b1f31e0eaaf7dcd59505ada2ca6e8646da347f95 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 5 Nov 2021 08:24:58 -0700 Subject: [PATCH 035/455] min_forward & min_backward thresholds must not be disabled (#1815) Signed-off-by: Shane Loretz --- rclcpp/test/rclcpp/test_time_source.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index 4a572c7a70..c355ea6227 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -304,8 +304,8 @@ class CallbackObject TEST_F(TestTimeSource, callbacks) { CallbackObject cbo; rcl_jump_threshold_t jump_threshold; - jump_threshold.min_forward.nanoseconds = 0; - jump_threshold.min_backward.nanoseconds = 0; + jump_threshold.min_forward.nanoseconds = 1; + jump_threshold.min_backward.nanoseconds = -1; jump_threshold.on_clock_change = true; rclcpp::TimeSource ts(node); @@ -396,8 +396,8 @@ TEST_F(TestTimeSource, callbacks) { TEST_F(TestTimeSource, callback_handler_erasure) { CallbackObject cbo; rcl_jump_threshold_t jump_threshold; - jump_threshold.min_forward.nanoseconds = 0; - jump_threshold.min_backward.nanoseconds = 0; + jump_threshold.min_forward.nanoseconds = 1; + jump_threshold.min_backward.nanoseconds = -1; jump_threshold.on_clock_change = true; rclcpp::TimeSource ts(node); From e0e96681d9f0f5bd1f220b3d3107a612dad8321b Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 5 Nov 2021 13:40:08 -0700 Subject: [PATCH 036/455] Update maintainers (#1817) Adding Jacob, removing Mabel. Signed-off-by: Jacob Perron --- rclcpp/package.xml | 3 ++- rclcpp_action/package.xml | 3 ++- rclcpp_components/package.xml | 3 ++- rclcpp_lifecycle/package.xml | 3 ++- 4 files changed, 8 insertions(+), 4 deletions(-) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index e4356c6684..a031169edf 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -5,10 +5,11 @@ 13.1.0 The ROS client library in C++. Ivan Paunovic - Mabel Zhang + Jacob Perron William Woodall Apache License 2.0 Dirk Thomas + Mabel Zhang ament_cmake_ros python3 diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index de5cc6941e..0019a1a8d4 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -5,10 +5,11 @@ 13.1.0 Adds action APIs for C++. Ivan Paunovic - Mabel Zhang + Jacob Perron William Woodall Apache License 2.0 Dirk Thomas + Mabel Zhang ament_cmake_ros diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index a2ee5664a3..0252630e44 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -5,10 +5,11 @@ 13.1.0 Package containing tools for dynamically loadable components Ivan Paunovic - Mabel Zhang + Jacob Perron William Woodall Apache License 2.0 Michael Carroll + Mabel Zhang ament_cmake_ros diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 5e9a7c0fe6..4cf13843ff 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -5,10 +5,11 @@ 13.1.0 Package containing a prototype for lifecycle implementation Ivan Paunovic - Mabel Zhang + Jacob Perron William Woodall Apache License 2.0 Karsten Knese + Mabel Zhang ament_cmake_ros From 0781ea543cc523aa8ecae266847ab42900e2a1fd Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 8 Nov 2021 23:51:05 -0800 Subject: [PATCH 037/455] Remove author by request (#1818) Signed-off-by: Jacob Perron --- rclcpp/package.xml | 1 - rclcpp_action/package.xml | 1 - rclcpp_components/package.xml | 1 - rclcpp_lifecycle/package.xml | 1 - 4 files changed, 4 deletions(-) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index a031169edf..480c0848fc 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -9,7 +9,6 @@ William Woodall Apache License 2.0 Dirk Thomas - Mabel Zhang ament_cmake_ros python3 diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 0019a1a8d4..1fa83b8f60 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -9,7 +9,6 @@ William Woodall Apache License 2.0 Dirk Thomas - Mabel Zhang ament_cmake_ros diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 0252630e44..db7b8fbaae 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -9,7 +9,6 @@ William Woodall Apache License 2.0 Michael Carroll - Mabel Zhang ament_cmake_ros diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 4cf13843ff..235427442c 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -9,7 +9,6 @@ William Woodall Apache License 2.0 Karsten Knese - Mabel Zhang ament_cmake_ros From f7bb88fc8fabcaf5008dfe87b5aec7d2269ba147 Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Thu, 11 Nov 2021 02:12:47 -0500 Subject: [PATCH 038/455] Use parantheses around logging macro parameter (#1820) * Use parantheses around logging macro parameter This allows the macro to expand "correctly", i.e. macro argument expression is fully evaluated before use. Signed-off-by: Abrar Rahman Protyasha * Remove redundant parantheses around macro param `decltype(X)` already provides sufficient "scoping" to the macro parameter `X`. Signed-off-by: Abrar Rahman Protyasha * Add test case for expressions as logging param Signed-off-by: Abrar Rahman Protyasha --- rclcpp/resource/logging.hpp.em | 2 +- rclcpp/test/rclcpp/test_logging.cpp | 6 ++++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/rclcpp/resource/logging.hpp.em b/rclcpp/resource/logging.hpp.em index e8009678c7..228cd7cd48 100644 --- a/rclcpp/resource/logging.hpp.em +++ b/rclcpp/resource/logging.hpp.em @@ -149,7 +149,7 @@ def get_rclcpp_suffix_from_features(features): @[ if params]@ @(''.join([' ' + p + ', \\\n' for p in params if p != stream_arg]))@ @[ end if]@ - logger.get_name(), \ + (logger).get_name(), \ @[ if 'stream' not in feature_combination]@ __VA_ARGS__); \ @[ else]@ diff --git a/rclcpp/test/rclcpp/test_logging.cpp b/rclcpp/test/rclcpp/test_logging.cpp index 7fce7db39a..8e2214f8e6 100644 --- a/rclcpp/test/rclcpp/test_logging.cpp +++ b/rclcpp/test/rclcpp/test_logging.cpp @@ -229,6 +229,12 @@ TEST_F(TestLoggingMacros, test_throttle) { } } +TEST_F(TestLoggingMacros, test_parameter_expression) { + RCLCPP_DEBUG_STREAM(*&g_logger, "message"); + EXPECT_EQ(1u, g_log_calls); + EXPECT_EQ("message", g_last_log_event.message); +} + bool log_function(rclcpp::Logger logger) { RCLCPP_INFO(logger, "successful log"); From e03e98220d7c4a79bfc934d43d762e017b493e9c Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Fri, 12 Nov 2021 16:53:52 -0500 Subject: [PATCH 039/455] Call CMake function to generate version header (#1805) Provides compile-time version information for `rclcpp`. Signed-off-by: Abrar Rahman Protyasha --- rclcpp/CMakeLists.txt | 2 ++ rclcpp/package.xml | 1 + 2 files changed, 3 insertions(+) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 76876b4b2d..125cab556b 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -253,3 +253,5 @@ if(TEST cppcheck) # must set the property after ament_package() set_tests_properties(cppcheck PROPERTIES TIMEOUT 500) endif() + +ament_cmake_gen_version_h() diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 480c0848fc..36f5fa1d80 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -11,6 +11,7 @@ Dirk Thomas ament_cmake_ros + ament_cmake_gen_version_h python3 ament_index_cpp From 87d754b2190edf53a3f7b9f95812e5a80313f649 Mon Sep 17 00:00:00 2001 From: mauropasse Date: Mon, 29 Nov 2021 20:58:47 +0000 Subject: [PATCH 040/455] Use rclcpp::guard_condition (#1612) * Use rclcpp::GuardCondition Signed-off-by: Mauro Passerino * Pass GuardCondition ptr instead of ref to remove_guard_condition Before the api was taking a reference to a guard condition, then getting the address of it. But if a node had expired, we can't get the orig gc dereferencing a pointer, nor can we get an address of an out-of-scope guard condition. Signed-off-by: Mauro Passerino * Address PR comments Signed-off-by: Mauro Passerino Co-authored-by: Mauro Passerino --- rclcpp/CMakeLists.txt | 1 + .../add_guard_condition_to_rcl_wait_set.hpp | 39 ++++++++++ rclcpp/include/rclcpp/executor.hpp | 4 +- .../static_executor_entities_collector.hpp | 4 +- .../subscription_intra_process_base.hpp | 8 +- .../subscription_intra_process_buffer.hpp | 28 +------ rclcpp/include/rclcpp/graph_listener.hpp | 3 +- rclcpp/include/rclcpp/guard_condition.hpp | 9 ++- rclcpp/include/rclcpp/memory_strategy.hpp | 6 +- .../rclcpp/node_interfaces/node_base.hpp | 8 +- .../node_interfaces/node_base_interface.hpp | 14 +--- rclcpp/include/rclcpp/qos_event.hpp | 2 +- .../strategies/allocator_memory_strategy.hpp | 26 ++----- .../detail/storage_policy_common.hpp | 5 +- rclcpp/include/rclcpp/waitable.hpp | 9 +-- .../add_guard_condition_to_rcl_wait_set.cpp | 39 ++++++++++ rclcpp/src/rclcpp/executor.cpp | 78 +++++++++---------- .../static_executor_entities_collector.cpp | 17 ++-- .../static_single_threaded_executor.cpp | 16 +--- rclcpp/src/rclcpp/graph_listener.cpp | 33 ++------ rclcpp/src/rclcpp/guard_condition.cpp | 15 +++- .../src/rclcpp/node_interfaces/node_base.cpp | 41 ++-------- .../src/rclcpp/node_interfaces/node_graph.cpp | 12 +-- .../rclcpp/node_interfaces/node_services.cpp | 28 +++---- .../rclcpp/node_interfaces/node_timers.cpp | 10 ++- .../rclcpp/node_interfaces/node_topics.cpp | 26 +++---- .../rclcpp/node_interfaces/node_waitables.cpp | 14 ++-- rclcpp/src/rclcpp/qos_event.cpp | 3 +- .../subscription_intra_process_base.cpp | 8 +- .../test/rclcpp/executors/test_executors.cpp | 37 ++------- ...est_static_executor_entities_collector.cpp | 6 +- .../node_interfaces/test_node_graph.cpp | 4 +- .../node_interfaces/test_node_services.cpp | 4 +- .../node_interfaces/test_node_timers.cpp | 2 +- .../node_interfaces/test_node_topics.cpp | 2 +- .../node_interfaces/test_node_waitables.cpp | 4 +- .../test_allocator_memory_strategy.cpp | 49 ++++++------ rclcpp/test/rclcpp/test_graph_listener.cpp | 8 +- rclcpp/test/rclcpp/test_memory_strategy.cpp | 2 +- rclcpp/test/rclcpp/test_qos_event.cpp | 2 +- .../test_dynamic_storage.cpp | 2 +- .../wait_set_policies/test_static_storage.cpp | 2 +- .../test_storage_policy_common.cpp | 7 +- .../test_thread_safe_synchronization.cpp | 2 +- .../include/rclcpp_action/client.hpp | 2 +- .../include/rclcpp_action/server.hpp | 2 +- rclcpp_action/src/client.cpp | 6 +- rclcpp_action/src/server.cpp | 6 +- rclcpp_action/test/test_client.cpp | 2 +- rclcpp_action/test/test_server.cpp | 2 +- 50 files changed, 309 insertions(+), 350 deletions(-) create mode 100644 rclcpp/include/rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp create mode 100644 rclcpp/src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 125cab556b..0072aaca8a 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -41,6 +41,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/clock.cpp src/rclcpp/context.cpp src/rclcpp/contexts/default_context.cpp + src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp src/rclcpp/detail/resolve_parameter_overrides.cpp src/rclcpp/detail/rmw_implementation_specific_payload.cpp src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp diff --git a/rclcpp/include/rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp b/rclcpp/include/rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp new file mode 100644 index 0000000000..5f1a7e0df8 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp @@ -0,0 +1,39 @@ +// 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__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_ +#define RCLCPP__DETAIL__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_ + +#include "rclcpp/guard_condition.hpp" + +namespace rclcpp +{ +namespace detail +{ + +/// Adds the guard condition to a waitset +/** + * \param[in] wait_set reference to a wait set where to add the guard condition + * \param[in] guard_condition reference to the guard_condition to be added + */ +RCLCPP_PUBLIC +void +add_guard_condition_to_rcl_wait_set( + rcl_wait_set_t & wait_set, + const rclcpp::GuardCondition & guard_condition); + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_ diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index ca00f1309e..dfd88ccea2 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -525,7 +525,7 @@ class Executor std::atomic_bool spinning; /// Guard condition for signaling the rmw layer to wake up for special events. - rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition(); + rclcpp::GuardCondition interrupt_guard_condition_; std::shared_ptr shutdown_guard_condition_; @@ -549,7 +549,7 @@ class Executor spin_once_impl(std::chrono::nanoseconds timeout); typedef std::map> WeakNodesToGuardConditionsMap; diff --git a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp index 2c4613c845..bda28cb74b 100644 --- a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp @@ -102,7 +102,7 @@ class StaticExecutorEntitiesCollector final * \throws std::runtime_error if it couldn't add guard condition to wait set */ RCLCPP_PUBLIC - bool + void add_to_wait_set(rcl_wait_set_t * wait_set) override; RCLCPP_PUBLIC @@ -328,7 +328,7 @@ class StaticExecutorEntitiesCollector final WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_; typedef std::map> WeakNodesToGuardConditionsMap; WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index ae57521999..ba575c763c 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -25,6 +25,7 @@ #include "rcl/error_handling.h" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/waitable.hpp" @@ -41,9 +42,10 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable RCLCPP_PUBLIC SubscriptionIntraProcessBase( + rclcpp::Context::SharedPtr context, const std::string & topic_name, const rclcpp::QoS & qos_profile) - : topic_name_(topic_name), qos_profile_(qos_profile) + : gc_(context), topic_name_(topic_name), qos_profile_(qos_profile) {} virtual ~SubscriptionIntraProcessBase() = default; @@ -53,7 +55,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable get_number_of_ready_guard_conditions() {return 1;} RCLCPP_PUBLIC - bool + void add_to_wait_set(rcl_wait_set_t * wait_set); virtual bool @@ -79,7 +81,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable protected: std::recursive_mutex reentrant_mutex_; - rcl_guard_condition_t gc_; + rclcpp::GuardCondition gc_; private: virtual void diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 05a830633c..3ec335f9f6 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -67,36 +67,13 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase const std::string & topic_name, const rclcpp::QoS & qos_profile, rclcpp::IntraProcessBufferType buffer_type) - : SubscriptionIntraProcessBase(topic_name, qos_profile) + : SubscriptionIntraProcessBase(context, topic_name, qos_profile) { // Create the intra-process buffer. buffer_ = rclcpp::experimental::create_intra_process_buffer( buffer_type, qos_profile, allocator); - - // Create the guard condition. - rcl_guard_condition_options_t guard_condition_options = - rcl_guard_condition_get_default_options(); - - gc_ = rcl_get_zero_initialized_guard_condition(); - rcl_ret_t ret = rcl_guard_condition_init( - &gc_, context->get_rcl_context().get(), guard_condition_options); - - if (RCL_RET_OK != ret) { - throw std::runtime_error( - "SubscriptionIntraProcessBuffer init error initializing guard condition"); - } - } - - virtual ~SubscriptionIntraProcessBuffer() - { - if (rcl_guard_condition_fini(&gc_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Failed to destroy guard condition: %s", - rcutils_get_error_string().str); - } } bool @@ -130,8 +107,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase void trigger_guard_condition() { - rcl_ret_t ret = rcl_trigger_guard_condition(&gc_); - (void)ret; + gc_.trigger(); } BufferUniquePtr buffer_; diff --git a/rclcpp/include/rclcpp/graph_listener.hpp b/rclcpp/include/rclcpp/graph_listener.hpp index 30305ad2ff..79357b952e 100644 --- a/rclcpp/include/rclcpp/graph_listener.hpp +++ b/rclcpp/include/rclcpp/graph_listener.hpp @@ -24,6 +24,7 @@ #include "rcl/guard_condition.h" #include "rcl/wait.h" #include "rclcpp/context.hpp" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/visibility_control.hpp" @@ -187,7 +188,7 @@ class GraphListener : public std::enable_shared_from_this mutable std::mutex node_graph_interfaces_mutex_; std::vector node_graph_interfaces_; - rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition(); + rclcpp::GuardCondition interrupt_guard_condition_; rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set(); }; diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp index 010c127a07..5104a6446c 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -47,7 +47,9 @@ class GuardCondition RCLCPP_PUBLIC explicit GuardCondition( rclcpp::Context::SharedPtr context = - rclcpp::contexts::get_global_default_context()); + rclcpp::contexts::get_global_default_context(), + rcl_guard_condition_options_t guard_condition_options = + rcl_guard_condition_get_default_options()); RCLCPP_PUBLIC virtual @@ -58,6 +60,11 @@ class GuardCondition rclcpp::Context::SharedPtr get_context() const; + /// Return the underlying rcl guard condition structure. + RCLCPP_PUBLIC + rcl_guard_condition_t & + get_rcl_guard_condition(); + /// Return the underlying rcl guard condition structure. RCLCPP_PUBLIC const rcl_guard_condition_t & diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index da79643e4e..0993c223c9 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -64,9 +64,11 @@ class RCLCPP_PUBLIC MemoryStrategy virtual void clear_handles() = 0; virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0; - virtual void add_guard_condition(const rcl_guard_condition_t * guard_condition) = 0; + virtual void + add_guard_condition(const rclcpp::GuardCondition & guard_condition) = 0; - virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0; + virtual void + remove_guard_condition(const rclcpp::GuardCondition * guard_condition) = 0; virtual void get_next_subscription( diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index 8a19a6c9df..74616a1cbc 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -114,13 +114,9 @@ class NodeBase : public NodeBaseInterface get_associated_with_executor_atomic() override; RCLCPP_PUBLIC - rcl_guard_condition_t * + rclcpp::GuardCondition & get_notify_guard_condition() override; - RCLCPP_PUBLIC - std::unique_lock - acquire_notify_guard_condition_lock() const override; - RCLCPP_PUBLIC bool get_use_intra_process_default() const override; @@ -149,7 +145,7 @@ class NodeBase : public NodeBaseInterface /// Guard condition for notifying the Executor of changes to this node. mutable std::recursive_mutex notify_guard_condition_mutex_; - rcl_guard_condition_t notify_guard_condition_ = rcl_get_zero_initialized_guard_condition(); + rclcpp::GuardCondition notify_guard_condition_; bool notify_guard_condition_is_valid_; }; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index ebe7b1b98e..b0f8fbd65e 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -24,6 +24,7 @@ #include "rclcpp/callback_group.hpp" #include "rclcpp/context.hpp" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" @@ -142,24 +143,17 @@ class NodeBaseInterface std::atomic_bool & get_associated_with_executor_atomic() = 0; - /// Return guard condition that should be notified when the internal node state changes. + /// Return a guard condition that should be notified when the internal node state changes. /** * For example, this should be notified when a publisher is added or removed. * - * \return the rcl_guard_condition_t if it is valid, else nullptr + * \return the GuardCondition if it is valid, else thow runtime error */ RCLCPP_PUBLIC virtual - rcl_guard_condition_t * + rclcpp::GuardCondition & get_notify_guard_condition() = 0; - /// Acquire and return a scoped lock that protects the notify guard condition. - /** This should be used when triggering the notify guard condition. */ - RCLCPP_PUBLIC - virtual - std::unique_lock - acquire_notify_guard_condition_lock() const = 0; - /// Return the default preference for using intra process communication. RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index b3bf61cea5..04231e1395 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -94,7 +94,7 @@ class QOSEventHandlerBase : public Waitable /// Add the Waitable to a wait set. RCLCPP_PUBLIC - bool + void add_to_wait_set(rcl_wait_set_t * wait_set) override; /// Check if the Waitable is ready. diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 43ef71682e..d0bb9c7df2 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -21,6 +21,7 @@ #include "rcl/allocator.h" #include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" #include "rclcpp/memory_strategy.hpp" #include "rclcpp/node.hpp" #include "rclcpp/visibility_control.hpp" @@ -61,17 +62,17 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy allocator_ = std::make_shared(); } - void add_guard_condition(const rcl_guard_condition_t * guard_condition) override + void add_guard_condition(const rclcpp::GuardCondition & guard_condition) override { for (const auto & existing_guard_condition : guard_conditions_) { - if (existing_guard_condition == guard_condition) { + if (existing_guard_condition == &guard_condition) { return; } } - guard_conditions_.push_back(guard_condition); + guard_conditions_.push_back(&guard_condition); } - void remove_guard_condition(const rcl_guard_condition_t * guard_condition) override + void remove_guard_condition(const rclcpp::GuardCondition * guard_condition) override { for (auto it = guard_conditions_.begin(); it != guard_conditions_.end(); ++it) { if (*it == guard_condition) { @@ -240,22 +241,11 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } for (auto guard_condition : guard_conditions_) { - if (rcl_wait_set_add_guard_condition(wait_set, guard_condition, NULL) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add guard_condition to wait set: %s", - rcl_get_error_string().str); - return false; - } + detail::add_guard_condition_to_rcl_wait_set(*wait_set, *guard_condition); } for (auto waitable : waitable_handles_) { - if (!waitable->add_to_wait_set(wait_set)) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add waitable to wait set: %s", rcl_get_error_string().str); - return false; - } + waitable->add_to_wait_set(wait_set); } return true; } @@ -509,7 +499,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy using VectorRebind = std::vector::template rebind_alloc>; - VectorRebind guard_conditions_; + VectorRebind guard_conditions_; VectorRebind> subscription_handles_; VectorRebind> service_handles_; 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 24ed8b9130..2db3ff630a 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 @@ -383,10 +383,7 @@ class StoragePolicyCommon continue; } rclcpp::Waitable & waitable = *waitable_ptr_pair.second; - bool successful = waitable.add_to_wait_set(&rcl_wait_set_); - if (!successful) { - throw std::runtime_error("waitable unexpectedly failed to be added to wait set"); - } + waitable.add_to_wait_set(&rcl_wait_set_); } } diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 85deabda85..c78b0a885f 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -103,12 +103,11 @@ class Waitable /// Add the Waitable to a wait set. /** * \param[in] wait_set A handle to the wait set to add the Waitable to. - * \return `true` if the Waitable is added successfully, `false` otherwise. * \throws rclcpp::execptions::RCLError from rcl_wait_set_add_*() */ RCLCPP_PUBLIC virtual - bool + void add_to_wait_set(rcl_wait_set_t * wait_set) = 0; /// Check if the Waitable is ready. @@ -145,8 +144,7 @@ class Waitable * ```cpp * // ... create a wait set and a Waitable * // Add the Waitable to the wait set - * bool add_ret = waitable.add_to_wait_set(wait_set); - * // ... error handling + * waitable.add_to_wait_set(wait_set); * // Wait * rcl_ret_t wait_ret = rcl_wait(wait_set); * // ... error handling @@ -172,8 +170,7 @@ class Waitable * ```cpp * // ... create a wait set and a Waitable * // Add the Waitable to the wait set - * bool add_ret = waitable.add_to_wait_set(wait_set); - * // ... error handling + * waitable.add_to_wait_set(wait_set); * // Wait * rcl_ret_t wait_ret = rcl_wait(wait_set); * // ... error handling diff --git a/rclcpp/src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp b/rclcpp/src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp new file mode 100644 index 0000000000..85b4c6594a --- /dev/null +++ b/rclcpp/src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp @@ -0,0 +1,39 @@ +// 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. + +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" +#include "rclcpp/exceptions.hpp" + +namespace rclcpp +{ +namespace detail +{ + +void +add_guard_condition_to_rcl_wait_set( + rcl_wait_set_t & wait_set, + const rclcpp::GuardCondition & guard_condition) +{ + const auto & gc = guard_condition.get_rcl_guard_condition(); + + rcl_ret_t ret = rcl_wait_set_add_guard_condition(&wait_set, &gc, NULL); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to add guard condition to wait set"); + } +} + +} // namespace detail +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index a5d4050e70..d3f6cb361f 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -45,19 +45,13 @@ using rclcpp::FutureReturnCode; Executor::Executor(const rclcpp::ExecutorOptions & options) : spinning(false), + interrupt_guard_condition_(options.context), shutdown_guard_condition_(std::make_shared(options.context)), memory_strategy_(options.memory_strategy) { // Store the context for later use. context_ = options.context; - rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); - rcl_ret_t ret = rcl_guard_condition_init( - &interrupt_guard_condition_, context_->get_rcl_context().get(), guard_condition_options); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "Failed to create interrupt guard condition in Executor constructor"); - } - shutdown_callback_handle_ = context_->add_on_shutdown_callback( [weak_gc = std::weak_ptr{shutdown_guard_condition_}]() { auto strong_gc = weak_gc.lock(); @@ -68,13 +62,13 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) // The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond, // and one for the executor's guard cond (interrupt_guard_condition_) - memory_strategy_->add_guard_condition(&shutdown_guard_condition_->get_rcl_guard_condition()); + memory_strategy_->add_guard_condition(*shutdown_guard_condition_.get()); // Put the executor's guard condition in - memory_strategy_->add_guard_condition(&interrupt_guard_condition_); + memory_strategy_->add_guard_condition(interrupt_guard_condition_); rcl_allocator_t allocator = memory_strategy_->get_allocator(); - ret = rcl_wait_set_init( + rcl_ret_t ret = rcl_wait_set_init( &wait_set_, 0, 2, 0, 0, 0, 0, context_->get_rcl_context().get(), @@ -84,12 +78,6 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) "rclcpp", "failed to create wait set: %s", rcl_get_error_string().str); rcl_reset_error(); - if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy guard condition: %s", rcl_get_error_string().str); - rcl_reset_error(); - } throw_from_rcl_error(ret, "Failed to create wait set in Executor constructor"); } } @@ -119,7 +107,7 @@ Executor::~Executor() weak_groups_to_nodes_associated_with_executor_.clear(); weak_groups_to_nodes_.clear(); for (const auto & pair : weak_nodes_to_guard_conditions_) { - auto & guard_condition = pair.second; + auto guard_condition = pair.second; memory_strategy_->remove_guard_condition(guard_condition); } weak_nodes_to_guard_conditions_.clear(); @@ -131,15 +119,9 @@ Executor::~Executor() "failed to destroy wait set: %s", rcl_get_error_string().str); rcl_reset_error(); } - // Finalize the interrupt guard condition. - if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy guard condition: %s", rcl_get_error_string().str); - rcl_reset_error(); - } // Remove and release the sigint guard condition - memory_strategy_->remove_guard_condition(&shutdown_guard_condition_->get_rcl_guard_condition()); + memory_strategy_->remove_guard_condition(shutdown_guard_condition_.get()); + memory_strategy_->remove_guard_condition(&interrupt_guard_condition_); // Remove shutdown callback handle registered to Context if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) { @@ -234,17 +216,20 @@ Executor::add_callback_group_to_map( // Also add to the map that contains all callback groups weak_groups_to_nodes_.insert(std::make_pair(weak_group_ptr, node_ptr)); if (is_new_node) { - rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr); - weak_nodes_to_guard_conditions_[node_weak_ptr] = node_ptr->get_notify_guard_condition(); + const auto & gc = node_ptr->get_notify_guard_condition(); + weak_nodes_to_guard_conditions_[node_ptr] = &gc; if (notify) { // Interrupt waiting to handle new node - rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group add"); + try { + interrupt_guard_condition_.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on callback group add: ") + ex.what()); } } // Add the node's notify condition to the guard condition handles - memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition()); + memory_strategy_->add_guard_condition(gc); } } @@ -315,15 +300,17 @@ Executor::remove_callback_group_from_map( if (!has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) && !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_)) { - rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr); - weak_nodes_to_guard_conditions_.erase(node_weak_ptr); + weak_nodes_to_guard_conditions_.erase(node_ptr); if (notify) { - rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group remove"); + try { + interrupt_guard_condition_.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on callback group remove: ") + ex.what()); } } - memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition()); + memory_strategy_->remove_guard_condition(&node_ptr->get_notify_guard_condition()); } } @@ -494,9 +481,11 @@ void Executor::cancel() { spinning.store(false); - rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "Failed to trigger guard condition in cancel"); + try { + interrupt_guard_condition_.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string("Failed to trigger guard condition in cancel: ") + ex.what()); } } @@ -541,9 +530,12 @@ Executor::execute_any_executable(AnyExecutable & any_exec) any_exec.callback_group->can_be_taken_from().store(true); // Wake the wait, because it may need to be recalculated or work that // was previously blocked is now available. - rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "Failed to trigger guard condition from execute_any_executable"); + try { + interrupt_guard_condition_.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition from execute_any_executable: ") + ex.what()); } } diff --git a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp index b6ceb5b331..bf50e062f3 100644 --- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp @@ -23,6 +23,7 @@ #include "rclcpp/memory_strategy.hpp" #include "rclcpp/executors/static_single_threaded_executor.hpp" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" using rclcpp::executors::StaticExecutorEntitiesCollector; @@ -108,7 +109,8 @@ StaticExecutorEntitiesCollector::execute(std::shared_ptr & data) std::lock_guard guard{new_nodes_mutex_}; for (const auto & weak_node : new_nodes_) { if (auto node_ptr = weak_node.lock()) { - weak_nodes_to_guard_conditions_[node_ptr] = node_ptr->get_notify_guard_condition(); + const auto & gc = node_ptr->get_notify_guard_condition(); + weak_nodes_to_guard_conditions_[node_ptr] = &gc; } } new_nodes_.clear(); @@ -265,18 +267,14 @@ StaticExecutorEntitiesCollector::refresh_wait_set(std::chrono::nanoseconds timeo } } -bool +void StaticExecutorEntitiesCollector::add_to_wait_set(rcl_wait_set_t * wait_set) { // Add waitable guard conditions (one for each registered node) into the wait set. for (const auto & pair : weak_nodes_to_guard_conditions_) { auto & gc = pair.second; - rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, gc, NULL); - if (ret != RCL_RET_OK) { - throw std::runtime_error("Executor waitable: couldn't add guard condition to wait set"); - } + detail::add_guard_condition_to_rcl_wait_set(*wait_set, *gc); } - return true; } size_t StaticExecutorEntitiesCollector::get_number_of_ready_guard_conditions() @@ -441,8 +439,9 @@ StaticExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set) auto found_guard_condition = std::find_if( weak_nodes_to_guard_conditions_.begin(), weak_nodes_to_guard_conditions_.end(), [&](std::pair pair) -> bool { - return pair.second == p_wait_set->guard_conditions[i]; + const GuardCondition *> pair) -> bool { + const rcl_guard_condition_t & rcl_gc = pair.second->get_rcl_guard_condition(); + return &rcl_gc == p_wait_set->guard_conditions[i]; }); if (found_guard_condition != weak_nodes_to_guard_conditions_.end()) { return true; diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 8350521fe3..683d300c06 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -139,9 +139,7 @@ StaticSingleThreadedExecutor::add_callback_group( bool is_new_node = entities_collector_->add_callback_group(group_ptr, node_ptr); if (is_new_node && notify) { // Interrupt waiting to handle new node - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string().str); - } + interrupt_guard_condition_.trigger(); } } @@ -152,9 +150,7 @@ StaticSingleThreadedExecutor::add_node( bool is_new_node = entities_collector_->add_node(node_ptr); if (is_new_node && notify) { // Interrupt waiting to handle new node - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string().str); - } + interrupt_guard_condition_.trigger(); } } @@ -171,9 +167,7 @@ StaticSingleThreadedExecutor::remove_callback_group( bool node_removed = entities_collector_->remove_callback_group(group_ptr); // If the node was matched and removed, interrupt waiting if (node_removed && notify) { - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string().str); - } + interrupt_guard_condition_.trigger(); } } @@ -187,9 +181,7 @@ StaticSingleThreadedExecutor::remove_node( } // If the node was matched and removed, interrupt waiting if (notify) { - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string().str); - } + interrupt_guard_condition_.trigger(); } } diff --git a/rclcpp/src/rclcpp/graph_listener.cpp b/rclcpp/src/rclcpp/graph_listener.cpp index 0af47acc5b..e0b516b595 100644 --- a/rclcpp/src/rclcpp/graph_listener.cpp +++ b/rclcpp/src/rclcpp/graph_listener.cpp @@ -22,6 +22,7 @@ #include "rcl/error_handling.h" #include "rcl/types.h" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/node.hpp" @@ -40,19 +41,9 @@ GraphListener::GraphListener(const std::shared_ptr & parent_context) : weak_parent_context_(parent_context), rcl_parent_context_(parent_context->get_rcl_context()), is_started_(false), - is_shutdown_(false) + is_shutdown_(false), + interrupt_guard_condition_(parent_context) { - // TODO(wjwwood): make a guard condition class in rclcpp so this can be tracked - // automatically with the rcl guard condition - // hold on to this context to prevent it from going out of scope while this - // guard condition is using it. - rcl_ret_t ret = rcl_guard_condition_init( - &interrupt_guard_condition_, - rcl_parent_context_.get(), - rcl_guard_condition_get_default_options()); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to create interrupt guard condition"); - } } GraphListener::~GraphListener() @@ -159,10 +150,7 @@ GraphListener::run_loop() throw_from_rcl_error(ret, "failed to clear wait set"); } // Put the interrupt guard condition in the wait set. - ret = rcl_wait_set_add_guard_condition(&wait_set_, &interrupt_guard_condition_, NULL); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to add interrupt guard condition to wait set"); - } + detail::add_guard_condition_to_rcl_wait_set(wait_set_, interrupt_guard_condition_); // Put graph guard conditions for each node into the wait set. std::vector graph_gc_indexes(node_graph_interfaces_size, 0u); @@ -211,19 +199,16 @@ GraphListener::run_loop() } static void -interrupt_(rcl_guard_condition_t * interrupt_guard_condition) +interrupt_(GuardCondition * interrupt_guard_condition) { - rcl_ret_t ret = rcl_trigger_guard_condition(interrupt_guard_condition); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to trigger the interrupt guard condition"); - } + interrupt_guard_condition->trigger(); } static void acquire_nodes_lock_( std::mutex * node_graph_interfaces_barrier_mutex, std::mutex * node_graph_interfaces_mutex, - rcl_guard_condition_t * interrupt_guard_condition) + GuardCondition * interrupt_guard_condition) { { // Acquire this lock to prevent the run loop from re-locking the @@ -351,10 +336,6 @@ GraphListener::__shutdown() interrupt_(&interrupt_guard_condition_); listener_thread_.join(); } - rcl_ret_t ret = rcl_guard_condition_fini(&interrupt_guard_condition_); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to finalize interrupt guard condition"); - } if (is_started_) { cleanup_wait_set(); } diff --git a/rclcpp/src/rclcpp/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp index 22ca3f3223..cae0c4ce5c 100644 --- a/rclcpp/src/rclcpp/guard_condition.cpp +++ b/rclcpp/src/rclcpp/guard_condition.cpp @@ -20,19 +20,20 @@ namespace rclcpp { -GuardCondition::GuardCondition(rclcpp::Context::SharedPtr context) +GuardCondition::GuardCondition( + rclcpp::Context::SharedPtr context, + rcl_guard_condition_options_t guard_condition_options) : context_(context), rcl_guard_condition_{rcl_get_zero_initialized_guard_condition()} { if (!context_) { throw std::invalid_argument("context argument unexpectedly nullptr"); } - rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); rcl_ret_t ret = rcl_guard_condition_init( &this->rcl_guard_condition_, context_->get_rcl_context().get(), guard_condition_options); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to create guard condition"); } } @@ -45,7 +46,7 @@ GuardCondition::~GuardCondition() } catch (const std::exception & exception) { RCLCPP_ERROR( rclcpp::get_logger("rclcpp"), - "Error in destruction of rcl guard condition: %s", exception.what()); + "failed to finalize guard condition: %s", exception.what()); } } } @@ -56,6 +57,12 @@ GuardCondition::get_context() const return context_; } +rcl_guard_condition_t & +GuardCondition::get_rcl_guard_condition() +{ + return rcl_guard_condition_; +} + const rcl_guard_condition_t & GuardCondition::get_rcl_guard_condition() const { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index f6d851dc54..e68d1a01eb 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -132,30 +132,15 @@ NodeBase::NodeBase( node_handle_(nullptr), default_callback_group_(nullptr), associated_with_executor_(false), + notify_guard_condition_(context), notify_guard_condition_is_valid_(false) { - // Setup the guard condition that is notified when changes occur in the graph. - rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); - rcl_ret_t ret = rcl_guard_condition_init( - ¬ify_guard_condition_, context_->get_rcl_context().get(), guard_condition_options); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "failed to create interrupt guard condition"); - } - - // Setup a safe exit lamda to clean up the guard condition in case of an error here. - auto finalize_notify_guard_condition = [this]() { - // Finalize the interrupt guard condition. - if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy guard condition: %s", rcl_get_error_string().str); - } - }; - // Create the rcl node and store it in a shared_ptr with a custom destructor. std::unique_ptr rcl_node(new rcl_node_t(rcl_get_zero_initialized_node())); std::shared_ptr logging_mutex = get_global_logging_mutex(); + + rcl_ret_t ret; { std::lock_guard guard(*logging_mutex); // TODO(ivanpauno): /rosout Qos should be reconfigurable. @@ -168,9 +153,6 @@ NodeBase::NodeBase( context_->get_rcl_context().get(), &rcl_node_options); } if (ret != RCL_RET_OK) { - // Finalize the interrupt guard condition. - finalize_notify_guard_condition(); - if (ret == RCL_RET_NODE_INVALID_NAME) { rcl_reset_error(); // discard rcl_node_init error int validation_result; @@ -235,11 +217,6 @@ NodeBase::~NodeBase() { std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); notify_guard_condition_is_valid_ = false; - if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy guard condition: %s", rcl_get_error_string().str); - } } } @@ -340,20 +317,14 @@ NodeBase::get_associated_with_executor_atomic() return associated_with_executor_; } -rcl_guard_condition_t * +rclcpp::GuardCondition & NodeBase::get_notify_guard_condition() { std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); if (!notify_guard_condition_is_valid_) { - return nullptr; + throw std::runtime_error("failed to get notify guard condition because it is invalid"); } - return ¬ify_guard_condition_; -} - -std::unique_lock -NodeBase::acquire_notify_guard_condition_lock() const -{ - return std::unique_lock(notify_guard_condition_mutex_); + return notify_guard_condition_; } bool diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index c993a4ed6f..e13ec7cd4c 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -533,12 +533,12 @@ NodeGraph::notify_graph_change() } } graph_cv_.notify_all(); - { - auto notify_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - rcl_ret_t ret = rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to trigger notify guard condition"); - } + auto & node_gc = node_base_->get_notify_guard_condition(); + try { + node_gc.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string("failed to notify wait set on graph change: ") + ex.what()); } } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp index dee39cb403..14ab1c82c4 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp @@ -41,14 +41,12 @@ NodeServices::add_service( } // Notify the executor that a new service was created using the parent Node. - { - auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { - throw std::runtime_error( - std::string("Failed to notify wait set on service creation: ") + - rmw_get_error_string().str - ); - } + auto & node_gc = node_base_->get_notify_guard_condition(); + try { + node_gc.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string("failed to notify wait set on service creation: ") + ex.what()); } } @@ -68,14 +66,12 @@ NodeServices::add_client( } // Notify the executor that a new client was created using the parent Node. - { - auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { - throw std::runtime_error( - std::string("Failed to notify wait set on client creation: ") + - rmw_get_error_string().str - ); - } + auto & node_gc = node_base_->get_notify_guard_condition(); + try { + node_gc.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string("failed to notify wait set on client creation: ") + ex.what()); } } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp index 6936e26759..b463e8a0e7 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp @@ -41,11 +41,15 @@ NodeTimers::add_timer( } else { node_base_->get_default_callback_group()->add_timer(timer); } - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { + + auto & node_gc = node_base_->get_notify_guard_condition(); + try { + node_gc.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( - std::string("Failed to notify wait set on timer creation: ") + - rmw_get_error_string().str); + std::string("failed to notify wait set on timer creation: ") + ex.what()); } + TRACEPOINT( rclcpp_timer_link_node, static_cast(timer->get_timer_handle().get()), diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index c57fbcee5d..fa188a2f9a 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -60,13 +60,12 @@ NodeTopics::add_publisher( } // Notify the executor that a new publisher was created using the parent Node. - { - auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { - throw std::runtime_error( - std::string("Failed to notify wait set on publisher creation: ") + - rmw_get_error_string().str); - } + auto & node_gc = node_base_->get_notify_guard_condition(); + try { + node_gc.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string("failed to notify wait set on publisher creation: ") + ex.what()); } } @@ -108,13 +107,12 @@ NodeTopics::add_subscription( } // Notify the executor that a new subscription was created using the parent Node. - { - auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - auto ret = rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()); - if (ret != RCL_RET_OK) { - using rclcpp::exceptions::throw_from_rcl_error; - throw_from_rcl_error(ret, "failed to notify wait set on subscription creation"); - } + auto & node_gc = node_base_->get_notify_guard_condition(); + try { + node_gc.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string("failed to notify wait set on subscription creation: ") + ex.what()); } } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp index 0fd083788a..6f243f6025 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp @@ -41,14 +41,12 @@ NodeWaitables::add_waitable( } // Notify the executor that a new waitable was created using the parent Node. - { - auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { - throw std::runtime_error( - std::string("Failed to notify wait set on waitable creation: ") + - rmw_get_error_string().str - ); - } + auto & node_gc = node_base_->get_notify_guard_condition(); + try { + node_gc.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string("failed to notify wait set on waitable creation: ") + ex.what()); } } diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index 8af3918c48..daf1d3e01e 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -51,14 +51,13 @@ QOSEventHandlerBase::get_number_of_ready_events() } /// Add the Waitable to a wait set. -bool +void QOSEventHandlerBase::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_); if (RCL_RET_OK != ret) { exceptions::throw_from_rcl_error(ret, "Couldn't add event to wait set"); } - return true; } /// Check if the Waitable is ready. diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index 2738161675..b13123b65b 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -13,16 +13,14 @@ // limitations under the License. #include "rclcpp/experimental/subscription_intra_process_base.hpp" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" using rclcpp::experimental::SubscriptionIntraProcessBase; -bool +void SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set) { - std::lock_guard lock(reentrant_mutex_); - - rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL); - return RCL_RET_OK == ret; + detail::add_guard_condition_to_rcl_wait_set(*wait_set, gc_); } const char * diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 1cf90d9901..1fa2cbb4dd 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -30,7 +30,9 @@ #include "rcl/error_handling.h" #include "rcl/time.h" #include "rclcpp/clock.hpp" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" #include "rclcpp/duration.hpp" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/rclcpp.hpp" #include "test_msgs/msg/empty.hpp" @@ -352,40 +354,17 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) { class TestWaitable : public rclcpp::Waitable { public: - TestWaitable() - { - rcl_guard_condition_options_t guard_condition_options = - rcl_guard_condition_get_default_options(); - - gc_ = rcl_get_zero_initialized_guard_condition(); - rcl_ret_t ret = rcl_guard_condition_init( - &gc_, - rclcpp::contexts::get_global_default_context()->get_rcl_context().get(), - guard_condition_options); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - } + TestWaitable() = default; - ~TestWaitable() - { - rcl_ret_t ret = rcl_guard_condition_fini(&gc_); - if (RCL_RET_OK != ret) { - fprintf(stderr, "failed to call rcl_guard_condition_fini\n"); - } - } - - bool + void add_to_wait_set(rcl_wait_set_t * wait_set) override { - rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL); - return RCL_RET_OK == ret; + rclcpp::detail::add_guard_condition_to_rcl_wait_set(*wait_set, gc_); } - bool trigger() + void trigger() { - rcl_ret_t ret = rcl_trigger_guard_condition(&gc_); - return RCL_RET_OK == ret; + gc_.trigger(); } bool @@ -420,7 +399,7 @@ class TestWaitable : public rclcpp::Waitable private: size_t count_ = 0; - rcl_guard_condition_t gc_; + rclcpp::GuardCondition gc_; }; TYPED_TEST(TestExecutors, spinAll) { 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 dc81b5611e..1ea91029f4 100644 --- a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp @@ -230,7 +230,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) { class TestWaitable : public rclcpp::Waitable { public: - bool add_to_wait_set(rcl_wait_set_t *) override {return true;} + void add_to_wait_set(rcl_wait_set_t *) override {} bool is_ready(rcl_wait_set_t *) override {return true;} @@ -513,9 +513,9 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_to_wait_set_nullptr) { entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); - RCLCPP_EXPECT_THROW_EQ( + EXPECT_THROW( entities_collector_->add_to_wait_set(nullptr), - std::runtime_error("Executor waitable: couldn't add guard condition to wait set")); + 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_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp index 03f654a928..c25af15422 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -556,9 +556,9 @@ TEST_F(TestNodeGraph, notify_graph_change_rcl_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); - EXPECT_THROW( + RCLCPP_EXPECT_THROW_EQ( node()->get_node_graph_interface()->notify_graph_change(), - rclcpp::exceptions::RCLError); + std::runtime_error("failed to notify wait set on graph change: error not set")); } TEST_F(TestNodeGraph, get_info_by_topic) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp index a7607f5d76..ea55a1aac2 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp @@ -103,7 +103,7 @@ TEST_F(TestNodeService, add_service_rcl_trigger_guard_condition_error) "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( node_services->add_service(service, callback_group), - std::runtime_error("Failed to notify wait set on service creation: error not set")); + std::runtime_error("failed to notify wait set on service creation: error not set")); } TEST_F(TestNodeService, add_client) @@ -130,7 +130,7 @@ TEST_F(TestNodeService, add_client_rcl_trigger_guard_condition_error) "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( node_services->add_client(client, callback_group), - std::runtime_error("Failed to notify wait set on client creation: error not set")); + std::runtime_error("failed to notify wait set on client creation: error not set")); } TEST_F(TestNodeService, resolve_service_name) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp index d368e0e875..d038a4b44d 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp @@ -87,5 +87,5 @@ TEST_F(TestNodeTimers, add_timer_rcl_trigger_guard_condition_error) "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( node_timers->add_timer(timer, callback_group), - std::runtime_error("Failed to notify wait set on timer creation: error not set")); + std::runtime_error("failed to notify wait set on timer creation: error not set")); } diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp index bb59c83bb2..a99633bec6 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp @@ -129,7 +129,7 @@ TEST_F(TestNodeTopics, add_publisher_rcl_trigger_guard_condition_error) "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( node_topics->add_publisher(publisher, callback_group), - std::runtime_error("Failed to notify wait set on publisher creation: error not set")); + std::runtime_error("failed to notify wait set on publisher creation: error not set")); } TEST_F(TestNodeTopics, add_subscription) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp index a12ef36eab..a1f35c0cdc 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp @@ -28,7 +28,7 @@ class TestWaitable : public rclcpp::Waitable { public: - bool add_to_wait_set(rcl_wait_set_t *) override {return false;} + void add_to_wait_set(rcl_wait_set_t *) override {} bool is_ready(rcl_wait_set_t *) override {return false;} std::shared_ptr @@ -96,5 +96,5 @@ TEST_F(TestNodeWaitables, add_waitable_rcl_trigger_guard_condition_error) "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( node_waitables->add_waitable(waitable, callback_group), - std::runtime_error("Failed to notify wait set on waitable creation: error not set")); + std::runtime_error("failed to notify wait set on waitable creation: error not set")); } diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index 09279ed024..c068e8f640 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -39,9 +39,11 @@ static bool test_waitable_result = false; class TestWaitable : public rclcpp::Waitable { public: - bool add_to_wait_set(rcl_wait_set_t *) override + void add_to_wait_set(rcl_wait_set_t *) override { - return test_waitable_result; + if (!test_waitable_result) { + throw std::runtime_error("TestWaitable add_to_wait_set failed"); + } } bool is_ready(rcl_wait_set_t *) override @@ -453,19 +455,19 @@ TEST_F(TestAllocatorMemoryStrategy, construct_destruct) { } TEST_F(TestAllocatorMemoryStrategy, add_remove_guard_conditions) { - rcl_guard_condition_t guard_condition1 = rcl_get_zero_initialized_guard_condition(); - rcl_guard_condition_t guard_condition2 = rcl_get_zero_initialized_guard_condition(); - rcl_guard_condition_t guard_condition3 = rcl_get_zero_initialized_guard_condition(); + rclcpp::GuardCondition guard_condition1; + rclcpp::GuardCondition guard_condition2; + rclcpp::GuardCondition guard_condition3; - EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition1)); - EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition2)); - EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition3)); + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition1)); + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition2)); + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition3)); EXPECT_EQ(3u, allocator_memory_strategy()->number_of_guard_conditions()); // Adding a second time should not add to vector - EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition1)); - EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition2)); - EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition3)); + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition1)); + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition2)); + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition3)); EXPECT_EQ(3u, allocator_memory_strategy()->number_of_guard_conditions()); EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition1)); @@ -569,24 +571,17 @@ TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_client) { TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_guard_condition) { auto node = create_node_with_disabled_callback_groups("node"); - rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition(); auto context = node->get_node_base_interface()->get_context(); - rcl_context_t * rcl_context = context->get_rcl_context().get(); - rcl_guard_condition_options_t guard_condition_options = { - rcl_get_default_allocator()}; - - EXPECT_EQ( - RCL_RET_OK, - rcl_guard_condition_init(&guard_condition, rcl_context, guard_condition_options)); - RCPPUTILS_SCOPE_EXIT( - { - EXPECT_EQ(RCL_RET_OK, rcl_guard_condition_fini(&guard_condition)); - }); - allocator_memory_strategy()->add_guard_condition(&guard_condition); + rclcpp::GuardCondition guard_condition(context); + + EXPECT_NO_THROW(rclcpp::GuardCondition guard_condition(context);); + + allocator_memory_strategy()->add_guard_condition(guard_condition); + RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities(); insufficient_capacities.size_of_guard_conditions = 0; - EXPECT_TRUE(TestAddHandlesToWaitSet(node, insufficient_capacities)); + EXPECT_THROW(TestAddHandlesToWaitSet(node, insufficient_capacities), std::runtime_error); } TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_timer) { @@ -607,7 +602,9 @@ TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_waitable) { EXPECT_TRUE(allocator_memory_strategy()->add_handles_to_wait_set(nullptr)); test_waitable_result = false; - EXPECT_FALSE(allocator_memory_strategy()->add_handles_to_wait_set(nullptr)); + EXPECT_THROW( + allocator_memory_strategy()->add_handles_to_wait_set(nullptr), + std::runtime_error); // This calls TestWaitable's functions, so rcl errors are not set EXPECT_FALSE(rcl_error_is_set()); diff --git a/rclcpp/test/rclcpp/test_graph_listener.cpp b/rclcpp/test/rclcpp/test_graph_listener.cpp index bd66c686d7..24c370e65d 100644 --- a/rclcpp/test/rclcpp/test_graph_listener.cpp +++ b/rclcpp/test/rclcpp/test_graph_listener.cpp @@ -87,7 +87,7 @@ TEST_F(TestGraphListener, error_construct_graph_listener) { auto graph_listener_error = std::make_shared(get_global_default_context()); graph_listener_error.reset(); - }, std::runtime_error("failed to create interrupt guard condition: error not set")); + }, std::runtime_error("failed to create guard condition: error not set")); } // Required for mocking_utils below @@ -169,7 +169,7 @@ TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_set_add_guard_condi "lib:rclcpp", rcl_wait_set_add_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( graph_listener_test->run_protected(), - std::runtime_error("failed to add interrupt guard condition to wait set: error not set")); + std::runtime_error("failed to add guard condition to wait set: error not set")); } TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_error) { @@ -292,9 +292,7 @@ TEST_F(TestGraphListener, test_graph_listener_shutdown_guard_fini_error_throw) { auto mock_wait_set_fini = mocking_utils::patch_and_return( "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR); - RCLCPP_EXPECT_THROW_EQ( - graph_listener_test->shutdown(), - std::runtime_error("failed to finalize interrupt guard condition: error not set")); + EXPECT_NO_THROW(graph_listener_test->shutdown()); graph_listener_test->mock_cleanup_wait_set(); } diff --git a/rclcpp/test/rclcpp/test_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp index f20926bafa..c919d1981d 100644 --- a/rclcpp/test/rclcpp/test_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/test_memory_strategy.cpp @@ -35,7 +35,7 @@ typedef std::map take_data() override {return nullptr;} void execute(std::shared_ptr & data) override {(void)data;} diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index 960ffdcb4c..ebf0e14afb 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -315,7 +315,7 @@ 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_TRUE(handler.add_to_wait_set(&wait_set)); + EXPECT_NO_THROW(handler.add_to_wait_set(&wait_set)); } { 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 7a15e2cf49..9afb97536a 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp @@ -51,7 +51,7 @@ class TestWaitable : public rclcpp::Waitable TestWaitable() : is_ready_(false) {} - bool add_to_wait_set(rcl_wait_set_t *) override {return true;} + void add_to_wait_set(rcl_wait_set_t *) override {} bool is_ready(rcl_wait_set_t *) override {return is_ready_;} 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 1e4d5f805e..470d0de361 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp @@ -50,7 +50,7 @@ class TestWaitable : public rclcpp::Waitable public: TestWaitable() : is_ready_(false) {} - bool add_to_wait_set(rcl_wait_set_t *) override {return true;} + void add_to_wait_set(rcl_wait_set_t *) override {} bool is_ready(rcl_wait_set_t *) override {return is_ready_;} 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 ba0f4d16e8..d217286da1 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,7 +50,12 @@ class TestWaitable : public rclcpp::Waitable public: TestWaitable() : is_ready_(false), add_to_wait_set_(false) {} - bool add_to_wait_set(rcl_wait_set_t *) override {return add_to_wait_set_;} + 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_;} 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 99d07f1a19..347714bbf7 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,7 +50,7 @@ class TestWaitable : public rclcpp::Waitable public: TestWaitable() : is_ready_(false) {} - bool add_to_wait_set(rcl_wait_set_t *) override {return true;} + void add_to_wait_set(rcl_wait_set_t *) override {} bool is_ready(rcl_wait_set_t *) override {return is_ready_;} diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 8294f2be68..ed590247d9 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -108,7 +108,7 @@ class ClientBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC - bool + void add_to_wait_set(rcl_wait_set_t * wait_set) override; /// \internal diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index f9488fc1d5..63c99d51e0 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -109,7 +109,7 @@ class ServerBase : public rclcpp::Waitable /// Add all entities to a wait set. /// \internal RCLCPP_ACTION_PUBLIC - bool + void 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. diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 5839a4f932..84559bfa9e 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -252,12 +252,14 @@ ClientBase::get_number_of_ready_services() return pimpl_->num_services; } -bool +void 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); - return RCL_RET_OK == ret; + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "ClientBase::add_to_wait_set() failed"); + } } bool diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index d1477300f1..e4765e9461 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -164,13 +164,15 @@ ServerBase::get_number_of_ready_guard_conditions() return pimpl_->num_guard_conditions_; } -bool +void 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); - return RCL_RET_OK == ret; + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "ServerBase::add_to_wait_set() failed"); + } } bool diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index ae01964fd5..b911e3d71b 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -397,7 +397,7 @@ 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_TRUE(action_client->add_to_wait_set(&wait_set)); + ASSERT_NO_THROW(action_client->add_to_wait_set(&wait_set)); EXPECT_TRUE(action_client->is_ready(&wait_set)); { diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 4d33c4db6d..25b38db34c 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -1107,7 +1107,7 @@ TEST_F(TestGoalRequestServer, is_ready_rcl_error) { { EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)); }); - ASSERT_TRUE(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)); auto mock = mocking_utils::patch_and_return( From 39dad4d9a738e7016eadd80f3b31e0763cbbe7bd Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 30 Nov 2021 10:37:52 -0800 Subject: [PATCH 041/455] Synchronize rcl and std::chrono steady clocks in Clock::sleep_until (#1830) * Synchronize RCL and std::chrono steady clocks Signed-off-by: Shane Loretz * Warn about gcc steady clock bugs Signed-off-by: Shane Loretz * Try to make warning message clearer Signed-off-by: Shane Loretz --- rclcpp/include/rclcpp/clock.hpp | 10 ++++++++++ rclcpp/src/rclcpp/clock.cpp | 10 +++++++--- 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index f70e5efeea..82dbc1bec5 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -88,6 +88,16 @@ class Clock * false. There is not a consistent choice of sleeping time when the time source changes, * so this is up to the caller to call again if needed. * + * \warning When using gcc < 10 or when using gcc >= 10 and pthreads lacks the function + * `pthread_cond_clockwait`, steady clocks may sleep using the system clock. + * If so, steady clock sleep times can be affected by system clock time jumps. + * Depending on the steady clock's epoch and resolution in comparison to the system clock's, + * an overflow when converting steady clock durations to system clock times may cause + * undefined behavior. + * For more info see these issues: + * https://gcc.gnu.org/bugzilla/show_bug.cgi?id=41861 + * https://gcc.gnu.org/bugzilla/show_bug.cgi?id=58931 + * * \param until absolute time according to current clock type to sleep until. * \param context the rclcpp context the clock should use to check that ROS is still initialized. * \return true immediately if `until` is in the past diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index c9e283ebba..6184df8607 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -105,13 +105,17 @@ Clock::sleep_until(Time until, Context::SharedPtr context) }); if (this_clock_type == RCL_STEADY_TIME) { - auto steady_time = std::chrono::steady_clock::time_point( - std::chrono::nanoseconds(until.nanoseconds())); + // Synchronize because RCL steady clock epoch might differ from chrono::steady_clock epoch + const Time rcl_entry = now(); + const std::chrono::steady_clock::time_point chrono_entry = std::chrono::steady_clock::now(); + const Duration delta_t = until - rcl_entry; + const std::chrono::steady_clock::time_point chrono_until = + chrono_entry + std::chrono::nanoseconds(delta_t.nanoseconds()); // loop over spurious wakeups but notice shutdown std::unique_lock lock(impl_->clock_mutex_); while (now() < until && context->is_valid()) { - cv.wait_until(lock, steady_time); + cv.wait_until(lock, chrono_until); } } else if (this_clock_type == RCL_SYSTEM_TIME) { auto system_time = std::chrono::system_clock::time_point( From 6f6e9e8ce9e79533b808b976418576e83091b43a Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 30 Nov 2021 14:02:32 -0800 Subject: [PATCH 042/455] Add Clock::sleep_for() (#1828) Signed-off-by: Shane Loretz --- rclcpp/include/rclcpp/clock.hpp | 24 ++++ rclcpp/src/rclcpp/clock.cpp | 6 + rclcpp/test/rclcpp/test_time.cpp | 183 ++++++++++++++++++++++++++++++- 3 files changed, 211 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 82dbc1bec5..42d3e01e7e 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -113,6 +113,30 @@ class Clock Time until, Context::SharedPtr context = contexts::get_global_default_context()); + /** + * Sleep for a specified Duration. + * + * Equivalent to + * + * ```cpp + * clock->sleep_until(clock->now() + rel_time, context) + * ``` + * + * The function will return immediately if `rel_time` is zero or negative. + * + * \param rel_time the length of time to sleep for. + * \param context the rclcpp context the clock should use to check that ROS is still initialized. + * \return true when the end time is reached + * \return false if time cannot be reached reliably, for example from shutdown or a change + * of time source. + * \throws std::runtime_error if the context is invalid + */ + RCLCPP_PUBLIC + bool + sleep_for( + Duration rel_time, + Context::SharedPtr context = contexts::get_global_default_context()); + /** * Returns the clock of the type `RCL_ROS_TIME` is active. * diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index 6184df8607..66c8db70e1 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -176,6 +176,12 @@ Clock::sleep_until(Time until, Context::SharedPtr context) return now() >= until; } +bool +Clock::sleep_for(Duration rel_time, Context::SharedPtr context) +{ + return sleep_until(now() + rel_time, context); +} + bool Clock::ros_time_is_active() { diff --git a/rclcpp/test/rclcpp/test_time.cpp b/rclcpp/test/rclcpp/test_time.cpp index 26bfe034eb..2f188b2d7c 100644 --- a/rclcpp/test/rclcpp/test_time.cpp +++ b/rclcpp/test/rclcpp/test_time.cpp @@ -487,7 +487,7 @@ TEST_F(TestClockSleep, bad_clock_type) { std::runtime_error("until's clock type does not match this clock's type")); } -TEST_F(TestClockSleep, invalid_context) { +TEST_F(TestClockSleep, sleep_until_invalid_context) { rclcpp::Clock clock(RCL_SYSTEM_TIME); auto until = clock.now(); @@ -508,7 +508,7 @@ TEST_F(TestClockSleep, invalid_context) { std::runtime_error("context cannot be slept with because it's invalid")); } -TEST_F(TestClockSleep, non_global_context) { +TEST_F(TestClockSleep, sleep_until_non_global_context) { rclcpp::Clock clock(RCL_SYSTEM_TIME); auto until = clock.now() + rclcpp::Duration(0, 1); @@ -669,3 +669,182 @@ TEST_F(TestClockSleep, sleep_until_basic_ros) { sleep_thread.join(); EXPECT_TRUE(sleep_succeeded); } + +TEST_F(TestClockSleep, sleep_for_invalid_context) { + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto rel_time = rclcpp::Duration(1, 0u); + + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_for(rel_time, nullptr), + std::runtime_error("context cannot be slept with because it's invalid")); + + auto uninitialized_context = std::make_shared(); + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_for(rel_time, uninitialized_context), + std::runtime_error("context cannot be slept with because it's invalid")); + + auto shutdown_context = std::make_shared(); + shutdown_context->init(0, nullptr); + shutdown_context->shutdown("i am a teapot"); + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_for(rel_time, shutdown_context), + std::runtime_error("context cannot be slept with because it's invalid")); +} + +TEST_F(TestClockSleep, sleep_for_non_global_context) { + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto rel_time = rclcpp::Duration(0, 1); + + auto non_global_context = std::make_shared(); + non_global_context->init(0, nullptr); + ASSERT_TRUE(clock.sleep_for(rel_time, non_global_context)); +} + +TEST_F(TestClockSleep, sleep_for_basic_system) { + const auto milliseconds = 300; + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto rel_time = rclcpp::Duration(0, RCUTILS_MS_TO_NS(milliseconds)); + + auto start = std::chrono::system_clock::now(); + ASSERT_TRUE(clock.sleep_for(rel_time)); + auto end = std::chrono::system_clock::now(); + + EXPECT_GE(end - start, std::chrono::milliseconds(milliseconds)); +} + +TEST_F(TestClockSleep, sleep_for_basic_steady) { + const auto milliseconds = 300; + rclcpp::Clock clock(RCL_STEADY_TIME); + auto rel_time = rclcpp::Duration(0, RCUTILS_MS_TO_NS(milliseconds)); + + auto steady_start = std::chrono::steady_clock::now(); + ASSERT_TRUE(clock.sleep_for(rel_time)); + auto steady_end = std::chrono::steady_clock::now(); + + EXPECT_GE(steady_end - steady_start, std::chrono::milliseconds(milliseconds)); +} + +TEST_F(TestClockSleep, sleep_for_steady_past_returns_immediately) { + rclcpp::Clock clock(RCL_STEADY_TIME); + auto rel_time = rclcpp::Duration(-1000, 0); + // This should return immediately + ASSERT_TRUE(clock.sleep_for(rel_time)); +} + +TEST_F(TestClockSleep, sleep_for_system_past_returns_immediately) { + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto rel_time = rclcpp::Duration(-1000, 0); + // This should return immediately + ASSERT_TRUE(clock.sleep_for(rel_time)); +} + +TEST_F(TestClockSleep, sleep_for_ros_time_enable_interrupt) { + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source; + time_source.attachNode(node); + time_source.attachClock(clock); + + // 5 second timeout, but it should be interrupted right away + const auto rel_time = rclcpp::Duration(5, 0); + + // Try sleeping with ROS time off, then turn it on to interrupt + bool sleep_succeeded = true; + auto sleep_thread = std::thread( + [clock, rel_time, &sleep_succeeded]() { + sleep_succeeded = clock->sleep_for(rel_time); + }); + // yield execution long enough to let the sleep thread get to waiting on the condition variable + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + auto set_parameters_results = param_client->set_parameters( + {rclcpp::Parameter("use_sim_time", true)}); + for (auto & result : set_parameters_results) { + ASSERT_TRUE(result.successful); + } + sleep_thread.join(); + EXPECT_FALSE(sleep_succeeded); +} + +TEST_F(TestClockSleep, sleep_for_ros_time_disable_interrupt) { + param_client->set_parameters({rclcpp::Parameter("use_sim_time", true)}); + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source; + time_source.attachNode(node); + time_source.attachClock(clock); + + // /clock shouldn't be publishing, shouldn't be possible to reach timeout + const auto rel_time = rclcpp::Duration(600, 0); + + // Try sleeping with ROS time off, then turn it on to interrupt + bool sleep_succeeded = true; + auto sleep_thread = std::thread( + [clock, rel_time, &sleep_succeeded]() { + sleep_succeeded = clock->sleep_for(rel_time); + }); + // yield execution long enough to let the sleep thread get to waiting on the condition variable + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + auto set_parameters_results = param_client->set_parameters( + {rclcpp::Parameter("use_sim_time", false)}); + for (auto & result : set_parameters_results) { + ASSERT_TRUE(result.successful); + } + sleep_thread.join(); + EXPECT_FALSE(sleep_succeeded); +} + +TEST_F(TestClockSleep, sleep_for_shutdown_interrupt) { + param_client->set_parameters({rclcpp::Parameter("use_sim_time", true)}); + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source; + time_source.attachNode(node); + time_source.attachClock(clock); + + // the timeout doesn't matter here - no /clock is being published, so it should never wake + const auto rel_time = rclcpp::Duration(600, 0); + + bool sleep_succeeded = true; + auto sleep_thread = std::thread( + [clock, rel_time, &sleep_succeeded]() { + sleep_succeeded = clock->sleep_for(rel_time); + }); + // yield execution long enough to let the sleep thread get to waiting on the condition variable + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + rclcpp::shutdown(); + sleep_thread.join(); + EXPECT_FALSE(sleep_succeeded); +} + +TEST_F(TestClockSleep, sleep_for_basic_ros) { + rclcpp::Clock clock(RCL_ROS_TIME); + rcl_clock_t * rcl_clock = clock.get_clock_handle(); + + ASSERT_EQ(RCL_ROS_TIME, clock.get_clock_type()); + + // Not zero, because 0 means time not initialized + const rcl_time_point_value_t start_time = 1337; + const rcl_time_point_value_t end_time = start_time + 1; + + // Initialize time + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(rcl_clock)); + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(rcl_clock, start_time)); + + const auto rel_time = rclcpp::Duration(0, 1u); + + bool sleep_succeeded = false; + auto sleep_thread = std::thread( + [&clock, rel_time, &sleep_succeeded]() { + sleep_succeeded = clock.sleep_for(rel_time); + }); + + // yield execution long enough to let the sleep thread get to waiting on the condition variable + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + // False because still sleeping + EXPECT_FALSE(sleep_succeeded); + + // Jump time to the end + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(rcl_clock, end_time)); + ASSERT_EQ(end_time, clock.now().nanoseconds()); + + sleep_thread.join(); + EXPECT_TRUE(sleep_succeeded); +} From 536df11ee0fdac3e4753754d11052068983c4d5c Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 7 Dec 2021 03:32:08 +0800 Subject: [PATCH 043/455] Make node base sharable (#1832) Signed-off-by: Chen Lihui --- .../rclcpp/node_interfaces/node_base.hpp | 2 +- .../src/rclcpp/node_interfaces/node_base.cpp | 107 +++--------------- rclcpp/test/rclcpp/test_memory_strategy.cpp | 5 +- 3 files changed, 21 insertions(+), 93 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index 74616a1cbc..675aff0483 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -34,7 +34,7 @@ namespace node_interfaces { /// Implementation of the NodeBase part of the Node API. -class NodeBase : public NodeBaseInterface +class NodeBase : public NodeBaseInterface, public std::enable_shared_from_this { public: RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBase) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index e68d1a01eb..8be8364dc6 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -15,7 +15,6 @@ #include #include #include -#include #include #include "rclcpp/node_interfaces/node_base.hpp" @@ -32,93 +31,6 @@ using rclcpp::exceptions::throw_from_rcl_error; using rclcpp::node_interfaces::NodeBase; -namespace -{ -/// A class to manage the lifetime of a node handle and its context -/** - * This class bundles together the lifetime of the rcl_node_t handle with the - * lifetime of the RCL context. This ensures that the context will remain alive - * for as long as the rcl_node_t handle is alive. - */ -class NodeHandleWithContext -{ -public: - /// Make an instance of a NodeHandleWithContext - /** - * The make function returns a std::shared_ptr which is actually - * an alias for a std::shared_ptr. There is no use for - * accessing a NodeHandleWithContext instance directly, because its only job - * is to couple together the lifetime of a context with the lifetime of a - * node handle that depends on it. - */ - static std::shared_ptr - make( - rclcpp::Context::SharedPtr context, - std::shared_ptr logging_mutex, - rcl_node_t * node_handle) - { - auto aliased_ptr = std::make_shared( - NodeHandleWithContext( - std::move(context), - std::move(logging_mutex), - node_handle)); - - return std::shared_ptr(std::move(aliased_ptr), node_handle); - } - - // This class should not be copied. It should only exist in the - // std::shared_ptr that it was originally provided in. - NodeHandleWithContext(const NodeHandleWithContext &) = delete; - NodeHandleWithContext & operator=(const NodeHandleWithContext &) = delete; - NodeHandleWithContext & operator=(NodeHandleWithContext &&) = delete; - - NodeHandleWithContext(NodeHandleWithContext && other) - : context_(std::move(other.context_)), - logging_mutex_(std::move(other.logging_mutex_)), - node_handle_(other.node_handle_) - { - other.node_handle_ = nullptr; - } - - ~NodeHandleWithContext() - { - if (!node_handle_) { - // If the node_handle_ is null, then this object was moved-from. We don't - // need to do any cleanup. - return; - } - - std::lock_guard guard(*logging_mutex_); - // TODO(ivanpauno): Instead of mutually excluding rcl_node_fini with the global logger mutex, - // rcl_logging_rosout_fini_publisher_for_node could be decoupled from there and be called - // here directly. - if (rcl_node_fini(node_handle_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Error in destruction of rcl node handle: %s", rcl_get_error_string().str); - } - delete node_handle_; - } - -private: - // The constructor is private because this class is only meant to be aliased - // into a std::shared_ptr and should not be constructed any other - // way. - NodeHandleWithContext( - rclcpp::Context::SharedPtr context, - std::shared_ptr logging_mutex, - rcl_node_t * node_handle) - : context_(std::move(context)), - logging_mutex_(std::move(logging_mutex)), - node_handle_(node_handle) - {} - - rclcpp::Context::SharedPtr context_; - std::shared_ptr logging_mutex_; - rcl_node_t * node_handle_; -}; -} // anonymous namespace - NodeBase::NodeBase( const std::string & node_name, const std::string & namespace_, @@ -201,7 +113,20 @@ NodeBase::NodeBase( throw_from_rcl_error(ret, "failed to initialize rcl node"); } - node_handle_ = NodeHandleWithContext::make(context_, logging_mutex, rcl_node.release()); + node_handle_.reset( + rcl_node.release(), + [logging_mutex](rcl_node_t * node) -> void { + std::lock_guard guard(*logging_mutex); + // TODO(ivanpauno): Instead of mutually excluding rcl_node_fini with the global logger mutex, + // rcl_logging_rosout_fini_publisher_for_node could be decoupled from there and be called + // here directly. + if (rcl_node_fini(node) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rcl node handle: %s", rcl_get_error_string().str); + } + delete node; + }); // Create the default callback group. using rclcpp::CallbackGroupType; @@ -259,13 +184,13 @@ NodeBase::get_rcl_node_handle() const std::shared_ptr NodeBase::get_shared_rcl_node_handle() { - return node_handle_; + return std::shared_ptr(shared_from_this(), node_handle_.get()); } std::shared_ptr NodeBase::get_shared_rcl_node_handle() const { - return node_handle_; + return std::shared_ptr(shared_from_this(), node_handle_.get()); } rclcpp::CallbackGroup::SharedPtr diff --git a/rclcpp/test/rclcpp/test_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp index c919d1981d..b6fff57a92 100644 --- a/rclcpp/test/rclcpp/test_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/test_memory_strategy.cpp @@ -307,6 +307,8 @@ TEST_F(TestMemoryStrategy, get_node_by_group) { EXPECT_EQ( node_handle, memory_strategy()->get_node_by_group(callback_group, weak_groups_to_nodes)); + // Clear the handles to not hold NodeBase. + memory_strategy()->clear_handles(); } // Node goes out of scope // Callback group still exists, so lookup returns nullptr because node is destroyed. EXPECT_EQ( @@ -363,8 +365,9 @@ TEST_F(TestMemoryStrategy, get_group_by_subscription) { callback_group, memory_strategy()->get_group_by_subscription(subscription, weak_groups_to_nodes)); } // Node goes out of scope + // NodeBase(SubscriptionBase->rcl_node_t->NodeBase) is still alive. EXPECT_EQ( - nullptr, + callback_group, memory_strategy()->get_group_by_subscription(subscription, weak_groups_to_nodes)); } From 321c74c2b3ae0336528f561a039fd554a28f4543 Mon Sep 17 00:00:00 2001 From: gezp Date: Wed, 8 Dec 2021 01:24:53 +0800 Subject: [PATCH 044/455] create component_container_isolated (#1781) Signed-off-by: zhenpeng ge --- rclcpp_components/CMakeLists.txt | 13 ++- .../rclcpp_components/component_manager.hpp | 18 +++- .../component_manager_isolated.hpp | 99 +++++++++++++++++++ .../src/component_container_isolated.cpp | 49 +++++++++ rclcpp_components/src/component_manager.cpp | 25 +++-- .../test/test_component_manager_api.cpp | 24 ++++- 6 files changed, 218 insertions(+), 10 deletions(-) create mode 100644 rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp create mode 100644 rclcpp_components/src/component_container_isolated.cpp diff --git a/rclcpp_components/CMakeLists.txt b/rclcpp_components/CMakeLists.txt index 5098fd8631..fe82f6beb1 100644 --- a/rclcpp_components/CMakeLists.txt +++ b/rclcpp_components/CMakeLists.txt @@ -61,9 +61,20 @@ ament_target_dependencies(component_container_mt "rclcpp" ) +add_executable( + component_container_isolated + src/component_container_isolated.cpp +) +target_link_libraries(component_container_isolated component_manager) +ament_target_dependencies(component_container_isolated + "rclcpp" +) + + if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") target_link_libraries(component_container "stdc++fs") target_link_libraries(component_container_mt "stdc++fs") + target_link_libraries(component_container_isolated "stdc++fs") endif() if(BUILD_TESTING) @@ -135,7 +146,7 @@ install( # Install executables install( - TARGETS component_container component_container_mt + TARGETS component_container component_container_mt component_container_isolated RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/rclcpp_components/include/rclcpp_components/component_manager.hpp b/rclcpp_components/include/rclcpp_components/component_manager.hpp index b2f1de0827..a8342c3987 100644 --- a/rclcpp_components/include/rclcpp_components/component_manager.hpp +++ b/rclcpp_components/include/rclcpp_components/component_manager.hpp @@ -140,6 +140,22 @@ class ComponentManager : public rclcpp::Node virtual rclcpp::NodeOptions create_node_options(const std::shared_ptr request); + /// Add component node to executor model, it's invoked in on_load_node() + /** + * \param node_id node_id of loaded component node in node_wrappers_ + */ + RCLCPP_COMPONENTS_PUBLIC + virtual void + add_node_to_executor(uint64_t node_id); + + /// Remove component node from executor model, it's invoked in on_unload_node() + /** + * \param node_id node_id of loaded component node in node_wrappers_ + */ + RCLCPP_COMPONENTS_PUBLIC + virtual void + remove_node_from_executor(uint64_t node_id); + /// Service callback to load a new node in the component /** * This function allows to add parameters, remap rules, a specific node, name a namespace @@ -231,7 +247,7 @@ class ComponentManager : public rclcpp::Node on_list_nodes(request_header, request, response); } -private: +protected: std::weak_ptr executor_; uint64_t unique_id_ {1}; diff --git a/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp b/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp new file mode 100644 index 0000000000..308c3d9b30 --- /dev/null +++ b/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp @@ -0,0 +1,99 @@ +// 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_COMPONENTS__COMPONENT_MANAGER_ISOLATED_HPP__ +#define RCLCPP_COMPONENTS__COMPONENT_MANAGER_ISOLATED_HPP__ + +#include +#include +#include +#include +#include +#include + +#include "rclcpp_components/component_manager.hpp" + + +namespace rclcpp_components +{ +/// ComponentManagerIsolated uses dedicated single-threaded executors for each components. +template +class ComponentManagerIsolated : public rclcpp_components::ComponentManager +{ + using rclcpp_components::ComponentManager::ComponentManager; + + struct DedicatedExecutorWrapper + { + std::shared_ptr executor; + std::thread thread; + std::promise promise; + }; + +public: + ~ComponentManagerIsolated() + { + if (node_wrappers_.size()) { + for (auto & executor_wrapper : dedicated_executor_wrappers_) { + executor_wrapper.second.promise.set_value(); + executor_wrapper.second.executor->cancel(); + executor_wrapper.second.thread.join(); + } + node_wrappers_.clear(); + } + } + +protected: + /// Add component node to executor model, it's invoked in on_load_node() + /** + * \param node_id node_id of loaded component node in node_wrappers_ + */ + RCLCPP_COMPONENTS_PUBLIC + void + add_node_to_executor(uint64_t node_id) override + { + DedicatedExecutorWrapper executor_wrapper; + auto exec = std::make_shared(); + exec->add_node(node_wrappers_[node_id].get_node_base_interface()); + executor_wrapper.executor = exec; + executor_wrapper.thread = std::thread( + [exec, cancel_token = executor_wrapper.promise.get_future()]() { + exec->spin_until_future_complete(cancel_token); + }); + dedicated_executor_wrappers_[node_id] = std::move(executor_wrapper); + } + /// Remove component node from executor model, it's invoked in on_unload_node() + /** + * \param node_id node_id of loaded component node in node_wrappers_ + */ + RCLCPP_COMPONENTS_PUBLIC + void + remove_node_from_executor(uint64_t node_id) override + { + auto executor_wrapper = dedicated_executor_wrappers_.find(node_id); + if (executor_wrapper != dedicated_executor_wrappers_.end()) { + executor_wrapper->second.promise.set_value(); + executor_wrapper->second.executor->cancel(); + executor_wrapper->second.thread.join(); + dedicated_executor_wrappers_.erase(executor_wrapper); + } + } + +private: + std::unordered_map dedicated_executor_wrappers_; +}; + +} // namespace rclcpp_components + +#endif // RCLCPP_COMPONENTS__COMPONENT_MANAGER_ISOLATED_HPP__ diff --git a/rclcpp_components/src/component_container_isolated.cpp b/rclcpp_components/src/component_container_isolated.cpp new file mode 100644 index 0000000000..96ba8b1a03 --- /dev/null +++ b/rclcpp_components/src/component_container_isolated.cpp @@ -0,0 +1,49 @@ +// 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. + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_components/component_manager_isolated.hpp" + +int main(int argc, char * argv[]) +{ + /// Component container with dedicated single-threaded executors for each components. + rclcpp::init(argc, argv); + // parse arguments + bool use_multi_threaded_executor{false}; + std::vector args = rclcpp::remove_ros_arguments(argc, argv); + for (auto & arg : args) { + if (arg == std::string("--use_multi_threaded_executor")) { + use_multi_threaded_executor = true; + } + } + // create executor and component manager + auto exec = std::make_shared(); + rclcpp::Node::SharedPtr node; + if (use_multi_threaded_executor) { + using ComponentManagerIsolated = + rclcpp_components::ComponentManagerIsolated; + node = std::make_shared(exec); + } else { + using ComponentManagerIsolated = + rclcpp_components::ComponentManagerIsolated; + node = std::make_shared(exec); + } + exec->add_node(node); + exec->spin(); +} diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index cc2157f0d5..49b6e5871f 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -166,6 +166,22 @@ ComponentManager::create_node_options(const std::shared_ptr r return options; } +void +ComponentManager::add_node_to_executor(uint64_t node_id) +{ + if (auto exec = executor_.lock()) { + exec->add_node(node_wrappers_[node_id].get_node_base_interface(), true); + } +} + +void +ComponentManager::remove_node_from_executor(uint64_t node_id) +{ + if (auto exec = executor_.lock()) { + exec->remove_node(node_wrappers_[node_id].get_node_base_interface()); + } +} + void ComponentManager::on_load_node( const std::shared_ptr request_header, @@ -214,10 +230,9 @@ ComponentManager::on_load_node( throw ComponentManagerException("Component constructor threw an exception"); } + add_node_to_executor(node_id); + auto node = node_wrappers_[node_id].get_node_base_interface(); - if (auto exec = executor_.lock()) { - exec->add_node(node, true); - } response->full_node_name = node->get_fully_qualified_name(); response->unique_id = node_id; response->success = true; @@ -253,9 +268,7 @@ ComponentManager::on_unload_node( response->error_message = ss.str(); RCLCPP_WARN(get_logger(), "%s", ss.str().c_str()); } else { - if (auto exec = executor_.lock()) { - exec->remove_node(wrapper->second.get_node_base_interface()); - } + remove_node_from_executor(request->unique_id); node_wrappers_.erase(wrapper); response->success = true; } diff --git a/rclcpp_components/test/test_component_manager_api.cpp b/rclcpp_components/test/test_component_manager_api.cpp index 84b11fe7e9..f1a4700e6e 100644 --- a/rclcpp_components/test/test_component_manager_api.cpp +++ b/rclcpp_components/test/test_component_manager_api.cpp @@ -22,6 +22,7 @@ #include "composition_interfaces/srv/list_nodes.hpp" #include "rclcpp_components/component_manager.hpp" +#include "rclcpp_components/component_manager_isolated.hpp" using namespace std::chrono_literals; @@ -36,11 +37,18 @@ class TestComponentManager : public ::testing::Test // TODO(hidmic): split up tests once Node bring up/tear down races // are solved https://github.com/ros2/rclcpp/issues/863 -TEST_F(TestComponentManager, components_api) +void test_components_api(bool use_dedicated_executor) { auto exec = std::make_shared(); auto node = rclcpp::Node::make_shared("test_component_manager"); - auto manager = std::make_shared(exec); + std::shared_ptr manager; + if (use_dedicated_executor) { + using ComponentManagerIsolated = + rclcpp_components::ComponentManagerIsolated; + manager = std::make_shared(exec); + } else { + manager = std::make_shared(exec); + } exec->add_node(manager); exec->add_node(node); @@ -321,3 +329,15 @@ TEST_F(TestComponentManager, components_api) } } } + +TEST_F(TestComponentManager, components_api) +{ + { + SCOPED_TRACE("ComponentManager"); + test_components_api(false); + } + { + SCOPED_TRACE("ComponentManagerIsolated"); + test_components_api(true); + } +} From 0775e2f6e7b53b5b5cab1702e4f3fa1206f3b59b Mon Sep 17 00:00:00 2001 From: gezp Date: Thu, 9 Dec 2021 22:41:47 +0800 Subject: [PATCH 045/455] remove RCLCPP_COMPONENTS_PUBLIC in class ComponentManagerIsolated (#1843) Signed-off-by: zhenpeng ge --- .../include/rclcpp_components/component_manager_isolated.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp b/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp index 308c3d9b30..8ff9a60665 100644 --- a/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp +++ b/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp @@ -59,7 +59,6 @@ class ComponentManagerIsolated : public rclcpp_components::ComponentManager /** * \param node_id node_id of loaded component node in node_wrappers_ */ - RCLCPP_COMPONENTS_PUBLIC void add_node_to_executor(uint64_t node_id) override { @@ -77,7 +76,6 @@ class ComponentManagerIsolated : public rclcpp_components::ComponentManager /** * \param node_id node_id of loaded component node in node_wrappers_ */ - RCLCPP_COMPONENTS_PUBLIC void remove_node_from_executor(uint64_t node_id) override { From ee20dd31e676c4803e2393bc86ce76f0a21f9b73 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Tue, 14 Dec 2021 02:40:16 +0900 Subject: [PATCH 046/455] Add parameter to configure number of thread (#1708) * Add parameter to configure number of thread Signed-off-by: wep21 --- .../rclcpp_components/component_manager.hpp | 11 ++++++++++- .../src/component_container_mt.cpp | 11 ++++++++++- rclcpp_components/src/component_manager.cpp | 18 ++++++++++++++++++ 3 files changed, 38 insertions(+), 2 deletions(-) diff --git a/rclcpp_components/include/rclcpp_components/component_manager.hpp b/rclcpp_components/include/rclcpp_components/component_manager.hpp index a8342c3987..6238284278 100644 --- a/rclcpp_components/include/rclcpp_components/component_manager.hpp +++ b/rclcpp_components/include/rclcpp_components/component_manager.hpp @@ -99,7 +99,8 @@ class ComponentManager : public rclcpp::Node */ RCLCPP_COMPONENTS_PUBLIC ComponentManager( - std::weak_ptr executor, + std::weak_ptr executor = + std::weak_ptr(), std::string node_name = "ComponentManager", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() .start_parameter_services(false) @@ -130,6 +131,14 @@ class ComponentManager : public rclcpp::Node virtual std::shared_ptr create_component_factory(const ComponentResource & resource); + /// Member function to set a executor in the component + /** + * \param executor executor to be set + */ + RCLCPP_COMPONENTS_PUBLIC + virtual void + set_executor(const std::weak_ptr executor); + protected: /// Create node options for loaded component /** diff --git a/rclcpp_components/src/component_container_mt.cpp b/rclcpp_components/src/component_container_mt.cpp index aa4784e426..9dcbade712 100644 --- a/rclcpp_components/src/component_container_mt.cpp +++ b/rclcpp_components/src/component_container_mt.cpp @@ -22,8 +22,17 @@ int main(int argc, char * argv[]) { /// Component container with a multi-threaded executor. rclcpp::init(argc, argv); + auto exec = std::make_shared(); - auto node = std::make_shared(exec); + auto node = std::make_shared(); + if (node->has_parameter("thread_num")) { + const auto thread_num = node->get_parameter("thread_num").as_int(); + exec = std::make_shared( + rclcpp::ExecutorOptions{}, thread_num); + node->set_executor(exec); + } else { + node->set_executor(exec); + } exec->add_node(node); exec->spin(); } diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index 49b6e5871f..ec69ed4b68 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -46,6 +46,18 @@ ComponentManager::ComponentManager( listNodes_srv_ = create_service( "~/_container/list_nodes", std::bind(&ComponentManager::on_list_nodes, this, _1, _2, _3)); + + { + rcl_interfaces::msg::ParameterDescriptor desc{}; + desc.description = "Number of thread"; + rcl_interfaces::msg::IntegerRange range{}; + range.from_value = 1; + range.to_value = std::thread::hardware_concurrency(); + desc.integer_range.push_back(range); + desc.read_only = true; + this->declare_parameter( + "thread_num", static_cast(std::thread::hardware_concurrency()), desc); + } } ComponentManager::~ComponentManager() @@ -166,6 +178,12 @@ ComponentManager::create_node_options(const std::shared_ptr r return options; } +void +ComponentManager::set_executor(const std::weak_ptr executor) +{ + executor_ = executor; +} + void ComponentManager::add_node_to_executor(uint64_t node_id) { From b2b676d3172ada509e58fa552a676a446809d83c Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Tue, 14 Dec 2021 16:01:34 -0800 Subject: [PATCH 047/455] use private member to keep the all reference underneath. (#1845) Signed-off-by: Tomoya Fujita --- rclcpp/include/rclcpp/subscription.hpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 0b42549978..a77f4f8ef1 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -146,19 +146,19 @@ class Subscription : public SubscriptionBase options_(options), message_memory_strategy_(message_memory_strategy) { - if (options.event_callbacks.deadline_callback) { + if (options_.event_callbacks.deadline_callback) { this->add_event_handler( - options.event_callbacks.deadline_callback, + options_.event_callbacks.deadline_callback, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); } - if (options.event_callbacks.liveliness_callback) { + if (options_.event_callbacks.liveliness_callback) { this->add_event_handler( - options.event_callbacks.liveliness_callback, + options_.event_callbacks.liveliness_callback, RCL_SUBSCRIPTION_LIVELINESS_CHANGED); } - if (options.event_callbacks.incompatible_qos_callback) { + if (options_.event_callbacks.incompatible_qos_callback) { this->add_event_handler( - options.event_callbacks.incompatible_qos_callback, + options_.event_callbacks.incompatible_qos_callback, RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); } else if (options_.use_default_callbacks) { // Register default callback when not specified @@ -172,14 +172,14 @@ class Subscription : public SubscriptionBase // pass } } - if (options.event_callbacks.message_lost_callback) { + if (options_.event_callbacks.message_lost_callback) { this->add_event_handler( - options.event_callbacks.message_lost_callback, + options_.event_callbacks.message_lost_callback, RCL_SUBSCRIPTION_MESSAGE_LOST); } // Setup intra process publishing if requested. - if (rclcpp::detail::resolve_use_intra_process(options, *node_base)) { + if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) { using rclcpp::detail::resolve_intra_process_buffer_type; // Check if the QoS is compatible with intra-process. @@ -201,11 +201,11 @@ class Subscription : public SubscriptionBase auto context = node_base->get_context(); subscription_intra_process_ = std::make_shared( callback, - options.get_allocator(), + options_.get_allocator(), context, this->get_topic_name(), // important to get like this, as it has the fully-qualified name qos_profile, - resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback)); + resolve_intra_process_buffer_type(options_.intra_process_buffer_type, callback)); TRACEPOINT( rclcpp_subscription_init, static_cast(get_subscription_handle().get()), From 8ac848bbc2b1f5e5ce2d53b9085942dada67eb7e Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 17 Dec 2021 13:23:44 -0500 Subject: [PATCH 048/455] Fixes for uncrustify 0.72 (#1844) Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/function_traits.hpp | 4 ++-- .../rclcpp/wait_set_policies/sequential_synchronization.hpp | 2 +- .../rclcpp/wait_set_policies/thread_safe_synchronization.hpp | 2 +- rclcpp/test/utils/rclcpp_gtest_macros.hpp | 4 ++-- rclcpp_action/src/client.cpp | 2 +- 5 files changed, 7 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/function_traits.hpp b/rclcpp/include/rclcpp/function_traits.hpp index 4004476842..4dd3cc28db 100644 --- a/rclcpp/include/rclcpp/function_traits.hpp +++ b/rclcpp/include/rclcpp/function_traits.hpp @@ -83,7 +83,7 @@ template> #elif defined _GLIBCXX_RELEASE // glibc++ (GNU C++ >= 7.1) -struct function_traits> +struct function_traits> #elif defined __GLIBCXX__ // glibc++ (GNU C++) struct function_traits(FArgs ...)>> #elif defined _MSC_VER // MS Visual Studio @@ -100,7 +100,7 @@ template> #elif defined _GLIBCXX_RELEASE // glibc++ (GNU C++ >= 7.1) -struct function_traits> +struct function_traits> #elif defined __GLIBCXX__ // glibc++ (GNU C++) struct function_traits(FArgs ...)>> #elif defined _MSC_VER // MS Visual Studio diff --git a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp index 5ebf32bb72..be2e569c40 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp @@ -203,7 +203,7 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon std::shared_ptr && waitable, std::shared_ptr && associated_entity, std::function< - void(std::shared_ptr&&, std::shared_ptr&&) + void(std::shared_ptr&&, std::shared_ptr &&) > add_waitable_function) { // Explicitly no thread synchronization. diff --git a/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp index ea74769ad4..4a4fb16547 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp @@ -216,7 +216,7 @@ class ThreadSafeSynchronization : public detail::SynchronizationPolicyCommon std::shared_ptr && waitable, std::shared_ptr && associated_entity, std::function< - void(std::shared_ptr&&, std::shared_ptr&&) + void(std::shared_ptr&&, std::shared_ptr &&) > add_waitable_function) { using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; diff --git a/rclcpp/test/utils/rclcpp_gtest_macros.hpp b/rclcpp/test/utils/rclcpp_gtest_macros.hpp index a7b1b97d77..7dd708f974 100644 --- a/rclcpp/test/utils/rclcpp_gtest_macros.hpp +++ b/rclcpp/test/utils/rclcpp_gtest_macros.hpp @@ -162,7 +162,7 @@ ::testing::AssertionResult AreThrowableContentsEqual( #define RCLCPP_EXPECT_THROW_EQ(throwing_statement, expected_exception) \ do { \ ::testing::AssertionResult \ - is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable = \ + is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable = \ ::testing::AssertionSuccess(); \ CHECK_THROW_EQ_IMPL( \ throwing_statement, \ @@ -183,7 +183,7 @@ ::testing::AssertionResult AreThrowableContentsEqual( #define RCLCPP_ASSERT_THROW_EQ(throwing_statement, expected_exception) \ do { \ ::testing::AssertionResult \ - is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable = \ + is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable = \ ::testing::AssertionSuccess(); \ CHECK_THROW_EQ_IMPL( \ throwing_statement, \ diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 84559bfa9e..2f39a50e52 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -44,7 +44,7 @@ class ClientBaseImpl : node_graph_(node_graph), node_handle(node_base->get_shared_rcl_node_handle()), logger(node_logging->get_logger().get_child("rclcpp_action")), - random_bytes_generator(std::random_device{} ()) + random_bytes_generator(std::random_device{}()) { std::weak_ptr weak_node_handle(node_handle); client_handle = std::shared_ptr( From d2e563afd57954df8fb0d3cdd5021205649cb5c2 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 17 Dec 2021 19:23:51 +0000 Subject: [PATCH 049/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 22 ++++++++++++++++++++++ rclcpp_action/CHANGELOG.rst | 9 +++++++++ rclcpp_components/CHANGELOG.rst | 10 ++++++++++ rclcpp_lifecycle/CHANGELOG.rst | 7 +++++++ 4 files changed, 48 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 220c16e898..bb5ec645f7 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,28 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fixes for uncrustify 0.72 (`#1844 `_) +* use private member to keep the all reference underneath. (`#1845 `_) +* Make node base sharable (`#1832 `_) +* Add Clock::sleep_for() (`#1828 `_) +* Synchronize rcl and std::chrono steady clocks in Clock::sleep_until (`#1830 `_) +* Use rclcpp::guard_condition (`#1612 `_) +* Call CMake function to generate version header (`#1805 `_) +* Use parantheses around logging macro parameter (`#1820 `_) +* Remove author by request (`#1818 `_) +* Update maintainers (`#1817 `_) +* min_forward & min_backward thresholds must not be disabled (`#1815 `_) +* Re-add Clock::sleep_until (`#1814 `_) +* Fix lifetime of context so it remains alive while its dependent node handles are still in use (`#1754 `_) +* Add the interface for pre-shutdown callback (`#1714 `_) +* Take message ownership from moved LoanedMessage (`#1808 `_) +* Suppress clang dead-store warnings in the benchmarks. (`#1802 `_) +* Wait for publisher and subscription to match (`#1777 `_) +* Fix unused QoS profile for clock subscription and make ClockQoS the default (`#1801 `_) +* Contributors: Abrar Rahman Protyasha, Barry Xu, Chen Lihui, Chris Lalancette, Grey, Jacob Perron, Nikolai Morin, Shane Loretz, Tomoya Fujita, mauropasse + 13.1.0 (2021-10-18) ------------------- * Fix dangerous std::bind capture in TimeSource implementation. (`#1768 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 94f2fc7842..0698a0cdf7 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,15 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fixes for uncrustify 0.72 (`#1844 `_) +* Use rclcpp::guard_condition (`#1612 `_) +* Remove author by request (`#1818 `_) +* Update maintainers (`#1817 `_) +* Suppress clang dead-store warnings in the benchmarks. (`#1802 `_) +* Contributors: Chris Lalancette, Jacob Perron, mauropasse + 13.1.0 (2021-10-18) ------------------- * Deprecate the `void shared_ptr` subscription callback signatures (`#1713 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 7951a187a5..765a05cc0b 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add parameter to configure number of thread (`#1708 `_) +* remove RCLCPP_COMPONENTS_PUBLIC in class ComponentManagerIsolated (`#1843 `_) +* create component_container_isolated (`#1781 `_) +* Remove author by request (`#1818 `_) +* Update maintainers (`#1817 `_) +* Suppress clang dead-store warnings in the benchmarks. (`#1802 `_) +* Contributors: Chris Lalancette, Daisuke Nishimatsu, Jacob Perron, gezp + 13.1.0 (2021-10-18) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 0498541596..5910b430f7 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,13 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove author by request (`#1818 `_) +* Update maintainers (`#1817 `_) +* Suppress clang dead-store warnings in the benchmarks. (`#1802 `_) +* Contributors: Chris Lalancette, Jacob Perron + 13.1.0 (2021-10-18) ------------------- * Update forward declarations of `rcl_lifecycle` types (`#1788 `_) From 9342c257b8a427663f62c05de853aa2ef1b78e6f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 17 Dec 2021 19:24:02 +0000 Subject: [PATCH 050/455] 14.0.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index bb5ec645f7..88bde8e867 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +14.0.0 (2021-12-17) +------------------- * Fixes for uncrustify 0.72 (`#1844 `_) * use private member to keep the all reference underneath. (`#1845 `_) * Make node base sharable (`#1832 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 36f5fa1d80..de866ad039 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 13.1.0 + 14.0.0 The ROS client library in C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 0698a0cdf7..838ecd86bb 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +14.0.0 (2021-12-17) +------------------- * Fixes for uncrustify 0.72 (`#1844 `_) * Use rclcpp::guard_condition (`#1612 `_) * Remove author by request (`#1818 `_) diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 1fa83b8f60..0ab56ca454 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 13.1.0 + 14.0.0 Adds action APIs for C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 765a05cc0b..5ed7eeae02 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +14.0.0 (2021-12-17) +------------------- * Add parameter to configure number of thread (`#1708 `_) * remove RCLCPP_COMPONENTS_PUBLIC in class ComponentManagerIsolated (`#1843 `_) * create component_container_isolated (`#1781 `_) diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index db7b8fbaae..fa93648255 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 13.1.0 + 14.0.0 Package containing tools for dynamically loadable components Ivan Paunovic Jacob Perron diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 5910b430f7..6806e91cd8 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +14.0.0 (2021-12-17) +------------------- * Remove author by request (`#1818 `_) * Update maintainers (`#1817 `_) * Suppress clang dead-store warnings in the benchmarks. (`#1802 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 235427442c..371e3cff53 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 13.1.0 + 14.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Jacob Perron From 152dbc6e38e18c18a5883b51c33162d237f891e0 Mon Sep 17 00:00:00 2001 From: Bi0T1N Date: Mon, 20 Dec 2021 20:56:46 +0100 Subject: [PATCH 051/455] Add tests for function templates of declare_parameter (#1747) * Add function template tests for all defined types of declare_parameter Signed-off-by: Nico Neumann --- rclcpp/test/rclcpp/test_node.cpp | 157 +++++++++++++++++++++++++++++++ 1 file changed, 157 insertions(+) diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index d8c57c3605..fc2f564c75 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -2614,6 +2614,163 @@ TEST_F(TestNode, declare_parameter_with_vector) { } } +// test non-array data types for declare parameter function templates that are explicitly defined +TEST_F(TestNode, declare_parameter_allowed_simple_types_function_templates) { + auto node = std::make_shared( + "test_declare_parameter_allowed_simple_types_function_templates"_unq); + { + // declare parameter and then get types to check + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto name4 = "parameter"_unq; + auto name5 = "parameter"_unq; + auto name6 = "parameter"_unq; + auto name7 = "parameter"_unq; + + node->declare_parameter(name1, false); + node->declare_parameter(name2, 1234); + node->declare_parameter(name3, 12340); + node->declare_parameter(name4, static_cast(12.34)); + node->declare_parameter(name5, 12.34); // called float64 in ros2 design parameters page + node->declare_parameter(name6, "test string"); + auto str = "test param"; + node->declare_parameter(name7, str); + + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + EXPECT_TRUE(node->has_parameter(name4)); + EXPECT_TRUE(node->has_parameter(name5)); + EXPECT_TRUE(node->has_parameter(name6)); + EXPECT_TRUE(node->has_parameter(name7)); + + auto results = node->get_parameter_types({name1, name2, name3, name4, name5, name6, name7}); + EXPECT_EQ(results.size(), 7u); + EXPECT_EQ(results[0], rcl_interfaces::msg::ParameterType::PARAMETER_BOOL); + EXPECT_EQ(results[1], rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + EXPECT_EQ(results[2], rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + EXPECT_EQ(results[3], rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE); + EXPECT_EQ(results[4], rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE); + EXPECT_EQ(results[5], rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + EXPECT_EQ(results[6], rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + } + { + // declare parameter and then get values to check + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto name4 = "parameter"_unq; + auto name5 = "parameter"_unq; + auto name6 = "parameter"_unq; + auto name7 = "parameter"_unq; + + node->declare_parameter(name1, false); + node->declare_parameter(name2, 4321); + node->declare_parameter(name3, 43210); + node->declare_parameter(name4, static_cast(43.21)); + node->declare_parameter(name5, 12.34); // called float64 in ros2 design parameters page + node->declare_parameter(name6, "test string"); + auto str = "test param"; + node->declare_parameter(name7, str); + + std::vector expected = { + {name1, false}, + {name2, 4321}, + {name3, 43210}, + {name4, static_cast(43.21)}, + {name5, 12.34}, + {name6, "test string"}, + {name7, str} + }; + EXPECT_EQ(node->get_parameters({name1, name2, name3, name4, name5, name6, name7}), expected); + } +} + +// test array data types for declare parameter function templates that are explicitly defined +TEST_F(TestNode, declare_parameter_allowed_array_types_function_templates) { + auto node = std::make_shared( + "test_declare_parameter_allowed_array_types_function_templates"_unq); + { + // declare parameter and then get types to check + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto name4 = "parameter"_unq; + auto name5 = "parameter"_unq; + auto name6 = "parameter"_unq; + auto name7 = "parameter"_unq; + + node->declare_parameter>(name1, std::vector{3, 4, 5, 7, 9}); + node->declare_parameter>(name2, std::vector{false, true}); + node->declare_parameter>(name3, std::vector{1234, 2345}); + node->declare_parameter>(name4, std::vector{12340, 9876}); + node->declare_parameter>( + name5, std::vector{static_cast(12.34), + static_cast(98.78)}); + node->declare_parameter>( + name6, + std::vector{12.34, 55.66}); // called float64 in ros2 design parameters page + node->declare_parameter>( + name7, std::vector{"test string", + "another test str"}); + + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + EXPECT_TRUE(node->has_parameter(name4)); + EXPECT_TRUE(node->has_parameter(name5)); + EXPECT_TRUE(node->has_parameter(name6)); + EXPECT_TRUE(node->has_parameter(name7)); + + auto results = node->get_parameter_types({name1, name2, name3, name4, name5, name6, name7}); + EXPECT_EQ(results.size(), 7u); + EXPECT_EQ(results[0], rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY); + EXPECT_EQ(results[1], rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY); + EXPECT_EQ(results[2], rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY); + EXPECT_EQ(results[3], rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY); + EXPECT_EQ(results[4], rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY); + EXPECT_EQ(results[5], rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY); + EXPECT_EQ(results[6], rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY); + } + { + // declare parameter and then get values to check + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto name4 = "parameter"_unq; + auto name5 = "parameter"_unq; + auto name6 = "parameter"_unq; + auto name7 = "parameter"_unq; + + std::vector byte_arr = {0xD, 0xE, 0xA, 0xD}; + node->declare_parameter>(name1, byte_arr); + node->declare_parameter>(name2, std::vector{true, false, true}); + node->declare_parameter>(name3, std::vector{22, 33, 55, 77}); + node->declare_parameter>(name4, std::vector{456, 765}); + node->declare_parameter>( + name5, std::vector{static_cast(99.11), + static_cast(11.99)}); + node->declare_parameter>( + name6, + std::vector{12.21, 55.55, 98.89}); // called float64 in ros2 design parameters page + node->declare_parameter>( + name7, std::vector{"ros2", + "colcon", "ignition"}); + + std::vector expected = { + {name1, std::vector{0xD, 0xE, 0xA, 0xD}}, + {name2, std::vector{true, false, true}}, + {name3, std::vector{22, 33, 55, 77}}, + {name4, std::vector{456, 765}}, + {name5, std::vector{static_cast(99.11), static_cast(11.99)}}, + {name6, std::vector{12.21, 55.55, 98.89}}, + {name7, std::vector{"ros2", "colcon", "ignition"}} + }; + EXPECT_EQ(node->get_parameters({name1, name2, name3, name4, name5, name6, name7}), expected); + } +} + void expect_qos_profile_eq( const rmw_qos_profile_t & qos1, const rmw_qos_profile_t & qos2, bool is_publisher) { From 802bfc2c7442e7d285cd9ac3aaa88e3358683b54 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Tue, 21 Dec 2021 12:36:01 +0800 Subject: [PATCH 052/455] Add wait_for_all_acked support (#1662) * Add wait_for_all_acked support Signed-off-by: Barry Xu * Update the description of wait_for_all_acked Signed-off-by: Barry Xu * Use rcpputils::convert_to_nanoseconds() for time conversion Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/publisher_base.hpp | 42 +++++++++++++ rclcpp/test/rclcpp/test_publisher.cpp | 79 +++++++++++++++++++++++- 2 files changed, 120 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index c67a0439e0..01bdbb585f 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -33,6 +34,7 @@ #include "rclcpp/qos_event.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" +#include "rcpputils/time.hpp" namespace rclcpp { @@ -203,6 +205,46 @@ class PublisherBase : public std::enable_shared_from_this std::vector get_network_flow_endpoints() const; + /// Wait until all published messages are acknowledged or until the specified timeout elapses. + /** + * This method waits until all published messages are acknowledged by all matching + * subscriptions or the given timeout elapses. + * + * If the timeout is negative then this method will block indefinitely until all published + * messages are acknowledged. + * If the timeout is zero then this method will not block, it will check if all published + * messages are acknowledged and return immediately. + * If the timeout is greater than zero, this method will wait until all published messages are + * acknowledged or the timeout elapses. + * + * This method only waits for acknowledgments if the publisher's QoS profile is RELIABLE. + * Otherwise this method will immediately return `true`. + * + * \param[in] timeout the duration to wait for all published messages to be acknowledged. + * \return `true` if all published messages were acknowledged before the given timeout + * elapsed, otherwise `false`. + * \throws rclcpp::exceptions::RCLError if middleware doesn't support or internal error occurs + * \throws std::invalid_argument if timeout is greater than std::chrono::nanoseconds::max() or + * less than std::chrono::nanoseconds::min() + */ + template + bool + wait_for_all_acked( + std::chrono::duration timeout = + std::chrono::duration(-1)) const + { + rcl_duration_value_t rcl_timeout = rcpputils::convert_to_nanoseconds(timeout).count(); + + rcl_ret_t ret = rcl_publisher_wait_for_all_acked(publisher_handle_.get(), rcl_timeout); + if (ret == RCL_RET_OK) { + return true; + } else if (ret == RCL_RET_TIMEOUT) { + return false; + } else { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } + protected: template void diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index bec8bbb17a..3be64e9647 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -14,8 +14,9 @@ #include -#include +#include #include +#include #include #include @@ -28,6 +29,7 @@ #include "../utils/rclcpp_gtest_macros.hpp" #include "test_msgs/msg/empty.hpp" +#include "test_msgs/msg/strings.hpp" class TestPublisher : public ::testing::Test { @@ -536,3 +538,78 @@ TEST_F(TestPublisher, get_network_flow_endpoints_errors) { EXPECT_NO_THROW(publisher->get_network_flow_endpoints()); } } + +TEST_F(TestPublisher, check_wait_for_all_acked_return) { + initialize(); + const rclcpp::QoS publisher_qos(1); + auto publisher = node->create_publisher("topic", publisher_qos); + + { + // Using 'self' instead of 'lib:rclcpp' because `rcl_publisher_wait_for_all_acked` is entirely + // defined in a header + auto mock = mocking_utils::patch_and_return( + "self", rcl_publisher_wait_for_all_acked, RCL_RET_OK); + EXPECT_TRUE(publisher->wait_for_all_acked(std::chrono::milliseconds(-1))); + } + + { + // Using 'self' instead of 'lib:rclcpp' because `rcl_publisher_wait_for_all_acked` is entirely + // defined in a header + auto mock = mocking_utils::patch_and_return( + "self", rcl_publisher_wait_for_all_acked, RCL_RET_TIMEOUT); + EXPECT_FALSE(publisher->wait_for_all_acked(std::chrono::milliseconds(-1))); + } + + { + // Using 'self' instead of 'lib:rclcpp' because `rcl_publisher_wait_for_all_acked` is entirely + // defined in a header + auto mock = mocking_utils::patch_and_return( + "self", rcl_publisher_wait_for_all_acked, RCL_RET_UNSUPPORTED); + EXPECT_THROW( + publisher->wait_for_all_acked(std::chrono::milliseconds(-1)), + rclcpp::exceptions::RCLError); + } + + { + // Using 'self' instead of 'lib:rclcpp' because `rcl_publisher_wait_for_all_acked` is entirely + // defined in a header + auto mock = mocking_utils::patch_and_return( + "self", rcl_publisher_wait_for_all_acked, RCL_RET_ERROR); + EXPECT_THROW( + publisher->wait_for_all_acked(std::chrono::milliseconds(-1)), + rclcpp::exceptions::RCLError); + } +} + +class TestPublisherWaitForAllAcked + : public TestPublisher, public ::testing::WithParamInterface> +{ +}; + +TEST_P(TestPublisherWaitForAllAcked, check_wait_for_all_acked_with_QosPolicy) { + initialize(); + + auto do_nothing = [](std::shared_ptr) {}; + auto pub = node->create_publisher("topic", std::get<0>(GetParam())); + auto sub = node->create_subscription( + "topic", + std::get<1>(GetParam()), + do_nothing); + + auto msg = std::make_shared(); + for (int i = 0; i < 20; i++) { + ASSERT_NO_THROW(pub->publish(*msg)); + } + EXPECT_TRUE(pub->wait_for_all_acked(std::chrono::milliseconds(500))); +} + +INSTANTIATE_TEST_SUITE_P( + TestWaitForAllAckedWithParm, + TestPublisherWaitForAllAcked, + ::testing::Values( + std::pair( + rclcpp::QoS(1).reliable(), rclcpp::QoS(1).reliable()), + std::pair( + rclcpp::QoS(1).best_effort(), rclcpp::QoS(1).best_effort()), + std::pair( + rclcpp::QoS(1).reliable(), rclcpp::QoS(1).best_effort()))); From 7a2ee2328151ebbc29ea3aed611b231eacbd51d8 Mon Sep 17 00:00:00 2001 From: "M. Mostafa Farzan" Date: Fri, 24 Dec 2021 09:58:46 +0330 Subject: [PATCH 053/455] Use UninitializedStaticallyTypedParameterException (#1689) * Use UninitializedStaticallyTypedParameterException Signed-off-by: Mohammad Farzan Co-authored-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/exceptions/exceptions.hpp | 17 +++++++++++++++++ rclcpp/include/rclcpp/node_impl.hpp | 16 ++++++++++------ rclcpp/test/rclcpp/test_node.cpp | 6 ++++++ 3 files changed, 33 insertions(+), 6 deletions(-) diff --git a/rclcpp/include/rclcpp/exceptions/exceptions.hpp b/rclcpp/include/rclcpp/exceptions/exceptions.hpp index 524ccf73ea..bf67c50d6d 100644 --- a/rclcpp/include/rclcpp/exceptions/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions/exceptions.hpp @@ -254,6 +254,23 @@ class InvalidParameterTypeException : public std::runtime_error {} }; +/// Thrown if user attempts to create an uninitialized statically typed parameter +/** + * (see https://github.com/ros2/rclcpp/issues/1691) + */ +class UninitializedStaticallyTypedParameterException : public std::runtime_error +{ +public: + /// Construct an instance. + /** + * \param[in] name the name of the parameter. + */ + RCLCPP_PUBLIC + explicit UninitializedStaticallyTypedParameterException(const std::string & name) + : std::runtime_error("Statically typed parameter '" + name + "' must be initialized.") + {} +}; + /// Thrown if parameter is already declared. class ParameterAlreadyDeclaredException : public std::runtime_error { diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index df6039cf49..46f54aa054 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -220,12 +220,16 @@ Node::declare_parameter( // get advantage of parameter value template magic to get // the correct rclcpp::ParameterType from ParameterT rclcpp::ParameterValue value{ParameterT{}}; - return this->declare_parameter( - name, - value.get_type(), - parameter_descriptor, - ignore_override - ).get(); + try { + return this->declare_parameter( + name, + value.get_type(), + parameter_descriptor, + ignore_override + ).get(); + } catch (const ParameterTypeException &) { + throw exceptions::UninitializedStaticallyTypedParameterException(name); + } } template diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index fc2f564c75..f3fb13119f 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -715,6 +715,12 @@ TEST_F(TestNode, declare_parameter_with_overrides) { "parameter_type_mismatch", rclcpp::ParameterType::PARAMETER_INTEGER);}, rclcpp::exceptions::InvalidParameterTypeException); } + { + // statically typed parameter must be initialized + EXPECT_THROW( + {node->declare_parameter("static_and_uninitialized");}, + rclcpp::exceptions::UninitializedStaticallyTypedParameterException); + } { // cannot pass an expected type and a descriptor with dynamic_typing=True rcl_interfaces::msg::ParameterDescriptor descriptor{}; From 5613d613cf199ccfa7da2a2935dbae4f32252f18 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 5 Jan 2022 10:39:54 -0500 Subject: [PATCH 054/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 7 +++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 3 +++ 4 files changed, 16 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 88bde8e867..0da6768a1d 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use UninitializedStaticallyTypedParameterException (`#1689 `_) +* Add wait_for_all_acked support (`#1662 `_) +* Add tests for function templates of declare_parameter (`#1747 `_) +* Contributors: Barry Xu, Bi0T1N, M. Mostafa Farzan + 14.0.0 (2021-12-17) ------------------- * Fixes for uncrustify 0.72 (`#1844 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 838ecd86bb..ef8907e934 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 14.0.0 (2021-12-17) ------------------- * Fixes for uncrustify 0.72 (`#1844 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 5ed7eeae02..017d35e8d2 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 14.0.0 (2021-12-17) ------------------- * Add parameter to configure number of thread (`#1708 `_) diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 6806e91cd8..2f0b543ac3 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 14.0.0 (2021-12-17) ------------------- * Remove author by request (`#1818 `_) From 2d6e6364cda34af4c8d005f5c7dde0bdab4a8d78 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 5 Jan 2022 10:40:02 -0500 Subject: [PATCH 055/455] 14.1.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 0da6768a1d..29372f7a82 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +14.1.0 (2022-01-05) +------------------- * Use UninitializedStaticallyTypedParameterException (`#1689 `_) * Add wait_for_all_acked support (`#1662 `_) * Add tests for function templates of declare_parameter (`#1747 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index de866ad039..30a915c0a5 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 14.0.0 + 14.1.0 The ROS client library in C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index ef8907e934..7d5c727ac2 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +14.1.0 (2022-01-05) +------------------- 14.0.0 (2021-12-17) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 0ab56ca454..692e1de1fb 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 14.0.0 + 14.1.0 Adds action APIs for C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 017d35e8d2..c7083506a9 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +14.1.0 (2022-01-05) +------------------- 14.0.0 (2021-12-17) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index fa93648255..3c4ded1cf0 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 14.0.0 + 14.1.0 Package containing tools for dynamically loadable components Ivan Paunovic Jacob Perron diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 2f0b543ac3..a15fc96cb3 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +14.1.0 (2022-01-05) +------------------- 14.0.0 (2021-12-17) ------------------- diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 371e3cff53..e827de98bf 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 14.0.0 + 14.1.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Jacob Perron From 9583ec7855aeed7f76ab332ec5729f1e5d0c2fe6 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 6 Jan 2022 14:31:17 -0800 Subject: [PATCH 056/455] Add rclcpp_components::component (#1855) Signed-off-by: Shane Loretz --- rclcpp_components/CMakeLists.txt | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/rclcpp_components/CMakeLists.txt b/rclcpp_components/CMakeLists.txt index fe82f6beb1..c7dec3e65f 100644 --- a/rclcpp_components/CMakeLists.txt +++ b/rclcpp_components/CMakeLists.txt @@ -20,6 +20,15 @@ find_package(composition_interfaces REQUIRED) find_package(rclcpp REQUIRED) find_package(rcpputils REQUIRED) +# Add an interface library that can be dependend upon by libraries who register components +add_library(component INTERFACE) +target_include_directories(component INTERFACE + "$" + "$") +target_link_libraries(component INTERFACE + class_loader::class_loader + rclcpp::rclcpp) + add_library( component_manager SHARED @@ -87,10 +96,7 @@ if(BUILD_TESTING) set(components "") add_library(test_component SHARED test/components/test_component.cpp) - target_include_directories(test_component PUBLIC include) - ament_target_dependencies(test_component - "class_loader" - "rclcpp") + target_link_libraries(test_component PRIVATE component) #rclcpp_components_register_nodes(test_component "test_rclcpp_components::TestComponent") set(components "${components}test_rclcpp_components::TestComponentFoo;$\n") set(components "${components}test_rclcpp_components::TestComponentBar;$\n") @@ -138,7 +144,7 @@ if(BUILD_TESTING) endif() install( - TARGETS component_manager EXPORT component_manager + TARGETS component component_manager EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -165,7 +171,7 @@ install( # specific order: dependents before dependencies ament_export_include_directories(include) ament_export_libraries(component_manager) -ament_export_targets(component_manager) +ament_export_targets(export_${PROJECT_NAME}) ament_export_dependencies(ament_index_cpp) ament_export_dependencies(class_loader) ament_export_dependencies(composition_interfaces) From 8093648ee378ebe39ee3d0b11e0f58fecfc02b1a Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Fri, 7 Jan 2022 09:36:11 -0300 Subject: [PATCH 057/455] Fix rclcpp documentation build (#1779) * Update Doxyfile * Update docblocks with warnings * Use leading * instead of trailing [] for pointer types * Help Doxygen parse std::enable_if<> condition * Add documentation-only SFINAE functions' declarations * Avoid function pointer type syntax in function arguments. * Add documentation-only SFINAE function traits. Signed-off-by: Chris Lalancette Signed-off-by: Michel Hidalgo --- rclcpp/Doxyfile | 11 ++++++++- rclcpp/include/rclcpp/context.hpp | 2 +- .../include/rclcpp/detail/qos_parameters.hpp | 18 ++++++++++++-- .../subscription_callback_type_helper.hpp | 2 ++ rclcpp/include/rclcpp/detail/utilities.hpp | 2 +- .../include/rclcpp/exceptions/exceptions.hpp | 5 ++-- rclcpp/include/rclcpp/executor.hpp | 1 + .../static_executor_entities_collector.hpp | 3 ++- .../experimental/intra_process_manager.hpp | 1 + rclcpp/include/rclcpp/function_traits.hpp | 12 +++++++--- rclcpp/include/rclcpp/generic_publisher.hpp | 1 - .../get_message_type_support_handle.hpp | 24 ++++++++++++------- rclcpp/include/rclcpp/guard_condition.hpp | 2 ++ .../node_interfaces/node_graph_interface.hpp | 4 ++++ rclcpp/include/rclcpp/parameter_client.hpp | 2 +- rclcpp/include/rclcpp/time_source.hpp | 2 ++ rclcpp/include/rclcpp/utilities.hpp | 13 ++++++---- rclcpp/src/rclcpp/context.cpp | 2 +- rclcpp/src/rclcpp/detail/utilities.cpp | 2 +- rclcpp/src/rclcpp/utilities.cpp | 8 +++---- 20 files changed, 85 insertions(+), 32 deletions(-) diff --git a/rclcpp/Doxyfile b/rclcpp/Doxyfile index 2a7846b1f8..01541e33d0 100644 --- a/rclcpp/Doxyfile +++ b/rclcpp/Doxyfile @@ -21,7 +21,16 @@ GENERATE_LATEX = NO ENABLE_PREPROCESSING = YES MACRO_EXPANSION = YES EXPAND_ONLY_PREDEF = YES -PREDEFINED = RCLCPP_PUBLIC= +PREDEFINED += DOXYGEN_ONLY +PREDEFINED += RCLCPP_LOCAL= +PREDEFINED += RCLCPP_PUBLIC= +PREDEFINED += RCLCPP_PUBLIC_TYPE= +PREDEFINED += RCUTILS_WARN_UNUSED= +PREDEFINED += RCPPUTILS_TSA_GUARDED_BY(x)= +PREDEFINED += RCPPUTILS_TSA_PT_GUARDED_BY(x)= +PREDEFINED += RCPPUTILS_TSA_REQUIRES(x)= + +DOT_GRAPH_MAX_NODES = 101 # Tag files that do not exist will produce a warning and cross-project linking will not work. TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 227fc3928c..2d8db29d21 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -127,7 +127,7 @@ class Context : public std::enable_shared_from_this void init( int argc, - char const * const argv[], + char const * const * argv, const rclcpp::InitOptions & init_options = rclcpp::InitOptions()); /// Return true if the context is valid, otherwise false. diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp index cd8d9ffef7..651e58e7d2 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -104,6 +104,7 @@ declare_parameter_or_get( } } +#ifdef DOXYGEN_ONLY /// \internal Declare QoS parameters for the given entity. /** * \tparam NodeT Node pointer or reference type. @@ -116,12 +117,23 @@ declare_parameter_or_get( * \param default_qos User provided qos. It will be used as a default for the parameters declared. * \return qos profile based on the user provided parameter overrides. */ +template +rclcpp::QoS + declare_qos_parameters( + const ::rclcpp::QosOverridingOptions & options, + NodeT & node, + const std::string & topic_name, + const ::rclcpp::QoS & default_qos, + EntityQosParametersTraits); + +#else + template std::enable_if_t< - rclcpp::node_interfaces::has_node_parameters_interface< + (rclcpp::node_interfaces::has_node_parameters_interface< decltype(std::declval::type>())>::value || std::is_same, - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr>::value, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr>::value), rclcpp::QoS> declare_qos_parameters( const ::rclcpp::QosOverridingOptions & options, @@ -204,6 +216,8 @@ declare_qos_parameters( return default_qos; } +#endif + /// \internal Helper function to get a rmw qos policy value from a string. #define RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( \ kind_lower, kind_upper, parameter_value, rclcpp_qos) \ diff --git a/rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp b/rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp index ec24038d79..0ebe6efd66 100644 --- a/rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp +++ b/rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp @@ -38,6 +38,7 @@ namespace detail * * This issue, with the lambda's, can be demonstrated with this minimal program: * + * \code{.cpp} * #include * #include * @@ -52,6 +53,7 @@ namespace detail * std::function)> cb = [](std::shared_ptr){}; * f(cb); * } + * \endcode * * If this program ever starts working in a future version of C++, this class * may become redundant. diff --git a/rclcpp/include/rclcpp/detail/utilities.hpp b/rclcpp/include/rclcpp/detail/utilities.hpp index 62012ba0c8..d1c5d2549a 100644 --- a/rclcpp/include/rclcpp/detail/utilities.hpp +++ b/rclcpp/include/rclcpp/detail/utilities.hpp @@ -30,7 +30,7 @@ namespace detail std::vector get_unparsed_ros_arguments( - int argc, char const * const argv[], + int argc, char const * const * argv, rcl_arguments_t * arguments, rcl_allocator_t allocator); diff --git a/rclcpp/include/rclcpp/exceptions/exceptions.hpp b/rclcpp/include/rclcpp/exceptions/exceptions.hpp index bf67c50d6d..ecb2a09905 100644 --- a/rclcpp/include/rclcpp/exceptions/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions/exceptions.hpp @@ -109,6 +109,8 @@ class UnimplementedError : public std::runtime_error : std::runtime_error(msg) {} }; +typedef void (* reset_error_function_t)(); + /// Throw a C++ std::exception which was created based on an rcl error. /** * Passing nullptr for reset_error is safe and will avoid calling any function @@ -129,7 +131,7 @@ throw_from_rcl_error [[noreturn]] ( rcl_ret_t ret, const std::string & prefix = "", const rcl_error_state_t * error_state = nullptr, - void (* reset_error)() = rcl_reset_error); + reset_error_function_t reset_error = rcl_reset_error); /* *INDENT-ON* */ class RCLErrorBase @@ -306,7 +308,6 @@ class ParameterUninitializedException : public std::runtime_error /// Construct an instance. /** * \param[in] name the name of the parameter. - * \param[in] message custom exception message. */ explicit ParameterUninitializedException(const std::string & name) : std::runtime_error("parameter '" + name + "' is not initialized") diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index dfd88ccea2..77d6a5f447 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -455,6 +455,7 @@ class Executor /// Return true if the node has been added to this executor. /** * \param[in] node_ptr a shared pointer that points to a node base interface + * \param[in] weak_groups_to_nodes map to nodes to lookup * \return true if the node is associated with the executor, otherwise false */ RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp index bda28cb74b..f9fd2ff672 100644 --- a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp @@ -299,7 +299,8 @@ class StaticExecutorEntitiesCollector final /// Return true if the node belongs to the collector /** - * \param[in] group_ptr a node base interface shared pointer + * \param[in] node_ptr a node base interface shared pointer + * \param[in] weak_groups_to_nodes map to nodes to lookup * \return boolean whether a node belongs the collector */ bool diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index a927a81856..16c6039b76 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -173,6 +173,7 @@ class IntraProcessManager * * \param intra_process_publisher_id the id of the publisher of this message. * \param message the message that is being stored. + * \param allocator for allocations when buffering messages. */ template< typename MessageT, diff --git a/rclcpp/include/rclcpp/function_traits.hpp b/rclcpp/include/rclcpp/function_traits.hpp index 4dd3cc28db..b6af0aeb47 100644 --- a/rclcpp/include/rclcpp/function_traits.hpp +++ b/rclcpp/include/rclcpp/function_traits.hpp @@ -80,7 +80,9 @@ struct function_traits: function_traits -#if defined _LIBCPP_VERSION // libc++ (Clang) +#if defined DOXYGEN_ONLY +struct function_traits> +#elif defined _LIBCPP_VERSION // libc++ (Clang) struct function_traits> #elif defined _GLIBCXX_RELEASE // glibc++ (GNU C++ >= 7.1) struct function_traits> @@ -97,7 +99,9 @@ struct function_traits< // std::bind for object const methods template -#if defined _LIBCPP_VERSION // libc++ (Clang) +#if defined DOXYGEN_ONLY +struct function_traits> +#elif defined _LIBCPP_VERSION // libc++ (Clang) struct function_traits> #elif defined _GLIBCXX_RELEASE // glibc++ (GNU C++ >= 7.1) struct function_traits> @@ -114,7 +118,9 @@ struct function_traits< // std::bind for free functions template -#if defined _LIBCPP_VERSION // libc++ (Clang) +#if defined DOXYGEN_ONLY +struct function_traits> +#elif defined _LIBCPP_VERSION // libc++ (Clang) struct function_traits> #elif defined __GLIBCXX__ // glibc++ (GNU C++) struct function_traits> diff --git a/rclcpp/include/rclcpp/generic_publisher.hpp b/rclcpp/include/rclcpp/generic_publisher.hpp index 7cadda5d1d..e379f09427 100644 --- a/rclcpp/include/rclcpp/generic_publisher.hpp +++ b/rclcpp/include/rclcpp/generic_publisher.hpp @@ -60,7 +60,6 @@ class GenericPublisher : public rclcpp::PublisherBase * \param topic_name Topic name * \param topic_type Topic type * \param qos %QoS settings - * \param callback Callback for new messages of serialized form * \param options %Publisher options. * Not all publisher options are currently respected, the only relevant options for this * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`. diff --git a/rclcpp/include/rclcpp/get_message_type_support_handle.hpp b/rclcpp/include/rclcpp/get_message_type_support_handle.hpp index 2f19528baf..cbc2e6035b 100644 --- a/rclcpp/include/rclcpp/get_message_type_support_handle.hpp +++ b/rclcpp/include/rclcpp/get_message_type_support_handle.hpp @@ -28,7 +28,17 @@ namespace rclcpp { -/// Specialization for when MessageT is an actual ROS message type. +#ifdef DOXYGEN_ONLY + +/// Returns the message type support for the given `MessageT` type. +/** + * \tparam MessageT an actual ROS message type or an adapted type using `rclcpp::TypeAdapter` + */ +template +constexpr const rosidl_message_type_support_t & get_message_type_support_handle(); + +#else + template constexpr typename std::enable_if_t< @@ -44,7 +54,6 @@ get_message_type_support_handle() return *handle; } -/// Specialization for when MessageT is an adapted type using rclcpp::TypeAdapter. template constexpr typename std::enable_if_t< @@ -63,12 +72,9 @@ get_message_type_support_handle() return *handle; } -/// Specialization for when MessageT is not a ROS message nor an adapted type. -/** - * This specialization is a pass through runtime check, which allows a better - * static_assert to catch this issue further down the line. - * This should never get to be called in practice, and is purely defensive. - */ +// This specialization is a pass through runtime check, which allows a better +// static_assert to catch this issue further down the line. +// This should never get to be called in practice, and is purely defensive. template< typename AdaptedType > @@ -85,6 +91,8 @@ get_message_type_support_handle() "should never be called"); } +#endif + } // namespace rclcpp #endif // RCLCPP__GET_MESSAGE_TYPE_SUPPORT_HANDLE_HPP_ diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp index 5104a6446c..fce42f1d4b 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -40,6 +40,8 @@ class GuardCondition * Defaults to using the global default context singleton. * Shared ownership of the context is held with the guard condition until * destruction. + * \param[in] guard_condition_options Optional guard condition options to be used. + * Defaults to using the default guard condition options. * \throws std::invalid_argument if the context is nullptr. * \throws rclcpp::exceptions::RCLError based exceptions when underlying * rcl functions fail. diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp index 243c22cf5e..3958f5b2d9 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -357,6 +357,8 @@ class NodeGraphInterface /// Return the topic endpoint information about publishers on a given topic. /** * \param[in] topic_name the actual topic name used; it will not be automatically remapped. + * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name, + * otherwise it should be a valid ROS topic name. * \sa rclcpp::Node::get_publishers_info_by_topic */ RCLCPP_PUBLIC @@ -367,6 +369,8 @@ class NodeGraphInterface /// Return the topic endpoint information about subscriptions on a given topic. /** * \param[in] topic_name the actual topic name used; it will not be automatically remapped. + * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name, + * otherwise it should be a valid ROS topic name. * \sa rclcpp::Node::get_subscriptions_info_by_topic */ RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 4a15a35141..e03d7f519f 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -185,7 +185,7 @@ class AsyncParametersClient /** * This function filters the parameters to be set based on the node name. * - * \param yaml_filename the full name of the yaml file + * \param parameter_map named parameters to be loaded * \return the future of the set_parameter service used to load the parameters * \throw InvalidParametersException if there is no parameter to set */ diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp index 9d067d933c..84984da6e4 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -56,6 +56,7 @@ class TimeSource * * \param node std::shared pointer to a initialized node * \param qos QoS that will be used when creating a `/clock` subscription. + * \param use_clock_thread whether to spin the attached node in a separate thread */ RCLCPP_PUBLIC explicit TimeSource( @@ -68,6 +69,7 @@ class TimeSource * An Empty TimeSource class * * \param qos QoS that will be used when creating a `/clock` subscription. + * \param use_clock_thread whether to spin the attached node in a separate thread. */ RCLCPP_PUBLIC explicit TimeSource( diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index e9d8408bb3..fa48a010c4 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -65,13 +65,16 @@ enum class SignalHandlerOptions * * \sa rclcpp::Context::init() for more details on arguments and possible exceptions * - * \param signal_handler_options option to indicate which signal handlers should be installed. + * \param[in] argc number of command-line arguments to parse. + * \param[in] argv array of command-line arguments to parse. + * \param[in] init_options initialization options to apply. + * \param[in] signal_handler_options option to indicate which signal handlers should be installed. */ RCLCPP_PUBLIC void init( int argc, - char const * const argv[], + char const * const * argv, const InitOptions & init_options = InitOptions(), SignalHandlerOptions signal_handler_options = SignalHandlerOptions::All); @@ -87,7 +90,7 @@ init( * * This function is thread-safe. * - * \param signal_handler_options option to indicate which signal handlers should be installed. + * \param[in] signal_handler_options option to indicate which signal handlers should be installed. * \return true if signal handler was installed by this function, false if already installed. */ RCLCPP_PUBLIC @@ -136,7 +139,7 @@ RCLCPP_PUBLIC std::vector init_and_remove_ros_arguments( int argc, - char const * const argv[], + char const * const * argv, const InitOptions & init_options = InitOptions()); /// Remove ROS-specific arguments from argument vector. @@ -154,7 +157,7 @@ init_and_remove_ros_arguments( */ RCLCPP_PUBLIC std::vector -remove_ros_arguments(int argc, char const * const argv[]); +remove_ros_arguments(int argc, char const * const * argv); /// Check rclcpp's status. /** diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index fff5753954..d3d123c065 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -192,7 +192,7 @@ __delete_context(rcl_context_t * context) void Context::init( int argc, - char const * const argv[], + char const * const * argv, const rclcpp::InitOptions & init_options) { std::lock_guard init_lock(init_mutex_); diff --git a/rclcpp/src/rclcpp/detail/utilities.cpp b/rclcpp/src/rclcpp/detail/utilities.cpp index 179756f6a3..0166f2dceb 100644 --- a/rclcpp/src/rclcpp/detail/utilities.cpp +++ b/rclcpp/src/rclcpp/detail/utilities.cpp @@ -31,7 +31,7 @@ namespace detail std::vector get_unparsed_ros_arguments( - int argc, char const * const argv[], + int argc, char const * const * argv, rcl_arguments_t * arguments, rcl_allocator_t allocator) { diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index dcc79c96b8..a6ead85c03 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -33,7 +33,7 @@ namespace rclcpp void init( int argc, - char const * const argv[], + char const * const * argv, const InitOptions & init_options, SignalHandlerOptions signal_handler_options) { @@ -71,7 +71,7 @@ uninstall_signal_handlers() static std::vector _remove_ros_arguments( - char const * const argv[], + char const * const * argv, const rcl_arguments_t * args, rcl_allocator_t alloc) { @@ -112,7 +112,7 @@ _remove_ros_arguments( std::vector init_and_remove_ros_arguments( int argc, - char const * const argv[], + char const * const * argv, const InitOptions & init_options) { init(argc, argv, init_options); @@ -123,7 +123,7 @@ init_and_remove_ros_arguments( } std::vector -remove_ros_arguments(int argc, char const * const argv[]) +remove_ros_arguments(int argc, char const * const * argv) { rcl_allocator_t alloc = rcl_get_default_allocator(); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); From 6b321edb4fabcc57f666f77a4dc091e95948070b Mon Sep 17 00:00:00 2001 From: Gonzo <42421541+gonzodepedro@users.noreply.github.com> Date: Fri, 7 Jan 2022 16:45:06 -0300 Subject: [PATCH 058/455] Add non transform capabilities for intra-process (#1849) That is, make it so that if both a publisher and a subscriber using intra-process and using TypeAdaptation are the same type, we don't need to convert to a ROS Message type on the publisher and from a ROS Message type before delivering to the subscriber. Instead, we store the original message type in the subscription buffer, and deliver it straight through to the subscriber. This has two main benefits: 1. It is better for performance; we are eliding unnecessary work. 2. We can use TypeAdaptation to pass custom handles (like hardware accelerator handles) between a publisher and subscriber. That means we can avoid unnecessary overhead when using a hardware accelerator, like synchronizing between the hardware accelerator and the main CPU. The main way this is accomplished is by causing the SubscriptionIntraProcessBuffer to store messages of the Subscribed Type (which in the case of a ROS message type subscriber is a ROS message type, but in the case of a TypeAdapter type is the custom type). When a publish with a Type Adapted type happens, it delays conversion of the message, and passes the message type from the user down into the IntraProcessManager. The IntraProcessManager checks to see if it can cast the SubscriptionBuffer to the custom type being published first. If it can, then it knows it can deliver the message directly into the SubscriptionBuffer with no conversion necessary. If it can't, it then sees if the SubscriptionBuffer is of a ROS message type. If it is, then it performs the conversion as necessary, and then stores it. In all cases, the Subscription is then triggered underneath so that the message will eventually be delivered by an executor. We also add tests to ensure that in the case where the publisher and subscriber and using the same type adapter, no conversion happens when delivering the message. Signed-off-by: Chris Lalancette Signed-off-by: Gonzalo de Pedro --- .../rclcpp/any_subscription_callback.hpp | 140 ++- .../experimental/intra_process_manager.hpp | 182 +++- .../ros_message_intra_process_buffer.hpp | 68 ++ .../subscription_intra_process.hpp | 47 +- .../subscription_intra_process_base.hpp | 16 +- .../subscription_intra_process_buffer.hpp | 103 +- rclcpp/include/rclcpp/publisher.hpp | 87 +- rclcpp/include/rclcpp/subscription.hpp | 12 +- rclcpp/test/rclcpp/CMakeLists.txt | 12 + .../rclcpp/test_any_subscription_callback.cpp | 114 +- .../rclcpp/test_intra_process_manager.cpp | 38 +- .../test_publisher_with_type_adapter.cpp | 10 + ...ption_publisher_with_same_type_adapter.cpp | 991 ++++++++++++++++++ 13 files changed, 1603 insertions(+), 217 deletions(-) create mode 100644 rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp create mode 100644 rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 62e116af56..56af35c518 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -476,6 +476,22 @@ class AnySubscriptionCallback } } + std::unique_ptr + convert_custom_type_to_ros_message_unique_ptr(const SubscribedType & msg) + { + if constexpr (rclcpp::TypeAdapter::is_specialized::value) { + auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator_, 1); + ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator_, ptr); + rclcpp::TypeAdapter::convert_to_ros_message(msg, *ptr); + return std::unique_ptr(ptr, ros_message_type_deleter_); + } else { + static_assert( + !sizeof(MessageT *), + "convert_custom_type_to_ros_message_unique_ptr() " + "unexpectedly called without specialized TypeAdapter"); + } + } + // Dispatch when input is a ros message and the output could be anything. void dispatch( @@ -658,7 +674,7 @@ class AnySubscriptionCallback void dispatch_intra_process( - std::shared_ptr message, + std::shared_ptr message, const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, static_cast(this), true); @@ -677,65 +693,89 @@ class AnySubscriptionCallback // conditions for custom type if constexpr (is_ta && std::is_same_v) { - auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); - callback(*local_message); + callback(*message); } else if constexpr (is_ta && std::is_same_v) { // NOLINT - auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); - callback(*local_message, message_info); + callback(*message, message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + callback(create_custom_unique_ptr_from_custom_shared_ptr_message(message)); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + callback(create_custom_unique_ptr_from_custom_shared_ptr_message(message), message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + callback(message); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + callback(message, message_info); } // conditions for ros message type - else if constexpr (std::is_same_v) { // NOLINT - callback(*message); - } else if constexpr (std::is_same_v) { - callback(*message, message_info); + else if constexpr (std::is_same_v) { // NOLINT[readability/braces] + if constexpr (is_ta) { + auto local = convert_custom_type_to_ros_message_unique_ptr(*message); + callback(*local); + } else { + callback(*message); + } + } else if constexpr (std::is_same_v) { // NOLINT[readability/braces] + if constexpr (is_ta) { + auto local = convert_custom_type_to_ros_message_unique_ptr(*message); + callback(*local, message_info); + } else { + callback(*message, message_info); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message)); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message)); + } else { + callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message)); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info); + } else { + callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(message); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message)); + } else { + callback(message); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(message, message_info); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info); + } else { + callback(message, message_info); + } } // condition to catch SerializedMessage types else if constexpr ( // NOLINT[readability/braces] @@ -764,7 +804,7 @@ class AnySubscriptionCallback void dispatch_intra_process( - std::unique_ptr message, + std::unique_ptr message, const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, static_cast(this), true); @@ -778,70 +818,98 @@ class AnySubscriptionCallback // Dispatch. std::visit( [&message, &message_info, this](auto && callback) { + // clang complains that 'this' lambda capture is unused, which is true + // in *some* specializations of this template, but not others. Just + // quiet it down. + (void)this; + using T = std::decay_t; static constexpr bool is_ta = rclcpp::TypeAdapter::is_specialized::value; // conditions for custom type if constexpr (is_ta && std::is_same_v) { - auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); - callback(*local_message); + callback(*message); } else if constexpr (is_ta && std::is_same_v) { // NOLINT - auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); - callback(*local_message, message_info); + callback(*message, message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| - std::is_same_v - )) + std::is_same_v)) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + callback(std::move(message)); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + callback(std::move(message), message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + callback(std::move(message)); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + callback(std::move(message), message_info); } // conditions for ros message type - else if constexpr (std::is_same_v) { // NOLINT - callback(*message); - } else if constexpr (std::is_same_v) { - callback(*message, message_info); + else if constexpr (std::is_same_v) { // NOLINT[readability/braces] + if constexpr (is_ta) { + auto local = convert_custom_type_to_ros_message_unique_ptr(*message); + callback(*local); + } else { + callback(*message); + } + } else if constexpr (std::is_same_v) { // NOLINT[readability/braces] + if constexpr (is_ta) { + auto local = convert_custom_type_to_ros_message_unique_ptr(*message); + callback(*local, message_info); + } else { + callback(*message, message_info); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(std::move(message)); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message)); + } else { + callback(std::move(message)); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(std::move(message), message_info); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info); + } else { + callback(std::move(message), message_info); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(std::move(message)); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message)); + } else { + callback(std::move(message)); + } } else if constexpr ( // NOLINT[readability/braces] std::is_same_v|| std::is_same_v) { - callback(std::move(message), message_info); + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info); + } else { + callback(std::move(message), message_info); + } } // condition to catch SerializedMessage types else if constexpr ( // NOLINT[readability/braces] diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 16c6039b76..0950669e9a 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -29,8 +29,10 @@ #include #include #include +#include #include "rclcpp/allocator/allocator_deleter.hpp" +#include "rclcpp/experimental/ros_message_intra_process_buffer.hpp" #include "rclcpp/experimental/subscription_intra_process.hpp" #include "rclcpp/experimental/subscription_intra_process_base.hpp" #include "rclcpp/experimental/subscription_intra_process_buffer.hpp" @@ -38,6 +40,7 @@ #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/publisher_base.hpp" +#include "rclcpp/type_adapter.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -177,8 +180,10 @@ class IntraProcessManager */ template< typename MessageT, - typename Alloc = std::allocator, - typename Deleter = std::default_delete> + typename ROSMessageType, + typename Alloc, + typename Deleter = std::default_delete + > void do_intra_process_publish( uint64_t intra_process_publisher_id, @@ -204,7 +209,7 @@ class IntraProcessManager // None of the buffers require ownership, so we promote the pointer std::shared_ptr msg = std::move(message); - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( msg, sub_ids.take_shared_subscriptions); } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT sub_ids.take_shared_subscriptions.size() <= 1) @@ -218,8 +223,7 @@ class IntraProcessManager concatenated_vector.end(), sub_ids.take_ownership_subscriptions.begin(), sub_ids.take_ownership_subscriptions.end()); - - this->template add_owned_msg_to_buffers( + this->template add_owned_msg_to_buffers( std::move(message), concatenated_vector, allocator); @@ -230,17 +234,19 @@ class IntraProcessManager // for the buffers that do not require ownership auto shared_msg = std::allocate_shared(allocator, *message); - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, sub_ids.take_shared_subscriptions); - this->template add_owned_msg_to_buffers( + this->template add_owned_msg_to_buffers( std::move(message), sub_ids.take_ownership_subscriptions, allocator); } } template< typename MessageT, - typename Alloc = std::allocator, - typename Deleter = std::default_delete> + typename ROSMessageType, + typename Alloc, + typename Deleter = std::default_delete + > std::shared_ptr do_intra_process_publish_and_return_shared( uint64_t intra_process_publisher_id, @@ -266,7 +272,7 @@ class IntraProcessManager // If there are no owning, just convert to shared. std::shared_ptr shared_msg = std::move(message); if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, sub_ids.take_shared_subscriptions); } return shared_msg; @@ -276,17 +282,16 @@ class IntraProcessManager auto shared_msg = std::allocate_shared(allocator, *message); if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, sub_ids.take_shared_subscriptions); } if (!sub_ids.take_ownership_subscriptions.empty()) { - this->template add_owned_msg_to_buffers( + this->template add_owned_msg_to_buffers( std::move(message), sub_ids.take_ownership_subscriptions, allocator); } - return shared_msg; } } @@ -339,41 +344,83 @@ class IntraProcessManager template< typename MessageT, typename Alloc, - typename Deleter> + typename Deleter, + typename ROSMessageType> void add_shared_msg_to_buffers( std::shared_ptr message, std::vector subscription_ids) { + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + + using PublishedType = typename rclcpp::TypeAdapter::custom_type; + using PublishedTypeAllocatorTraits = allocator::AllocRebind; + using PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type; + using PublishedTypeDeleter = allocator::Deleter; + for (auto id : subscription_ids) { auto subscription_it = subscriptions_.find(id); if (subscription_it == subscriptions_.end()) { throw std::runtime_error("subscription has unexpectedly gone out of scope"); } auto subscription_base = subscription_it->second.lock(); - if (subscription_base) { - auto subscription = std::dynamic_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcessBuffer - >(subscription_base); - if (nullptr == subscription) { - throw std::runtime_error( - "failed to dynamic cast SubscriptionIntraProcessBase to " - "SubscriptionIntraProcessBuffer, which " - "can happen when the publisher and subscription use different " - "allocator types, which is not supported"); - } + if (subscription_base == nullptr) { + subscriptions_.erase(id); + continue; + } + + auto subscription = std::dynamic_pointer_cast< + rclcpp::experimental::SubscriptionIntraProcessBuffer + >(subscription_base); + if (subscription != nullptr) { + subscription->provide_intra_process_data(message); + continue; + } - subscription->provide_intra_process_message(message); + auto ros_message_subscription = std::dynamic_pointer_cast< + rclcpp::experimental::SubscriptionROSMsgIntraProcessBuffer + >(subscription_base); + if (nullptr == ros_message_subscription) { + throw std::runtime_error( + "failed to dynamic cast SubscriptionIntraProcessBase to " + "SubscriptionIntraProcessBuffer, or to " + "SubscriptionROSMsgIntraProcessBuffer which can happen when the publisher and " + "subscription use different allocator types, which is not supported"); + } + + if constexpr (rclcpp::TypeAdapter::is_specialized::value) { + ROSMessageType ros_msg; + rclcpp::TypeAdapter::convert_to_ros_message(*message, ros_msg); + ros_message_subscription->provide_intra_process_message( + std::make_shared(ros_msg)); } else { - subscriptions_.erase(id); + if constexpr (std::is_same::value) { + ros_message_subscription->provide_intra_process_message(message); + } else { + if constexpr (std::is_same::ros_message_type, ROSMessageType>::value) + { + ROSMessageType ros_msg; + rclcpp::TypeAdapter::convert_to_ros_message( + *message, ros_msg); + ros_message_subscription->provide_intra_process_message( + std::make_shared(ros_msg)); + } + } } } } template< typename MessageT, - typename Alloc = std::allocator, - typename Deleter = std::default_delete> + typename Alloc, + typename Deleter, + typename ROSMessageType> void add_owned_msg_to_buffers( std::unique_ptr message, @@ -383,39 +430,84 @@ class IntraProcessManager using MessageAllocTraits = allocator::AllocRebind; using MessageUniquePtr = std::unique_ptr; + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + + using PublishedType = typename rclcpp::TypeAdapter::custom_type; + using PublishedTypeAllocatorTraits = allocator::AllocRebind; + using PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type; + using PublishedTypeDeleter = allocator::Deleter; + for (auto it = subscription_ids.begin(); it != subscription_ids.end(); it++) { auto subscription_it = subscriptions_.find(*it); if (subscription_it == subscriptions_.end()) { throw std::runtime_error("subscription has unexpectedly gone out of scope"); } auto subscription_base = subscription_it->second.lock(); - if (subscription_base) { - auto subscription = std::dynamic_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcessBuffer - >(subscription_base); - if (nullptr == subscription) { - throw std::runtime_error( - "failed to dynamic cast SubscriptionIntraProcessBase to " - "SubscriptionIntraProcessBuffer, which " - "can happen when the publisher and subscription use different " - "allocator types, which is not supported"); - } + if (subscription_base == nullptr) { + subscriptions_.erase(subscription_it); + continue; + } + auto subscription = std::dynamic_pointer_cast< + rclcpp::experimental::SubscriptionIntraProcessBuffer + >(subscription_base); + if (subscription != nullptr) { if (std::next(it) == subscription_ids.end()) { // If this is the last subscription, give up ownership - subscription->provide_intra_process_message(std::move(message)); + subscription->provide_intra_process_data(std::move(message)); } else { // Copy the message since we have additional subscriptions to serve - MessageUniquePtr copy_message; Deleter deleter = message.get_deleter(); auto ptr = MessageAllocTraits::allocate(allocator, 1); MessageAllocTraits::construct(allocator, ptr, *message); - copy_message = MessageUniquePtr(ptr, deleter); - subscription->provide_intra_process_message(std::move(copy_message)); + subscription->provide_intra_process_data(std::move(MessageUniquePtr(ptr, deleter))); } + + continue; + } + + auto ros_message_subscription = std::dynamic_pointer_cast< + rclcpp::experimental::SubscriptionROSMsgIntraProcessBuffer + >(subscription_base); + if (nullptr == ros_message_subscription) { + throw std::runtime_error( + "failed to dynamic cast SubscriptionIntraProcessBase to " + "SubscriptionIntraProcessBuffer, or to " + "SubscriptionROSMsgIntraProcessBuffer which can happen when the publisher and " + "subscription use different allocator types, which is not supported"); + } + + if constexpr (rclcpp::TypeAdapter::is_specialized::value) { + ROSMessageTypeAllocator ros_message_alloc(allocator); + auto ptr = ros_message_alloc.allocate(1); + ros_message_alloc.construct(ptr); + ROSMessageTypeDeleter deleter; + allocator::set_allocator_for_deleter(&deleter, &allocator); + rclcpp::TypeAdapter::convert_to_ros_message(*message, *ptr); + auto ros_msg = std::unique_ptr(ptr, deleter); + ros_message_subscription->provide_intra_process_message(std::move(ros_msg)); } else { - subscriptions_.erase(subscription_it); + if constexpr (std::is_same::value) { + if (std::next(it) == subscription_ids.end()) { + // If this is the last subscription, give up ownership + ros_message_subscription->provide_intra_process_message(std::move(message)); + } else { + // Copy the message since we have additional subscriptions to serve + Deleter deleter = message.get_deleter(); + allocator::set_allocator_for_deleter(&deleter, &allocator); + auto ptr = MessageAllocTraits::allocate(allocator, 1); + MessageAllocTraits::construct(allocator, ptr, *message); + + ros_message_subscription->provide_intra_process_message( + std::move(MessageUniquePtr(ptr, deleter))); + } + } } } } diff --git a/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp new file mode 100644 index 0000000000..7b8b3b833d --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp @@ -0,0 +1,68 @@ +// 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__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_ +#define RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_ + +#include +#include + +#include "rcl/error_handling.h" + +#include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/context.hpp" +#include "rclcpp/experimental/subscription_intra_process_base.hpp" +#include "tracetools/tracetools.h" + +namespace rclcpp +{ +namespace experimental +{ + +template< + typename RosMessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete +> +class SubscriptionROSMsgIntraProcessBuffer : public SubscriptionIntraProcessBase +{ +public: + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + + using ConstMessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + SubscriptionROSMsgIntraProcessBuffer( + rclcpp::Context::SharedPtr context, + const std::string & topic_name, + const rclcpp::QoS & qos_profile) + : SubscriptionIntraProcessBase(context, topic_name, qos_profile) + {} + + virtual ~SubscriptionROSMsgIntraProcessBuffer() + {} + + virtual void + provide_intra_process_message(ConstMessageSharedPtr message) = 0; + + virtual void + provide_intra_process_message(MessageUniquePtr message) = 0; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index f174cd5c07..54857562d0 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -43,40 +43,47 @@ namespace experimental template< typename MessageT, - typename Alloc = std::allocator, - typename Deleter = std::default_delete, - typename CallbackMessageT = MessageT> + typename SubscribedType, + typename SubscribedTypeAlloc = std::allocator, + typename SubscribedTypeDeleter = std::default_delete, + typename ROSMessageType = SubscribedType, + typename Alloc = std::allocator +> class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< - MessageT, - Alloc, - Deleter + SubscribedType, + SubscribedTypeAlloc, + SubscribedTypeDeleter, + ROSMessageType > { using SubscriptionIntraProcessBufferT = SubscriptionIntraProcessBuffer< - MessageT, - Alloc, - Deleter + SubscribedType, + SubscribedTypeAlloc, + SubscribedTypeDeleter, + ROSMessageType >; public: RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess) - using MessageAllocTraits = typename SubscriptionIntraProcessBufferT::MessageAllocTraits; - using MessageAlloc = typename SubscriptionIntraProcessBufferT::MessageAlloc; - using ConstMessageSharedPtr = typename SubscriptionIntraProcessBufferT::ConstMessageSharedPtr; - using MessageUniquePtr = typename SubscriptionIntraProcessBufferT::MessageUniquePtr; + using MessageAllocTraits = + typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocatorTraits; + using MessageAlloc = typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocator; + using ConstMessageSharedPtr = typename SubscriptionIntraProcessBufferT::ConstDataSharedPtr; + using MessageUniquePtr = typename SubscriptionIntraProcessBufferT::SubscribedTypeUniquePtr; using BufferUniquePtr = typename SubscriptionIntraProcessBufferT::BufferUniquePtr; SubscriptionIntraProcess( - AnySubscriptionCallback callback, + AnySubscriptionCallback callback, std::shared_ptr allocator, rclcpp::Context::SharedPtr context, const std::string & topic_name, const rclcpp::QoS & qos_profile, rclcpp::IntraProcessBufferType buffer_type) - : SubscriptionIntraProcessBuffer( - allocator, + : SubscriptionIntraProcessBuffer( + std::make_shared(*allocator), context, topic_name, qos_profile, @@ -98,7 +105,7 @@ class SubscriptionIntraProcess virtual ~SubscriptionIntraProcess() = default; std::shared_ptr - take_data() + take_data() override { ConstMessageSharedPtr shared_msg; MessageUniquePtr unique_msg; @@ -115,9 +122,9 @@ class SubscriptionIntraProcess ); } - void execute(std::shared_ptr & data) + void execute(std::shared_ptr & data) override { - execute_impl(data); + execute_impl(data); } protected: @@ -154,7 +161,7 @@ class SubscriptionIntraProcess shared_ptr.reset(); } - AnySubscriptionCallback any_callback_; + AnySubscriptionCallback any_callback_; }; } // namespace experimental diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index ba575c763c..f0ee39ce47 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -52,21 +52,11 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable RCLCPP_PUBLIC size_t - get_number_of_ready_guard_conditions() {return 1;} + get_number_of_ready_guard_conditions() override {return 1;} RCLCPP_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set); - - virtual bool - is_ready(rcl_wait_set_t * wait_set) = 0; - - virtual - std::shared_ptr - take_data() = 0; - - virtual void - execute(std::shared_ptr & data) = 0; + add_to_wait_set(rcl_wait_set_t * wait_set) override; virtual bool use_take_shared_method() const = 0; @@ -83,10 +73,10 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable std::recursive_mutex reentrant_mutex_; rclcpp::GuardCondition gc_; -private: virtual void trigger_guard_condition() = 0; +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 3ec335f9f6..2e20c3de9b 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -20,8 +20,8 @@ #include #include #include -#include #include +#include #include #include "rcl/error_handling.h" @@ -30,6 +30,7 @@ #include "rclcpp/experimental/buffers/intra_process_buffer.hpp" #include "rclcpp/experimental/create_intra_process_buffer.hpp" #include "rclcpp/experimental/subscription_intra_process_base.hpp" +#include "rclcpp/experimental/ros_message_intra_process_buffer.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/waitable.hpp" @@ -41,24 +42,39 @@ namespace experimental { template< - typename MessageT, - typename Alloc = std::allocator, - typename Deleter = std::default_delete + typename SubscribedType, + typename Alloc = std::allocator, + typename Deleter = std::default_delete, + /// MessageT::ros_message_type if MessageT is a TypeAdapter, + /// otherwise just MessageT. + typename ROSMessageType = SubscribedType > -class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase +class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuffer::allocator_type, + allocator::Deleter::allocator_type, + ROSMessageType>> { public: RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcessBuffer) - using MessageAllocTraits = allocator::AllocRebind; - using MessageAlloc = typename MessageAllocTraits::allocator_type; - using ConstMessageSharedPtr = std::shared_ptr; - using MessageUniquePtr = std::unique_ptr; + using SubscribedTypeAllocatorTraits = allocator::AllocRebind; + using SubscribedTypeAllocator = typename SubscribedTypeAllocatorTraits::allocator_type; + using SubscribedTypeDeleter = allocator::Deleter; + + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + + using ConstMessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + using ConstDataSharedPtr = std::shared_ptr; + using SubscribedTypeUniquePtr = std::unique_ptr; using BufferUniquePtr = typename rclcpp::experimental::buffers::IntraProcessBuffer< - MessageT, + SubscribedType, Alloc, - Deleter + SubscribedTypeDeleter >::UniquePtr; SubscriptionIntraProcessBuffer( @@ -67,50 +83,97 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase const std::string & topic_name, const rclcpp::QoS & qos_profile, rclcpp::IntraProcessBufferType buffer_type) - : SubscriptionIntraProcessBase(context, topic_name, qos_profile) + : SubscriptionROSMsgIntraProcessBuffer( + context, topic_name, qos_profile), + subscribed_type_allocator_(*allocator) { + allocator::set_allocator_for_deleter(&subscribed_type_deleter_, &subscribed_type_allocator_); + // Create the intra-process buffer. - buffer_ = rclcpp::experimental::create_intra_process_buffer( + buffer_ = rclcpp::experimental::create_intra_process_buffer( buffer_type, qos_profile, - allocator); + std::make_shared(subscribed_type_allocator_)); } bool - is_ready(rcl_wait_set_t * wait_set) + is_ready(rcl_wait_set_t * wait_set) override { (void) wait_set; return buffer_->has_data(); } + SubscribedTypeUniquePtr + convert_ros_message_to_subscribed_type_unique_ptr(const ROSMessageType & msg) + { + if constexpr (!std::is_same::value) { + auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1); + SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr); + rclcpp::TypeAdapter::convert_to_custom(msg, *ptr); + return SubscribedTypeUniquePtr(ptr, subscribed_type_deleter_); + } else { + throw std::runtime_error( + "convert_ros_message_to_subscribed_type_unique_ptr " + "unexpectedly called without TypeAdapter"); + } + } + + void + provide_intra_process_message(ConstMessageSharedPtr message) override + { + if constexpr (std::is_same::value) { + buffer_->add_shared(std::move(message)); + trigger_guard_condition(); + } else { + buffer_->add_shared(convert_ros_message_to_subscribed_type_unique_ptr(*message)); + trigger_guard_condition(); + } + } + + void + provide_intra_process_message(MessageUniquePtr message) override + { + if constexpr (std::is_same::value) { + buffer_->add_unique(std::move(message)); + trigger_guard_condition(); + } else { + buffer_->add_unique(convert_ros_message_to_subscribed_type_unique_ptr(*message)); + trigger_guard_condition(); + } + } + void - provide_intra_process_message(ConstMessageSharedPtr message) + provide_intra_process_data(ConstDataSharedPtr message) { buffer_->add_shared(std::move(message)); trigger_guard_condition(); } void - provide_intra_process_message(MessageUniquePtr message) + provide_intra_process_data(SubscribedTypeUniquePtr message) { buffer_->add_unique(std::move(message)); trigger_guard_condition(); } bool - use_take_shared_method() const + use_take_shared_method() const override { return buffer_->use_take_shared_method(); } protected: void - trigger_guard_condition() + trigger_guard_condition() override { - gc_.trigger(); + this->gc_.trigger(); } BufferUniquePtr buffer_; + SubscribedTypeAllocator subscribed_type_allocator_; + SubscribedTypeDeleter subscribed_type_deleter_; }; } // namespace experimental diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index bebedda845..6afb397177 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -81,7 +81,6 @@ class Publisher : public PublisherBase /// MessageT::custom_type if MessageT is a TypeAdapter, otherwise just MessageT. using PublishedType = typename rclcpp::TypeAdapter::custom_type; - /// MessageT::ros_message_type if MessageT is a TypeAdapter, otherwise just MessageT. using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; using PublishedTypeAllocatorTraits = allocator::AllocRebind; @@ -263,10 +262,11 @@ class Publisher : public PublisherBase get_subscription_count() > get_intra_process_subscription_count(); if (inter_process_publish_needed) { - auto shared_msg = this->do_intra_process_publish_and_return_shared(std::move(msg)); + auto shared_msg = + this->do_intra_process_ros_message_publish_and_return_shared(std::move(msg)); this->do_inter_process_publish(*shared_msg); } else { - this->do_intra_process_publish(std::move(msg)); + this->do_intra_process_ros_message_publish(std::move(msg)); } } @@ -319,13 +319,28 @@ class Publisher : public PublisherBase > publish(std::unique_ptr msg) { - // TODO(wjwwood): later update this to give the unique_ptr to the intra - // process manager and let it decide if it needs to be converted or not. - // For now, convert it unconditionally and pass it the ROSMessageType - // publish function specialization. - auto unique_ros_msg = this->create_ros_message_unique_ptr(); - rclcpp::TypeAdapter::convert_to_ros_message(*msg, *unique_ros_msg); - this->publish(std::move(unique_ros_msg)); + // Avoid allocating when not using intra process. + if (!intra_process_is_enabled_) { + // In this case we're not using intra process. + ROSMessageType ros_msg; + rclcpp::TypeAdapter::convert_to_ros_message(*msg, ros_msg); + return this->do_inter_process_publish(ros_msg); + } + + bool inter_process_publish_needed = + get_subscription_count() > get_intra_process_subscription_count(); + + if (inter_process_publish_needed) { + ROSMessageType ros_msg; + // TODO(clalancette): This is unnecessarily doing an additional conversion + // that may have already been done in do_intra_process_publish_and_return_shared(). + // We should just reuse that effort. + rclcpp::TypeAdapter::convert_to_ros_message(*msg, ros_msg); + this->do_intra_process_publish(std::move(msg)); + this->do_inter_process_publish(ros_msg); + } else { + this->do_intra_process_publish(std::move(msg)); + } } /// Publish a message on the topic. @@ -346,12 +361,7 @@ class Publisher : public PublisherBase > publish(const T & msg) { - // TODO(wjwwood): later update this to give the unique_ptr to the intra - // process manager and let it decide if it needs to be converted or not. - // For now, convert it unconditionally and pass it the ROSMessageType - // publish function specialization. - - // Avoid allocating when not using intra process. + // Avoid double allocating when not using intra process. if (!intra_process_is_enabled_) { // Convert to the ROS message equivalent and publish it. ROSMessageType ros_msg; @@ -359,13 +369,12 @@ class Publisher : public PublisherBase // In this case we're not using intra process. return this->do_inter_process_publish(ros_msg); } - // Otherwise we have to allocate memory in a unique_ptr, convert it, - // and pass it along. + + // Otherwise we have to allocate memory in a unique_ptr and pass it along. // As the message is not const, a copy should be made. // A shared_ptr could also be constructed here. - auto unique_ros_msg = this->create_ros_message_unique_ptr(); - rclcpp::TypeAdapter::convert_to_ros_message(msg, *unique_ros_msg); - this->publish(std::move(unique_ros_msg)); + auto unique_msg = this->duplicate_type_adapt_message_as_unique_ptr(msg); + this->publish(std::move(unique_msg)); } void @@ -491,7 +500,25 @@ class Publisher : public PublisherBase } void - do_intra_process_publish(std::unique_ptr msg) + do_intra_process_publish(std::unique_ptr msg) + { + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process publish called after destruction of intra process manager"); + } + if (!msg) { + throw std::runtime_error("cannot publish msg which is a null pointer"); + } + + ipm->template do_intra_process_publish( + intra_process_publisher_id_, + std::move(msg), + published_type_allocator_); + } + + void + do_intra_process_ros_message_publish(std::unique_ptr msg) { auto ipm = weak_ipm_.lock(); if (!ipm) { @@ -502,14 +529,14 @@ class Publisher : public PublisherBase throw std::runtime_error("cannot publish msg which is a null pointer"); } - ipm->template do_intra_process_publish( + ipm->template do_intra_process_publish( intra_process_publisher_id_, std::move(msg), ros_message_type_allocator_); } std::shared_ptr - do_intra_process_publish_and_return_shared( + do_intra_process_ros_message_publish_and_return_shared( std::unique_ptr msg) { auto ipm = weak_ipm_.lock(); @@ -521,13 +548,14 @@ class Publisher : public PublisherBase throw std::runtime_error("cannot publish msg which is a null pointer"); } - return ipm->template do_intra_process_publish_and_return_sharedtemplate do_intra_process_publish_and_return_shared( intra_process_publisher_id_, std::move(msg), ros_message_type_allocator_); } + /// Return a new unique_ptr using the ROSMessageType of the publisher. std::unique_ptr create_ros_message_unique_ptr() @@ -546,6 +574,15 @@ class Publisher : public PublisherBase return std::unique_ptr(ptr, ros_message_type_deleter_); } + /// Duplicate a given type adapted message as a unique_ptr. + std::unique_ptr + duplicate_type_adapt_message_as_unique_ptr(const PublishedType & msg) + { + auto ptr = PublishedTypeAllocatorTraits::allocate(published_type_allocator_, 1); + PublishedTypeAllocatorTraits::construct(published_type_allocator_, ptr, msg); + return std::unique_ptr(ptr, published_type_deleter_); + } + /// Copy of original options passed during construction. /** * It is important to save a copy of this so that the rmw payload which it diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index a77f4f8ef1..2d52388e3b 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -276,7 +276,7 @@ class Subscription : public SubscriptionBase /// Take the next message from the inter-process subscription. /** - * This verison takes a SubscribedType which is different frmo the + * This version takes a SubscribedType which is different from the * ROSMessageType when a rclcpp::TypeAdapter is in used. * * \sa take(ROSMessageType &, rclcpp::MessageInfo &) @@ -406,10 +406,12 @@ class Subscription : public SubscriptionBase SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_{nullptr}; using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< - ROSMessageType, - AllocatorT, - ROSMessageTypeDeleter, - MessageT>; + MessageT, + SubscribedType, + SubscribedTypeAllocator, + SubscribedTypeDeleter, + ROSMessageT, + AllocatorT>; std::shared_ptr subscription_intra_process_; }; diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index d78a1adef5..34bdc0daed 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -400,6 +400,18 @@ if(TARGET test_subscription_with_type_adapter) ${cpp_typesupport_target}) endif() +ament_add_gtest(test_subscription_publisher_with_same_type_adapter test_subscription_publisher_with_same_type_adapter.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" +) +if(TARGET test_subscription_publisher_with_same_type_adapter) + ament_target_dependencies(test_subscription_publisher_with_same_type_adapter + "statistics_msgs" + ) + target_link_libraries(test_subscription_publisher_with_same_type_adapter + ${PROJECT_NAME} + ${cpp_typesupport_target}) +endif() + ament_add_gtest(test_publisher_subscription_count_api test_publisher_subscription_count_api.cpp) if(TARGET test_publisher_subscription_count_api) ament_target_dependencies(test_publisher_subscription_count_api diff --git a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp index 32abdf2934..02daee010b 100644 --- a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp +++ b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp @@ -25,6 +25,29 @@ #include "test_msgs/msg/empty.hpp" #include "test_msgs/msg/empty.h" +// Type adapter to be used in tests. +struct MyEmpty {}; + +template<> +struct rclcpp::TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = MyEmpty; + using ros_message_type = test_msgs::msg::Empty; + + static + void + convert_to_ros_message(const custom_type &, ros_message_type &) + {} + + static + void + convert_to_custom(const ros_message_type &, custom_type &) + {} +}; + +using MyTA = rclcpp::TypeAdapter; + class TestAnySubscriptionCallback : public ::testing::Test { public: @@ -43,6 +66,24 @@ class TestAnySubscriptionCallback : public ::testing::Test rclcpp::MessageInfo message_info_; }; +class TestAnySubscriptionCallbackTA : public ::testing::Test +{ +public: + TestAnySubscriptionCallbackTA() {} + + static + std::unique_ptr + get_unique_ptr_msg() + { + return std::make_unique(); + } + +protected: + rclcpp::AnySubscriptionCallback any_subscription_callback_; + std::shared_ptr msg_shared_ptr_{std::make_shared()}; + rclcpp::MessageInfo message_info_; +}; + void construct_with_null_allocator() { // suppress deprecated function warning @@ -97,29 +138,6 @@ TEST_F(TestAnySubscriptionCallback, unset_dispatch_throw) { // Parameterized test to test across all callback types and dispatch types. // -// Type adapter to be used in tests. -struct MyEmpty {}; - -template<> -struct rclcpp::TypeAdapter -{ - using is_specialized = std::true_type; - using custom_type = MyEmpty; - using ros_message_type = test_msgs::msg::Empty; - - static - void - convert_to_ros_message(const custom_type &, ros_message_type &) - {} - - static - void - convert_to_custom(const ros_message_type &, custom_type &) - {} -}; - -using MyTA = rclcpp::TypeAdapter; - template class InstanceContextImpl { @@ -177,7 +195,7 @@ class DispatchTests {}; class DispatchTestsWithTA - : public TestAnySubscriptionCallback, + : public TestAnySubscriptionCallbackTA, public ::testing::WithParamInterface> {}; @@ -193,27 +211,35 @@ format_parameter_with_ta(const ::testing::TestParamInfo as input */ \ - TEST_P(DispatchTests_name, test_inter_shared_dispatch) { \ - auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); \ - any_subscription_callback_to_test.dispatch(msg_shared_ptr_, message_info_); \ - } \ - \ - /* Testing dispatch with shared_ptr as input */ \ - TEST_P(DispatchTests_name, test_intra_shared_dispatch) { \ - auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); \ - any_subscription_callback_to_test.dispatch_intra_process(msg_shared_ptr_, message_info_); \ - } \ - \ - /* Testing dispatch with unique_ptr as input */ \ - TEST_P(DispatchTests_name, test_intra_unique_dispatch) { \ - auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); \ - any_subscription_callback_to_test.dispatch_intra_process(get_unique_ptr_msg(), message_info_); \ - } +/* Testing dispatch with shared_ptr as input */ +TEST_P(DispatchTests, test_inter_shared_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch(msg_shared_ptr_, message_info_); +} + +/* Testing dispatch with shared_ptr as input */ +TEST_P(DispatchTests, test_intra_shared_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch_intra_process(msg_shared_ptr_, message_info_); +} + +/* Testing dispatch with unique_ptr as input */ +TEST_P(DispatchTests, test_intra_unique_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch_intra_process(get_unique_ptr_msg(), message_info_); +} + +/* Testing dispatch with shared_ptr as input */ +TEST_P(DispatchTestsWithTA, test_intra_shared_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch_intra_process(msg_shared_ptr_, message_info_); +} -PARAMETERIZED_TESTS(DispatchTests) -PARAMETERIZED_TESTS(DispatchTestsWithTA) +/* Testing dispatch with unique_ptr as input */ +TEST_P(DispatchTestsWithTA, test_intra_unique_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch_intra_process(get_unique_ptr_msg(), message_info_); +} // Generic classes for testing callbacks using std::bind to class methods. template diff --git a/rclcpp/test/rclcpp/test_intra_process_manager.cpp b/rclcpp/test/rclcpp/test_intra_process_manager.cpp index 59437aa560..c78900ac1d 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager.cpp @@ -22,6 +22,7 @@ #define RCLCPP_BUILDING_LIBRARY 1 #include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/context.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/qos.hpp" #include "rmw/types.h" @@ -194,9 +195,14 @@ class SubscriptionIntraProcessBase public: RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase) - explicit SubscriptionIntraProcessBase(rclcpp::QoS qos = rclcpp::QoS(10)) - : qos_profile(qos), topic_name("topic") - {} + explicit SubscriptionIntraProcessBase( + rclcpp::Context::SharedPtr context, + const std::string & topic = "topic", + rclcpp::QoS qos = rclcpp::QoS(10)) + : qos_profile(qos), topic_name(topic) + { + (void)context; + } virtual ~SubscriptionIntraProcessBase() {} @@ -212,24 +218,26 @@ class SubscriptionIntraProcessBase const char * get_topic_name() { - return topic_name; + return topic_name.c_str(); } rclcpp::QoS qos_profile; - const char * topic_name; + std::string topic_name; }; template< typename MessageT, typename Alloc = std::allocator, - typename Deleter = std::default_delete> + typename Deleter = std::default_delete, + typename ROSMessageType = MessageT +> class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase { public: RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcessBuffer) explicit SubscriptionIntraProcessBuffer(rclcpp::QoS qos) - : SubscriptionIntraProcessBase(qos), take_shared_method(false) + : SubscriptionIntraProcessBase(nullptr, "topic", qos), take_shared_method(false) { buffer = std::make_unique>(); } @@ -246,6 +254,18 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase buffer->add(std::move(msg)); } + void + provide_intra_process_data(std::shared_ptr msg) + { + buffer->add(msg); + } + + void + provide_intra_process_data(std::unique_ptr msg) + { + buffer->add(std::move(msg)); + } + std::uintptr_t pop() { @@ -267,7 +287,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase template< typename MessageT, - typename Alloc = std::allocator, + typename Alloc = std::allocator, typename Deleter = std::default_delete> class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< MessageT, @@ -328,7 +348,7 @@ void Publisher::publish(MessageUniquePtr msg) throw std::runtime_error("cannot publish msg which is a null pointer"); } - ipm->template do_intra_process_publish( + ipm->template do_intra_process_publish( intra_process_publisher_id_, std::move(msg), *message_allocator_); diff --git a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp index f5ab43cb7c..8c6ebaee57 100644 --- a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp @@ -172,8 +172,18 @@ TEST_F(TestPublisher, conversion_exception_is_passed_up) { for (auto is_intra_process : {true, false}) { rclcpp::NodeOptions options; options.use_intra_process_comms(is_intra_process); + + auto callback = + [](const rclcpp::msg::String::ConstSharedPtr msg) -> void + { + (void)msg; + }; + initialize(options); auto pub = node->create_publisher("topic_name", 1); + // A subscription is created to ensure the existence of a buffer in the intra proccess + // manager which will trigger the faulty conversion. + auto sub = node->create_subscription("topic_name", 1, callback); EXPECT_THROW(pub->publish(1), std::runtime_error); } } diff --git a/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp b/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp new file mode 100644 index 0000000000..5cee82c3ef --- /dev/null +++ b/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp @@ -0,0 +1,991 @@ +// 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. + + +#include + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rclcpp/msg/string.hpp" +#include "statistics_msgs/msg/statistic_data_point.hpp" + +static const int g_max_loops = 200; +static const std::chrono::milliseconds g_sleep_per_loop(10); + +class test_intra_process_within_one_node : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +namespace rclcpp +{ + +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = std::string; + using ros_message_type = rclcpp::msg::String; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + (void) source; + (void) destination; + throw std::runtime_error("This should not happen"); + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + (void) source; + (void) destination; + throw std::runtime_error("This should not happen"); + } +}; + +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = double; + using ros_message_type = statistics_msgs::msg::StatisticDataPoint; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + destination.data_type = 0; + destination.data = source; + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + destination = source.data; + } +}; + +} // namespace rclcpp + +void wait_for_message_to_be_received( + bool & is_received, + const std::shared_ptr & node) +{ + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + executor.spin_once(std::chrono::milliseconds(0)); + int i = 0; + while (!is_received && i < g_max_loops) { + printf("spin_node_once() - callback (1) expected - try %d/%d\n", ++i, g_max_loops); + executor.spin_once(g_sleep_per_loop); + } +} + +/* + * Testing that subscriber receives type adapted types and ROS message types with intra proccess communications. + */ +TEST_F( + test_intra_process_within_one_node, + type_adapted_messages_unique_pub_received_by_intra_process_subscription) +{ + using StringTypeAdapter = rclcpp::TypeAdapter; + const std::string message_data = "Message Data"; + const std::string topic_name = "topic_name"; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + + auto pub = node->create_publisher(topic_name, 1); + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string &, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), msg.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string & with message info, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg, const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), msg.c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr with message info, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr &, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr & with message info, + // publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } +} + +TEST_F( + test_intra_process_within_one_node, + type_adapted_messages_const_ref_pub_received_by_intra_process_subscription) +{ + using StringTypeAdapter = rclcpp::TypeAdapter; + const std::string message_data = "Message Data"; + const std::string topic_name = "topic_name"; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + + auto pub = node->create_publisher(topic_name, 1); + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string &, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), msg.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string & with message info, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg, const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), msg.c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr with message info, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, + // publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr &, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr & with message info, + // publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } +} + +TEST_F( + test_intra_process_within_one_node, + type_adapted_messages_ros_message_ref_pub_received_by_intra_process_subscription) +{ + using DoubleTypeAdapter = rclcpp::TypeAdapter; + const double message_data = 0.894; + const std::string topic_name = "topic_name"; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + + auto pub = node->create_publisher(topic_name, 1); + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string &, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const double & msg) -> void { + is_received = true; + ASSERT_EQ(message_data, msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string & with message info, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const double & msg, const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr with message info, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, + // publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr &, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr & with message info, + // publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } +} + +TEST_F( + test_intra_process_within_one_node, + type_adapted_messages_unique_ptr_ros_message_pub_received_by_intra_process_subscription) +{ + using DoubleTypeAdapter = rclcpp::TypeAdapter; + const double message_data = 0.7508; + const std::string topic_name = "topic_name"; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + + auto pub = node->create_publisher(topic_name, 1); + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string &, publish with unique statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + const double & msg) -> void { + is_received = true; + ASSERT_EQ(message_data, msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string & with message info, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + const double & msg, const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr with message info, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr &, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr & with message info, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } +} From 5e314c253e72f5e11a6e66d004bed3d26baa199e Mon Sep 17 00:00:00 2001 From: Doug Smith Date: Tue, 11 Jan 2022 16:52:52 -0600 Subject: [PATCH 059/455] Rename stringstream in macros to a more unique name (#1862) * Rename stringstream in macros to a more unique name Signed-off-by: Doug Smith --- rclcpp/resource/logging.hpp.em | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rclcpp/resource/logging.hpp.em b/rclcpp/resource/logging.hpp.em index 228cd7cd48..7b5b0f349f 100644 --- a/rclcpp/resource/logging.hpp.em +++ b/rclcpp/resource/logging.hpp.em @@ -141,8 +141,8 @@ def get_rclcpp_suffix_from_features(features): }; \ @[ end if] \ @[ if 'stream' in feature_combination]@ - std::stringstream ss; \ - ss << @(stream_arg); \ + std::stringstream rclcpp_stream_ss_; \ + rclcpp_stream_ss_ << @(stream_arg); \ @[ end if]@ RCUTILS_LOG_@(severity)@(get_suffix_from_features(feature_combination))_NAMED( \ @{params = ['get_time_point' if p == 'clock' and 'throttle' in feature_combination else p for p in params]}@ @@ -153,7 +153,7 @@ def get_rclcpp_suffix_from_features(features): @[ if 'stream' not in feature_combination]@ __VA_ARGS__); \ @[ else]@ - "%s", ss.str().c_str()); \ + "%s", rclcpp_stream_ss_.str().c_str()); \ @[ end if]@ } while (0) From 80f93d1dbbe8398524ceec637f50ce2e3f1e9103 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 12 Jan 2022 14:24:49 -0800 Subject: [PATCH 060/455] Fix include order and relative paths for cpplint (#1859) * Fix include order and relative paths for cpplint Relates to https://github.com/ament/ament_lint/pull/324 Signed-off-by: Jacob Perron * Use double-quotes for other includes This is backwards compatible with older versions of cpplint. Signed-off-by: Jacob Perron --- .../detail/resolve_parameter_overrides.cpp | 2 +- .../executors/multi_threaded_executor.cpp | 3 +- rclcpp/src/rclcpp/signal_handler.cpp | 2 +- .../rclcpp/test_intra_process_manager.cpp | 2 +- .../include/rclcpp_action/client.hpp | 24 +++++----- .../rclcpp_action/client_goal_handle.hpp | 12 ++--- .../include/rclcpp_action/create_client.hpp | 4 +- .../include/rclcpp_action/create_server.hpp | 16 +++---- .../include/rclcpp_action/server.hpp | 16 +++---- .../rclcpp_action/server_goal_handle.hpp | 10 ++--- rclcpp_action/include/rclcpp_action/types.hpp | 10 ++--- rclcpp_action/src/client.cpp | 10 ++--- rclcpp_action/src/server.cpp | 20 ++++----- rclcpp_action/src/server_goal_handle.cpp | 10 ++--- rclcpp_action/test/test_client.cpp | 44 +++++++++---------- rclcpp_action/test/test_server.cpp | 18 ++++---- .../test/test_server_goal_handle.cpp | 16 +++---- 17 files changed, 110 insertions(+), 109 deletions(-) diff --git a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp index 15f567f7c5..a62121b37f 100644 --- a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp +++ b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "./resolve_parameter_overrides.hpp" +#include "resolve_parameter_overrides.hpp" #include #include diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index 849815fed5..bb477690be 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -72,8 +72,9 @@ MultiThreadedExecutor::get_number_of_threads() } void -MultiThreadedExecutor::run(size_t) +MultiThreadedExecutor::run(size_t this_thread_number) { + (void)this_thread_number; while (rclcpp::ok(this->context_) && spinning.load()) { rclcpp::AnyExecutable any_exec; { diff --git a/rclcpp/src/rclcpp/signal_handler.cpp b/rclcpp/src/rclcpp/signal_handler.cpp index 55ef988786..35ceb56781 100644 --- a/rclcpp/src/rclcpp/signal_handler.cpp +++ b/rclcpp/src/rclcpp/signal_handler.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "./signal_handler.hpp" +#include "signal_handler.hpp" #include #include diff --git a/rclcpp/test/rclcpp/test_intra_process_manager.cpp b/rclcpp/test/rclcpp/test_intra_process_manager.cpp index c78900ac1d..45d916b004 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager.cpp @@ -321,7 +321,7 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< #define SubscriptionIntraProcessBase mock::SubscriptionIntraProcessBase #define SubscriptionIntraProcessBuffer mock::SubscriptionIntraProcessBuffer #define SubscriptionIntraProcess mock::SubscriptionIntraProcess -#include "../src/rclcpp/intra_process_manager.cpp" +#include "../src/rclcpp/intra_process_manager.cpp" // NOLINT #undef Publisher #undef PublisherBase #undef IntraProcessBuffer diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index ed590247d9..a4ba5726e1 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -15,18 +15,6 @@ #ifndef RCLCPP_ACTION__CLIENT_HPP_ #define RCLCPP_ACTION__CLIENT_HPP_ -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - #include #include #include @@ -37,6 +25,18 @@ #include #include +#include "rclcpp/exceptions.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/waitable.hpp" + +#include "rosidl_runtime_c/action_type_support_struct.h" +#include "rosidl_typesupport_cpp/action_type_support.hpp" + #include "rclcpp_action/client_goal_handle.hpp" #include "rclcpp_action/exceptions.hpp" #include "rclcpp_action/types.hpp" diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index e2861e3e79..4989a71bbc 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -15,17 +15,17 @@ #ifndef RCLCPP_ACTION__CLIENT_GOAL_HANDLE_HPP_ #define RCLCPP_ACTION__CLIENT_GOAL_HANDLE_HPP_ -#include - -#include -#include -#include - #include #include #include #include +#include "rcl_action/action_client.h" + +#include "action_msgs/msg/goal_status.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/time.hpp" + #include "rclcpp_action/exceptions.hpp" #include "rclcpp_action/types.hpp" #include "rclcpp_action/visibility_control.hpp" diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index 8538a8205e..f594bca78d 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -15,11 +15,11 @@ #ifndef RCLCPP_ACTION__CREATE_CLIENT_HPP_ #define RCLCPP_ACTION__CREATE_CLIENT_HPP_ -#include - #include #include +#include "rclcpp/node.hpp" + #include "rclcpp_action/client.hpp" #include "rclcpp_action/visibility_control.hpp" diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index 9116fc05ee..3aa0e4dea7 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -15,17 +15,17 @@ #ifndef RCLCPP_ACTION__CREATE_SERVER_HPP_ #define RCLCPP_ACTION__CREATE_SERVER_HPP_ -#include - -#include -#include -#include -#include -#include - #include #include +#include "rcl_action/action_server.h" + +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_waitables_interface.hpp" + #include "rclcpp_action/server.hpp" #include "rclcpp_action/visibility_control.hpp" diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 63c99d51e0..2f79bee041 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -15,14 +15,6 @@ #ifndef RCLCPP_ACTION__SERVER_HPP_ #define RCLCPP_ACTION__SERVER_HPP_ -#include -#include -#include -#include -#include -#include -#include - #include #include #include @@ -30,6 +22,14 @@ #include #include +#include "rcl_action/action_server.h" +#include "rosidl_runtime_c/action_type_support_struct.h" +#include "rosidl_typesupport_cpp/action_type_support.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/waitable.hpp" + #include "rclcpp_action/visibility_control.hpp" #include "rclcpp_action/server_goal_handle.hpp" #include "rclcpp_action/types.hpp" diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index d41b32eb4f..857ca7332a 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -15,15 +15,15 @@ #ifndef RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_ #define RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_ -#include -#include - -#include - #include #include #include +#include "rcl_action/types.h" +#include "rcl_action/goal_handle.h" + +#include "action_msgs/msg/goal_status.hpp" + #include "rclcpp_action/visibility_control.hpp" #include "rclcpp_action/types.hpp" diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index addf4a8307..09717b16ea 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -15,15 +15,15 @@ #ifndef RCLCPP_ACTION__TYPES_HPP_ #define RCLCPP_ACTION__TYPES_HPP_ -#include - -#include -#include - #include #include #include +#include "rcl_action/types.h" + +#include "action_msgs/msg/goal_status.hpp" +#include "action_msgs/msg/goal_info.hpp" + #include "rclcpp_action/visibility_control.hpp" namespace rclcpp_action diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 2f39a50e52..cedab99e92 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -12,11 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include - #include #include #include @@ -25,6 +20,11 @@ #include #include +#include "rcl_action/action_client.h" +#include "rcl_action/wait.h" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" + #include "rclcpp_action/client.hpp" #include "rclcpp_action/exceptions.hpp" diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index e4765e9461..5ec88281da 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -12,16 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include - -#include - -#include -#include -#include -#include - #include #include #include @@ -30,6 +20,16 @@ #include #include +#include "rcl_action/action_server.h" +#include "rcl_action/wait.h" + +#include "rcpputils/scope_exit.hpp" + +#include "action_msgs/msg/goal_status_array.hpp" +#include "action_msgs/srv/cancel_goal.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp_action/server.hpp" + using rclcpp_action::ServerBase; using rclcpp_action::GoalUUID; diff --git a/rclcpp_action/src/server_goal_handle.cpp b/rclcpp_action/src/server_goal_handle.cpp index f0709e19cb..7df0ebf0e1 100644 --- a/rclcpp_action/src/server_goal_handle.cpp +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include -#include -#include +#include "rcl_action/action_server.h" +#include "rcl_action/goal_handle.h" -#include +#include "rclcpp_action/server_goal_handle.hpp" +#include "rclcpp/exceptions.hpp" namespace rclcpp_action { diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index b911e3d71b..b99aa236bd 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -12,27 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - #include #include #include @@ -41,13 +20,34 @@ #include #include +#include "gtest/gtest.h" + +#include "rcl/allocator.h" +#include "rcl/time.h" +#include "rcl/types.h" + +#include "rcl_action/names.h" +#include "rcl_action/default_qos.h" +#include "rcl_action/wait.h" + +#include "rclcpp/clock.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/time.hpp" + +#include "test_msgs/action/fibonacci.hpp" + #include "rclcpp_action/exceptions.hpp" #include "rclcpp_action/create_client.hpp" #include "rclcpp_action/client.hpp" #include "rclcpp_action/qos.hpp" #include "rclcpp_action/types.hpp" -#include "./mocking_utils/patch.hpp" +#include "mocking_utils/patch.hpp" using namespace std::chrono_literals; diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 25b38db34c..8687f90fbe 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -12,22 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include -#include - -#include - #include #include +#include "gtest/gtest.h" + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rcpputils/scope_exit.hpp" +#include "test_msgs/action/fibonacci.hpp" + #include "rcl_action/action_server.h" #include "rcl_action/wait.h" #include "rclcpp_action/create_server.hpp" #include "rclcpp_action/server.hpp" -#include "./mocking_utils/patch.hpp" +#include "mocking_utils/patch.hpp" using Fibonacci = test_msgs::action::Fibonacci; using CancelResponse = typename Fibonacci::Impl::CancelGoalService::Response; diff --git a/rclcpp_action/test/test_server_goal_handle.cpp b/rclcpp_action/test/test_server_goal_handle.cpp index cd56ba4a8f..eb392c8bf7 100644 --- a/rclcpp_action/test/test_server_goal_handle.cpp +++ b/rclcpp_action/test/test_server_goal_handle.cpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include - -#include - #include #include +#include "gtest/gtest.h" + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "test_msgs/action/fibonacci.hpp" + #include "action_msgs/msg/goal_info.h" #include "rclcpp_action/server_goal_handle.hpp" -#include "./mocking_utils/patch.hpp" +#include "mocking_utils/patch.hpp" class FibonacciServerGoalHandle : public rclcpp_action::ServerGoalHandle From 1688f05aead3968bcea6cab27b346260f633f0b0 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 14 Jan 2022 08:23:16 -0500 Subject: [PATCH 061/455] Cleanup includes. (#1857) Remove ones that aren't needed, and add in ones that are actually needed in the respective files. Signed-off-by: Chris Lalancette --- .../experimental/buffers/intra_process_buffer.hpp | 1 + .../buffers/ring_buffer_implementation.hpp | 4 ---- .../experimental/create_intra_process_buffer.hpp | 4 +--- .../include/rclcpp/experimental/executable_list.hpp | 1 - .../rclcpp/experimental/intra_process_manager.hpp | 8 ++------ .../experimental/subscription_intra_process.hpp | 11 ++++------- .../experimental/subscription_intra_process_base.hpp | 7 +------ .../subscription_intra_process_buffer.hpp | 9 ++------- rclcpp/include/rclcpp/node_interfaces/node_topics.hpp | 6 +++++- rclcpp/include/rclcpp/publisher.hpp | 2 ++ rclcpp/include/rclcpp/subscription_factory.hpp | 3 +-- rclcpp/src/rclcpp/node_interfaces/node_topics.cpp | 9 +++++++++ 12 files changed, 28 insertions(+), 37 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp index aef38a028d..c2e747e774 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp @@ -16,6 +16,7 @@ #define RCLCPP__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_ #include +#include #include #include diff --git a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp index ad01946193..c01240b429 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp @@ -15,10 +15,6 @@ #ifndef RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ #define RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ -#include -#include -#include -#include #include #include #include diff --git a/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp index 1d232d14fe..4d7668b964 100644 --- a/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp @@ -16,11 +16,9 @@ #define RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_ #include -#include +#include #include -#include "rcl/subscription.h" - #include "rclcpp/experimental/buffers/intra_process_buffer.hpp" #include "rclcpp/experimental/buffers/ring_buffer_implementation.hpp" #include "rclcpp/intra_process_buffer_type.hpp" diff --git a/rclcpp/include/rclcpp/experimental/executable_list.hpp b/rclcpp/include/rclcpp/experimental/executable_list.hpp index e1c70db3de..887dd6cd8f 100644 --- a/rclcpp/include/rclcpp/experimental/executable_list.hpp +++ b/rclcpp/include/rclcpp/experimental/executable_list.hpp @@ -15,7 +15,6 @@ #ifndef RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_ #define RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_ -#include #include #include "rclcpp/client.hpp" diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 0950669e9a..d8054be2d4 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -19,13 +19,9 @@ #include -#include -#include -#include -#include -#include +#include #include -#include +#include #include #include #include diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 54857562d0..803d940086 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -15,25 +15,22 @@ #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ -#include +#include -#include -#include #include #include #include +#include #include -#include "rcl/error_handling.h" +#include "rcl/types.h" #include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/context.hpp" #include "rclcpp/experimental/buffers/intra_process_buffer.hpp" -#include "rclcpp/experimental/create_intra_process_buffer.hpp" -#include "rclcpp/experimental/subscription_intra_process_base.hpp" #include "rclcpp/experimental/subscription_intra_process_buffer.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" -#include "rclcpp/waitable.hpp" #include "tracetools/tracetools.h" namespace rclcpp diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index f0ee39ce47..531e4c70e0 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -15,19 +15,14 @@ #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ -#include - -#include #include #include #include -#include -#include "rcl/error_handling.h" +#include "rcl/wait.h" #include "rclcpp/guard_condition.hpp" #include "rclcpp/qos.hpp" -#include "rclcpp/type_support_decl.hpp" #include "rclcpp/waitable.hpp" namespace rclcpp diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 2e20c3de9b..ecb02f501d 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -15,26 +15,21 @@ #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_ #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_ -#include - -#include -#include #include #include #include #include #include "rcl/error_handling.h" +#include "rcl/guard_condition.h" +#include "rcl/wait.h" -#include "rclcpp/any_subscription_callback.hpp" #include "rclcpp/experimental/buffers/intra_process_buffer.hpp" #include "rclcpp/experimental/create_intra_process_buffer.hpp" #include "rclcpp/experimental/subscription_intra_process_base.hpp" #include "rclcpp/experimental/ros_message_intra_process_buffer.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" -#include "rclcpp/waitable.hpp" -#include "tracetools/tracetools.h" namespace rclcpp { diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp index b4d8c5b2f6..33a87732d6 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp @@ -20,12 +20,16 @@ #include "rcl/publisher.h" #include "rcl/subscription.h" +#include "rclcpp/callback_group.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" -#include "rclcpp/publisher.hpp" +#include "rclcpp/publisher_base.hpp" #include "rclcpp/publisher_factory.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/subscription_factory.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 6afb397177..78b5700611 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -20,12 +20,14 @@ #include #include #include +#include #include #include "rcl/error_handling.h" #include "rcl/publisher.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" +#include "rosidl_runtime_cpp/traits.hpp" #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index 4da6c236bf..a1727eab5a 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -26,14 +26,13 @@ #include "rclcpp/any_subscription_callback.hpp" #include "rclcpp/get_message_type_support_handle.hpp" -#include "rclcpp/intra_process_buffer_type.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/subscription_options.hpp" #include "rclcpp/subscription_traits.hpp" -#include "rclcpp/visibility_control.hpp" #include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" +#include "rclcpp/visibility_control.hpp" namespace rclcpp { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index fa188a2f9a..284795ef6e 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -14,9 +14,18 @@ #include "rclcpp/node_interfaces/node_topics.hpp" +#include #include +#include "rclcpp/callback_group.hpp" #include "rclcpp/exceptions.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" +#include "rclcpp/publisher_base.hpp" +#include "rclcpp/publisher_factory.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/subscription_factory.hpp" +#include "rclcpp/qos.hpp" using rclcpp::exceptions::throw_from_rcl_error; From aa18ef51cc289c3ecdb1e6373f347edc2c808530 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 14 Jan 2022 14:38:13 -0500 Subject: [PATCH 062/455] Cleanup the TypeAdapt tests (#1858) * Cleanup test_publisher_with_type_adapter. It had a lot of infrastructure that it didn't need, so remove most of that. Also move the creation of the node to open-coded, since we weren't really saving anything. Finally make sure to reset the node pointers as appropriate, which cleans up errant error messages. * Cleanup test_subscription_with_type_adapter. It had a lot of infrastructure that it didn't need, so remove most of that. Also move the creation of the node to open-coded, since we weren't really saving anything. Finally make sure to reset the node pointers as appropriate, which cleans up errant error messages. Signed-off-by: Chris Lalancette --- rclcpp/test/rclcpp/CMakeLists.txt | 18 -------- .../test_publisher_with_type_adapter.cpp | 46 ++----------------- .../test_subscription_with_type_adapter.cpp | 46 ++----------------- 3 files changed, 8 insertions(+), 102 deletions(-) diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 34bdc0daed..331c39fb27 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -368,17 +368,8 @@ ament_add_gtest(test_publisher_with_type_adapter test_publisher_with_type_adapte APPEND_LIBRARY_DIRS "${append_library_dirs}" ) if(TARGET test_publisher_with_type_adapter) - ament_target_dependencies(test_publisher_with_type_adapter - "rcutils" - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) target_link_libraries(test_publisher_with_type_adapter ${PROJECT_NAME} - mimick ${cpp_typesupport_target}) endif() @@ -386,17 +377,8 @@ ament_add_gtest(test_subscription_with_type_adapter test_subscription_with_type_ APPEND_LIBRARY_DIRS "${append_library_dirs}" ) if(TARGET test_subscription_with_type_adapter) - ament_target_dependencies(test_subscription_with_type_adapter - "rcutils" - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) target_link_libraries(test_subscription_with_type_adapter ${PROJECT_NAME} - mimick ${cpp_typesupport_target}) endif() diff --git a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp index 8c6ebaee57..c041d86aee 100644 --- a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp @@ -16,32 +16,18 @@ #include #include -#include #include #include #include #include -#include #include "rclcpp/exceptions.hpp" #include "rclcpp/loaned_message.hpp" #include "rclcpp/rclcpp.hpp" -#include "../mocking_utils/patch.hpp" -#include "../utils/rclcpp_gtest_macros.hpp" - -#include "test_msgs/msg/empty.hpp" #include "rclcpp/msg/string.hpp" -#ifdef RMW_IMPLEMENTATION -# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX -# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) -#else -# define CLASSNAME(NAME, SUFFIX) NAME -#endif - - using namespace std::chrono_literals; static const int g_max_loops = 200; @@ -58,28 +44,6 @@ class TestPublisher : public ::testing::Test } } -protected: - void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) - { - node = std::make_shared("my_node", "/ns", node_options); - } - - void TearDown() - { - node.reset(); - } - - rclcpp::Node::SharedPtr node; -}; - -class CLASSNAME (test_intra_process_within_one_node, RMW_IMPLEMENTATION) : public ::testing::Test -{ -public: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - static void TearDownTestCase() { rclcpp::shutdown(); @@ -128,7 +92,7 @@ struct TypeAdapter { (void) source; (void) destination; - throw std::runtime_error("This should happen"); + throw std::runtime_error("This should not happen"); } static void @@ -150,7 +114,7 @@ TEST_F(TestPublisher, various_creation_signatures) { for (auto is_intra_process : {true, false}) { rclcpp::NodeOptions options; options.use_intra_process_comms(is_intra_process); - initialize(options); + auto node = std::make_shared("my_node", "/ns", options); { using StringTypeAdapter = rclcpp::TypeAdapter; auto publisher = node->create_publisher("topic", 42); @@ -179,7 +143,7 @@ TEST_F(TestPublisher, conversion_exception_is_passed_up) { (void)msg; }; - initialize(options); + auto node = std::make_shared("my_node", "/ns", options); auto pub = node->create_publisher("topic_name", 1); // A subscription is created to ensure the existence of a buffer in the intra proccess // manager which will trigger the faulty conversion. @@ -192,7 +156,7 @@ TEST_F(TestPublisher, conversion_exception_is_passed_up) { * Testing that publisher sends type adapted types and ROS message types with intra proccess communications. */ TEST_F( - CLASSNAME(test_intra_process_within_one_node, RMW_IMPLEMENTATION), + TestPublisher, check_type_adapted_message_is_sent_and_received_intra_process) { using StringTypeAdapter = rclcpp::TypeAdapter; const std::string message_data = "Message Data"; @@ -281,7 +245,7 @@ TEST_F( TEST_F(TestPublisher, check_type_adapted_message_is_sent_and_received) { using StringTypeAdapter = rclcpp::TypeAdapter; - initialize(); + auto node = std::make_shared("my_node", "/ns", rclcpp::NodeOptions()); const std::string message_data = "Message Data"; const std::string topic_name = "topic_name"; diff --git a/rclcpp/test/rclcpp/test_subscription_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_subscription_with_type_adapter.cpp index 7668feadb0..8e74592f9e 100644 --- a/rclcpp/test/rclcpp/test_subscription_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_subscription_with_type_adapter.cpp @@ -16,32 +16,16 @@ #include #include -#include #include #include #include -#include -#include #include "rclcpp/exceptions.hpp" -#include "rclcpp/loaned_message.hpp" #include "rclcpp/rclcpp.hpp" -#include "../mocking_utils/patch.hpp" -#include "../utils/rclcpp_gtest_macros.hpp" - -#include "test_msgs/msg/empty.hpp" #include "rclcpp/msg/string.hpp" -#ifdef RMW_IMPLEMENTATION -# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX -# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) -#else -# define CLASSNAME(NAME, SUFFIX) NAME -#endif - - using namespace std::chrono_literals; static const int g_max_loops = 200; @@ -50,30 +34,6 @@ static const std::chrono::milliseconds g_sleep_per_loop(10); class TestSubscription : public ::testing::Test { -public: - static void SetUpTestCase() - { - if (!rclcpp::ok()) { - rclcpp::init(0, nullptr); - } - } - -protected: - void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) - { - node = std::make_shared("my_node", "/ns", node_options); - } - - void TearDown() - { - node.reset(); - } - - rclcpp::Node::SharedPtr node; -}; - -class CLASSNAME (test_intra_process_within_one_node, RMW_IMPLEMENTATION) : public ::testing::Test -{ public: static void SetUpTestCase() { @@ -148,7 +108,7 @@ bool wait_for_match( * Testing publisher creation signatures with a type adapter. */ TEST_F(TestSubscription, various_creation_signatures) { - initialize(); + auto node = std::make_shared("my_node", "/ns", rclcpp::NodeOptions()); { using StringTypeAdapter = rclcpp::TypeAdapter; auto sub = @@ -167,7 +127,7 @@ TEST_F(TestSubscription, various_creation_signatures) { * Testing that subscriber receives type adapted types and ROS message types with intra proccess communications. */ TEST_F( - CLASSNAME(test_intra_process_within_one_node, RMW_IMPLEMENTATION), + TestSubscription, check_type_adapted_messages_are_received_by_intra_process_subscription) { using StringTypeAdapter = rclcpp::TypeAdapter; const std::string message_data = "Message Data"; @@ -386,7 +346,7 @@ TEST_F( * Testing that subscriber receives type adapted types and ROS message types with inter proccess communications. */ TEST_F( - CLASSNAME(test_intra_process_within_one_node, RMW_IMPLEMENTATION), + TestSubscription, check_type_adapted_messages_are_received_by_inter_process_subscription) { using StringTypeAdapter = rclcpp::TypeAdapter; const std::string message_data = "Message Data"; From 3123f5a64376588d07800579cb36cbdc7ffe82ee Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 14 Jan 2022 18:00:06 -0300 Subject: [PATCH 063/455] Automatically transition lifecycle entities when node transitions (#1863) * Automatically transition LifecyclePublisher(s) between activated and inactive Signed-off-by: Ivan Santiago Paunovic * Fix: Add created publishers to the managed entities vector Signed-off-by: Ivan Santiago Paunovic * enabled_ -> activated_ Signed-off-by: Ivan Santiago Paunovic * Continue setting should_log_ as before Signed-off-by: Ivan Santiago Paunovic * Fix visibility attributes so it works on Windows Signed-off-by: Ivan Santiago Paunovic --- rclcpp_lifecycle/CMakeLists.txt | 1 + .../rclcpp_lifecycle/lifecycle_node.hpp | 10 ++- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 4 +- .../rclcpp_lifecycle/lifecycle_publisher.hpp | 47 +++---------- .../rclcpp_lifecycle/managed_entity.hpp | 68 +++++++++++++++++++ rclcpp_lifecycle/src/lifecycle_node.cpp | 20 +++++- .../src/lifecycle_node_interface_impl.hpp | 28 +++++++- rclcpp_lifecycle/src/managed_entity.cpp | 35 ++++++++++ .../test/test_lifecycle_publisher.cpp | 2 +- 9 files changed, 168 insertions(+), 47 deletions(-) create mode 100644 rclcpp_lifecycle/include/rclcpp_lifecycle/managed_entity.hpp create mode 100644 rclcpp_lifecycle/src/managed_entity.cpp diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index e73a5a3ffd..d635d54133 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(lifecycle_msgs REQUIRED) ### CPP High level library add_library(rclcpp_lifecycle src/lifecycle_node.cpp + src/managed_entity.cpp src/node_interfaces/lifecycle_node_interface.cpp src/state.cpp src/transition.cpp diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 4442304c0c..b091ea9c00 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -968,10 +968,18 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, bool register_on_error(std::function fcn); + RCLCPP_LIFECYCLE_PUBLIC + CallbackReturn + on_activate(const State & previous_state) override; + + RCLCPP_LIFECYCLE_PUBLIC + CallbackReturn + on_deactivate(const State & previous_state) override; + protected: RCLCPP_LIFECYCLE_PUBLIC void - add_publisher_handle(std::shared_ptr pub); + add_managed_entity(std::weak_ptr managed_entity); RCLCPP_LIFECYCLE_PUBLIC void diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index a7f1c686bc..22fd7f9c08 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -49,11 +49,13 @@ LifecycleNode::create_publisher( const rclcpp::PublisherOptionsWithAllocator & options) { using PublisherT = rclcpp_lifecycle::LifecyclePublisher; - return rclcpp::create_publisher( + auto pub = rclcpp::create_publisher( *this, topic_name, qos, options); + this->add_managed_entity(pub); + return pub; } // TODO(karsten1987): Create LifecycleSubscriber diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp index d1b5f80975..8df08dcb41 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -19,35 +19,20 @@ #include #include +#include "rclcpp/logging.hpp" #include "rclcpp/publisher.hpp" -#include "rclcpp/logging.hpp" +#include "rclcpp_lifecycle/managed_entity.hpp" + namespace rclcpp_lifecycle { -/// base class with only -/** - * pure virtual functions. A managed - * node can then deactivate or activate - * the publishing. - * It is more a convenient interface class - * than a necessary base class. - */ -class LifecyclePublisherInterface -{ -public: - virtual ~LifecyclePublisherInterface() {} - virtual void on_activate() = 0; - virtual void on_deactivate() = 0; - virtual bool is_activated() = 0; -}; - /// brief child class of rclcpp Publisher class. /** * Overrides all publisher functions to check for enabled/disabled state. */ template> -class LifecyclePublisher : public LifecyclePublisherInterface, +class LifecyclePublisher : public SimpleManagedEntity, public rclcpp::Publisher { public: @@ -64,7 +49,6 @@ class LifecyclePublisher : public LifecyclePublisherInterface, const rclcpp::QoS & qos, const rclcpp::PublisherOptionsWithAllocator & options) : rclcpp::Publisher(node_base, topic, qos, options), - enabled_(false), should_log_(true), logger_(rclcpp::get_logger("LifecyclePublisher")) { @@ -81,7 +65,7 @@ class LifecyclePublisher : public LifecyclePublisherInterface, virtual void publish(std::unique_ptr msg) { - if (!enabled_) { + if (!this->is_activated()) { log_publisher_not_enabled(); return; } @@ -97,32 +81,20 @@ class LifecyclePublisher : public LifecyclePublisherInterface, virtual void publish(const MessageT & msg) { - if (!enabled_) { + if (!this->is_activated()) { log_publisher_not_enabled(); return; } rclcpp::Publisher::publish(msg); } - virtual void - on_activate() - { - enabled_ = true; - } - - virtual void - on_deactivate() + void + on_activate() override { - enabled_ = false; + SimpleManagedEntity::on_activate(); should_log_ = true; } - virtual bool - is_activated() - { - return enabled_; - } - private: /// LifecyclePublisher log helper function /** @@ -146,7 +118,6 @@ class LifecyclePublisher : public LifecyclePublisherInterface, should_log_ = false; } - bool enabled_ = false; bool should_log_ = true; rclcpp::Logger logger_; }; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/managed_entity.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/managed_entity.hpp new file mode 100644 index 0000000000..4f83411e80 --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/managed_entity.hpp @@ -0,0 +1,68 @@ +// Copyright 2022 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_LIFECYCLE__MANAGED_ENTITY_HPP_ +#define RCLCPP_LIFECYCLE__MANAGED_ENTITY_HPP_ + +#include + +#include "rclcpp_lifecycle/visibility_control.h" + +namespace rclcpp_lifecycle +{ + +/// Base class for lifecycle entities, like `LifecyclePublisher`. +class ManagedEntityInterface +{ +public: + RCLCPP_LIFECYCLE_PUBLIC + virtual + ~ManagedEntityInterface() {} + + RCLCPP_LIFECYCLE_PUBLIC + virtual + void + on_activate() = 0; + + RCLCPP_LIFECYCLE_PUBLIC + virtual + void + on_deactivate() = 0; +}; + +/// A simple implementation of `ManagedEntityInterface`, which toogles a flag. +class SimpleManagedEntity : public ManagedEntityInterface +{ +public: + RCLCPP_LIFECYCLE_PUBLIC + ~SimpleManagedEntity() override = default; + + RCLCPP_LIFECYCLE_PUBLIC + void + on_activate() override; + + RCLCPP_LIFECYCLE_PUBLIC + void + on_deactivate() override; + + RCLCPP_LIFECYCLE_PUBLIC + bool + is_activated() const; + +private: + std::atomic activated_ = false; +}; + +} // namespace rclcpp_lifecycle +#endif // RCLCPP_LIFECYCLE__MANAGED_ENTITY_HPP_ diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index d2bd49bcf7..c012e50804 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -639,11 +639,25 @@ LifecycleNode::shutdown(LifecycleNodeInterface::CallbackReturn & cb_return_code) rcl_lifecycle_shutdown_label, cb_return_code); } +node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleNode::on_activate(const State &) +{ + impl_->on_activate(); + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleNode::on_deactivate(const State &) +{ + impl_->on_deactivate(); + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + void -LifecycleNode::add_publisher_handle( - std::shared_ptr pub) +LifecycleNode::add_managed_entity( + std::weak_ptr managed_entity) { - impl_->add_publisher_handle(pub); + impl_->add_managed_entity(managed_entity); } void diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 9f70d2c259..0fca4c19aa 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -501,9 +501,9 @@ class LifecycleNode::LifecycleNodeInterfaceImpl } void - add_publisher_handle(std::shared_ptr pub) + add_managed_entity(std::weak_ptr managed_entity) { - weak_pubs_.push_back(pub); + weak_managed_entities_.push_back(managed_entity); } void @@ -512,6 +512,28 @@ class LifecycleNode::LifecycleNodeInterfaceImpl weak_timers_.push_back(timer); } + void + on_activate() + { + for (const auto & weak_entity : weak_managed_entities_) { + auto entity = weak_entity.lock(); + if (entity) { + entity->on_activate(); + } + } + } + + void + on_deactivate() + { + for (const auto & weak_entity : weak_managed_entities_) { + auto entity = weak_entity.lock(); + if (entity) { + entity->on_activate(); + } + } + } + rcl_lifecycle_state_machine_t state_machine_; State current_state_; std::map< @@ -538,7 +560,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl GetTransitionGraphSrvPtr srv_get_transition_graph_; // to controllable things - std::vector> weak_pubs_; + std::vector> weak_managed_entities_; std::vector> weak_timers_; }; diff --git a/rclcpp_lifecycle/src/managed_entity.cpp b/rclcpp_lifecycle/src/managed_entity.cpp new file mode 100644 index 0000000000..8cb8fb46ec --- /dev/null +++ b/rclcpp_lifecycle/src/managed_entity.cpp @@ -0,0 +1,35 @@ +// Copyright 2022 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. + +#include "rclcpp_lifecycle/managed_entity.hpp" + +namespace rclcpp_lifecycle +{ + +void SimpleManagedEntity::on_activate() +{ + activated_.store(true); +} + +void SimpleManagedEntity::on_deactivate() +{ + activated_.store(false); +} + +bool SimpleManagedEntity::is_activated() const +{ + return activated_.load(); +} + +} // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp index 555354a8c3..278b1f9026 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp @@ -46,7 +46,7 @@ class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode publisher_ = std::make_shared>( get_node_base_interface().get(), std::string("topic"), rclcpp::QoS(10), options); - add_publisher_handle(publisher_); + add_managed_entity(publisher_); // For coverage this is being added here auto timer = create_wall_timer(std::chrono::seconds(1), []() {}); From 8afef51cfda177b18ba8d5bb679ca84cfd809dcd Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 14 Jan 2022 17:49:37 -0800 Subject: [PATCH 064/455] 15.0.0 --- rclcpp/CHANGELOG.rst | 10 ++++++++++ rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 5 +++++ rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 5 +++++ rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 5 +++++ rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 29 insertions(+), 4 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 29372f7a82..e71a1ab69a 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +15.0.0 (2022-01-14) +------------------- +* Cleanup the TypeAdapt tests (`#1858 `_) +* Cleanup includes (`#1857 `_) +* Fix include order and relative paths for cpplint (`#1859 `_) +* Rename stringstream in macros to a more unique name (`#1862 `_) +* Add non transform capabilities for intra-process (`#1849 `_) +* Fix rclcpp documentation build (`#1779 `_) +* Contributors: Chris Lalancette, Doug Smith, Gonzo, Jacob Perron, Michel Hidalgo + 14.1.0 (2022-01-05) ------------------- * Use UninitializedStaticallyTypedParameterException (`#1689 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 30a915c0a5..52cd61807b 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 14.1.0 + 15.0.0 The ROS client library in C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 7d5c727ac2..8b9312df43 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +15.0.0 (2022-01-14) +------------------- +* Fix include order and relative paths for cpplint (`#1859 `_) +* Contributors: Jacob Perron + 14.1.0 (2022-01-05) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 692e1de1fb..73ec8bfee7 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 14.1.0 + 15.0.0 Adds action APIs for C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index c7083506a9..6373079eb5 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +15.0.0 (2022-01-14) +------------------- +* Add rclcpp_components::component (`#1855 `_) +* Contributors: Shane Loretz + 14.1.0 (2022-01-05) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 3c4ded1cf0..b1d03eaa51 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 14.1.0 + 15.0.0 Package containing tools for dynamically loadable components Ivan Paunovic Jacob Perron diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index a15fc96cb3..3a355582c6 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +15.0.0 (2022-01-14) +------------------- +* Automatically transition lifecycle entities when node transitions (`#1863 `_) +* Contributors: Ivan Santiago Paunovic + 14.1.0 (2022-01-05) ------------------- diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index e827de98bf..e708c8c79a 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 14.1.0 + 15.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Jacob Perron From bca3fd7da18e779a6c1aee118440c962503ee6b2 Mon Sep 17 00:00:00 2001 From: gezp Date: Wed, 19 Jan 2022 21:29:33 +0800 Subject: [PATCH 065/455] add use_global_arguments for node options of component nodes (#1776) * add use_global_arguments for node options of component nodes Signed-off-by: zhenpeng ge keep use_global_arguments false default Signed-off-by: zhenpeng ge update warning message Signed-off-by: zhenpeng ge update warnning message Signed-off-by: zhenpeng ge add test Signed-off-by: zhenpeng ge * use forward_global_arguments instead of use_global_arguments Signed-off-by: zhenpeng ge --- rclcpp_components/src/component_manager.cpp | 12 ++++ .../test/test_component_manager_api.cpp | 55 +++++++++++++++++-- 2 files changed, 63 insertions(+), 4 deletions(-) diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index ec69ed4b68..036350a64f 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -172,6 +172,18 @@ ComponentManager::create_node_options(const std::shared_ptr r "Extra component argument 'use_intra_process_comms' must be a boolean"); } options.use_intra_process_comms(extra_argument.get_value()); + } else if (extra_argument.get_name() == "forward_global_arguments") { + if (extra_argument.get_type() != rclcpp::ParameterType::PARAMETER_BOOL) { + throw ComponentManagerException( + "Extra component argument 'forward_global_arguments' must be a boolean"); + } + options.use_global_arguments(extra_argument.get_value()); + if (extra_argument.get_value()) { + RCLCPP_WARN( + get_logger(), "forward_global_arguments is true by default in nodes, but is not " + "recommended in a component manager. If true, this will cause this node's behavior " + "to be influenced by global arguments, not only those targeted at this node."); + } } } diff --git a/rclcpp_components/test/test_component_manager_api.cpp b/rclcpp_components/test/test_component_manager_api.cpp index f1a4700e6e..dfb9db76a2 100644 --- a/rclcpp_components/test/test_component_manager_api.cpp +++ b/rclcpp_components/test/test_component_manager_api.cpp @@ -219,6 +219,49 @@ void test_components_api(bool use_dedicated_executor) EXPECT_EQ(result->unique_id, 0u); } + { + // forward_global_arguments + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponentFoo"; + request->node_name = "test_component_global_arguments"; + rclcpp::Parameter forward_global_arguments("forward_global_arguments", + rclcpp::ParameterValue(true)); + request->extra_arguments.push_back(forward_global_arguments.to_parameter_msg()); + + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + EXPECT_EQ(result->success, true); + EXPECT_EQ(result->error_message, ""); + EXPECT_EQ(result->full_node_name, "/test_component_global_arguments"); + EXPECT_EQ(result->unique_id, 7u); + } + + { + // forward_global_arguments is not a bool type parameter + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponentFoo"; + request->node_name = "test_component_global_arguments_str"; + + rclcpp::Parameter forward_global_arguments("forward_global_arguments", + rclcpp::ParameterValue("hello")); + request->extra_arguments.push_back(forward_global_arguments.to_parameter_msg()); + + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + EXPECT_EQ(result->success, false); + EXPECT_EQ( + result->error_message, + "Extra component argument 'forward_global_arguments' must be a boolean"); + EXPECT_EQ(result->full_node_name, ""); + EXPECT_EQ(result->unique_id, 0u); + } + auto node_names = node->get_node_names(); auto find_in_nodes = [node_names](std::string name) { @@ -247,20 +290,22 @@ void test_components_api(bool use_dedicated_executor) auto result_node_names = result->full_node_names; auto result_unique_ids = result->unique_ids; - EXPECT_EQ(result_node_names.size(), 6u); + EXPECT_EQ(result_node_names.size(), 7u); EXPECT_EQ(result_node_names[0], "/test_component_foo"); EXPECT_EQ(result_node_names[1], "/test_component_bar"); EXPECT_EQ(result_node_names[2], "/test_component_baz"); EXPECT_EQ(result_node_names[3], "/ns/test_component_bing"); EXPECT_EQ(result_node_names[4], "/test_component_remap"); EXPECT_EQ(result_node_names[5], "/test_component_intra_process"); - EXPECT_EQ(result_unique_ids.size(), 6u); + EXPECT_EQ(result_node_names[6], "/test_component_global_arguments"); + EXPECT_EQ(result_unique_ids.size(), 7u); EXPECT_EQ(result_unique_ids[0], 1u); EXPECT_EQ(result_unique_ids[1], 2u); EXPECT_EQ(result_unique_ids[2], 3u); EXPECT_EQ(result_unique_ids[3], 4u); EXPECT_EQ(result_unique_ids[4], 5u); EXPECT_EQ(result_unique_ids[5], 6u); + EXPECT_EQ(result_unique_ids[6], 7u); } } @@ -314,18 +359,20 @@ void test_components_api(bool use_dedicated_executor) auto result_node_names = result->full_node_names; auto result_unique_ids = result->unique_ids; - EXPECT_EQ(result_node_names.size(), 5u); + EXPECT_EQ(result_node_names.size(), 6u); EXPECT_EQ(result_node_names[0], "/test_component_bar"); EXPECT_EQ(result_node_names[1], "/test_component_baz"); EXPECT_EQ(result_node_names[2], "/ns/test_component_bing"); EXPECT_EQ(result_node_names[3], "/test_component_remap"); EXPECT_EQ(result_node_names[4], "/test_component_intra_process"); - EXPECT_EQ(result_unique_ids.size(), 5u); + EXPECT_EQ(result_node_names[5], "/test_component_global_arguments"); + EXPECT_EQ(result_unique_ids.size(), 6u); EXPECT_EQ(result_unique_ids[0], 2u); EXPECT_EQ(result_unique_ids[1], 3u); EXPECT_EQ(result_unique_ids[2], 4u); EXPECT_EQ(result_unique_ids[3], 5u); EXPECT_EQ(result_unique_ids[4], 6u); + EXPECT_EQ(result_unique_ids[5], 7u); } } } From 9ec0393e636d1dd165bf12c10dae437dee88743e Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Thu, 20 Jan 2022 12:36:17 +0000 Subject: [PATCH 066/455] add is_spinning() method to executor base class Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/executor.hpp | 9 +++++++++ rclcpp/src/rclcpp/executor.cpp | 6 ++++++ rclcpp/test/rclcpp/test_executor.cpp | 21 +++++++++++++++++++++ 3 files changed, 36 insertions(+) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 77d6a5f447..f3f6e9bfe4 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -401,6 +401,15 @@ class Executor void set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy); + /// Returns true if the executor is currently spinning. + /** + * This function can be called asynchronously from any thread. + * \return True if the executor is currently spinning. + */ + RCLCPP_PUBLIC + bool + is_spinning(); + protected: RCLCPP_PUBLIC void diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index d3f6cb361f..6ed3ace557 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -928,3 +928,9 @@ Executor::has_node( return other_ptr == node_ptr; }) != weak_groups_to_nodes.end(); } + +bool +Executor::is_spinning() +{ + return spinning; +} diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index fce0369edd..0b007d519b 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -556,3 +556,24 @@ TEST_F(TestExecutor, spin_until_future_complete_future_already_complete) { rclcpp::FutureReturnCode::SUCCESS, dummy.spin_until_future_complete(future, std::chrono::milliseconds(1))); } + +TEST_F(TestExecutor, is_spinning) { + DummyExecutor dummy; + ASSERT_FALSE(dummy.is_spinning()); + + auto node = std::make_shared("node", "ns"); + bool timer_called = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + timer_called = true; + EXPECT_TRUE(dummy.is_spinning()); + }); + + dummy.add_node(node); + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + dummy.spin_some(std::chrono::milliseconds(1)); + + ASSERT_TRUE(timer_called); +} From c54a6f1cd2b5bfbe530c362ad7e8fa22f753e325 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 20 Jan 2022 16:30:55 -0500 Subject: [PATCH 067/455] Cleanup time source object lifetimes (#1867) * Small cleanups in TimeSource. Simplify some code, and also make sure to throw an exception when use_sim_time is not of type bool. Also add a test for the latter. * Remove the sim_time_cb_handler. It was originally used to make sure that someone didn't change the 'use_sim_time' type from boolean to something else. But since the parameters interface now does that automatically for us, we don't need the explicit check here. I can think of one situation that this allows that wasn't allowed before. If the user defined 'use_sim_time' as a parameter override when constructing a node, and the type is bool, and they also defined the parameters as 'dynamic_typing', then this callback will still have effect. But presumably if the user went out of their way to change the parameter to dynamic_typing, they are trying to do something esoteric and so we should just let them. * ClocksState does not need to be a pointer. Instead, make it a regular member variable. That lets us get rid of all of the special handling for when it is a weak_ptr or not. It's lifetime is now just that of NodeState. * Stop using NodeState as a weak_ptr in the captures. This ended up causing the lifetime of the object to be weird. Instead, just capture 'this' which is sufficient. * Make sure destroy_clock_sub is first. * Switch to using just a regular object. Windows objects to trying to do "make_shared" in the RCLCPP macro, so just switch to a normal object here. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/time_source.hpp | 3 - rclcpp/src/rclcpp/time_source.cpp | 144 +++++++++--------------- rclcpp/test/rclcpp/test_time_source.cpp | 23 +++- 3 files changed, 70 insertions(+), 100 deletions(-) diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp index 84984da6e4..0de9b368e8 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -147,9 +147,6 @@ class TimeSource ~TimeSource(); private: - class ClocksState; - std::shared_ptr clocks_state_; - class NodeState; std::shared_ptr node_state_; diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 7e1226e1b2..42a9208251 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -33,7 +33,7 @@ namespace rclcpp { -class TimeSource::ClocksState : public std::enable_shared_from_this +class ClocksState final { public: ClocksState() @@ -182,12 +182,11 @@ class TimeSource::ClocksState : public std::enable_shared_from_this std::shared_ptr last_msg_set_; }; -class TimeSource::NodeState : public std::enable_shared_from_this +class TimeSource::NodeState final { public: - NodeState(std::weak_ptr clocks_state, const rclcpp::QoS & qos, bool use_clock_thread) - : clocks_state_(std::move(clocks_state)), - use_clock_thread_(use_clock_thread), + NodeState(const rclcpp::QoS & qos, bool use_clock_thread) + : use_clock_thread_(use_clock_thread), logger_(rclcpp::get_logger("rclcpp")), qos_(qos) { @@ -250,52 +249,31 @@ class TimeSource::NodeState : public std::enable_shared_from_this if (!node_parameters_->has_parameter(use_sim_time_name)) { use_sim_time_param = node_parameters_->declare_parameter( use_sim_time_name, - rclcpp::ParameterValue(false), - rcl_interfaces::msg::ParameterDescriptor()); + rclcpp::ParameterValue(false)); } else { use_sim_time_param = node_parameters_->get_parameter(use_sim_time_name).get_parameter_value(); } if (use_sim_time_param.get_type() == rclcpp::PARAMETER_BOOL) { if (use_sim_time_param.get()) { parameter_state_ = SET_TRUE; - // There should be no way to call this attachNode when the clocks_state_ pointer is not - // valid because it means the TimeSource is being destroyed - if (auto clocks_state_ptr = clocks_state_.lock()) { - clocks_state_ptr->enable_ros_time(); - create_clock_sub(); - } + clocks_state_.enable_ros_time(); + create_clock_sub(); } } else { RCLCPP_ERROR( logger_, "Invalid type '%s' for parameter 'use_sim_time', should be 'bool'", rclcpp::to_string(use_sim_time_param.get_type()).c_str()); + throw std::invalid_argument("Invalid type for parameter 'use_sim_time', should be 'bool'"); } - sim_time_cb_handler_ = node_parameters_->add_on_set_parameters_callback( - [use_sim_time_name](const std::vector & parameters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - for (const auto & parameter : parameters) { - if ( - parameter.get_name() == use_sim_time_name && - parameter.get_type() != rclcpp::PARAMETER_BOOL) - { - result.successful = false; - result.reason = "'" + use_sim_time_name + "' must be a bool"; - break; - } - } - return result; - }); // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609 parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event( node_topics_, - [state = std::weak_ptr(this->shared_from_this())]( - std::shared_ptr event) { - if (auto state_ptr = state.lock()) { - state_ptr->on_parameter_event(event); + [this](std::shared_ptr event) { + if (node_base_ != nullptr) { + this->on_parameter_event(event); } - // Do nothing if the pointer could not be locked because it means the TimeSource is now + // Do nothing if node_base_ is nullptr because it means the TimeSource is now // without an attached node }); } @@ -303,12 +281,10 @@ class TimeSource::NodeState : public std::enable_shared_from_this // Detach the attached node void detachNode() { - // There should be no way to call detachNode when the clocks_state_ pointer is not valid - // because it means the TimeSource is being destroyed - if (auto clocks_state_ptr = clocks_state_.lock()) { - clocks_state_ptr->disable_ros_time(); - } + // destroy_clock_sub() *must* be first here, to ensure that the executor + // can't possibly call any of the callbacks as we are cleaning up. destroy_clock_sub(); + clocks_state_.disable_ros_time(); parameter_subscription_.reset(); node_base_.reset(); node_topics_.reset(); @@ -316,28 +292,34 @@ class TimeSource::NodeState : public std::enable_shared_from_this node_services_.reset(); node_logging_.reset(); node_clock_.reset(); - if (sim_time_cb_handler_ && node_parameters_) { - node_parameters_->remove_on_set_parameters_callback(sim_time_cb_handler_.get()); - } - sim_time_cb_handler_.reset(); node_parameters_.reset(); } + void attachClock(std::shared_ptr clock) + { + clocks_state_.attachClock(std::move(clock)); + } + + void detachClock(std::shared_ptr clock) + { + clocks_state_.detachClock(std::move(clock)); + } + private: - std::weak_ptr clocks_state_; + ClocksState clocks_state_; // Dedicated thread for clock subscription. bool use_clock_thread_; std::thread clock_executor_thread_; // Preserve the node reference - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; - rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; - rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; - rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; - rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_{nullptr}; + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_{nullptr}; + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_{nullptr}; + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_{nullptr}; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_{nullptr}; + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_{nullptr}; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_{nullptr}; // Store (and update on node attach) logger for logging. Logger logger_; @@ -346,9 +328,7 @@ class TimeSource::NodeState : public std::enable_shared_from_this rclcpp::QoS qos_; // The subscription for the clock callback - using MessageT = rosgraph_msgs::msg::Clock; - using Alloc = std::allocator; - using SubscriptionT = rclcpp::Subscription; + using SubscriptionT = rclcpp::Subscription; std::shared_ptr clock_subscription_{nullptr}; std::mutex clock_sub_lock_; rclcpp::CallbackGroup::SharedPtr clock_callback_group_; @@ -358,21 +338,15 @@ class TimeSource::NodeState : public std::enable_shared_from_this // The clock callback itself void clock_cb(std::shared_ptr msg) { - auto clocks_state_ptr = clocks_state_.lock(); - if (!clocks_state_ptr) { - // The clock_state_ pointer is no longer valid, implying that the TimeSource object is being - // destroyed, so do nothing - return; - } - if (!clocks_state_ptr->is_ros_time_active() && SET_TRUE == this->parameter_state_) { - clocks_state_ptr->enable_ros_time(); + if (!clocks_state_.is_ros_time_active() && SET_TRUE == this->parameter_state_) { + clocks_state_.enable_ros_time(); } // Cache the last message in case a new clock is attached. - clocks_state_ptr->cache_last_msg(msg); + clocks_state_.cache_last_msg(msg); auto time_msg = std::make_shared(msg->clock); if (SET_TRUE == this->parameter_state_) { - clocks_state_ptr->set_all_clocks(time_msg, true); + clocks_state_.set_all_clocks(time_msg, true); } } @@ -421,13 +395,12 @@ class TimeSource::NodeState : public std::enable_shared_from_this node_topics_, "/clock", qos_, - [state = std::weak_ptr(this->shared_from_this())]( - std::shared_ptr msg) { - if (auto state_ptr = state.lock()) { - state_ptr->clock_cb(msg); + [this](std::shared_ptr msg) { + // We are using node_base_ as an indication if there is a node attached. + // Only call the clock_cb if that is the case. + if (node_base_ != nullptr) { + clock_cb(msg); } - // Do nothing if the pointer could not be locked because it means the TimeSource is now - // without an attached node }, options ); @@ -447,19 +420,12 @@ class TimeSource::NodeState : public std::enable_shared_from_this } // Parameter Event subscription - using ParamMessageT = rcl_interfaces::msg::ParameterEvent; - using ParamSubscriptionT = rclcpp::Subscription; + using ParamSubscriptionT = rclcpp::Subscription; std::shared_ptr parameter_subscription_; // Callback for parameter updates void on_parameter_event(std::shared_ptr event) { - auto clocks_state_ptr = clocks_state_.lock(); - if (!clocks_state_ptr) { - // The clock_state_ pointer is no longer valid, implying that the TimeSource object is being - // destroyed, so do nothing - return; - } // Filter out events on 'use_sim_time' parameter instances in other nodes. if (event->node != node_base_->get_fully_qualified_name()) { return; @@ -475,12 +441,12 @@ class TimeSource::NodeState : public std::enable_shared_from_this } if (it.second->value.bool_value) { parameter_state_ = SET_TRUE; - clocks_state_ptr->enable_ros_time(); + clocks_state_.enable_ros_time(); create_clock_sub(); } else { parameter_state_ = SET_FALSE; - clocks_state_ptr->disable_ros_time(); destroy_clock_sub(); + clocks_state_.disable_ros_time(); } } // Handle the case that use_sim_time was deleted. @@ -488,7 +454,7 @@ class TimeSource::NodeState : public std::enable_shared_from_this {rclcpp::ParameterEventsFilter::EventType::DELETED}); for (auto & it : deleted.get_events()) { (void) it; // if there is a match it's already matched, don't bother reading it. - // If the parameter is deleted mark it as unset but dont' change state. + // If the parameter is deleted mark it as unset but don't change state. parameter_state_ = UNSET; } } @@ -496,20 +462,14 @@ class TimeSource::NodeState : public std::enable_shared_from_this // An enum to hold the parameter state enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE}; UseSimTimeParameterState parameter_state_; - - // A handler for the use_sim_time parameter callback. - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr sim_time_cb_handler_{nullptr}; }; TimeSource::TimeSource( std::shared_ptr node, const rclcpp::QoS & qos, bool use_clock_thread) -: constructed_use_clock_thread_(use_clock_thread), - constructed_qos_(qos) +: TimeSource(qos, use_clock_thread) { - clocks_state_ = std::make_shared(); - node_state_ = std::make_shared(clocks_state_->weak_from_this(), qos, use_clock_thread); attachNode(node); } @@ -519,8 +479,7 @@ TimeSource::TimeSource( : constructed_use_clock_thread_(use_clock_thread), constructed_qos_(qos) { - clocks_state_ = std::make_shared(); - node_state_ = std::make_shared(clocks_state_->weak_from_this(), qos, use_clock_thread); + node_state_ = std::make_shared(qos, use_clock_thread); } void TimeSource::attachNode(rclcpp::Node::SharedPtr node) @@ -559,19 +518,18 @@ void TimeSource::detachNode() { node_state_.reset(); node_state_ = std::make_shared( - clocks_state_->weak_from_this(), constructed_qos_, constructed_use_clock_thread_); } void TimeSource::attachClock(std::shared_ptr clock) { - clocks_state_->attachClock(std::move(clock)); + node_state_->attachClock(std::move(clock)); } void TimeSource::detachClock(std::shared_ptr clock) { - clocks_state_->detachClock(std::move(clock)); + node_state_->detachClock(std::move(clock)); } bool TimeSource::get_use_clock_thread() diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index c355ea6227..2bf89a5b09 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -27,6 +27,8 @@ #include "rclcpp/time.hpp" #include "rclcpp/time_source.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + using namespace std::chrono_literals; class TestTimeSource : public ::testing::Test @@ -246,11 +248,25 @@ TEST_F(TestTimeSource, ROS_time_valid_sim_time) { } TEST_F(TestTimeSource, ROS_invalid_sim_time) { - rclcpp::TimeSource ts; - ts.attachNode(node); + rclcpp::TimeSource ts(node); EXPECT_FALSE(node->set_parameter(rclcpp::Parameter("use_sim_time", "not boolean")).successful); } +TEST(TimeSource, invalid_sim_time_parameter_override) +{ + rclcpp::init(0, nullptr); + + rclcpp::NodeOptions options; + options.automatically_declare_parameters_from_overrides(true); + options.append_parameter_override("use_sim_time", "not boolean"); + + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Node("my_node", options), + std::invalid_argument("Invalid type for parameter 'use_sim_time', should be 'bool'")); + + rclcpp::shutdown(); +} + TEST_F(TestTimeSource, clock) { rclcpp::TimeSource ts(node); auto ros_clock = std::make_shared(RCL_ROS_TIME); @@ -749,8 +765,7 @@ TEST_F(TestTimeSource, clock_sleep_until_with_ros_time_basic) { node->set_parameter({"use_sim_time", true}); auto clock = std::make_shared(RCL_ROS_TIME); - rclcpp::TimeSource time_source; - time_source.attachNode(node); + rclcpp::TimeSource time_source(node); time_source.attachClock(clock); // Wait until time source has definitely received a first ROS time from the pub node From 2520d398c7645361e37a312df7f523033501b7e5 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Fri, 4 Feb 2022 23:23:17 +0900 Subject: [PATCH 068/455] Add return value version of get_parameter_or (#1813) * Add return value version of get_parameter_or * Add test case Signed-off-by: Kenji Miyake Co-authored-by: Abrar Rahman Protyasha --- rclcpp/include/rclcpp/node.hpp | 18 ++++++++++++++++++ rclcpp/include/rclcpp/node_impl.hpp | 11 +++++++++++ rclcpp/test/rclcpp/test_node.cpp | 27 +++++++++++++++++++++++++++ 3 files changed, 56 insertions(+) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 7e5f402f8a..2150f60e1c 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -720,6 +720,24 @@ class Node : public std::enable_shared_from_this ParameterT & parameter, const ParameterT & alternative_value) const; + /// Return the parameter value, or the "alternative_value" if not set. + /** + * If the parameter was not set, then the "alternative_value" argument is returned. + * + * This method will not throw the rclcpp::exceptions::ParameterNotDeclaredException exception. + * + * In all cases, the parameter is never set or declared within the node. + * + * \param[in] name The name of the parameter to get. + * \param[in] alternative_value Value to be stored in output if the parameter was not set. + * \returns The value of the parameter. + */ + template + ParameterT + get_parameter_or( + const std::string & name, + const ParameterT & alternative_value) const; + /// Return the parameters by the given parameter names. /** * Like get_parameters(), this method may throw the diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 46f54aa054..086c2bb17c 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -313,6 +313,17 @@ Node::get_parameter_or( return got_parameter; } +template +ParameterT +Node::get_parameter_or( + const std::string & name, + const ParameterT & alternative_value) const +{ + ParameterT parameter; + get_parameter_or(name, parameter, alternative_value); + return parameter; +} + // this is a partially-specialized version of get_parameter above, // where our concrete type for ParameterT is std::map, but the to-be-determined // type is the value in the map. diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index f3fb13119f..7b4bade94b 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -2075,6 +2075,33 @@ TEST_F(TestNode, get_parameter_or_undeclared_parameters_allowed) { } } +// test get_parameter_or with return value +TEST_F(TestNode, get_parameter_or_with_return_value) { + auto node = std::make_shared( + "test_get_parameter_or_node"_unq); + { + // normal use (declare first) still works + auto name = "parameter"_unq; + + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + + { + const int value = node->get_parameter_or(name, 43); + EXPECT_EQ(value, 42); + } + } + { + // normal use, no declare first + auto name = "parameter"_unq; + + { + const int value = node->get_parameter_or(name, 43); + EXPECT_EQ(value, 43); + } + } +} + // test get_parameters with undeclared not allowed TEST_F(TestNode, get_parameters_undeclared_parameters_not_allowed) { auto node = std::make_shared( From 0d1747e0663572ce52c8dd7ab72476dd4415d8c5 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Mon, 7 Feb 2022 09:32:44 -0800 Subject: [PATCH 069/455] LifecycleNode::on_deactivate deactivate all managed entities. (#1885) Signed-off-by: Tomoya Fujita --- .../src/lifecycle_node_interface_impl.hpp | 2 +- .../test/test_lifecycle_publisher.cpp | 46 +++++++++++++++++++ 2 files changed, 47 insertions(+), 1 deletion(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 0fca4c19aa..51de8eab07 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -529,7 +529,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl for (const auto & weak_entity : weak_managed_entities_) { auto entity = weak_entity.lock(); if (entity) { - entity->on_activate(); + entity->on_deactivate(); } } } diff --git a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp index 278b1f9026..2a5d08f728 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp @@ -18,11 +18,17 @@ #include #include +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" + #include "test_msgs/msg/empty.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" +using lifecycle_msgs::msg::State; +using lifecycle_msgs::msg::Transition; + class TestDefaultStateMachine : public ::testing::Test { protected: @@ -80,7 +86,47 @@ class TestLifecyclePublisher : public ::testing::Test std::shared_ptr node_; }; +TEST_F(TestLifecyclePublisher, publish_managed_by_node) { + // transition via LifecycleNode + auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + auto ret = reset_key; + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, node_->get_current_state().id()); + node_->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE), ret); + ASSERT_EQ(success, ret); + ret = reset_key; + node_->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE), ret); + ASSERT_EQ(success, ret); + ret = reset_key; + EXPECT_TRUE(node_->publisher()->is_activated()); + { + auto msg_ptr = std::make_unique(); + EXPECT_NO_THROW(node_->publisher()->publish(*msg_ptr)); + } + { + auto msg_ptr = std::make_unique(); + EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr))); + } + node_->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE), ret); + ASSERT_EQ(success, ret); + ret = reset_key; + EXPECT_FALSE(node_->publisher()->is_activated()); + { + auto msg_ptr = std::make_unique(); + EXPECT_NO_THROW(node_->publisher()->publish(*msg_ptr)); + } + { + auto msg_ptr = std::make_unique(); + EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr))); + } +} + TEST_F(TestLifecyclePublisher, publish) { + // transition via LifecyclePublisher node_->publisher()->on_deactivate(); EXPECT_FALSE(node_->publisher()->is_activated()); { From 43db06dad7639da965d5447961e4da0dbf6f59ec Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Wed, 9 Feb 2022 14:45:39 +0000 Subject: [PATCH 070/455] Use spin() in component_manager_isolated.hpp (#1881) * replace spin_until_future_complete with spin in component_manager_isolated.hpp spin_until_future_complete() is more inefficient as it needs to check the state of the future and the timeout after every work iteration Signed-off-by: Alberto Soragna * fix uncrustify error Signed-off-by: Alberto Soragna --- .../component_manager_isolated.hpp | 37 ++++++++++++++----- 1 file changed, 28 insertions(+), 9 deletions(-) diff --git a/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp b/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp index 8ff9a60665..ea77532312 100644 --- a/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp +++ b/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp @@ -38,7 +38,6 @@ class ComponentManagerIsolated : public rclcpp_components::ComponentManager { std::shared_ptr executor; std::thread thread; - std::promise promise; }; public: @@ -46,9 +45,7 @@ class ComponentManagerIsolated : public rclcpp_components::ComponentManager { if (node_wrappers_.size()) { for (auto & executor_wrapper : dedicated_executor_wrappers_) { - executor_wrapper.second.promise.set_value(); - executor_wrapper.second.executor->cancel(); - executor_wrapper.second.thread.join(); + cancel_executor(executor_wrapper.second); } node_wrappers_.clear(); } @@ -67,8 +64,8 @@ class ComponentManagerIsolated : public rclcpp_components::ComponentManager exec->add_node(node_wrappers_[node_id].get_node_base_interface()); executor_wrapper.executor = exec; executor_wrapper.thread = std::thread( - [exec, cancel_token = executor_wrapper.promise.get_future()]() { - exec->spin_until_future_complete(cancel_token); + [exec]() { + exec->spin(); }); dedicated_executor_wrappers_[node_id] = std::move(executor_wrapper); } @@ -81,14 +78,36 @@ class ComponentManagerIsolated : public rclcpp_components::ComponentManager { auto executor_wrapper = dedicated_executor_wrappers_.find(node_id); if (executor_wrapper != dedicated_executor_wrappers_.end()) { - executor_wrapper->second.promise.set_value(); - executor_wrapper->second.executor->cancel(); - executor_wrapper->second.thread.join(); + cancel_executor(executor_wrapper->second); dedicated_executor_wrappers_.erase(executor_wrapper); } } private: + /// Stops a spinning executor avoiding race conditions. + /** + * @param executor_wrapper executor to stop and its associated thread + */ + void cancel_executor(DedicatedExecutorWrapper & executor_wrapper) + { + // We can't immediately call the cancel() API on an executor because if it is not + // already spinning, this operation will have no effect. + // We rely on the assumption that this class creates executors and then immediately makes them + // spin in a separate thread, i.e. the time gap between when the executor is created and when + // it starts to spin is small (although it's not negligible). + + while (!executor_wrapper.executor->is_spinning()) { + // This is an arbitrarily small delay to avoid busy looping + rclcpp::sleep_for(std::chrono::milliseconds(1)); + } + + // After the while loop we are sure that the executor is now spinning, so + // the call to cancel() will work. + executor_wrapper.executor->cancel(); + // Wait for the thread task to return + executor_wrapper.thread.join(); + } + std::unordered_map dedicated_executor_wrappers_; }; From 891fff0153981a13e9c68701f83fb1e171610377 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 10 Feb 2022 21:44:14 +0800 Subject: [PATCH 071/455] fix one subscription can wait_for_message twice (#1870) Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/wait_for_message.hpp | 3 ++ rclcpp/test/rclcpp/test_wait_for_message.cpp | 35 ++++++++++++++++++++ 2 files changed, 38 insertions(+) diff --git a/rclcpp/include/rclcpp/wait_for_message.hpp b/rclcpp/include/rclcpp/wait_for_message.hpp index 20c9df4db6..25c45ad782 100644 --- a/rclcpp/include/rclcpp/wait_for_message.hpp +++ b/rclcpp/include/rclcpp/wait_for_message.hpp @@ -18,6 +18,8 @@ #include #include +#include "rcpputils/scope_exit.hpp" + #include "rclcpp/node.hpp" #include "rclcpp/visibility_control.hpp" #include "rclcpp/wait_set.hpp" @@ -54,6 +56,7 @@ bool wait_for_message( rclcpp::WaitSet wait_set; wait_set.add_subscription(subscription); + RCPPUTILS_SCOPE_EXIT(wait_set.remove_subscription(subscription); ); wait_set.add_guard_condition(gc); auto ret = wait_set.wait(time_to_wait); if (ret.kind() != rclcpp::WaitResultKind::Ready) { diff --git a/rclcpp/test/rclcpp/test_wait_for_message.cpp b/rclcpp/test/rclcpp/test_wait_for_message.cpp index f0071698ca..dadc06f2da 100644 --- a/rclcpp/test/rclcpp/test_wait_for_message.cpp +++ b/rclcpp/test/rclcpp/test_wait_for_message.cpp @@ -72,3 +72,38 @@ TEST(TestUtilities, wait_for_message_indefinitely) { ASSERT_FALSE(received); } + +TEST(TestUtilities, wait_for_message_twice_one_sub) { + rclcpp::init(0, nullptr); + + auto node = std::make_shared("wait_for_message_node3"); + + using MsgT = test_msgs::msg::Strings; + auto pub = node->create_publisher("wait_for_message_topic", 10); + auto sub = node->create_subscription( + "wait_for_message_topic", 1, [](const std::shared_ptr) {}); + + MsgT out1; + MsgT out2; + auto received = false; + auto wait = std::async( + [&]() { + auto ret = rclcpp::wait_for_message(out1, sub, node->get_node_options().context(), 5s); + EXPECT_TRUE(ret); + ret = rclcpp::wait_for_message(out2, sub, node->get_node_options().context(), 5s); + EXPECT_TRUE(ret); + received = true; + }); + + for (auto i = 0u; i < 10 && received == false; ++i) { + pub->publish(*get_messages_strings()[0]); + std::this_thread::sleep_for(1s); + } + + ASSERT_NO_THROW(wait.get()); + ASSERT_TRUE(received); + EXPECT_EQ(out1, *get_messages_strings()[0]); + EXPECT_EQ(out2, *get_messages_strings()[0]); + + rclcpp::shutdown(); +} From 4c8cfa39f8ff787a1a15b725e58b896a5b851742 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Thu, 10 Feb 2022 09:58:36 -0800 Subject: [PATCH 072/455] use universal reference to support rvalue. (#1883) * use universal reference to support rvalue. Signed-off-by: Tomoya Fujita --- rclcpp/include/rclcpp/create_publisher.hpp | 2 +- rclcpp/test/rclcpp/test_publisher.cpp | 11 +++++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/create_publisher.hpp b/rclcpp/include/rclcpp/create_publisher.hpp index c633c74e2e..f6088a33c3 100644 --- a/rclcpp/include/rclcpp/create_publisher.hpp +++ b/rclcpp/include/rclcpp/create_publisher.hpp @@ -92,7 +92,7 @@ template< typename NodeT> std::shared_ptr create_publisher( - NodeT & node, + NodeT && node, const std::string & topic_name, const rclcpp::QoS & qos, const rclcpp::PublisherOptionsWithAllocator & options = ( diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 3be64e9647..f844e44e6c 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -156,6 +156,17 @@ TEST_F(TestPublisher, various_creation_signatures) { rclcpp::create_publisher(node, "topic", 42, options); (void)publisher; } + { + auto publisher = rclcpp::create_publisher( + node->get_node_topics_interface(), "topic", 42, rclcpp::PublisherOptions()); + (void)publisher; + } + { + auto node_topics_interface = node->get_node_topics_interface(); + auto publisher = rclcpp::create_publisher( + node_topics_interface, "topic", 42, rclcpp::PublisherOptions()); + (void)publisher; + } { auto node_parameters = node->get_node_parameters_interface(); auto node_topics = node->get_node_topics_interface(); From 025cd5ccc8202a52f7c7a3f037d8faf46f7dc3f3 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 15 Feb 2022 13:02:23 -0800 Subject: [PATCH 073/455] Use ament_generate_version_header (#1886) Signed-off-by: Shane Loretz --- rclcpp/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 0072aaca8a..c3b38cf7ee 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -255,4 +255,5 @@ if(TEST cppcheck) set_tests_properties(cppcheck PROPERTIES TIMEOUT 500) endif() -ament_cmake_gen_version_h() +ament_generate_version_header(${PROJECT_NAME} + INSTALL_PATH "include") From 4f778199b44f749664861d998e59d0f793b2caf3 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 16 Feb 2022 09:41:21 -0800 Subject: [PATCH 074/455] Install headers to include/${PROJECT_NAME} (#1888) * initial attempt, problems with ament_gen_version_h Signed-off-by: Shane Loretz * remove blank line Signed-off-by: Shane Loretz --- rclcpp/CMakeLists.txt | 14 ++++++++------ rclcpp_action/CMakeLists.txt | 11 +++++++---- rclcpp_components/CMakeLists.txt | 14 +++++++++----- rclcpp_lifecycle/CMakeLists.txt | 12 ++++++++---- 4 files changed, 32 insertions(+), 19 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index c3b38cf7ee..5cbe0b961a 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -187,7 +187,7 @@ endif() target_include_directories(${PROJECT_NAME} PUBLIC "$" "$" - "$") + "$") target_link_libraries(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT}) # specific order: dependents before dependencies ament_target_dependencies(${PROJECT_NAME} @@ -217,11 +217,14 @@ install( RUNTIME DESTINATION bin ) -# specific order: dependents before dependencies -ament_export_include_directories(include) +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") ament_export_libraries(${PROJECT_NAME}) + +# Export modern CMake targets ament_export_targets(${PROJECT_NAME}) +# specific order: dependents before dependencies ament_export_dependencies(ament_index_cpp) ament_export_dependencies(libstatistics_collector) ament_export_dependencies(rcl) @@ -247,7 +250,7 @@ ament_package() install( DIRECTORY include/ ${CMAKE_CURRENT_BINARY_DIR}/include/ - DESTINATION include + DESTINATION include/${PROJECT_NAME} ) if(TEST cppcheck) @@ -255,5 +258,4 @@ if(TEST cppcheck) set_tests_properties(cppcheck PROPERTIES TIMEOUT 500) endif() -ament_generate_version_header(${PROJECT_NAME} - INSTALL_PATH "include") +ament_generate_version_header(${PROJECT_NAME}) diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index e9bd327643..353e179244 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -34,7 +34,7 @@ add_library(${PROJECT_NAME} target_include_directories(${PROJECT_NAME} PUBLIC "$" - "$") + "$") ament_target_dependencies(${PROJECT_NAME} "action_msgs" @@ -51,7 +51,7 @@ target_compile_definitions(${PROJECT_NAME} install( DIRECTORY include/ - DESTINATION include) + DESTINATION include/${PROJECT_NAME}) install( TARGETS ${PROJECT_NAME} @@ -61,11 +61,14 @@ install( RUNTIME DESTINATION bin ) -# specific order: dependents before dependencies -ament_export_include_directories(include) +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") ament_export_libraries(${PROJECT_NAME}) + +# Export modern CMake targets ament_export_targets(${PROJECT_NAME}) +# specific order: dependents before dependencies ament_export_dependencies(ament_cmake) ament_export_dependencies(action_msgs) ament_export_dependencies(rclcpp) diff --git a/rclcpp_components/CMakeLists.txt b/rclcpp_components/CMakeLists.txt index c7dec3e65f..0b9cda7803 100644 --- a/rclcpp_components/CMakeLists.txt +++ b/rclcpp_components/CMakeLists.txt @@ -24,7 +24,7 @@ find_package(rcpputils REQUIRED) add_library(component INTERFACE) target_include_directories(component INTERFACE "$" - "$") + "$") target_link_libraries(component INTERFACE class_loader::class_loader rclcpp::rclcpp) @@ -36,7 +36,7 @@ add_library( ) target_include_directories(component_manager PUBLIC "$" - "$") + "$") ament_target_dependencies(component_manager "ament_index_cpp" "class_loader" @@ -159,7 +159,7 @@ install( # Install include directories install( DIRECTORY include/ - DESTINATION include + DESTINATION include/${PROJECT_NAME} ) # Install cmake @@ -168,10 +168,14 @@ install( DESTINATION share/${PROJECT_NAME} ) -# specific order: dependents before dependencies -ament_export_include_directories(include) +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") ament_export_libraries(component_manager) + +# Export modern CMake targets ament_export_targets(export_${PROJECT_NAME}) + +# specific order: dependents before dependencies ament_export_dependencies(ament_index_cpp) ament_export_dependencies(class_loader) ament_export_dependencies(composition_interfaces) diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index d635d54133..a823d44a68 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -27,7 +27,7 @@ add_library(rclcpp_lifecycle target_include_directories(${PROJECT_NAME} PUBLIC "$" - "$") + "$") # specific order: dependents before dependencies ament_target_dependencies(rclcpp_lifecycle "rclcpp" @@ -155,14 +155,18 @@ if(BUILD_TESTING) endif() endif() -# specific order: dependents before dependencies -ament_export_include_directories(include) +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") ament_export_libraries(${PROJECT_NAME}) + +# Export modern CMake targets ament_export_targets(${PROJECT_NAME}) + +# specific order: dependents before dependencies ament_export_dependencies(rclcpp) ament_export_dependencies(rcl_lifecycle) ament_export_dependencies(lifecycle_msgs) ament_package() install(DIRECTORY include/ - DESTINATION include) + DESTINATION include/${PROJECT_NAME}) From 32c03dde2dd973593da4dbe29a8bbd89dbaf6f44 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Sat, 19 Feb 2022 11:57:10 +0000 Subject: [PATCH 075/455] small improvements to node_main.cpp.in Signed-off-by: Alberto Soragna --- rclcpp_components/src/node_main.cpp.in | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp_components/src/node_main.cpp.in b/rclcpp_components/src/node_main.cpp.in index bfebc4aa7d..a6045ee00a 100644 --- a/rclcpp_components/src/node_main.cpp.in +++ b/rclcpp_components/src/node_main.cpp.in @@ -39,9 +39,9 @@ int main(int argc, char * argv[]) RCLCPP_DEBUG(logger, "Load library %s", library_name.c_str()); auto loader = new class_loader::ClassLoader(library_name); auto classes = loader->getAvailableClasses(); - for (auto clazz : classes) { + for (const auto & clazz : classes) { std::string name = clazz.c_str(); - if (!(name.compare(class_name))) { + if (name.compare(class_name) == 0) { RCLCPP_DEBUG(logger, "Instantiate class %s", clazz.c_str()); auto node_factory = loader->createInstance(clazz); auto wrapper = node_factory->create_node_instance(options); From c5a16dce11282f5f10967c346d8817142956714e Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Tue, 22 Feb 2022 14:49:37 +0100 Subject: [PATCH 076/455] Remove fastrtps customization on tests (#1887) Signed-off-by: Miguel Company --- .../test_allocator_memory_strategy.cpp | 10 +---- rclcpp/test/rclcpp/test_qos_event.cpp | 43 ++++++------------- 2 files changed, 15 insertions(+), 38 deletions(-) diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index c068e8f640..e1b916e9c0 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -497,14 +497,8 @@ TEST_F(TestAllocatorMemoryStrategy, add_remove_waitables) { TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_subscription) { RclWaitSetSizes expected_sizes = {}; expected_sizes.size_of_subscriptions = 1; - const std::string implementation_identifier = rmw_get_implementation_identifier(); - if (implementation_identifier == "rmw_cyclonedds_cpp" || - implementation_identifier == "rmw_connextdds") - { - // For cyclonedds and connext, a subscription will also add an event and waitable - expected_sizes.size_of_events += 1; - expected_sizes.size_of_waitables += 1; - } + expected_sizes.size_of_events = 1; + expected_sizes.size_of_waitables = 1; auto node_with_subscription = create_node_with_subscription("subscription_node"); EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_subscription, expected_sizes)); } diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index ebf0e14afb..f234591e33 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -37,9 +37,6 @@ class TestQosEvent : public ::testing::Test void SetUp() { - is_fastrtps = - std::string(rmw_get_implementation_identifier()).find("rmw_fastrtps") != std::string::npos; - node = std::make_shared("test_qos_event", "/ns"); message_callback = [node = node.get()](test_msgs::msg::Empty::ConstSharedPtr /*msg*/) { @@ -54,7 +51,6 @@ class TestQosEvent : public ::testing::Test static constexpr char topic_name[] = "test_topic"; rclcpp::Node::SharedPtr node; - bool is_fastrtps; std::function message_callback; }; @@ -101,12 +97,8 @@ TEST_F(TestQosEvent, test_publisher_constructor) "Offered incompatible qos - total %d (delta %d), last_policy_kind: %d", event.total_count, event.total_count_change, event.last_policy_kind); }; - try { - publisher = node->create_publisher( - topic_name, 10, options); - } catch (const rclcpp::UnsupportedEventTypeException & /*exc*/) { - EXPECT_TRUE(is_fastrtps); - } + publisher = node->create_publisher( + topic_name, 10, options); } /* @@ -151,12 +143,8 @@ TEST_F(TestQosEvent, test_subscription_constructor) "Requested incompatible qos - total %d (delta %d), last_policy_kind: %d", event.total_count, event.total_count_change, event.last_policy_kind); }; - try { - subscription = node->create_subscription( - topic_name, 10, message_callback, options); - } catch (const rclcpp::UnsupportedEventTypeException & /*exc*/) { - EXPECT_TRUE(is_fastrtps); - } + subscription = node->create_subscription( + topic_name, 10, message_callback, options); } /* @@ -210,22 +198,17 @@ TEST_F(TestQosEvent, test_default_incompatible_qos_callbacks) ex.add_node(node->get_node_base_interface()); // This future won't complete on fastrtps, so just timeout immediately - const auto timeout = (is_fastrtps) ? std::chrono::milliseconds(5) : std::chrono::seconds(10); + const auto timeout = std::chrono::seconds(10); ex.spin_until_future_complete(log_msgs_future, timeout); - if (is_fastrtps) { - EXPECT_EQ("", pub_log_msg); - EXPECT_EQ("", sub_log_msg); - } else { - EXPECT_EQ( - "New subscription discovered on topic '/ns/test_topic', requesting incompatible QoS. " - "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", - pub_log_msg); - EXPECT_EQ( - "New publisher discovered on topic '/ns/test_topic', offering incompatible QoS. " - "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", - sub_log_msg); - } + EXPECT_EQ( + "New subscription discovered on topic '/ns/test_topic', requesting incompatible QoS. " + "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", + pub_log_msg); + EXPECT_EQ( + "New publisher discovered on topic '/ns/test_topic', offering incompatible QoS. " + "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", + sub_log_msg); rcutils_logging_set_output_handler(original_output_handler); } From d8e1aed520047b5da28d65cbfbdf781358c14fc6 Mon Sep 17 00:00:00 2001 From: iRobot ROS <49500531+irobot-ros@users.noreply.github.com> Date: Thu, 24 Feb 2022 08:32:17 +0000 Subject: [PATCH 077/455] Add RMW listener APIs (#1579) * Add RMW listener APIs Signed-off-by: Alberto Soragna * split long log into two lines Signed-off-by: Alberto Soragna * Remove use_previous_event Signed-off-by: Mauro Passerino * Do not set gc listener callback Signed-off-by: Mauro Passerino * significant refactor to make callbacks type safe Signed-off-by: William Woodall * Add on_ready callback to waitables, add clear_* functions, add unit-tests Signed-off-by: Alberto Soragna * add mutex to set and clear listener APIs Signed-off-by: Alberto Soragna * allow to set ipc sub callback from regular subscription Signed-off-by: Alberto Soragna * fix minor failures Signed-off-by: Alberto Soragna * fix typos and errors in comments Signed-off-by: Alberto Soragna * fix comments Signed-off-by: Alberto Soragna * expose qos listener APIs from pub and sub; add unit-tests Signed-off-by: Alberto Soragna * add qos event unit-test for invalid callbacks Signed-off-by: Alberto Soragna * Use QoS depth to limit callbacks count Signed-off-by: Mauro Passerino * fix ipc-subscription Signed-off-by: Alberto Soragna * Rename CallbackMessageT -> ROSMessageType Signed-off-by: Mauro Passerino * Set callbacks to Actions Signed-off-by: Mauro Passerino * changes from upstream Signed-off-by: William Woodall * Unset callback on entities destruction Signed-off-by: Mauro Passerino * fix rebase errors Signed-off-by: Alberto Soragna * fix unit-tests Signed-off-by: Alberto Soragna * Add GuardCondition on trigger callback Signed-off-by: Mauro Passerino * Add tests for new GuardCondition APIs Signed-off-by: Mauro Passerino * Fix windows CI Signed-off-by: Mauro Passerino * Action unset callbacks only if were set Signed-off-by: Mauro Passerino * add missing include rcl event callback include directive Signed-off-by: Alberto Soragna * typos Signed-off-by: William Woodall * remove listener reference Signed-off-by: William Woodall * remove references to listener and move a method closer to impl Signed-off-by: William Woodall * cpplint Signed-off-by: William Woodall * Fix qos history check in subscription_intra_process.hpp Co-authored-by: Mauro Passerino Co-authored-by: William Woodall Co-authored-by: Alberto Soragna --- rclcpp/include/rclcpp/client.hpp | 101 +++++++- .../rclcpp/detail/cpp_callback_trampoline.hpp | 67 ++++++ .../subscription_intra_process_base.hpp | 119 ++++++++- .../subscription_intra_process_buffer.hpp | 4 + rclcpp/include/rclcpp/guard_condition.hpp | 17 ++ rclcpp/include/rclcpp/publisher_base.hpp | 77 +++++- rclcpp/include/rclcpp/qos_event.hpp | 115 ++++++++- rclcpp/include/rclcpp/service.hpp | 110 ++++++++- rclcpp/include/rclcpp/subscription.hpp | 17 +- rclcpp/include/rclcpp/subscription_base.hpp | 226 +++++++++++++++++- rclcpp/include/rclcpp/wait_set_template.hpp | 6 +- rclcpp/include/rclcpp/waitable.hpp | 40 ++++ rclcpp/src/rclcpp/client.cpp | 18 +- rclcpp/src/rclcpp/context.cpp | 2 - rclcpp/src/rclcpp/guard_condition.cpp | 53 +++- .../rclcpp/node_interfaces/node_topics.cpp | 6 +- rclcpp/src/rclcpp/publisher_base.cpp | 9 +- rclcpp/src/rclcpp/qos_event.cpp | 20 ++ rclcpp/src/rclcpp/service.cpp | 21 +- rclcpp/src/rclcpp/subscription_base.cpp | 34 ++- .../subscription_intra_process_base.cpp | 5 + rclcpp/src/rclcpp/waitable.cpp | 20 ++ rclcpp/test/rclcpp/test_client.cpp | 91 +++++++ rclcpp/test/rclcpp/test_guard_condition.cpp | 62 +++++ rclcpp/test/rclcpp/test_publisher.cpp | 3 +- rclcpp/test/rclcpp/test_qos_event.cpp | 105 ++++++++ rclcpp/test/rclcpp/test_service.cpp | 71 ++++++ rclcpp/test/rclcpp/test_subscription.cpp | 140 +++++++++++ rclcpp/test/rclcpp/test_wait_set.cpp | 4 +- .../include/rclcpp_action/client.hpp | 69 ++++++ .../include/rclcpp_action/server.hpp | 65 +++++ rclcpp_action/src/client.cpp | 158 ++++++++++++ rclcpp_action/src/server.cpp | 136 +++++++++++ 33 files changed, 1942 insertions(+), 49 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 1fc761f491..e169b890bb 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 @@ -29,20 +30,22 @@ #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" #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,6 +218,90 @@ class ClientBase bool exchange_in_use_by_wait_set_state(bool in_use_state); + /// 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. + * + * 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 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_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 { + 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"); + } + }; + + 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 + // 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_)); + } + + /// Unset the callback registered for new responses, if any. + void + clear_on_new_response_callback() + { + std::lock_guard lock(callback_mutex_); + if (on_new_response_callback_) { + set_on_new_response_callback(nullptr, nullptr); + on_new_response_callback_ = nullptr; + } + } + protected: RCLCPP_DISABLE_COPY(ClientBase) @@ -230,13 +317,21 @@ 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::recursive_mutex callback_mutex_; + std::function on_new_response_callback_{nullptr}; }; 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..be857e1b58 --- /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 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, + * 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 531e4c70e0..331c1b6da5 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -15,13 +15,16 @@ #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ +#include #include #include #include #include "rcl/wait.h" +#include "rmw/impl/cpp/demangle.hpp" #include "rclcpp/guard_condition.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/waitable.hpp" @@ -35,6 +38,11 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable public: RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase) + enum class EntityType + { + Subscription, + }; + RCLCPP_PUBLIC SubscriptionIntraProcessBase( rclcpp::Context::SharedPtr context, @@ -43,7 +51,8 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable : gc_(context), topic_name_(topic_name), qos_profile_(qos_profile) {} - virtual ~SubscriptionIntraProcessBase() = default; + RCLCPP_PUBLIC + virtual ~SubscriptionIntraProcessBase(); RCLCPP_PUBLIC size_t @@ -53,7 +62,17 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable void 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 @@ -64,13 +83,107 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable QoS get_actual_qos() const; + /// Set a callback to be called when each new message arrives. + /** + * 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. + * 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 + * 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. + * + * 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. + */ + 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(callback_mutex_); + on_new_message_callback_ = new_callback; + + if (unread_count_ > 0) { + if (qos_profile_.history() == HistoryPolicy::KeepAll) { + 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; + } + } + + /// Unset the callback registered for new messages, if any. + void + clear_on_ready_callback() override + { + std::lock_guard lock(callback_mutex_); + on_new_message_callback_ = nullptr; + } + protected: - std::recursive_mutex reentrant_mutex_; + std::recursive_mutex callback_mutex_; + std::function on_new_message_callback_ {nullptr}; + size_t unread_count_{0}; rclcpp::GuardCondition gc_; 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 ecb02f501d..3c71512677 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(); } + this->invoke_on_new_message(); } void @@ -137,6 +138,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff buffer_->add_unique(convert_ros_message_to_subscribed_type_unique_ptr(*message)); trigger_guard_condition(); } + this->invoke_on_new_message(); } void @@ -144,6 +146,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff { buffer_->add_shared(std::move(message)); trigger_guard_condition(); + this->invoke_on_new_message(); } void @@ -151,6 +154,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff { buffer_->add_unique(std::move(message)); trigger_guard_condition(); + this->invoke_on_new_message(); } bool diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp index fce42f1d4b..f6f5af9586 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -98,10 +98,27 @@ 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}; + rcl_wait_set_t * wait_set_{nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index 01bdbb585f..cdbd4bb758 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 publisher 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/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 04231e1395..59c99dda42 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -17,16 +17,21 @@ #include #include +#include #include #include #include "rcl/error_handling.h" +#include "rcl/event_callback.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 @@ -84,6 +89,11 @@ class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public st class QOSEventHandlerBase : public Waitable { public: + enum class EntityType + { + Event, + }; + RCLCPP_PUBLIC virtual ~QOSEventHandlerBase(); @@ -102,9 +112,111 @@ class QOSEventHandlerBase : public Waitable bool is_ready(rcl_wait_set_t * wait_set) override; + /// 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. + * + * 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 + * 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. + * + * 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 rmw_event_set_callback + * \sa rcl_event_set_callback + * + * \param[in] callback functor to be called when a new event occurs + */ + 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::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 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 ready' callback"); + } + }; + + 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 + // 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_)); + } + + /// Unset the callback registered for new events, if any. + void + clear_on_ready_callback() override + { + std::lock_guard lock(callback_mutex_); + if (on_new_event_callback_) { + set_on_new_event_callback(nullptr, nullptr); + on_new_event_callback_ = nullptr; + } + } + 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::recursive_mutex callback_mutex_; + std::function on_new_event_callback_{nullptr}; }; template @@ -117,9 +229,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 78fc5e048f..47704a4c09 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -19,22 +19,28 @@ #include #include #include +#include #include #include #include "rcl/error_handling.h" +#include "rcl/event_callback.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,6 +127,91 @@ class ServiceBase bool exchange_in_use_by_wait_set_state(bool in_use_state); + /// 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. + * + * + * 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 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_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 { + 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"); + } + }; + + 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 + // 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_)); + } + + /// Unset the callback registered for new requests, if any. + void + clear_on_new_request_callback() + { + std::lock_guard lock(callback_mutex_); + if (on_new_request_callback_) { + set_on_new_request_callback(nullptr, nullptr); + on_new_request_callback_ = nullptr; + } + } + protected: RCLCPP_DISABLE_COPY(ServiceBase) @@ -132,12 +223,21 @@ 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::recursive_mutex callback_mutex_; + std::function on_new_request_callback_{nullptr}; }; template 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 72ddc23be2..26650cd121 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -17,16 +17,20 @@ #include #include +#include #include #include #include #include +#include "rcl/event_callback.h" #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" @@ -99,9 +103,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. @@ -283,6 +288,209 @@ class SubscriptionBase : public std::enable_shared_from_this std::vector get_network_flow_endpoints() const; + /// 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_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 { + 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"); + } + }; + + 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 + // 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_)); + } + + /// Unset the callback registered for new messages, if any. + void + clear_on_new_message_callback() + { + std::lock_guard lock(callback_mutex_); + + 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. + /** + * 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; + } + + 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); + 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(); + } + + /// 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 subscription 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 @@ -297,7 +505,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 @@ -307,17 +515,24 @@ 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_; + std::unordered_map> event_handlers_; bool use_intra_process_; IntraProcessManagerWeakPtr weak_ipm_; uint64_t intra_process_subscription_id_; + std::shared_ptr subscription_intra_process_; private: RCLCPP_DISABLE_COPY(SubscriptionBase) @@ -329,6 +544,9 @@ 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::recursive_mutex callback_mutex_; + std::function on_new_message_callback_{nullptr}; }; } // namespace rclcpp 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/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index c78b0a885f..db06f70678 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 times the waitable was ready + * since the last time this callback was called. + * 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. + * 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 + * 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. + * + * 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/client.cpp b/rclcpp/src/rclcpp/client.cpp index 6a1d3934d4..2380b890f5 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; @@ -66,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(); } @@ -198,3 +200,17 @@ 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_on_new_response_callback(rcl_event_callback_t callback, const void * user_data) +{ + rcl_ret_t ret = rcl_client_set_on_new_response_callback( + client_handle_.get(), + callback, + user_data); + + if (RCL_RET_OK != ret) { + 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/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp index cae0c4ce5c..ea68c78d73 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,42 @@ 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) +{ + std::lock_guard lock(reentrant_mutex_); + + if (exchange_in_use_by_wait_set_state(true)) { + 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( + 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 diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index 284795ef6e..159409528d 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -64,7 +64,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); } @@ -105,7 +106,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..dd027f2e65 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" @@ -99,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(); @@ -154,7 +160,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/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index daf1d3e01e..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", @@ -67,4 +71,20 @@ QOSEventHandlerBase::is_ready(rcl_wait_set_t * wait_set) return wait_set->events[wait_set_event_index_] == &event_handle_; } +void +QOSEventHandlerBase::set_on_new_event_callback( + rcl_event_callback_t callback, + const void * user_data) +{ + rcl_ret_t ret = rcl_event_set_callback( + &event_handle_, + callback, + user_data); + + 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 QOS Event"); + } +} + } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 827062bdd3..d2f333cacf 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -28,11 +28,14 @@ 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() -{} +{ + clear_on_new_request_callback(); +} bool ServiceBase::take_type_erased_request(void * request_out, rmw_request_id_t & request_id_out) @@ -84,3 +87,17 @@ 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_on_new_request_callback(rcl_event_callback_t callback, const void * user_data) +{ + rcl_ret_t ret = rcl_service_set_on_new_request_callback( + service_handle_.get(), + callback, + user_data); + + if (RCL_RET_OK != ret) { + 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 472034f362..12841fe6b6 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" @@ -39,6 +40,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), @@ -83,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; } @@ -115,7 +124,8 @@ SubscriptionBase::get_subscription_handle() const return subscription_handle_; } -const std::vector> & +const +std::unordered_map> & SubscriptionBase::get_event_handlers() const { return event_handlers_; @@ -282,7 +292,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); } @@ -290,7 +301,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 +338,19 @@ std::vector SubscriptionBase::get_network_flow_endp return network_flow_endpoint_vector; } + +void +SubscriptionBase::set_on_new_message_callback( + rcl_event_callback_t callback, + const void * user_data) +{ + rcl_ret_t ret = rcl_subscription_set_on_new_message_callback( + subscription_handle_.get(), + callback, + user_data); + + 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"); + } +} 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 b76c7215e0..16ebb1b546 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,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 and make sure to call it on the waitable destructor."); +} 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_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); + } +} diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index f844e44e6c..6671fa1176 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -514,7 +514,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 f234591e33..221e2bdf0a 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: @@ -308,3 +310,106 @@ 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) +{ + 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.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, 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_DEADLINE_MISSED); + + { + test_msgs::msg::Empty msg; + publisher->publish(msg); + } + + std::this_thread::sleep_for(std::chrono::seconds(1)); + + 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_DEADLINE_MISSED); + + EXPECT_GT(c2, 1u); +} + +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;}; + + 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)); + + 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( + 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( + pub->set_on_new_qos_event_callback(invalid_cb, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED), + 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..a10c5c4eab 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -440,6 +440,146 @@ 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(rclcpp::NodeOptions().use_intra_process_comms(false)); + using test_msgs::msg::Empty; + + auto do_nothing = [](std::shared_ptr) {FAIL();}; + 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;}; + 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 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", 10, 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 */ 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}); diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index a4ba5726e1..9bcddadec0 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -23,8 +23,11 @@ #include #include #include +#include #include +#include "rcl/event_callback.h" + #include "rclcpp/exceptions.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" @@ -62,6 +65,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 +139,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 +290,31 @@ 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 listener_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); + + 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 2f79bee041..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" @@ -70,6 +71,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 +137,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 +298,29 @@ 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 listener_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); + + bool on_ready_callback_set_{false}; }; /// Action Server diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index cedab99e92..07982092d5 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,163 @@ 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(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); + + 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)); + } + + on_ready_callback_set_ = true; +} + +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() +{ + 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(); +} + std::shared_ptr ClientBase::take_data() { diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 5ec88281da..a07e203e28 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,138 @@ 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(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); + + 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)); + } + + on_ready_callback_set_ = true; +} + +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() +{ + 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 79241a3cdc2815e6156f5f83aaceab1014db1847 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Fri, 25 Feb 2022 09:20:00 -0800 Subject: [PATCH 078/455] spin_all with a zero timeout. (#1878) * spin_all with a zero timeout. Signed-off-by: Tomoya Fujita --- rclcpp/include/rclcpp/executor.hpp | 4 +++- .../rclcpp/executors/static_single_threaded_executor.hpp | 7 ++++--- rclcpp/src/rclcpp/executor.cpp | 4 ++-- .../rclcpp/executors/static_single_threaded_executor.cpp | 4 ++-- rclcpp/test/rclcpp/test_executor.cpp | 2 +- 5 files changed, 12 insertions(+), 9 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index f3f6e9bfe4..a2dd124ff0 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -305,7 +305,9 @@ class Executor * If the time that waitables take to be executed is longer than the period on which new waitables * become ready, this method will execute work repeatedly until `max_duration` has elapsed. * - * \param[in] max_duration The maximum amount of time to spend executing work. Must be positive. + * \param[in] max_duration The maximum amount of time to spend executing work, must be >= 0. + * `0` is potentially block forever until no more work is available. + * \throw throw std::invalid_argument if max_duration is less than 0. * Note that spin_all() may take longer than this time as it only returns once max_duration has * been exceeded. */ diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index 61da15e125..5294605eaf 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -99,9 +99,10 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor /// Static executor implementation of spin all /** - * This non-blocking function will execute entities until - * timeout or no more work available. If new entities get ready - * while executing work available, they will be executed + * This non-blocking function will execute entities until timeout (must be >= 0) + * or no more work available. + * If timeout is `0`, potentially it blocks forever until no more work is available. + * If new entities get ready while executing work available, they will be executed * as long as the timeout hasn't expired. * * Example: diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 6ed3ace557..73b7b8d80d 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -414,8 +414,8 @@ void Executor::spin_some(std::chrono::nanoseconds max_duration) void Executor::spin_all(std::chrono::nanoseconds max_duration) { - if (max_duration <= 0ns) { - throw std::invalid_argument("max_duration must be positive"); + if (max_duration < 0ns) { + throw std::invalid_argument("max_duration must be greater than or equal to 0"); } return this->spin_some_impl(max_duration, true); } diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 683d300c06..209fcde556 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -71,8 +71,8 @@ StaticSingleThreadedExecutor::spin_some(std::chrono::nanoseconds max_duration) void StaticSingleThreadedExecutor::spin_all(std::chrono::nanoseconds max_duration) { - if (max_duration <= std::chrono::nanoseconds(0)) { - throw std::invalid_argument("max_duration must be positive"); + if (max_duration < std::chrono::nanoseconds(0)) { + throw std::invalid_argument("max_duration must be greater than or equal to 0"); } return this->spin_some_impl(max_duration, true); } diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index 0b007d519b..bdbb0a1079 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -248,7 +248,7 @@ TEST_F(TestExecutor, spin_all_invalid_duration) { RCLCPP_EXPECT_THROW_EQ( dummy.spin_all(std::chrono::nanoseconds(-1)), - std::invalid_argument("max_duration must be positive")); + std::invalid_argument("max_duration must be greater than or equal to 0")); } TEST_F(TestExecutor, spin_some_in_spin_some) { From 4e3c6be76a8a1e91a3f1cafcfc80f52c7e072daf Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 1 Mar 2022 19:33:22 +0000 Subject: [PATCH 079/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 14 ++++++++++++++ rclcpp_action/CHANGELOG.rst | 6 ++++++ rclcpp_components/CHANGELOG.rst | 8 ++++++++ rclcpp_lifecycle/CHANGELOG.rst | 6 ++++++ 4 files changed, 34 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index e71a1ab69a..7d96989168 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* spin_all with a zero timeout. (`#1878 `_) +* Add RMW listener APIs (`#1579 `_) +* Remove fastrtps customization on tests (`#1887 `_) +* Install headers to include/${PROJECT_NAME} (`#1888 `_) +* Use ament_generate_version_header (`#1886 `_) +* use universal reference to support rvalue. (`#1883 `_) +* fix one subscription can wait_for_message twice (`#1870 `_) +* Add return value version of get_parameter_or (`#1813 `_) +* Cleanup time source object lifetimes (`#1867 `_) +* add is_spinning() method to executor base class +* Contributors: Alberto Soragna, Chen Lihui, Chris Lalancette, Kenji Miyake, Miguel Company, Shane Loretz, Tomoya Fujita, iRobot ROS + 15.0.0 (2022-01-14) ------------------- * Cleanup the TypeAdapt tests (`#1858 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 8b9312df43..92d277f7b8 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,12 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add RMW listener APIs (`#1579 `_) +* Install headers to include/${PROJECT_NAME} (`#1888 `_) +* Contributors: Shane Loretz, iRobot ROS + 15.0.0 (2022-01-14) ------------------- * Fix include order and relative paths for cpplint (`#1859 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 6373079eb5..694baa754d 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* small improvements to node_main.cpp.in +* Install headers to include/${PROJECT_NAME} (`#1888 `_) +* Use spin() in component_manager_isolated.hpp (`#1881 `_) +* add use_global_arguments for node options of component nodes (`#1776 `_) +* Contributors: Alberto Soragna, Shane Loretz, gezp + 15.0.0 (2022-01-14) ------------------- * Add rclcpp_components::component (`#1855 `_) diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 3a355582c6..48bf74f74d 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,12 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Install headers to include/${PROJECT_NAME} (`#1888 `_) +* LifecycleNode::on_deactivate deactivate all managed entities. (`#1885 `_) +* Contributors: Shane Loretz, Tomoya Fujita + 15.0.0 (2022-01-14) ------------------- * Automatically transition lifecycle entities when node transitions (`#1863 `_) From 16914e31a15417d5751c4cb611f591bbaa458eca Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 1 Mar 2022 19:33:31 +0000 Subject: [PATCH 080/455] 15.1.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 7d96989168..f368a5eb3f 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +15.1.0 (2022-03-01) +------------------- * spin_all with a zero timeout. (`#1878 `_) * Add RMW listener APIs (`#1579 `_) * Remove fastrtps customization on tests (`#1887 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 52cd61807b..43f6ae2527 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 15.0.0 + 15.1.0 The ROS client library in C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 92d277f7b8..cdb217533e 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +15.1.0 (2022-03-01) +------------------- * Add RMW listener APIs (`#1579 `_) * Install headers to include/${PROJECT_NAME} (`#1888 `_) * Contributors: Shane Loretz, iRobot ROS diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 73ec8bfee7..213f81fb7c 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 15.0.0 + 15.1.0 Adds action APIs for C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 694baa754d..6c4bae3687 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +15.1.0 (2022-03-01) +------------------- * small improvements to node_main.cpp.in * Install headers to include/${PROJECT_NAME} (`#1888 `_) * Use spin() in component_manager_isolated.hpp (`#1881 `_) diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index b1d03eaa51..c5941ac74b 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 15.0.0 + 15.1.0 Package containing tools for dynamically loadable components Ivan Paunovic Jacob Perron diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 48bf74f74d..ee0f49a0a1 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +15.1.0 (2022-03-01) +------------------- * Install headers to include/${PROJECT_NAME} (`#1888 `_) * LifecycleNode::on_deactivate deactivate all managed entities. (`#1885 `_) * Contributors: Shane Loretz, Tomoya Fujita diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index e708c8c79a..6a537dfcb4 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 15.0.0 + 15.1.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Jacob Perron From f2f7ffdf532fe37479bedf2797ef96de9513018f Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Tue, 8 Mar 2022 19:55:06 +0000 Subject: [PATCH 081/455] fix bugprone-exception-escape in node_main.cpp.in (#1895) Signed-off-by: Alberto Soragna --- rclcpp_components/src/node_main.cpp.in | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/rclcpp_components/src/node_main.cpp.in b/rclcpp_components/src/node_main.cpp.in index a6045ee00a..487331b251 100644 --- a/rclcpp_components/src/node_main.cpp.in +++ b/rclcpp_components/src/node_main.cpp.in @@ -43,7 +43,16 @@ int main(int argc, char * argv[]) std::string name = clazz.c_str(); if (name.compare(class_name) == 0) { RCLCPP_DEBUG(logger, "Instantiate class %s", clazz.c_str()); - auto node_factory = loader->createInstance(clazz); + std::shared_ptr node_factory = nullptr; + try { + node_factory = loader->createInstance(clazz); + } catch (const std::exception & ex) { + RCLCPP_ERROR(logger, "Failed to load library %s", ex.what()); + return 1; + } catch (...) { + RCLCPP_ERROR(logger, "Failed to load library"); + return 1; + } auto wrapper = node_factory->create_node_instance(options); auto node = wrapper.get_node_base_interface(); node_wrappers.push_back(wrapper); From 3bd6900f98968c4cb77c7d594e18af9f34d92270 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 9 Mar 2022 14:13:36 -0500 Subject: [PATCH 082/455] Micro-optimizations in rclcpp (#1896) * Use a single call to collect all CallbackGroup pointers. Believe it or not, the taking and releasing of mutex_ within the CallbackGroup class shows up in profiles. However, when collecting entities we really don't need to take it and drop it between each of subscriptions, services, clients, timers, and waitables. Just take it once at the beginning, collect everything, and then return, which removes a lot of unnecessary work with that mutex. Signed-off-by: Chris Lalancette * Pass shared_ptr as constref in the MemoryStrategy class. That way, in the case that the callee doesn't need to take a reference, we avoid creating and destroying a shared_ptr class. This reduces the overhead in using these functions, and seems to work fine in the default case. If a user wants to subclass the MemoryStrategy class, then they can explicitly create a shared_ptr copy by using the copy constructor. Signed-off-by: Chris Lalancette * Use constref shared_ptr when iterating. Believe it or not, the creation and destruction of the shared_ptr class itself shows up in profiles in these loops. Since we don't need to hold onto a reference while iterating (the class is already doing that), just use constref wherever we can which drastically reduces the overhead. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/callback_group.hpp | 8 ++++ rclcpp/include/rclcpp/memory_strategy.hpp | 20 ++++----- .../strategies/allocator_memory_strategy.hpp | 44 ++++++++---------- rclcpp/src/rclcpp/callback_group.cpp | 45 +++++++++++++++++++ rclcpp/src/rclcpp/memory_strategy.cpp | 20 ++++----- 5 files changed, 91 insertions(+), 46 deletions(-) diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index efa8352206..94bceced81 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -138,6 +138,14 @@ class CallbackGroup const CallbackGroupType & type() const; + RCLCPP_PUBLIC + void collect_all_ptrs( + std::function sub_func, + std::function service_func, + std::function client_func, + std::function timer_func, + std::function waitable_func) const; + /// Return a reference to the 'associated with executor' atomic boolean. /** * When a callback group is added to an executor this boolean is checked diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index 0993c223c9..fb5ba2a63f 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -100,52 +100,52 @@ class RCLCPP_PUBLIC MemoryStrategy static rclcpp::SubscriptionBase::SharedPtr get_subscription_by_handle( - std::shared_ptr subscriber_handle, + const std::shared_ptr & subscriber_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::ServiceBase::SharedPtr get_service_by_handle( - std::shared_ptr service_handle, + const std::shared_ptr & service_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::ClientBase::SharedPtr get_client_by_handle( - std::shared_ptr client_handle, + const std::shared_ptr & client_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::TimerBase::SharedPtr get_timer_by_handle( - std::shared_ptr timer_handle, + const std::shared_ptr & timer_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_by_group( - rclcpp::CallbackGroup::SharedPtr group, + const rclcpp::CallbackGroup::SharedPtr & group, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::CallbackGroup::SharedPtr get_group_by_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription, + const rclcpp::SubscriptionBase::SharedPtr & subscription, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::CallbackGroup::SharedPtr get_group_by_service( - rclcpp::ServiceBase::SharedPtr service, + const rclcpp::ServiceBase::SharedPtr & service, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::CallbackGroup::SharedPtr get_group_by_client( - rclcpp::ClientBase::SharedPtr client, + const rclcpp::ClientBase::SharedPtr & client, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::CallbackGroup::SharedPtr get_group_by_timer( - rclcpp::TimerBase::SharedPtr timer, + const rclcpp::TimerBase::SharedPtr & timer, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::CallbackGroup::SharedPtr get_group_by_waitable( - rclcpp::Waitable::SharedPtr waitable, + const rclcpp::Waitable::SharedPtr & waitable, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); }; diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index d0bb9c7df2..88698179d4 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -164,30 +164,22 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy if (!group || !group->can_be_taken_from().load()) { continue; } - group->find_subscription_ptrs_if( + + group->collect_all_ptrs( [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) { subscription_handles_.push_back(subscription->get_subscription_handle()); - return false; - }); - group->find_service_ptrs_if( + }, [this](const rclcpp::ServiceBase::SharedPtr & service) { service_handles_.push_back(service->get_service_handle()); - return false; - }); - group->find_client_ptrs_if( + }, [this](const rclcpp::ClientBase::SharedPtr & client) { client_handles_.push_back(client->get_client_handle()); - return false; - }); - group->find_timer_ptrs_if( + }, [this](const rclcpp::TimerBase::SharedPtr & timer) { timer_handles_.push_back(timer->get_timer_handle()); - return false; - }); - group->find_waitable_ptrs_if( + }, [this](const rclcpp::Waitable::SharedPtr & waitable) { waitable_handles_.push_back(waitable); - return false; }); } @@ -204,7 +196,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) override { - for (auto subscription : subscription_handles_) { + for (const std::shared_ptr & subscription : subscription_handles_) { if (rcl_wait_set_add_subscription(wait_set, subscription.get(), NULL) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", @@ -213,7 +205,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - for (auto client : client_handles_) { + for (const std::shared_ptr & client : client_handles_) { if (rcl_wait_set_add_client(wait_set, client.get(), NULL) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", @@ -222,7 +214,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - for (auto service : service_handles_) { + for (const std::shared_ptr & service : service_handles_) { if (rcl_wait_set_add_service(wait_set, service.get(), NULL) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", @@ -231,7 +223,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - for (auto timer : timer_handles_) { + for (const std::shared_ptr & timer : timer_handles_) { if (rcl_wait_set_add_timer(wait_set, timer.get(), NULL) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", @@ -244,7 +236,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy detail::add_guard_condition_to_rcl_wait_set(*wait_set, *guard_condition); } - for (auto waitable : waitable_handles_) { + for (const std::shared_ptr & waitable : waitable_handles_) { waitable->add_to_wait_set(wait_set); } return true; @@ -402,7 +394,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy { auto it = waitable_handles_.begin(); while (it != waitable_handles_.end()) { - auto waitable = *it; + std::shared_ptr & waitable = *it; if (waitable) { // Find the group for this handle and see if it can be serviced auto group = get_group_by_waitable(waitable, weak_groups_to_nodes); @@ -438,7 +430,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_subscriptions() const override { size_t number_of_subscriptions = subscription_handles_.size(); - for (auto waitable : waitable_handles_) { + for (const std::shared_ptr & waitable : waitable_handles_) { number_of_subscriptions += waitable->get_number_of_ready_subscriptions(); } return number_of_subscriptions; @@ -447,7 +439,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_services() const override { size_t number_of_services = service_handles_.size(); - for (auto waitable : waitable_handles_) { + for (const std::shared_ptr & waitable : waitable_handles_) { number_of_services += waitable->get_number_of_ready_services(); } return number_of_services; @@ -456,7 +448,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_events() const override { size_t number_of_events = 0; - for (auto waitable : waitable_handles_) { + for (const std::shared_ptr & waitable : waitable_handles_) { number_of_events += waitable->get_number_of_ready_events(); } return number_of_events; @@ -465,7 +457,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_clients() const override { size_t number_of_clients = client_handles_.size(); - for (auto waitable : waitable_handles_) { + for (const std::shared_ptr & waitable : waitable_handles_) { number_of_clients += waitable->get_number_of_ready_clients(); } return number_of_clients; @@ -474,7 +466,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_guard_conditions() const override { size_t number_of_guard_conditions = guard_conditions_.size(); - for (auto waitable : waitable_handles_) { + for (const std::shared_ptr & waitable : waitable_handles_) { number_of_guard_conditions += waitable->get_number_of_ready_guard_conditions(); } return number_of_guard_conditions; @@ -483,7 +475,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_timers() const override { size_t number_of_timers = timer_handles_.size(); - for (auto waitable : waitable_handles_) { + for (const std::shared_ptr & waitable : waitable_handles_) { number_of_timers += waitable->get_number_of_ready_timers(); } return number_of_timers; diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index 6b38578ba9..4b11156cf9 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -40,6 +40,51 @@ CallbackGroup::type() const return type_; } +void CallbackGroup::collect_all_ptrs( + std::function sub_func, + std::function service_func, + std::function client_func, + std::function timer_func, + std::function waitable_func) const +{ + std::lock_guard lock(mutex_); + + for (const rclcpp::SubscriptionBase::WeakPtr & weak_ptr : subscription_ptrs_) { + rclcpp::SubscriptionBase::SharedPtr ref_ptr = weak_ptr.lock(); + if (ref_ptr) { + sub_func(ref_ptr); + } + } + + for (const rclcpp::ServiceBase::WeakPtr & weak_ptr : service_ptrs_) { + rclcpp::ServiceBase::SharedPtr ref_ptr = weak_ptr.lock(); + if (ref_ptr) { + service_func(ref_ptr); + } + } + + for (const rclcpp::ClientBase::WeakPtr & weak_ptr : client_ptrs_) { + rclcpp::ClientBase::SharedPtr ref_ptr = weak_ptr.lock(); + if (ref_ptr) { + client_func(ref_ptr); + } + } + + for (const rclcpp::TimerBase::WeakPtr & weak_ptr : timer_ptrs_) { + rclcpp::TimerBase::SharedPtr ref_ptr = weak_ptr.lock(); + if (ref_ptr) { + timer_func(ref_ptr); + } + } + + for (const rclcpp::Waitable::WeakPtr & weak_ptr : waitable_ptrs_) { + rclcpp::Waitable::SharedPtr ref_ptr = weak_ptr.lock(); + if (ref_ptr) { + waitable_func(ref_ptr); + } + } +} + std::atomic_bool & CallbackGroup::get_associated_with_executor_atomic() { diff --git a/rclcpp/src/rclcpp/memory_strategy.cpp b/rclcpp/src/rclcpp/memory_strategy.cpp index bd32bdb169..cb69dc0d26 100644 --- a/rclcpp/src/rclcpp/memory_strategy.cpp +++ b/rclcpp/src/rclcpp/memory_strategy.cpp @@ -19,7 +19,7 @@ using rclcpp::memory_strategy::MemoryStrategy; rclcpp::SubscriptionBase::SharedPtr MemoryStrategy::get_subscription_by_handle( - std::shared_ptr subscriber_handle, + const std::shared_ptr & subscriber_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -40,7 +40,7 @@ MemoryStrategy::get_subscription_by_handle( rclcpp::ServiceBase::SharedPtr MemoryStrategy::get_service_by_handle( - std::shared_ptr service_handle, + const std::shared_ptr & service_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -61,7 +61,7 @@ MemoryStrategy::get_service_by_handle( rclcpp::ClientBase::SharedPtr MemoryStrategy::get_client_by_handle( - std::shared_ptr client_handle, + const std::shared_ptr & client_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -82,7 +82,7 @@ MemoryStrategy::get_client_by_handle( rclcpp::TimerBase::SharedPtr MemoryStrategy::get_timer_by_handle( - std::shared_ptr timer_handle, + const std::shared_ptr & timer_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -103,7 +103,7 @@ MemoryStrategy::get_timer_by_handle( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MemoryStrategy::get_node_by_group( - rclcpp::CallbackGroup::SharedPtr group, + const rclcpp::CallbackGroup::SharedPtr & group, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { if (!group) { @@ -121,7 +121,7 @@ MemoryStrategy::get_node_by_group( rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription, + const rclcpp::SubscriptionBase::SharedPtr & subscription, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -143,7 +143,7 @@ MemoryStrategy::get_group_by_subscription( rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_service( - rclcpp::ServiceBase::SharedPtr service, + const rclcpp::ServiceBase::SharedPtr & service, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -165,7 +165,7 @@ MemoryStrategy::get_group_by_service( rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_client( - rclcpp::ClientBase::SharedPtr client, + const rclcpp::ClientBase::SharedPtr & client, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -187,7 +187,7 @@ MemoryStrategy::get_group_by_client( rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_timer( - rclcpp::TimerBase::SharedPtr timer, + const rclcpp::TimerBase::SharedPtr & timer, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -209,7 +209,7 @@ MemoryStrategy::get_group_by_timer( rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_waitable( - rclcpp::Waitable::SharedPtr waitable, + const rclcpp::Waitable::SharedPtr & waitable, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { From 91f0b6449364b09d048e15c5b78ef9117760217b Mon Sep 17 00:00:00 2001 From: mauropasse Date: Fri, 11 Mar 2022 04:36:16 +0000 Subject: [PATCH 083/455] time_until_trigger returns max time if timer is cancelled (#1893) * time_until_trigger returns max time if timer is cancelled Signed-off-by: Mauro Passerino --- rclcpp/include/rclcpp/timer.hpp | 3 ++- rclcpp/src/rclcpp/timer.cpp | 4 +++- rclcpp/test/rclcpp/test_timer.cpp | 2 ++ 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 52df423d83..d55167e3ad 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -113,7 +113,8 @@ class TimerBase /// Check how long the timer has until its next scheduled callback. /** - * \return A std::chrono::duration representing the relative time until the next callback. + * \return A std::chrono::duration representing the relative time until the next callback + * or std::chrono::nanoseconds::max() if the timer is canceled. * \throws std::runtime_error if the rcl_timer_get_time_until_next_call returns a failure */ RCLCPP_PUBLIC diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 70414f38c6..788cdf8dce 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -119,7 +119,9 @@ TimerBase::time_until_trigger() int64_t time_until_next_call = 0; rcl_ret_t ret = rcl_timer_get_time_until_next_call( timer_handle_.get(), &time_until_next_call); - if (ret != RCL_RET_OK) { + if (ret == RCL_RET_TIMER_CANCELED) { + return std::chrono::nanoseconds::max(); + } else if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(ret, "Timer could not get time until next call"); } return std::chrono::nanoseconds(time_until_next_call); diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index 0928fcaa5d..7a6599dfe4 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -111,11 +111,13 @@ TEST_F(TestTimer, test_is_canceled_reset) // reset shouldn't affect state (not canceled yet) timer->reset(); + EXPECT_LE(timer->time_until_trigger().count(), std::chrono::nanoseconds::max().count()); EXPECT_FALSE(timer->is_canceled()); // cancel after reset timer->cancel(); EXPECT_TRUE(timer->is_canceled()); + EXPECT_EQ(timer->time_until_trigger().count(), std::chrono::nanoseconds::max().count()); // reset and cancel timer->reset(); From 2e39e0980365e08ff7d8e9a6f672aff78dc682cc Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 11 Mar 2022 14:46:07 -0500 Subject: [PATCH 084/455] Fix a bunch more rosdoc2 issues in rclcpp. (#1897) * Fix a bunch more rosdoc2 issues in rclcpp. There are a smattering of problems in here: 1. We weren't properly using PREDEFINE for all of our macros. 2. The Doxyfiles were referencing tag files that may not exist (this will be handled by rosdoc2 automatically). 3. The C++ parser in doxygen can't handle "friend classname", but can handle "friend class classname"; it shouldn't matter one way or the other to the compiler. 4. There were a couple of parameters that were not documented. 5. The doxygen C++ parser can't handle decltype in all situations. 6. There was a structure using a C-style declaration. This patch fixes all of the above issues. We still aren't totally clean on a rosdoc2 build, but we are a lot closer. Signed-off-by: Chris Lalancette --- rclcpp/Doxyfile | 8 ++++---- .../rclcpp/detail/cpp_callback_trampoline.hpp | 1 + rclcpp/include/rclcpp/executor.hpp | 2 +- rclcpp/include/rclcpp/logger.hpp | 2 +- rclcpp/include/rclcpp/parameter.hpp | 2 ++ .../detail/write_preferring_read_write_lock.hpp | 4 ++-- rclcpp_action/Doxyfile | 14 +++++++------- .../include/rclcpp_action/client_goal_handle.hpp | 6 +++--- .../include/rclcpp_action/server_goal_handle.hpp | 2 +- rclcpp_action/include/rclcpp_action/types.hpp | 1 + rclcpp_components/Doxyfile | 12 ++++++------ rclcpp_lifecycle/Doxyfile | 9 +++++---- 12 files changed, 34 insertions(+), 29 deletions(-) diff --git a/rclcpp/Doxyfile b/rclcpp/Doxyfile index 01541e33d0..3c76693821 100644 --- a/rclcpp/Doxyfile +++ b/rclcpp/Doxyfile @@ -33,10 +33,10 @@ PREDEFINED += RCPPUTILS_TSA_REQUIRES(x)= DOT_GRAPH_MAX_NODES = 101 # Tag files that do not exist will produce a warning and cross-project linking will not work. -TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" +#TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" # Consider changing "latest" to the version you want to reference (e.g. beta1 or 1.0.0) -TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/" -TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" -TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/" +#TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/" +#TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" +#TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/" # Uncomment to generate tag files for cross-project linking. #GENERATE_TAGFILE = "../../../../doxygen_tag_files/rclcpp.tag" diff --git a/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp b/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp index be857e1b58..4f46dfc939 100644 --- a/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp +++ b/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp @@ -46,6 +46,7 @@ namespace detail * \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 + * \param args the arguments to be forwarded to the callback * \returns whatever the callback returns, if anything */ template< diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index a2dd124ff0..ed2ddc4a0a 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -307,7 +307,7 @@ class Executor * * \param[in] max_duration The maximum amount of time to spend executing work, must be >= 0. * `0` is potentially block forever until no more work is available. - * \throw throw std::invalid_argument if max_duration is less than 0. + * \throw std::invalid_argument if max_duration is less than 0. * Note that spin_all() may take longer than this time as it only returns once max_duration has * been exceeded. */ diff --git a/rclcpp/include/rclcpp/logger.hpp b/rclcpp/include/rclcpp/logger.hpp index e1ab4945f3..c5e248028e 100644 --- a/rclcpp/include/rclcpp/logger.hpp +++ b/rclcpp/include/rclcpp/logger.hpp @@ -79,7 +79,7 @@ get_node_logger(const rcl_node_t * node); /// Get the current logging directory. /** * For more details of how the logging directory is determined, - * see \ref rcl_logging_get_logging_directory. + * see rcl_logging_get_logging_directory(). * * \returns the logging directory being used. * \throws rclcpp::exceptions::RCLError if an unexpected error occurs. diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index 68147ed731..85f088d025 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -264,6 +264,7 @@ get_value_helper(const rclcpp::Parameter * parameter) } // namespace detail +/// \cond template decltype(auto) Parameter::get_value() const @@ -275,6 +276,7 @@ Parameter::get_value() const throw exceptions::InvalidParameterTypeException(this->name_, ex.what()); } } +/// \endcond } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp index 90b9df0178..933bbf6895 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp @@ -191,7 +191,7 @@ class WritePreferringReadWriteLock final WritePreferringReadWriteLock & parent_lock_; - friend WritePreferringReadWriteLock; + friend class WritePreferringReadWriteLock; }; /// Write mutex for the WritePreferringReadWriteLock. @@ -212,7 +212,7 @@ class WritePreferringReadWriteLock final WritePreferringReadWriteLock & parent_lock_; - friend WritePreferringReadWriteLock; + friend class WritePreferringReadWriteLock; }; /// Return read mutex which can be used with standard constructs like std::lock_guard. diff --git a/rclcpp_action/Doxyfile b/rclcpp_action/Doxyfile index 4b97902c6c..e7c9fedb59 100644 --- a/rclcpp_action/Doxyfile +++ b/rclcpp_action/Doxyfile @@ -21,15 +21,15 @@ GENERATE_LATEX = NO ENABLE_PREPROCESSING = YES MACRO_EXPANSION = YES EXPAND_ONLY_PREDEF = YES -PREDEFINED = RCLCPP_PUBLIC= +PREDEFINED = RCLCPP_ACTION_PUBLIC= # Tag files that do not exist will produce a warning and cross-project linking will not work. -TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" +#TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" # Consider changing "latest" to the version you want to reference (e.g. beta1 or 1.0.0) -TAGFILES += "../../../../doxygen_tag_files/rclcpp.tag=http://docs.ros2.org/latest/api/rclcpp/" -TAGFILES += "../../../../doxygen_tag_files/rcl_action.tag=http://docs.ros2.org/latest/api/rcl_action/" -TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/" -TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" -TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/" +#TAGFILES += "../../../../doxygen_tag_files/rclcpp.tag=http://docs.ros2.org/latest/api/rclcpp/" +#TAGFILES += "../../../../doxygen_tag_files/rcl_action.tag=http://docs.ros2.org/latest/api/rcl_action/" +#TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/" +#TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" +#TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/" # Uncomment to generate tag files for cross-project linking. # GENERATE_TAGFILE = "../../../../doxygen_tag_files/rclcpp_action.tag" diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index 4989a71bbc..dd3a40671c 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -62,7 +62,7 @@ class ClientGoalHandle RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientGoalHandle) // A wrapper that defines the result of an action - typedef struct WrappedResult + struct WrappedResult { /// The unique identifier of the goal GoalUUID goal_id; @@ -70,7 +70,7 @@ class ClientGoalHandle ResultCode code; /// User defined fields sent back with an action typename ActionT::Result::SharedPtr result; - } WrappedResult; + }; using Feedback = typename ActionT::Feedback; using Result = typename ActionT::Result; @@ -104,7 +104,7 @@ class ClientGoalHandle private: // The templated Client creates goal handles - friend Client; + friend class Client; ClientGoalHandle( const GoalInfo & info, diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index 857ca7332a..ac9dd49492 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -273,7 +273,7 @@ class ServerGoalHandle : public ServerGoalHandleBase /// A unique id for the goal request. const GoalUUID uuid_; - friend Server; + friend class Server; std::function)> on_terminal_state_; std::function on_executing_; diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index 09717b16ea..4f8548ff76 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP_ACTION__TYPES_HPP_ #define RCLCPP_ACTION__TYPES_HPP_ +#include #include #include #include diff --git a/rclcpp_components/Doxyfile b/rclcpp_components/Doxyfile index da33050c4a..11fe1e178f 100644 --- a/rclcpp_components/Doxyfile +++ b/rclcpp_components/Doxyfile @@ -22,12 +22,12 @@ EXPAND_ONLY_PREDEF = YES PREDEFINED = RCLCPP_COMPONENTS_PUBLIC= # Tag files that do not exist will produce a warning and cross-project linking will not work. -TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" +#TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" # Consider changing "latest" to the version you want to reference (e.g. beta1 or 1.0.0) -TAGFILES += "../../../../doxygen_tag_files/class_loader.tag=http://docs.ros2.org/latest/api/class_loader/" -TAGFILES += "../../../../doxygen_tag_files/rclcpp.tag=http://docs.ros2.org/latest/api/rclcpp/" -TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/" -TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/" -TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" +#TAGFILES += "../../../../doxygen_tag_files/class_loader.tag=http://docs.ros2.org/latest/api/class_loader/" +#TAGFILES += "../../../../doxygen_tag_files/rclcpp.tag=http://docs.ros2.org/latest/api/rclcpp/" +#TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/" +#TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/" +#TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" # Uncomment to generate tag files for cross-project linking. GENERATE_TAGFILE = "../../../../doxygen_tag_files/rclcpp_components.tag" diff --git a/rclcpp_lifecycle/Doxyfile b/rclcpp_lifecycle/Doxyfile index 9a83ee5e85..3f7644b82e 100644 --- a/rclcpp_lifecycle/Doxyfile +++ b/rclcpp_lifecycle/Doxyfile @@ -22,12 +22,13 @@ ENABLE_PREPROCESSING = YES MACRO_EXPANSION = YES EXPAND_ONLY_PREDEF = YES PREDEFINED = RCLCPP_LIFECYCLE_PUBLIC= +PREDEFINED += RCUTILS_WARN_UNUSED # Tag files that do not exist will produce a warning and cross-project linking will not work. -TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" +#TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" # Consider changing "latest" to the version you want to reference (e.g. beta1 or 1.0.0) -TAGFILES += "../../../../doxygen_tag_files/rclcpp.tag=http://docs.ros2.org/latest/api/rclcpp/" -TAGFILES += "../../../../doxygen_tag_files/rcl_lifecycle.tag=http://docs.ros2.org/latest/api/rcl_lifecycle/" -TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" +#TAGFILES += "../../../../doxygen_tag_files/rclcpp.tag=http://docs.ros2.org/latest/api/rclcpp/" +#TAGFILES += "../../../../doxygen_tag_files/rcl_lifecycle.tag=http://docs.ros2.org/latest/api/rcl_lifecycle/" +#TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" # Uncomment to generate tag files for cross-project linking. GENERATE_TAGFILE = "../../../../doxygen_tag_files/rclcpp_lifecycle.tag" From 8b9cabfb9d41ed2e599d02d741501c839190d5a4 Mon Sep 17 00:00:00 2001 From: mauropasse Date: Wed, 16 Mar 2022 18:05:58 +0000 Subject: [PATCH 085/455] Add client/service QoS getters (#1784) Signed-off-by: Mauro Passerino Co-authored-by: Mauro Passerino --- rclcpp/include/rclcpp/client.hpp | 33 +++++++++ rclcpp/include/rclcpp/service.hpp | 33 +++++++++ rclcpp/src/rclcpp/client.cpp | 38 ++++++++++ rclcpp/src/rclcpp/service.cpp | 38 ++++++++++ rclcpp/test/rclcpp/test_client.cpp | 103 ++++++++++++++++++++++++++++ rclcpp/test/rclcpp/test_service.cpp | 101 +++++++++++++++++++++++++++ 6 files changed, 346 insertions(+) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index e169b890bb..e88fa8a949 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -40,6 +40,7 @@ #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" @@ -218,6 +219,38 @@ class ClientBase bool exchange_in_use_by_wait_set_state(bool in_use_state); + /// Get the actual request publsher QoS settings, after the defaults have been determined. + /** + * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT + * can only be resolved after the creation of the client, and it + * depends on the underlying rmw implementation. + * If the underlying setting in use can't be represented in ROS terms, + * it will be set to RMW_QOS_POLICY_*_UNKNOWN. + * May throw runtime_error when an unexpected error occurs. + * + * \return The actual request publsher qos settings. + * \throws std::runtime_error if failed to get qos settings + */ + RCLCPP_PUBLIC + rclcpp::QoS + get_request_publisher_actual_qos() const; + + /// Get the actual response subscription QoS settings, after the defaults have been determined. + /** + * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT + * can only be resolved after the creation of the client, and it + * depends on the underlying rmw implementation. + * If the underlying setting in use can't be represented in ROS terms, + * it will be set to RMW_QOS_POLICY_*_UNKNOWN. + * May throw runtime_error when an unexpected error occurs. + * + * \return The actual response subscription qos settings. + * \throws std::runtime_error if failed to get qos settings + */ + RCLCPP_PUBLIC + rclcpp::QoS + get_response_subscription_actual_qos() const; + /// 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 diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 47704a4c09..3f500eaa09 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -39,6 +39,7 @@ #include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" @@ -127,6 +128,38 @@ class ServiceBase bool exchange_in_use_by_wait_set_state(bool in_use_state); + /// Get the actual response publisher QoS settings, after the defaults have been determined. + /** + * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT + * can only be resolved after the creation of the service, and it + * depends on the underlying rmw implementation. + * If the underlying setting in use can't be represented in ROS terms, + * it will be set to RMW_QOS_POLICY_*_UNKNOWN. + * May throw runtime_error when an unexpected error occurs. + * + * \return The actual response publisher qos settings. + * \throws std::runtime_error if failed to get qos settings + */ + RCLCPP_PUBLIC + rclcpp::QoS + get_response_publisher_actual_qos() const; + + /// Get the actual request subscription QoS settings, after the defaults have been determined. + /** + * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT + * can only be resolved after the creation of the service, and it + * depends on the underlying rmw implementation. + * If the underlying setting in use can't be represented in ROS terms, + * it will be set to RMW_QOS_POLICY_*_UNKNOWN. + * May throw runtime_error when an unexpected error occurs. + * + * \return The actual request subscription qos settings. + * \throws std::runtime_error if failed to get qos settings + */ + RCLCPP_PUBLIC + rclcpp::QoS + get_request_subscription_actual_qos() const; + /// 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 diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 2380b890f5..17cc68e153 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -201,6 +201,44 @@ ClientBase::exchange_in_use_by_wait_set_state(bool in_use_state) return in_use_by_wait_set_.exchange(in_use_state); } +rclcpp::QoS +ClientBase::get_request_publisher_actual_qos() const +{ + const rmw_qos_profile_t * qos = + rcl_client_request_publisher_get_actual_qos(client_handle_.get()); + if (!qos) { + auto msg = + std::string("failed to get client's request publisher qos settings: ") + + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + + rclcpp::QoS request_publisher_qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); + + return request_publisher_qos; +} + +rclcpp::QoS +ClientBase::get_response_subscription_actual_qos() const +{ + const rmw_qos_profile_t * qos = + rcl_client_response_subscription_get_actual_qos(client_handle_.get()); + if (!qos) { + auto msg = + std::string("failed to get client's response subscription qos settings: ") + + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + + rclcpp::QoS response_subscription_qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); + + return response_subscription_qos; +} + void ClientBase::set_on_new_response_callback(rcl_event_callback_t callback, const void * user_data) { diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index d2f333cacf..7bc580ce68 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -88,6 +88,44 @@ ServiceBase::exchange_in_use_by_wait_set_state(bool in_use_state) return in_use_by_wait_set_.exchange(in_use_state); } +rclcpp::QoS +ServiceBase::get_response_publisher_actual_qos() const +{ + const rmw_qos_profile_t * qos = + rcl_service_response_publisher_get_actual_qos(service_handle_.get()); + if (!qos) { + auto msg = + std::string("failed to get service's response publisher qos settings: ") + + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + + rclcpp::QoS response_publisher_qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); + + return response_publisher_qos; +} + +rclcpp::QoS +ServiceBase::get_request_subscription_actual_qos() const +{ + const rmw_qos_profile_t * qos = + rcl_service_request_subscription_get_actual_qos(service_handle_.get()); + if (!qos) { + auto msg = + std::string("failed to get service's request subscription qos settings: ") + + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + + rclcpp::QoS request_subscription_qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); + + return request_subscription_qos; +} + void ServiceBase::set_on_new_request_callback(rcl_event_callback_t callback, const void * user_data) { diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp index 5e18fd8b4a..ee7e26d3a4 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -24,6 +24,7 @@ #include "rcl_interfaces/srv/list_parameters.hpp" #include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" #include "test_msgs/srv/empty.hpp" @@ -431,3 +432,105 @@ TEST_F(TestClient, on_new_response_callback) { std::function invalid_cb = nullptr; EXPECT_THROW(client->set_on_new_response_callback(invalid_cb), std::invalid_argument); } + +TEST_F(TestClient, rcl_client_request_publisher_get_actual_qos_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_client_request_publisher_get_actual_qos, nullptr); + auto client = node->create_client("service"); + RCLCPP_EXPECT_THROW_EQ( + client->get_request_publisher_actual_qos(), + std::runtime_error("failed to get client's request publisher qos settings: error not set")); +} + +TEST_F(TestClient, rcl_client_response_subscription_get_actual_qos_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_client_response_subscription_get_actual_qos, nullptr); + auto client = node->create_client("service"); + RCLCPP_EXPECT_THROW_EQ( + client->get_response_subscription_actual_qos(), + std::runtime_error("failed to get client's response subscription qos settings: error not set")); +} + +TEST_F(TestClient, client_qos) { + rmw_qos_profile_t qos_profile = rmw_qos_profile_services_default; + qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + uint64_t duration = 1; + qos_profile.deadline = {duration, duration}; + qos_profile.lifespan = {duration, duration}; + qos_profile.liveliness_lease_duration = {duration, duration}; + + auto client = + node->create_client("client", qos_profile); + + auto init_qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile), qos_profile); + auto rp_qos = client->get_request_publisher_actual_qos(); + auto rs_qos = client->get_response_subscription_actual_qos(); + + EXPECT_EQ(init_qos, rp_qos); + // Lifespan has no meaning for subscription/readers + rs_qos.lifespan(qos_profile.lifespan); + EXPECT_EQ(init_qos, rs_qos); +} + +TEST_F(TestClient, client_qos_depth) { + using namespace std::literals::chrono_literals; + + rmw_qos_profile_t client_qos_profile = rmw_qos_profile_default; + client_qos_profile.depth = 2; + + auto client = node->create_client("test_qos_depth", client_qos_profile); + + uint64_t server_cb_count_ = 0; + auto server_callback = [&]( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {server_cb_count_++;}; + + auto server_node = std::make_shared("server_node", "/ns"); + + rmw_qos_profile_t server_qos_profile = rmw_qos_profile_default; + + auto server = server_node->create_service( + "test_qos_depth", std::move(server_callback), server_qos_profile); + + auto request = std::make_shared(); + ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); + + using SharedFuture = rclcpp::Client::SharedFuture; + uint64_t client_cb_count_ = 0; + auto client_callback = [&client_cb_count_, &request_result](SharedFuture future_response) { + if (nullptr == future_response.get()) { + request_result = ::testing::AssertionFailure() << "Future response was null"; + } + client_cb_count_++; + }; + + uint64_t client_requests = 5; + for (uint64_t i = 0; i < client_requests; i++) { + client->async_send_request(request, client_callback); + std::this_thread::sleep_for(10ms); + } + + auto start = std::chrono::steady_clock::now(); + while ((server_cb_count_ < client_requests) && + (std::chrono::steady_clock::now() - start) < 2s) + { + rclcpp::spin_some(server_node); + std::this_thread::sleep_for(2ms); + } + + EXPECT_GT(server_cb_count_, client_qos_profile.depth); + + start = std::chrono::steady_clock::now(); + while ((client_cb_count_ < client_qos_profile.depth) && + (std::chrono::steady_clock::now() - start) < 1s) + { + rclcpp::spin_some(node); + } + + // Spin an extra time to check if client QoS depth has been ignored, + // so more client callbacks might be called than expected. + rclcpp::spin_some(node); + + EXPECT_EQ(client_cb_count_, client_qos_profile.depth); +} diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index 110e913da4..e3399be423 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -22,6 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" #include "rcl_interfaces/srv/list_parameters.hpp" #include "test_msgs/srv/empty.hpp" @@ -306,3 +307,103 @@ TEST_F(TestService, on_new_request_callback) { std::function invalid_cb = nullptr; EXPECT_THROW(server->set_on_new_request_callback(invalid_cb), std::invalid_argument); } + +TEST_F(TestService, rcl_service_response_publisher_get_actual_qos_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_response_publisher_get_actual_qos, nullptr); + auto callback = + [](const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + auto server = node->create_service("service", callback); + RCLCPP_EXPECT_THROW_EQ( + server->get_response_publisher_actual_qos(), + std::runtime_error("failed to get service's response publisher qos settings: error not set")); +} + +TEST_F(TestService, rcl_service_request_subscription_get_actual_qos_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_request_subscription_get_actual_qos, nullptr); + auto callback = + [](const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + auto server = node->create_service("service", callback); + RCLCPP_EXPECT_THROW_EQ( + server->get_request_subscription_actual_qos(), + std::runtime_error("failed to get service's request subscription qos settings: error not set")); +} + + +TEST_F(TestService, server_qos) { + rmw_qos_profile_t qos_profile = rmw_qos_profile_services_default; + qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + uint64_t duration = 1; + qos_profile.deadline = {duration, duration}; + qos_profile.lifespan = {duration, duration}; + qos_profile.liveliness_lease_duration = {duration, duration}; + + auto callback = [](const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + + auto server = node->create_service( + "service", callback, + qos_profile); + auto init_qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile), qos_profile); + auto rs_qos = server->get_request_subscription_actual_qos(); + auto rp_qos = server->get_response_publisher_actual_qos(); + + EXPECT_EQ(init_qos, rp_qos); + // Lifespan has no meaning for subscription/readers + rs_qos.lifespan(qos_profile.lifespan); + EXPECT_EQ(init_qos, rs_qos); +} + +TEST_F(TestService, server_qos_depth) { + using namespace std::literals::chrono_literals; + + uint64_t server_cb_count_ = 0; + auto server_callback = [&]( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {server_cb_count_++;}; + + auto server_node = std::make_shared("server_node", "/ns"); + + rmw_qos_profile_t server_qos_profile = rmw_qos_profile_default; + server_qos_profile.depth = 2; + + auto server = server_node->create_service( + "test_qos_depth", std::move(server_callback), server_qos_profile); + + rmw_qos_profile_t client_qos_profile = rmw_qos_profile_default; + auto client = node->create_client("test_qos_depth", client_qos_profile); + + ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); + auto request = std::make_shared(); + + using SharedFuture = rclcpp::Client::SharedFuture; + auto client_callback = [&request_result](SharedFuture future_response) { + if (nullptr == future_response.get()) { + request_result = ::testing::AssertionFailure() << "Future response was null"; + } + }; + + uint64_t client_requests = 5; + for (uint64_t i = 0; i < client_requests; i++) { + client->async_send_request(request, client_callback); + std::this_thread::sleep_for(10ms); + } + + auto start = std::chrono::steady_clock::now(); + while ((server_cb_count_ < server_qos_profile.depth) && + (std::chrono::steady_clock::now() - start) < 1s) + { + rclcpp::spin_some(server_node); + std::this_thread::sleep_for(1ms); + } + + // Spin an extra time to check if server QoS depth has been ignored, + // so more server responses might be processed than expected. + rclcpp::spin_some(server_node); + + EXPECT_EQ(server_cb_count_, server_qos_profile.depth); +} From 0f58bb8700e3dcdc1465d2465f5a0160db367b3f Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 16 Mar 2022 13:58:49 -0700 Subject: [PATCH 086/455] clang-tidy: explicit constructors (#1782) * mark some single-argument constructors explicit that should have been Signed-off-by: William Woodall * suppress clang-tidy [google-explicit-constructor] in some cases where it is a false-positive Signed-off-by: William Woodall * Revert "suppress clang-tidy [google-explicit-constructor] in some cases where it is a false-positive" This reverts commit eb6bf7e2df0dd27c945e97584ba9902ef9f61305. Signed-off-by: William Woodall --- rclcpp/include/rclcpp/allocator/allocator_deleter.hpp | 2 +- rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp | 2 +- rclcpp/include/rclcpp/parameter_client.hpp | 6 +++--- rclcpp/include/rclcpp/parameter_event_handler.hpp | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp index cfc1e15922..73fbc7b9f7 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp @@ -41,7 +41,7 @@ class AllocatorDeleter } template - AllocatorDeleter(const AllocatorDeleter & a) + explicit AllocatorDeleter(const AllocatorDeleter & a) { allocator_ = a.get_allocator(); } diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index a1befba300..c1de8051f2 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -52,7 +52,7 @@ class MultiThreadedExecutor : public rclcpp::Executor * \param timeout maximum time to wait */ RCLCPP_PUBLIC - MultiThreadedExecutor( + explicit MultiThreadedExecutor( const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions(), size_t number_of_threads = 0, bool yield_before_execute = false, diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index e03d7f519f..5e509bbf10 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -79,7 +79,7 @@ class AsyncParametersClient * \param[in] group (optional) The async parameter client will be added to this callback group. */ template - AsyncParametersClient( + explicit AsyncParametersClient( const std::shared_ptr node, const std::string & remote_node_name = "", const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters, @@ -102,7 +102,7 @@ class AsyncParametersClient * \param[in] group (optional) The async parameter client will be added to this callback group. */ template - AsyncParametersClient( + explicit AsyncParametersClient( NodeT * node, const std::string & remote_node_name = "", const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters, @@ -333,7 +333,7 @@ class SyncParametersClient {} template - SyncParametersClient( + explicit SyncParametersClient( NodeT * node, const std::string & remote_node_name = "", const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters) diff --git a/rclcpp/include/rclcpp/parameter_event_handler.hpp b/rclcpp/include/rclcpp/parameter_event_handler.hpp index 04de6c1862..c2ee55a540 100644 --- a/rclcpp/include/rclcpp/parameter_event_handler.hpp +++ b/rclcpp/include/rclcpp/parameter_event_handler.hpp @@ -165,7 +165,7 @@ class ParameterEventHandler * \param[in] qos The QoS settings to use for any subscriptions. */ template - ParameterEventHandler( + explicit ParameterEventHandler( NodeT node, const rclcpp::QoS & qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events))) From 9ae35e347ead6394d39fc573a1f9af999b5e72a7 Mon Sep 17 00:00:00 2001 From: Hirokazu Ishida <38597814+HiroIshida@users.noreply.github.com> Date: Sat, 19 Mar 2022 05:42:06 +0900 Subject: [PATCH 087/455] Select executor in node registration (#1898) * Select executor Signed-off-by: Hirokazu Ishida * Fix indent Signed-off-by: Hirokazu Ishida --- .../cmake/rclcpp_components_register_node.cmake | 9 ++++++++- rclcpp_components/src/node_main.cpp.in | 2 +- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/rclcpp_components/cmake/rclcpp_components_register_node.cmake b/rclcpp_components/cmake/rclcpp_components_register_node.cmake index 1a9d0cb1c2..c5b87af667 100644 --- a/rclcpp_components/cmake/rclcpp_components_register_node.cmake +++ b/rclcpp_components/cmake/rclcpp_components_register_node.cmake @@ -28,7 +28,7 @@ # :type RESOURCE_INDEX: string # macro(rclcpp_components_register_node target) - cmake_parse_arguments(ARGS "" "PLUGIN;EXECUTABLE;RESOURCE_INDEX" "" ${ARGN}) + cmake_parse_arguments(ARGS "" "PLUGIN;EXECUTABLE;EXECUTOR;RESOURCE_INDEX" "" ${ARGN}) if(ARGS_UNPARSED_ARGUMENTS) message(FATAL_ERROR "rclcpp_components_register_node() called with unused " "arguments: ${ARGS_UNPARSED_ARGUMENTS}") @@ -46,6 +46,13 @@ macro(rclcpp_components_register_node target) message(STATUS "Setting component resource index to non-default value ${resource_index}") endif() + # default to executor if not specified otherwise + set(executor "SingleThreadedExecutor") + if(NOT "${ARGS_EXECUTOR}" STREQUAL "") + set(executor ${ARGS_EXECUTOR}) + message(STATUS "Setting executor non-default value ${executor}") + endif() + set(component ${ARGS_PLUGIN}) set(node ${ARGS_EXECUTABLE}) _rclcpp_components_register_package_hook() diff --git a/rclcpp_components/src/node_main.cpp.in b/rclcpp_components/src/node_main.cpp.in index 487331b251..0ca5eb8c61 100644 --- a/rclcpp_components/src/node_main.cpp.in +++ b/rclcpp_components/src/node_main.cpp.in @@ -27,7 +27,7 @@ int main(int argc, char * argv[]) { auto args = rclcpp::init_and_remove_ros_arguments(argc, argv); rclcpp::Logger logger = rclcpp::get_logger(NODE_MAIN_LOGGER_NAME); - rclcpp::executors::SingleThreadedExecutor exec; + rclcpp::executors::@executor@ exec; rclcpp::NodeOptions options; options.arguments(args); std::vector loaders; From 6afec4805c682f6f8604fb8d8ae25909ed062a83 Mon Sep 17 00:00:00 2001 From: Andrea Sorbini Date: Mon, 21 Mar 2022 07:29:08 -0700 Subject: [PATCH 088/455] Increase timeout for acknowledgments to account for slower Connext settings. (#1901) Signed-off-by: Andrea Sorbini --- rclcpp/test/rclcpp/test_publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 6671fa1176..1679ca76f0 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -612,7 +612,7 @@ TEST_P(TestPublisherWaitForAllAcked, check_wait_for_all_acked_with_QosPolicy) { for (int i = 0; i < 20; i++) { ASSERT_NO_THROW(pub->publish(*msg)); } - EXPECT_TRUE(pub->wait_for_all_acked(std::chrono::milliseconds(500))); + EXPECT_TRUE(pub->wait_for_all_acked(std::chrono::milliseconds(6000))); } INSTANTIATE_TEST_SUITE_P( From 49c2dd481342333328ffacb5f949b89b95395e32 Mon Sep 17 00:00:00 2001 From: Andrea Sorbini Date: Mon, 21 Mar 2022 13:50:04 -0700 Subject: [PATCH 089/455] Update data callback tests to account for all published samples. (#1900) * Update data callback tests to account for all published samples. Signed-off-by: Andrea Sorbini --- rclcpp/test/rclcpp/test_client.cpp | 9 ++++++--- rclcpp/test/rclcpp/test_service.cpp | 10 +++++++--- rclcpp/test/rclcpp/test_subscription.cpp | 4 ++-- 3 files changed, 15 insertions(+), 8 deletions(-) diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp index ee7e26d3a4..7cb9b0af65 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -351,12 +351,15 @@ 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"); + rmw_qos_profile_t client_qos = rmw_qos_profile_services_default; + client_qos.depth = 3; + auto client = client_node->create_client("test_service", client_qos); 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 server = server_node->create_service( + "test_service", server_callback, client_qos); auto request = std::make_shared(); std::atomic c1 {0}; @@ -423,7 +426,7 @@ TEST_F(TestClient, on_new_response_callback) { start = std::chrono::steady_clock::now(); do { std::this_thread::sleep_for(100ms); - } while (c3 == 0 && std::chrono::steady_clock::now() - start < 10s); + } while (c3 < 3 && std::chrono::steady_clock::now() - start < 10s); EXPECT_EQ(c1.load(), 1u); EXPECT_EQ(c2.load(), 1u); diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index e3399be423..1dbb8ca9e4 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -246,13 +246,17 @@ 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); + rmw_qos_profile_t service_qos = rmw_qos_profile_services_default; + service_qos.depth = 3; + auto server = node->create_service( + "~/test_service", server_callback, service_qos); 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 client = node->create_client( + "~/test_service", service_qos); { auto request = std::make_shared(); client->async_send_request(request); @@ -298,7 +302,7 @@ TEST_F(TestService, on_new_request_callback) { start = std::chrono::steady_clock::now(); do { std::this_thread::sleep_for(100ms); - } while (c3 == 0 && std::chrono::steady_clock::now() - start < 10s); + } while (c3 < 3 && std::chrono::steady_clock::now() - start < 10s); EXPECT_EQ(c1.load(), 1u); EXPECT_EQ(c2.load(), 1u); diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index a10c5c4eab..6facada4dd 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -454,7 +454,7 @@ TEST_F(TestSubscription, on_new_message_callback) { 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); + auto pub = node->create_publisher("~/test_take", 3); { test_msgs::msg::Empty msg; pub->publish(msg); @@ -500,7 +500,7 @@ TEST_F(TestSubscription, on_new_message_callback) { start = std::chrono::steady_clock::now(); do { std::this_thread::sleep_for(100ms); - } while (c3 == 0 && std::chrono::steady_clock::now() - start < 10s); + } while (c3 < 3 && std::chrono::steady_clock::now() - start < 10s); EXPECT_EQ(c1.load(), 1u); EXPECT_EQ(c2.load(), 1u); From 011ea39e9909d74faaa0cbbbbb1def8d48760e9e Mon Sep 17 00:00:00 2001 From: Scott K Logan Date: Tue, 22 Mar 2022 15:53:08 -0700 Subject: [PATCH 090/455] Add missing ament dependency on rcl_interfaces (#1903) This package uses rcl_interfaces directly, and we've been getting away with the missing dependency because `rcl` has the entirety of `rcl_interfaces` as part of its link interface even tough it doesn't need to. If (when) `rcl` scopes its dependency to only the c generator and drops the cpp generator from its link interface, this package will fail to find the cpp symbols at link time. This change remedies that. Signed-off-by: Scott K Logan --- rclcpp/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 5cbe0b961a..9572f422a8 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -194,6 +194,7 @@ ament_target_dependencies(${PROJECT_NAME} "ament_index_cpp" "libstatistics_collector" "rcl" + "rcl_interfaces" "rcl_yaml_param_parser" "rcpputils" "rcutils" From d302e3c6280e7d6abf7a7cde8bca6498d2be7ddc Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Thu, 24 Mar 2022 17:37:17 -0700 Subject: [PATCH 091/455] 15.2.0 --- rclcpp/CHANGELOG.rst | 12 ++++++++++++ rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 5 +++++ rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 7 +++++++ rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 5 +++++ rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 33 insertions(+), 4 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index f368a5eb3f..d3e92d032d 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +15.2.0 (2022-03-24) +------------------- +* Add missing ament dependency on rcl_interfaces (`#1903 `_) +* Update data callback tests to account for all published samples (`#1900 `_) +* Increase timeout for acknowledgments to account for slower Connext settings (`#1901 `_) +* clang-tidy: explicit constructors (`#1782 `_) +* Add client/service QoS getters (`#1784 `_) +* Fix a bunch more rosdoc2 issues in rclcpp. (`#1897 `_) +* time_until_trigger returns max time if timer is cancelled (`#1893 `_) +* Micro-optimizations in rclcpp (`#1896 `_) +* Contributors: Andrea Sorbini, Chris Lalancette, Mauro Passerino, Scott K Logan, William Woodall + 15.1.0 (2022-03-01) ------------------- * spin_all with a zero timeout. (`#1878 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 43f6ae2527..171ccbc973 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 15.1.0 + 15.2.0 The ROS client library in C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index cdb217533e..62579cb8a6 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +15.2.0 (2022-03-24) +------------------- +* Fix rosdoc2 issues (`#1897 `_) +* Contributors: Chris Lalancette + 15.1.0 (2022-03-01) ------------------- * Add RMW listener APIs (`#1579 `_) diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 213f81fb7c..da34c5a646 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 15.1.0 + 15.2.0 Adds action APIs for C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 6c4bae3687..d16ee35d82 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +15.2.0 (2022-03-24) +------------------- +* Select executor in node registration (`#1898 `_) +* Fix rosdoc2 issues in rclcpp (`#1897 `_) +* Fix bugprone-exception-escape in node_main.cpp.in (`#1895 `_) +* Contributors: Alberto Soragna, Chris Lalancette, Hirokazu Ishida + 15.1.0 (2022-03-01) ------------------- * small improvements to node_main.cpp.in diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index c5941ac74b..747cccf75b 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 15.1.0 + 15.2.0 Package containing tools for dynamically loadable components Ivan Paunovic Jacob Perron diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index ee0f49a0a1..50cb5d6a53 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +15.2.0 (2022-03-24) +------------------- +* Fix rosdoc2 issues (`#1897 `_) +* Contributors: Chris Lalancette + 15.1.0 (2022-03-01) ------------------- * Install headers to include/${PROJECT_NAME} (`#1888 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 6a537dfcb4..00fa76af24 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 15.1.0 + 15.2.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Jacob Perron From 492770c12f5427effdb7f24a03aebc55b2532d63 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Mon, 28 Mar 2022 13:06:17 +0800 Subject: [PATCH 092/455] Add publish by loaned message in GenericPublisher (#1856) * Add publish by loaned message in GenericPublisher Signed-off-by: Barry Xu Co-authored-by: Alban Tamisier --- rclcpp/include/rclcpp/generic_publisher.hpp | 17 ++++ rclcpp/include/rclcpp/publisher_base.hpp | 2 + rclcpp/src/rclcpp/generic_publisher.cpp | 45 +++++++++ rclcpp/src/rclcpp/publisher_base.cpp | 4 +- rclcpp/test/rclcpp/test_generic_pubsub.cpp | 101 ++++++++++++++++---- 5 files changed, 152 insertions(+), 17 deletions(-) diff --git a/rclcpp/include/rclcpp/generic_publisher.hpp b/rclcpp/include/rclcpp/generic_publisher.hpp index e379f09427..e1b46002bc 100644 --- a/rclcpp/include/rclcpp/generic_publisher.hpp +++ b/rclcpp/include/rclcpp/generic_publisher.hpp @@ -31,6 +31,8 @@ #include "rclcpp/typesupport_helpers.hpp" #include "rclcpp/visibility_control.hpp" +#include "rmw/rmw.h" + namespace rclcpp { @@ -116,9 +118,24 @@ class GenericPublisher : public rclcpp::PublisherBase RCLCPP_PUBLIC void publish(const rclcpp::SerializedMessage & message); + /** + * Publish a rclcpp::SerializedMessage via loaned message after de-serialization. + * + * \param message a serialized message + * \throws anything rclcpp::exceptions::throw_from_rcl_error can show + */ + RCLCPP_PUBLIC + void publish_as_loaned_msg(const rclcpp::SerializedMessage & message); + private: // The type support library should stay loaded, so it is stored in the GenericPublisher std::shared_ptr ts_lib_; + + void * borrow_loaned_message(); + void deserialize_message( + const rmw_serialized_message_t & serialized_message, + void * deserialized_msg); + void publish_loaned_message(void * loaned_message); }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index cdbd4bb758..8416757a53 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -346,6 +346,8 @@ class PublisherBase : public std::enable_shared_from_this uint64_t intra_process_publisher_id_; rmw_gid_t rmw_gid_; + + const rosidl_message_type_support_t type_support_; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/generic_publisher.cpp b/rclcpp/src/rclcpp/generic_publisher.cpp index 733aa5dd5c..87ace1eb19 100644 --- a/rclcpp/src/rclcpp/generic_publisher.cpp +++ b/rclcpp/src/rclcpp/generic_publisher.cpp @@ -31,4 +31,49 @@ void GenericPublisher::publish(const rclcpp::SerializedMessage & message) } } +void GenericPublisher::publish_as_loaned_msg(const rclcpp::SerializedMessage & message) +{ + auto loaned_message = borrow_loaned_message(); + deserialize_message(message.get_rcl_serialized_message(), loaned_message); + publish_loaned_message(loaned_message); +} + +void * GenericPublisher::borrow_loaned_message() +{ + void * loaned_message = nullptr; + auto return_code = rcl_borrow_loaned_message( + get_publisher_handle().get(), &type_support_, &loaned_message); + + if (return_code != RMW_RET_OK) { + if (return_code == RCL_RET_UNSUPPORTED) { + rclcpp::exceptions::throw_from_rcl_error( + return_code, + "current middleware cannot support loan messages"); + } else { + rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to borrow loaned msg"); + } + } + return loaned_message; +} + +void GenericPublisher::deserialize_message( + const rmw_serialized_message_t & serialized_message, + void * deserialized_msg) +{ + auto return_code = rmw_deserialize(&serialized_message, &type_support_, deserialized_msg); + if (return_code != RMW_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to deserialize msg"); + } +} + +void GenericPublisher::publish_loaned_message(void * loaned_message) +{ + auto return_code = rcl_publish_loaned_message( + get_publisher_handle().get(), loaned_message, NULL); + + if (return_code != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to publish loaned message"); + } +} + } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index dd027f2e65..723d56e0d5 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -47,7 +47,9 @@ PublisherBase::PublisherBase( const rosidl_message_type_support_t & type_support, const rcl_publisher_options_t & publisher_options) : rcl_node_handle_(node_base->get_shared_rcl_node_handle()), - intra_process_is_enabled_(false), intra_process_publisher_id_(0) + intra_process_is_enabled_(false), + intra_process_publisher_id_(0), + type_support_(type_support) { auto custom_deleter = [node_handle = this->rcl_node_handle_](rcl_publisher_t * rcl_pub) { diff --git a/rclcpp/test/rclcpp/test_generic_pubsub.cpp b/rclcpp/test/rclcpp/test_generic_pubsub.cpp index ff34ec52b3..0d2bec588a 100644 --- a/rclcpp/test/rclcpp/test_generic_pubsub.cpp +++ b/rclcpp/test/rclcpp/test_generic_pubsub.cpp @@ -61,34 +61,37 @@ class RclcppGenericNodeFixture : public Test publishers_.push_back(publisher); } - std::vector subscribe_raw_messages( - size_t expected_messages_number, const std::string & topic_name, const std::string & type) + template + std::vector subscribe_raw_messages( + size_t expected_recv_msg_count, const std::string & topic_name, const std::string & type) { - std::vector messages; + std::vector messages; size_t counter = 0; auto subscription = node_->create_generic_subscription( topic_name, type, rclcpp::QoS(1), - [&counter, &messages](std::shared_ptr message) { - test_msgs::msg::Strings string_message; - rclcpp::Serialization serializer; - serializer.deserialize_message(message.get(), &string_message); - messages.push_back(string_message.string_value); + [&counter, &messages, this](std::shared_ptr message) { + T2 deserialized_message; + rclcpp::Serialization serializer; + serializer.deserialize_message(message.get(), &deserialized_message); + messages.push_back(this->get_data_from_msg(deserialized_message)); counter++; }); - while (counter < expected_messages_number) { + while (counter < expected_recv_msg_count) { rclcpp::spin_some(node_); } return messages; } - rclcpp::SerializedMessage serialize_string_message(const std::string & message) + template + rclcpp::SerializedMessage serialize_message(const T1 & data) { - test_msgs::msg::Strings string_message; - string_message.string_value = message; - rclcpp::Serialization ser; + T2 message; + write_message(data, message); + + rclcpp::Serialization ser; SerializedMessage result; - ser.serialize_message(&string_message, &result); + ser.serialize_message(&message, &result); return result; } @@ -115,6 +118,27 @@ class RclcppGenericNodeFixture : public Test std::shared_ptr node_; rclcpp::Node::SharedPtr publisher_node_; std::vector> publishers_; + +private: + void write_message(const std::string & data, test_msgs::msg::Strings & message) + { + message.string_value = data; + } + + void write_message(const int64_t & data, test_msgs::msg::BasicTypes & message) + { + message.int64_value = data; + } + + std::string get_data_from_msg(const test_msgs::msg::Strings & message) + { + return message.string_value; + } + + int64_t get_data_from_msg(const test_msgs::msg::BasicTypes & message) + { + return message.int64_value; + } }; @@ -130,7 +154,7 @@ TEST_F(RclcppGenericNodeFixture, publisher_and_subscriber_work) auto subscriber_future_ = std::async( std::launch::async, [this, topic_name, type] { - return subscribe_raw_messages(1, topic_name, type); + return subscribe_raw_messages(1, topic_name, type); }); // TODO(karsten1987): Port 'wait_for_sub' to rclcpp @@ -147,7 +171,7 @@ TEST_F(RclcppGenericNodeFixture, publisher_and_subscriber_work) ASSERT_TRUE(success); for (const auto & message : test_messages) { - publisher->publish(serialize_string_message(message)); + publisher->publish(serialize_message(message)); } auto subscribed_messages = subscriber_future_.get(); @@ -155,6 +179,51 @@ TEST_F(RclcppGenericNodeFixture, publisher_and_subscriber_work) EXPECT_THAT(subscribed_messages[0], StrEq("Hello World")); } +TEST_F(RclcppGenericNodeFixture, publish_loaned_msg_work) +{ + // We currently publish more messages because they can get lost + std::vector test_messages = {100, 100}; + std::string topic_name = "/int64_topic"; + std::string type = "test_msgs/msg/BasicTypes"; + + auto publisher = node_->create_generic_publisher(topic_name, type, rclcpp::QoS(1)); + + if (publisher->can_loan_messages()) { + auto subscriber_future_ = std::async( + std::launch::deferred, [this, topic_name, type] { + return subscribe_raw_messages( + 1, topic_name, type); + }); + + auto allocator = node_->get_node_options().allocator(); + auto success = false; + auto ret = rcl_wait_for_subscribers( + node_->get_node_base_interface()->get_rcl_node_handle(), + &allocator, + topic_name.c_str(), + 1u, + static_cast(2e9), + &success); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_TRUE(success); + + for (const auto & message : test_messages) { + publisher->publish_as_loaned_msg( + serialize_message(message)); + } + + auto subscribed_messages = subscriber_future_.get(); + EXPECT_THAT(subscribed_messages, SizeIs(Not(0))); + EXPECT_EQ(subscribed_messages[0], test_messages[0]); + } else { + ASSERT_THROW( + { + publisher->publish_as_loaned_msg( + serialize_message(test_messages[0])); + }, rclcpp::exceptions::RCLError); + } +} + TEST_F(RclcppGenericNodeFixture, generic_subscription_uses_qos) { // If the GenericSubscription does not use the provided QoS profile, From eac006317692b7eff3aed671eadbff3d72cca098 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ga=C3=ABl=20=C3=89corchard?= Date: Mon, 28 Mar 2022 15:23:11 +0200 Subject: [PATCH 093/455] Add test-dep ament_cmake_google_benchmark (#1904) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Gaël Écorchard --- rclcpp/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 171ccbc973..dbc984ca64 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -39,6 +39,7 @@ tracetools ament_cmake_gmock + ament_cmake_google_benchmark ament_cmake_gtest ament_lint_auto ament_lint_common From 12de518956a86aa4a4c5905ac7b16e5a00011abd Mon Sep 17 00:00:00 2001 From: Abrar Rahman Protyasha Date: Wed, 30 Mar 2022 12:57:22 -0400 Subject: [PATCH 094/455] [NodeParameters] Set name in param info pre-check (#1908) * [NodeParameters] Set name if user didn't populate If the user provided parameter descriptor already contains a name, then we should not override said name, under the good faith assumption that the user knows what they are doing. Signed-off-by: Abrar Rahman Protyasha --- rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 92d42bd8e7..6a76f8b523 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -236,6 +236,9 @@ __check_parameters( } else { descriptor = parameter_infos[name].descriptor; } + if (descriptor.name.empty()) { + descriptor.name = name; + } const auto new_type = parameter.get_type(); const auto specified_type = static_cast(descriptor.type); result.successful = descriptor.dynamic_typing || specified_type == new_type; From 06e6da414ac29b18c80e2e3db01acf71c91829d2 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 30 Mar 2022 18:54:09 +0000 Subject: [PATCH 095/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 7 +++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 3 +++ 4 files changed, 16 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index d3e92d032d..c584b31d54 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [NodeParameters] Set name in param info pre-check (`#1908 `_) +* Add test-dep ament_cmake_google_benchmark (`#1904 `_) +* Add publish by loaned message in GenericPublisher (`#1856 `_) +* Contributors: Abrar Rahman Protyasha, Barry Xu, Gaël Écorchard + 15.2.0 (2022-03-24) ------------------- * Add missing ament dependency on rcl_interfaces (`#1903 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 62579cb8a6..cb66b03111 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 15.2.0 (2022-03-24) ------------------- * Fix rosdoc2 issues (`#1897 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index d16ee35d82..e54193f997 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 15.2.0 (2022-03-24) ------------------- * Select executor in node registration (`#1898 `_) diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 50cb5d6a53..0059966ef6 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 15.2.0 (2022-03-24) ------------------- * Fix rosdoc2 issues (`#1897 `_) From 0699aeb8515f5f4f34c0fc66493099421dad3d6c Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 30 Mar 2022 18:54:18 +0000 Subject: [PATCH 096/455] 15.3.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index c584b31d54..1d1e4be73e 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +15.3.0 (2022-03-30) +------------------- * [NodeParameters] Set name in param info pre-check (`#1908 `_) * Add test-dep ament_cmake_google_benchmark (`#1904 `_) * Add publish by loaned message in GenericPublisher (`#1856 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index dbc984ca64..4d793bd3cd 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 15.2.0 + 15.3.0 The ROS client library in C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index cb66b03111..049d5cec06 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +15.3.0 (2022-03-30) +------------------- 15.2.0 (2022-03-24) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index da34c5a646..b60636edb8 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 15.2.0 + 15.3.0 Adds action APIs for C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index e54193f997..ab8346e938 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +15.3.0 (2022-03-30) +------------------- 15.2.0 (2022-03-24) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 747cccf75b..98769c47d8 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 15.2.0 + 15.3.0 Package containing tools for dynamically loadable components Ivan Paunovic Jacob Perron diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 0059966ef6..47237b91b5 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +15.3.0 (2022-03-30) +------------------- 15.2.0 (2022-03-24) ------------------- diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 00fa76af24..5f65931a12 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 15.2.0 + 15.3.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Jacob Perron From 03fa731d2345c41e5a4e436bf0730012ece96ce5 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 4 Apr 2022 11:50:50 +0800 Subject: [PATCH 097/455] add content-filtered-topic interfaces (#1561) * to support a feature of content filtered topic Signed-off-by: Chen Lihui * update for checking memory allocated Signed-off-by: Chen Lihui * set expression parameter only if filter is valid Signed-off-by: Chen Lihui * add test Signed-off-by: Chen Lihui * use a default empty value for expresion parameters Signed-off-by: Chen Lihui * remove std_msgs dependency Signed-off-by: Chen Lihui * use new rcl interface Signed-off-by: Chen Lihui * remove unused include header files and fix unscrutify Signed-off-by: Chen Lihui * update comment Signed-off-by: Chen Lihui * update comment Signed-off-by: Chen Lihui * rename Signed-off-by: Chen Lihui * refactor test Signed-off-by: Chen Lihui * relate to `rcutils_string_array_t expression_parameters` changed in rmw Signed-off-by: Chen Lihui * remove the implementation temporary, add them with fallback in the feature 1. add DEFINE_CONTENT_FILTER with default(ON) to build content filter interfaces 2. user need a compile option(CONTENT_FILTER_ENABLE) to turn on the feature to set the filter or call the interfaces Signed-off-by: Chen Lihui * update comments and check the option to build content filter test Signed-off-by: Chen Lihui * add DDS content filter implementation Signed-off-by: Chen Lihui * address review Signed-off-by: Chen Lihui * rcl api changed Signed-off-by: Chen Lihui * increase the filter propagation time Signed-off-by: Chen Lihui * add rclcpp::ContentFilterOptions and use it as the return type of get_content_filter Signed-off-by: Chen Lihui * increase the maximun time to wait message event and content filter propagation Signed-off-by: Chen Lihui * cft member should be defined in the structure. to check the macro for interfaces is enough. Signed-off-by: Chen Lihui * set test timeout to 120 Signed-off-by: Chen Lihui * update doc Signed-off-by: Chen Lihui --- rclcpp/CMakeLists.txt | 5 + rclcpp/include/rclcpp/subscription_base.hpp | 37 +++ .../subscription_content_filter_options.hpp | 38 +++ .../include/rclcpp/subscription_options.hpp | 19 ++ rclcpp/include/rclcpp/utilities.hpp | 9 + rclcpp/src/rclcpp/subscription_base.cpp | 92 ++++++ rclcpp/src/rclcpp/utilities.cpp | 13 + rclcpp/test/rclcpp/CMakeLists.txt | 20 ++ .../test_subscription_content_filter.cpp | 312 ++++++++++++++++++ 9 files changed, 545 insertions(+) create mode 100644 rclcpp/include/rclcpp/subscription_content_filter_options.hpp create mode 100644 rclcpp/test/rclcpp/test_subscription_content_filter.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 9572f422a8..de424342ed 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -2,6 +2,11 @@ cmake_minimum_required(VERSION 3.12) project(rclcpp) +option(DEFINE_CONTENT_FILTER "Content filter feature support" ON) +if(DEFINE_CONTENT_FILTER) + add_definitions(-DCONTENT_FILTER_ENABLE) +endif() + find_package(Threads REQUIRED) find_package(ament_cmake_ros REQUIRED) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 26650cd121..0ad098b027 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -39,6 +39,7 @@ #include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" #include "rclcpp/serialized_message.hpp" +#include "rclcpp/subscription_content_filter_options.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" @@ -491,6 +492,42 @@ class SubscriptionBase : public std::enable_shared_from_this event_handlers_[event_type]->clear_on_ready_callback(); } +#ifdef CONTENT_FILTER_ENABLE + /// Check if content filtered topic feature of the subscription instance is enabled. + /** + * \return boolean flag indicating if the content filtered topic of this subscription is enabled. + */ + RCLCPP_PUBLIC + bool + is_cft_enabled() const; + + /// Set the filter expression and expression parameters for the subscription. + /** + * \param[in] filter_expression A filter expression to set. + * \sa ContentFilterOptions::filter_expression + * An empty string ("") will clear the content filter setting of the subscription. + * \param[in] expression_parameters Array of expression parameters to set. + * \sa ContentFilterOptions::expression_parameters + * \throws RCLBadAlloc if memory cannot be allocated + * \throws RCLError if an unexpect error occurs + */ + RCLCPP_PUBLIC + void + set_content_filter( + const std::string & filter_expression, + const std::vector & expression_parameters = {}); + + /// Get the filter expression and expression parameters for the subscription. + /** + * \return rclcpp::ContentFilterOptions The content filter options to get. + * \throws RCLBadAlloc if memory cannot be allocated + * \throws RCLError if an unexpect error occurs + */ + RCLCPP_PUBLIC + rclcpp::ContentFilterOptions + get_content_filter() const; +#endif // CONTENT_FILTER_ENABLE + protected: template void diff --git a/rclcpp/include/rclcpp/subscription_content_filter_options.hpp b/rclcpp/include/rclcpp/subscription_content_filter_options.hpp new file mode 100644 index 0000000000..8de034ddf5 --- /dev/null +++ b/rclcpp/include/rclcpp/subscription_content_filter_options.hpp @@ -0,0 +1,38 @@ +// Copyright 2022 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__SUBSCRIPTION_CONTENT_FILTER_OPTIONS_HPP_ +#define RCLCPP__SUBSCRIPTION_CONTENT_FILTER_OPTIONS_HPP_ + +#include +#include + +namespace rclcpp +{ + +/// Options to configure content filtered topic in the subscription. +struct ContentFilterOptions +{ + /// Filter expression is similar to the WHERE part of an SQL clause. + std::string filter_expression; + /** + * Expression parameters is the tokens placeholder ‘parameters’ (i.e., "%n" tokens begin from 0) + * in the filter_expression. The maximum expression_parameters size is 100. + */ + std::vector expression_parameters; +}; + +} // namespace rclcpp + +#endif // RCLCPP__SUBSCRIPTION_CONTENT_FILTER_OPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index d6d2d4c60d..da81b8ffb0 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -28,6 +28,7 @@ #include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" #include "rclcpp/qos_overriding_options.hpp" +#include "rclcpp/subscription_content_filter_options.hpp" #include "rclcpp/topic_statistics_state.hpp" #include "rclcpp/visibility_control.hpp" @@ -81,6 +82,8 @@ struct SubscriptionOptionsBase TopicStatisticsOptions topic_stats_options; QosOverridingOptions qos_overriding_options; + + ContentFilterOptions content_filter_options; }; /// Structure containing optional configuration for Subscriptions. @@ -123,6 +126,22 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase rmw_implementation_payload->modify_rmw_subscription_options(result.rmw_subscription_options); } +#ifdef CONTENT_FILTER_ENABLE + // Copy content_filter_options into rcl_subscription_options. + if (!content_filter_options.filter_expression.empty()) { + std::vector cstrings = + get_c_vector_string(content_filter_options.expression_parameters); + rcl_ret_t ret = rcl_subscription_options_set_content_filter_options( + get_c_string(content_filter_options.filter_expression), + cstrings.size(), + cstrings.data(), + &result); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to set content_filter_options"); + } + } +#endif // CONTENT_FILTER_ENABLE return result; } diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index fa48a010c4..5274cba33a 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -321,6 +321,15 @@ RCLCPP_PUBLIC const char * get_c_string(const std::string & string_in); +/// Return the std::vector of C string from the given std::vector. +/** + * \param[in] strings_in is a std::vector of std::string + * \return the std::vector of C string from the std::vector + */ +RCLCPP_PUBLIC +std::vector +get_c_vector_string(const std::vector & strings_in); + } // namespace rclcpp #endif // RCLCPP__UTILITIES_HPP_ diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 12841fe6b6..dfe3c30ead 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -20,6 +20,8 @@ #include #include +#include "rcpputils/scope_exit.hpp" + #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/experimental/intra_process_manager.hpp" @@ -354,3 +356,93 @@ SubscriptionBase::set_on_new_message_callback( throw_from_rcl_error(ret, "failed to set the on new message callback for subscription"); } } + +#ifdef CONTENT_FILTER_ENABLE +bool +SubscriptionBase::is_cft_enabled() const +{ + return rcl_subscription_is_cft_enabled(subscription_handle_.get()); +} + +void +SubscriptionBase::set_content_filter( + const std::string & filter_expression, + const std::vector & expression_parameters) +{ + rcl_subscription_content_filter_options_t options = + rcl_get_zero_initialized_subscription_content_filter_options(); + + std::vector cstrings = + get_c_vector_string(expression_parameters); + rcl_ret_t ret = rcl_subscription_content_filter_options_init( + subscription_handle_.get(), + get_c_string(filter_expression), + cstrings.size(), + cstrings.data(), + &options); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to init subscription content_filtered_topic option"); + } + RCPPUTILS_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_content_filter_options_fini( + subscription_handle_.get(), &options); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Failed to fini subscription content_filtered_topic option: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + }); + + ret = rcl_subscription_set_content_filter( + subscription_handle_.get(), + &options); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to set cft expression parameters"); + } +} + +rclcpp::ContentFilterOptions +SubscriptionBase::get_content_filter() const +{ + rclcpp::ContentFilterOptions ret_options; + rcl_subscription_content_filter_options_t options = + rcl_get_zero_initialized_subscription_content_filter_options(); + + rcl_ret_t ret = rcl_subscription_get_content_filter( + subscription_handle_.get(), + &options); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get cft expression parameters"); + } + + RCPPUTILS_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_content_filter_options_fini( + subscription_handle_.get(), &options); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Failed to fini subscription content_filtered_topic option: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + }); + + rmw_subscription_content_filter_options_t & content_filter_options = + options.rmw_subscription_content_filter_options; + ret_options.filter_expression = content_filter_options.filter_expression; + + for (size_t i = 0; i < content_filter_options.expression_parameters.size; ++i) { + ret_options.expression_parameters.push_back( + content_filter_options.expression_parameters.data[i]); + } + + return ret_options; +} +#endif // CONTENT_FILTER_ENABLE diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index a6ead85c03..f300060010 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -214,4 +214,17 @@ get_c_string(const std::string & string_in) return string_in.c_str(); } +std::vector +get_c_vector_string(const std::vector & strings_in) +{ + std::vector cstrings; + cstrings.reserve(strings_in.size()); + + for (size_t i = 0; i < strings_in.size(); ++i) { + cstrings.push_back(strings_in[i].c_str()); + } + + return cstrings; +} + } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 331c39fb27..2d7e9a3001 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -733,3 +733,23 @@ ament_add_gtest(test_graph_listener test_graph_listener.cpp) if(TARGET test_graph_listener) target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick) endif() + +if(DEFINE_CONTENT_FILTER) + function(test_subscription_content_filter_for_rmw_implementation) + set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) + ament_add_gmock(test_subscription_content_filter${target_suffix} + test_subscription_content_filter.cpp + ENV ${rmw_implementation_env_var} + TIMEOUT 120 + ) + if(TARGET test_subscription_content_filter${target_suffix}) + target_link_libraries(test_subscription_content_filter${target_suffix} ${PROJECT_NAME} mimick) + ament_target_dependencies(test_subscription_content_filter${target_suffix} + "rcpputils" + "rosidl_typesupport_cpp" + "test_msgs" + ) + endif() + endfunction() + call_for_each_rmw_implementation(test_subscription_content_filter_for_rmw_implementation) +endif() diff --git a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp new file mode 100644 index 0000000000..942aa1274e --- /dev/null +++ b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp @@ -0,0 +1,312 @@ +// Copyright 2022 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. + +#include + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/wait_for_message.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/msg/basic_types.hpp" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +class CLASSNAME (TestContentFilterSubscription, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_content_filter_node", "/ns"); + context = node->get_node_options().context(); + qos.reliable().transient_local(); + + auto options = rclcpp::SubscriptionOptions(); + options.content_filter_options.filter_expression = filter_expression_init; + options.content_filter_options.expression_parameters = expression_parameters_1; + + auto callback = [](std::shared_ptr) {}; + sub = node->create_subscription( + "content_filter_topic", qos, callback, options); + } + + void TearDown() + { + sub.reset(); + node.reset(); + } + + template + bool wait_for(const Condition & condition, const Duration & timeout) + { + using clock = std::chrono::system_clock; + auto start = clock::now(); + while (!condition()) { + if ((clock::now() - start) > timeout) { + return false; + } + rclcpp::spin_some(node); + } + return true; + } + +protected: + rclcpp::Node::SharedPtr node; + rclcpp::Context::SharedPtr context; + rclcpp::QoS qos{rclcpp::KeepLast{10}}; + rclcpp::Subscription::SharedPtr sub; + + std::string filter_expression_init = "int32_value = %0"; + std::vector expression_parameters_1 = {"3"}; + std::vector expression_parameters_2 = {"4"}; +}; + +bool operator==(const test_msgs::msg::BasicTypes & m1, const test_msgs::msg::BasicTypes & m2) +{ + return m1.bool_value == m2.bool_value && + m1.byte_value == m2.byte_value && + m1.char_value == m2.char_value && + m1.float32_value == m2.float32_value && + m1.float64_value == m2.float64_value && + m1.int8_value == m2.int8_value && + m1.uint8_value == m2.uint8_value && + m1.int16_value == m2.int16_value && + m1.uint16_value == m2.uint16_value && + m1.int32_value == m2.int32_value && + m1.uint32_value == m2.uint32_value && + m1.int64_value == m2.int64_value && + m1.uint64_value == m2.uint64_value; +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), is_cft_enabled) { + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_is_cft_enabled, false); + EXPECT_FALSE(sub->is_cft_enabled()); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_is_cft_enabled, true); + EXPECT_TRUE(sub->is_cft_enabled()); + } +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), get_content_filter_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_get_content_filter, RCL_RET_ERROR); + + rclcpp::ContentFilterOptions options; + EXPECT_THROW( + options = sub->get_content_filter(), + rclcpp::exceptions::RCLError); +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), set_content_filter_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_set_content_filter, RCL_RET_ERROR); + + std::string filter_expression = "int32_value = %0"; + std::string expression_parameter = "100"; + EXPECT_THROW( + sub->set_content_filter(filter_expression, {expression_parameter}), + rclcpp::exceptions::RCLError); +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), get_content_filter) { + rclcpp::ContentFilterOptions options; + + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + options = sub->get_content_filter()); + + EXPECT_EQ(options.filter_expression, filter_expression_init); + EXPECT_EQ(options.expression_parameters, expression_parameters_1); + } else { + EXPECT_THROW( + options = sub->get_content_filter(), + rclcpp::exceptions::RCLError); + } +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), set_content_filter) { + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->set_content_filter(filter_expression_init, expression_parameters_2)); + } else { + EXPECT_THROW( + sub->set_content_filter(filter_expression_init, expression_parameters_2), + rclcpp::exceptions::RCLError); + } +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_get_begin) { + using namespace std::chrono_literals; + { + test_msgs::msg::BasicTypes msg; + rclcpp::MessageInfo msg_info; + EXPECT_FALSE(sub->take(msg, msg_info)); + } + { + rclcpp::PublisherOptions po; + auto pub = node->create_publisher("content_filter_topic", qos, po); + + auto connected = [pub, sub = this->sub]() -> bool { + return pub->get_subscription_count() && sub->get_publisher_count(); + }; + ASSERT_TRUE(wait_for(connected, 10s)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 3; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 10s); + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->set_content_filter(filter_expression_init, expression_parameters_2)); + // waiting to allow for filter propagation + std::this_thread::sleep_for(std::chrono::seconds(10)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 3; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 10s); + EXPECT_FALSE(receive); + } + } +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_get_later) { + using namespace std::chrono_literals; + { + test_msgs::msg::BasicTypes msg; + rclcpp::MessageInfo msg_info; + EXPECT_FALSE(sub->take(msg, msg_info)); + } + { + rclcpp::PublisherOptions po; + auto pub = node->create_publisher("content_filter_topic", qos, po); + + auto connected = [pub, sub = this->sub]() -> bool { + return pub->get_subscription_count() && sub->get_publisher_count(); + }; + ASSERT_TRUE(wait_for(connected, 10s)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 4; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 10s); + if (sub->is_cft_enabled()) { + EXPECT_FALSE(receive); + } else { + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + } + + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->set_content_filter(filter_expression_init, expression_parameters_2)); + // waiting to allow for filter propagation + std::this_thread::sleep_for(std::chrono::seconds(10)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 4; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 10s); + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + } + } +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_reset) { + using namespace std::chrono_literals; + { + test_msgs::msg::BasicTypes msg; + rclcpp::MessageInfo msg_info; + EXPECT_FALSE(sub->take(msg, msg_info)); + } + { + rclcpp::PublisherOptions po; + auto pub = node->create_publisher("content_filter_topic", qos, po); + + auto connected = [pub, sub = this->sub]() -> bool { + return pub->get_subscription_count() && sub->get_publisher_count(); + }; + ASSERT_TRUE(wait_for(connected, 10s)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 4; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 10s); + if (sub->is_cft_enabled()) { + EXPECT_FALSE(receive); + } else { + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + } + + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->set_content_filter("")); + // waiting to allow for filter propagation + std::this_thread::sleep_for(std::chrono::seconds(10)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 4; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 10s); + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + } + } +} From 6c06a290500f34e738d2a182b5d462585cb3b447 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Tue, 5 Apr 2022 06:25:30 +0100 Subject: [PATCH 098/455] add take_data_by_entity_id API to waitable (#1892) * add take_data_by_entity_id API to waitable Signed-off-by: Alberto Soragna * use size_t for entity id Signed-off-by: William Woodall Co-authored-by: William Woodall --- .../subscription_intra_process_base.hpp | 9 ++++++- rclcpp/include/rclcpp/qos_event.hpp | 9 ++++++- rclcpp/include/rclcpp/waitable.hpp | 18 +++++++++++++ rclcpp/src/rclcpp/waitable.cpp | 9 +++++++ .../include/rclcpp_action/client.hpp | 7 +++++- .../include/rclcpp_action/server.hpp | 6 ++++- rclcpp_action/src/client.cpp | 25 +++++++++++++++++++ rclcpp_action/src/server.cpp | 20 +++++++++++++++ 8 files changed, 99 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 331c1b6da5..037d1aaa2a 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -38,7 +38,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable public: RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase) - enum class EntityType + enum class EntityType : std::size_t { Subscription, }; @@ -68,6 +68,13 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable std::shared_ptr take_data() override = 0; + std::shared_ptr + take_data_by_entity_id(size_t id) override + { + (void)id; + return take_data(); + } + void execute(std::shared_ptr & data) override = 0; diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 59c99dda42..f258c5d5ba 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -89,7 +89,7 @@ class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public st class QOSEventHandlerBase : public Waitable { public: - enum class EntityType + enum class EntityType : std::size_t { Event, }; @@ -259,6 +259,13 @@ class QOSEventHandler : public QOSEventHandlerBase return std::static_pointer_cast(std::make_shared(callback_info)); } + std::shared_ptr + take_data_by_entity_id(size_t id) override + { + (void)id; + return take_data(); + } + /// Execute any entities of the Waitable that are ready. void execute(std::shared_ptr & data) override diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index db06f70678..1a0d8b61f1 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -160,6 +160,24 @@ class Waitable std::shared_ptr take_data() = 0; + /// Take the data so that it can be consumed with `execute`. + /** + * This function allows to specify an entity ID to take the data from. + * Entity IDs are identifiers that can be defined by waitable-derived + * classes that are composed of several distinct entities. + * The main use-case is in conjunction with the listener APIs. + * + * \param[in] id the id of the entity from which to take + * \returns the type-erased data taken from entity specified + * + * \sa rclcpp::Waitable::take_data + * \sa rclcpp::Waitable::set_on_ready_callback + */ + RCLCPP_PUBLIC + virtual + std::shared_ptr + take_data_by_entity_id(size_t id); + /// Execute data that is passed in. /** * Before calling this method, the Waitable should be added to a wait set, diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index 16ebb1b546..6c1284cb22 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -54,6 +54,15 @@ Waitable::get_number_of_ready_guard_conditions() return 0u; } +std::shared_ptr +Waitable::take_data_by_entity_id(size_t id) +{ + (void)id; + throw std::runtime_error( + "Custom waitables should override take_data_by_entity_id " + "if they want to use it."); +} + bool Waitable::exchange_in_use_by_wait_set_state(bool in_use_state) { diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 9bcddadec0..5eac5419f1 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -66,7 +66,7 @@ class ClientBase : public rclcpp::Waitable virtual ~ClientBase(); /// Enum to identify entities belonging to the action client - enum class EntityType + enum class EntityType : std::size_t { GoalClient, ResultClient, @@ -134,6 +134,11 @@ class ClientBase : public rclcpp::Waitable std::shared_ptr take_data() override; + /// \internal + RCLCPP_ACTION_PUBLIC + std::shared_ptr + take_data_by_entity_id(size_t id) override; + /// \internal RCLCPP_ACTION_PUBLIC void diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 554cb1cf56..892de5743b 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -72,7 +72,7 @@ class ServerBase : public rclcpp::Waitable { public: /// Enum to identify entities belonging to the action server - enum class EntityType + enum class EntityType : std::size_t { GoalService, ResultService, @@ -131,6 +131,10 @@ class ServerBase : public rclcpp::Waitable std::shared_ptr take_data() override; + RCLCPP_ACTION_PUBLIC + std::shared_ptr + take_data_by_entity_id(size_t id) override; + /// Act on entities in the wait set which are ready to be acted upon. /// \internal RCLCPP_ACTION_PUBLIC diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 07982092d5..f9144ecf49 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -589,6 +589,31 @@ ClientBase::take_data() } } +std::shared_ptr +ClientBase::take_data_by_entity_id(size_t id) +{ + // Mark as ready the entity from which we want to take data + switch (static_cast(id)) { + case EntityType::GoalClient: + pimpl_->is_goal_response_ready = true; + break; + case EntityType::ResultClient: + pimpl_->is_result_response_ready = true; + break; + case EntityType::CancelClient: + pimpl_->is_cancel_response_ready = true; + break; + case EntityType::FeedbackSubscription: + pimpl_->is_feedback_ready = true; + break; + case EntityType::StatusSubscription: + pimpl_->is_status_ready = true; + break; + } + + return take_data(); +} + void ClientBase::execute(std::shared_ptr & data) { diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index a07e203e28..b0afb9aa50 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -268,6 +268,25 @@ ServerBase::take_data() } } +std::shared_ptr +ServerBase::take_data_by_entity_id(size_t id) +{ + // Mark as ready the entity from which we want to take data + switch (static_cast(id)) { + case EntityType::GoalService: + pimpl_->goal_request_ready_ = true; + break; + case EntityType::ResultService: + pimpl_->result_request_ready_ = true; + break; + case EntityType::CancelService: + pimpl_->cancel_request_ready_ = true; + break; + } + + return take_data(); +} + void ServerBase::execute(std::shared_ptr & data) { @@ -398,6 +417,7 @@ ServerBase::execute_cancel_request_received(std::shared_ptr & data) } auto request = std::get<1>(*shared_ptr); auto request_header = std::get<2>(*shared_ptr); + pimpl_->cancel_request_ready_ = false; // Convert c++ message to C message rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request(); From 85a7046ac39bfc0d787ed2bbe62574424e31b871 Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Tue, 5 Apr 2022 14:17:00 -0700 Subject: [PATCH 099/455] 15.4.0 Signed-off-by: Audrow Nash --- rclcpp/CHANGELOG.rst | 6 ++++++ rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 5 +++++ rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 21 insertions(+), 4 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 1d1e4be73e..9842ec7e35 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +15.4.0 (2022-04-05) +------------------- +* add take_data_by_entity_id API to waitable (`#1892 `_) +* add content-filtered-topic interfaces (`#1561 `_) +* Contributors: Alberto Soragna, Chen Lihui + 15.3.0 (2022-03-30) ------------------- * [NodeParameters] Set name in param info pre-check (`#1908 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 4d793bd3cd..b62bcddc33 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 15.3.0 + 15.4.0 The ROS client library in C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 049d5cec06..2b652ee089 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +15.4.0 (2022-04-05) +------------------- +* add take_data_by_entity_id API to waitable (`#1892 `_) +* Contributors: Alberto Soragna + 15.3.0 (2022-03-30) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index b60636edb8..6e88ac86b6 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 15.3.0 + 15.4.0 Adds action APIs for C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index ab8346e938..149c22fa29 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +15.4.0 (2022-04-05) +------------------- + 15.3.0 (2022-03-30) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 98769c47d8..29f58c24e9 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 15.3.0 + 15.4.0 Package containing tools for dynamically loadable components Ivan Paunovic Jacob Perron diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 47237b91b5..fc1d83b05f 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +15.4.0 (2022-04-05) +------------------- + 15.3.0 (2022-03-30) ------------------- diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 5f65931a12..b85a973b63 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 15.3.0 + 15.4.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Jacob Perron From 6815022909086487f2b8dc14c0540b00d4cf61a0 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 8 Apr 2022 16:23:43 -0700 Subject: [PATCH 100/455] remove things that were deprecated during galactic (#1913) Signed-off-by: William Woodall --- .../rclcpp/any_subscription_callback.hpp | 6 -- rclcpp/include/rclcpp/duration.hpp | 7 -- rclcpp/include/rclcpp/node.hpp | 16 --- .../node_interfaces/node_parameters.hpp | 19 ---- .../node_parameters_interface.hpp | 20 ---- rclcpp/src/rclcpp/duration.cpp | 5 - rclcpp/src/rclcpp/node.cpp | 18 ---- .../node_interfaces/node_parameters.cpp | 8 -- .../rclcpp/test_any_subscription_callback.cpp | 29 ----- rclcpp/test/rclcpp/test_node.cpp | 16 --- .../include/rclcpp_action/client.hpp | 102 +----------------- rclcpp_action/test/test_client.cpp | 58 ---------- .../rclcpp_lifecycle/lifecycle_node.hpp | 16 --- rclcpp_lifecycle/src/lifecycle_node.cpp | 18 ---- 14 files changed, 4 insertions(+), 334 deletions(-) diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 56af35c518..ea26fad397 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -354,12 +354,6 @@ class AnySubscriptionCallback allocator::set_allocator_for_deleter(&ros_message_type_deleter_, &ros_message_type_allocator_); } - [[deprecated("use AnySubscriptionCallback(const AllocatorT & allocator) instead")]] - explicit - AnySubscriptionCallback(std::shared_ptr allocator) // NOLINT[runtime/explicit] - : AnySubscriptionCallback(*NotNull(allocator.get(), "invalid allocator").pointer) - {} - AnySubscriptionCallback(const AnySubscriptionCallback &) = default; /// Generic function for setting the callback. diff --git a/rclcpp/include/rclcpp/duration.hpp b/rclcpp/include/rclcpp/duration.hpp index c6b790fc7a..28b3718660 100644 --- a/rclcpp/include/rclcpp/duration.hpp +++ b/rclcpp/include/rclcpp/duration.hpp @@ -38,13 +38,6 @@ class RCLCPP_PUBLIC Duration */ Duration(int32_t seconds, uint32_t nanoseconds); - /// Construct duration from the specified nanoseconds. - [[deprecated( - "Use Duration::from_nanoseconds instead or std::chrono_literals. For example:" - "rclcpp::Duration::from_nanoseconds(int64_variable);" - "rclcpp::Duration(0ns);")]] - explicit Duration(rcl_duration_value_t nanoseconds); - /// Construct duration from the specified std::chrono::nanoseconds. explicit Duration(std::chrono::nanoseconds nanoseconds); diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 2150f60e1c..65b8797716 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -395,22 +395,6 @@ class Node : public std::enable_shared_from_this rcl_interfaces::msg::ParameterDescriptor{}, bool ignore_override = false); - /// Declare a parameter - [[deprecated( - "declare_parameter() with only a name is deprecated and will be deleted in the future.\n" \ - "If you want to declare a parameter that won't change type without a default value use:\n" \ - "`node->declare_parameter(name)`, where e.g. ParameterT=int64_t.\n\n" \ - "If you want to declare a parameter that can dynamically change type use:\n" \ - "```\n" \ - "rcl_interfaces::msg::ParameterDescriptor descriptor;\n" \ - "descriptor.dynamic_typing = true;\n" \ - "node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);\n" \ - "```" - )]] - RCLCPP_PUBLIC - const rclcpp::ParameterValue & - declare_parameter(const std::string & name); - /// Declare and initialize a parameter with a type. /** * See the non-templated declare_parameter() on this class for details. diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index 552fbc6979..6f0d5b0ed2 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -103,25 +103,6 @@ class NodeParameters : public NodeParametersInterface virtual ~NodeParameters(); -// This is overriding a deprecated method, so we need to ignore the deprecation warning here. -// Users of the method will still get a warning! -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - [[deprecated(RCLCPP_INTERNAL_NODE_PARAMETERS_INTERFACE_DEPRECATE_DECLARE)]] - RCLCPP_PUBLIC - const rclcpp::ParameterValue & - declare_parameter(const std::string & name) override; -#ifndef _WIN32 -# pragma GCC diagnostic pop -#else -# pragma warning(pop) -#endif - RCLCPP_PUBLIC const rclcpp::ParameterValue & declare_parameter( diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index d9981516c7..743c1b8d4f 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -45,17 +45,6 @@ struct OnSetParametersCallbackHandle OnParametersSetCallbackType callback; }; -#define RCLCPP_INTERNAL_NODE_PARAMETERS_INTERFACE_DEPRECATE_DECLARE \ - "declare_parameter() with only a name is deprecated and will be deleted in the future.\n" \ - "If you want to declare a parameter that won't change type without a default value use:\n" \ - "`node_params->declare_parameter(name, type)`, with e.g. type=rclcpp::PARAMETER_INTEGER.\n\n" \ - "If you want to declare a parameter that can dynamically change type use:\n" \ - "```\n" \ - "rcl_interfaces::msg::ParameterDescriptor descriptor;\n" \ - "descriptor.dynamic_typing = true;\n" \ - "node_params->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);\n" \ - "```" - /// Pure virtual interface class for the NodeParameters part of the Node API. class NodeParametersInterface { @@ -66,15 +55,6 @@ class NodeParametersInterface virtual ~NodeParametersInterface() = default; - /// Declare a parameter. - /** - * \sa rclcpp::Node::declare_parameter - */ - [[deprecated(RCLCPP_INTERNAL_NODE_PARAMETERS_INTERFACE_DEPRECATE_DECLARE)]] - virtual - const rclcpp::ParameterValue & - declare_parameter(const std::string & name) = 0; - /// Declare and initialize a parameter. /** * \sa rclcpp::Node::declare_parameter diff --git a/rclcpp/src/rclcpp/duration.cpp b/rclcpp/src/rclcpp/duration.cpp index 6c20757d46..2966cc4899 100644 --- a/rclcpp/src/rclcpp/duration.cpp +++ b/rclcpp/src/rclcpp/duration.cpp @@ -37,11 +37,6 @@ Duration::Duration(int32_t seconds, uint32_t nanoseconds) rcl_duration_.nanoseconds += nanoseconds; } -Duration::Duration(rcl_duration_value_t nanoseconds) -{ - rcl_duration_.nanoseconds = nanoseconds; -} - Duration::Duration(std::chrono::nanoseconds nanoseconds) { rcl_duration_.nanoseconds = nanoseconds.count(); diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 140fa5f993..8107458f43 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -310,24 +310,6 @@ Node::create_callback_group( return node_base_->create_callback_group(group_type, automatically_add_to_executor_with_node); } -const rclcpp::ParameterValue & -Node::declare_parameter(const std::string & name) -{ -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - return this->node_parameters_->declare_parameter(name); -#ifndef _WIN32 -# pragma GCC diagnostic pop -#else -# pragma warning(pop) -#endif -} - const rclcpp::ParameterValue & Node::declare_parameter( const std::string & name, diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 6a76f8b523..aeaaa9a439 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -467,14 +467,6 @@ declare_parameter_helper( return parameters.at(name).value; } -const rclcpp::ParameterValue & -NodeParameters::declare_parameter(const std::string & name) -{ - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.dynamic_typing = true; - return this->declare_parameter(name, rclcpp::ParameterValue{}, descriptor, false); -} - const rclcpp::ParameterValue & NodeParameters::declare_parameter( const std::string & name, diff --git a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp index 02daee010b..4fd3f32626 100644 --- a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp +++ b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp @@ -84,35 +84,6 @@ class TestAnySubscriptionCallbackTA : public ::testing::Test rclcpp::MessageInfo message_info_; }; -void construct_with_null_allocator() -{ -// suppress deprecated function warning -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else // !defined(_WIN32) -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - - // We need to wrap this in a function because `EXPECT_THROW` is a macro, and thinks - // that the comma in here splits macro arguments, not the template arguments. - rclcpp::AnySubscriptionCallback any_subscription_callback(nullptr); - -// remove warning suppression -#if !defined(_WIN32) -# pragma GCC diagnostic pop -#else // !defined(_WIN32) -# pragma warning(pop) -#endif -} - -TEST(AnySubscriptionCallback, null_allocator) { - EXPECT_THROW( - construct_with_null_allocator(), - std::invalid_argument); -} - TEST_F(TestAnySubscriptionCallback, construct_destruct) { // Default constructor. rclcpp::AnySubscriptionCallback asc1; diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index 7b4bade94b..58af342561 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -3138,20 +3138,4 @@ TEST_F(TestNode, static_and_dynamic_typing) { "uninitialized_not_valid_except_dynamic_typing", rclcpp::ParameterValue{}), rclcpp::exceptions::InvalidParameterTypeException); } - { -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - auto param = node->declare_parameter("deprecated_way_dynamic_typing"); - EXPECT_EQ(param, rclcpp::ParameterValue{}); -#ifndef _WIN32 -# pragma GCC diagnostic pop -#else -# pragma warning(pop) -#endif - } } diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 5eac5419f1..a9bf2a5008 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -342,107 +342,13 @@ class Client : public ClientBase using Feedback = typename ActionT::Feedback; using GoalHandle = ClientGoalHandle; using WrappedResult = typename GoalHandle::WrappedResult; + using GoalResponseCallback = std::function; using FeedbackCallback = typename GoalHandle::FeedbackCallback; using ResultCallback = typename GoalHandle::ResultCallback; using CancelRequest = typename ActionT::Impl::CancelGoalService::Request; using CancelResponse = typename ActionT::Impl::CancelGoalService::Response; using CancelCallback = std::function; - /// Compatibility wrapper for `goal_response_callback`. - class GoalResponseCallback - { -public: - using NewSignature = std::function; - using OldSignature = std::function)>; - - GoalResponseCallback() = default; - - GoalResponseCallback(std::nullptr_t) {} // NOLINT, intentionally implicit. - - // implicit constructor - [[deprecated( - "Use new goal response callback signature " - "`std::function::GoalHandle::SharedPtr)>` " - "instead of the old " - "`std::function::GoalHandle::SharedPtr>)>`.\n" - "e.g.:\n" - "```cpp\n" - "Client::SendGoalOptions options;\n" - "options.goal_response_callback = [](Client::GoalHandle::SharedPtr goal) {\n" - " // do something with `goal` here\n" - "};")]] - GoalResponseCallback(OldSignature old_callback) // NOLINT, intentionally implicit. - : old_callback_(std::move(old_callback)) {} - - GoalResponseCallback(NewSignature new_callback) // NOLINT, intentionally implicit. - : new_callback_(std::move(new_callback)) {} - - GoalResponseCallback & - operator=(OldSignature old_callback) {old_callback_ = std::move(old_callback); return *this;} - - GoalResponseCallback & - operator=(NewSignature new_callback) {new_callback_ = std::move(new_callback); return *this;} - - void - operator()(typename GoalHandle::SharedPtr goal_handle) const - { - if (new_callback_) { - new_callback_(std::move(goal_handle)); - return; - } - if (old_callback_) { - throw std::runtime_error{ - "Cannot call GoalResponseCallback(GoalHandle::SharedPtr) " - "if using the old goal response callback signature."}; - } - throw std::bad_function_call{}; - } - - [[deprecated( - "Calling " - "`void goal_response_callback(" - " std::shared_future::GoalHandle::SharedPtr> goal_handle_shared_future)`" - " is deprecated.")]] - void - operator()(std::shared_future goal_handle_future) const - { - if (old_callback_) { - old_callback_(std::move(goal_handle_future)); - return; - } - if (new_callback_) { - new_callback_(std::move(goal_handle_future).get_future().share()); - return; - } - throw std::bad_function_call{}; - } - - explicit operator bool() const noexcept { - return new_callback_ || old_callback_; - } - -private: - friend class Client; - void - operator()( - typename GoalHandle::SharedPtr goal_handle, - std::shared_future goal_handle_future) const - { - if (new_callback_) { - new_callback_(std::move(goal_handle)); - return; - } - if (old_callback_) { - old_callback_(std::move(goal_handle_future)); - return; - } - throw std::bad_function_call{}; - } - - NewSignature new_callback_; - OldSignature old_callback_; - }; - /// Options for sending a goal. /** * This struct is used to pass parameters to the function `async_send_goal`. @@ -524,14 +430,14 @@ class Client : public ClientBase goal_request->goal = goal; this->send_goal_request( std::static_pointer_cast(goal_request), - [this, goal_request, options, promise, future](std::shared_ptr response) mutable + [this, goal_request, options, promise](std::shared_ptr response) mutable { using GoalResponse = typename ActionT::Impl::SendGoalService::Response; auto goal_response = std::static_pointer_cast(response); if (!goal_response->accepted) { promise->set_value(nullptr); if (options.goal_response_callback) { - options.goal_response_callback(nullptr, future); + options.goal_response_callback(nullptr); } return; } @@ -547,7 +453,7 @@ class Client : public ClientBase } promise->set_value(goal_handle); if (options.goal_response_callback) { - options.goal_response_callback(goal_handle, future); + options.goal_response_callback(goal_handle); } if (options.result_callback) { diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index b99aa236bd..b94a82d500 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -536,64 +536,6 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait } } -TEST_F(TestClientAgainstServer, async_send_goal_with_deprecated_goal_response_callback) -{ - auto action_client = rclcpp_action::create_client(client_node, action_name); - ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); - - bool goal_response_received = false; - auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); - -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - send_goal_ops.goal_response_callback = - [&goal_response_received] - (std::shared_future goal_future) - { - if (goal_future.get()) { - goal_response_received = true; - } - }; -#if !defined(_WIN32) -# pragma GCC diagnostic pop -#else -# pragma warning(pop) -#endif - - { - ActionGoal bad_goal; - bad_goal.order = -1; - auto future_goal_handle = action_client->async_send_goal(bad_goal, send_goal_ops); - dual_spin_until_future_complete(future_goal_handle); - auto goal_handle = future_goal_handle.get(); - EXPECT_FALSE(goal_response_received); - EXPECT_EQ(nullptr, goal_handle); - } - - { - ActionGoal goal; - goal.order = 4; - auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); - dual_spin_until_future_complete(future_goal_handle); - auto goal_handle = future_goal_handle.get(); - EXPECT_TRUE(goal_response_received); - EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); - EXPECT_FALSE(goal_handle->is_feedback_aware()); - EXPECT_FALSE(goal_handle->is_result_aware()); - auto future_result = action_client->async_get_result(goal_handle); - EXPECT_TRUE(goal_handle->is_result_aware()); - dual_spin_until_future_complete(future_result); - auto wrapped_result = future_result.get(); - ASSERT_EQ(5u, wrapped_result.result->sequence.size()); - EXPECT_EQ(3, wrapped_result.result->sequence.back()); - } -} - TEST_F(TestClientAgainstServer, async_send_goal_with_feedback_callback_wait_for_result) { auto action_client = rclcpp_action::create_client(client_node, action_name); diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index b091ea9c00..6048d0d691 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -333,22 +333,6 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rcl_interfaces::msg::ParameterDescriptor{}, bool ignore_override = false); - /// Declare a parameter - [[deprecated( - "declare_parameter() with only a name is deprecated and will be deleted in the future.\n" \ - "If you want to declare a parameter that won't change type without a default value use:\n" \ - "`node->declare_parameter(name)`, where e.g. ParameterT=int64_t.\n\n" \ - "If you want to declare a parameter that can dynamically change type use:\n" \ - "```\n" \ - "rcl_interfaces::msg::ParameterDescriptor descriptor;\n" \ - "descriptor.dynamic_typing = true;\n" \ - "node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);\n" \ - "```" - )]] - RCLCPP_LIFECYCLE_PUBLIC - const rclcpp::ParameterValue & - declare_parameter(const std::string & name); - /// Declare and initialize a parameter with a type. /** * \sa rclcpp::Node::declare_parameter diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index c012e50804..06378b594b 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -182,24 +182,6 @@ LifecycleNode::declare_parameter( name, default_value, parameter_descriptor, ignore_override); } -const rclcpp::ParameterValue & -LifecycleNode::declare_parameter(const std::string & name) -{ -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - return this->node_parameters_->declare_parameter(name); -#ifndef _WIN32 -# pragma GCC diagnostic pop -#else -# pragma warning(pop) -#endif -} - const rclcpp::ParameterValue & LifecycleNode::declare_parameter( const std::string & name, From 76aae4f799fa11a3e942479877408080f10cabaf Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 8 Apr 2022 16:25:08 -0700 Subject: [PATCH 101/455] changelogs Signed-off-by: William Woodall --- rclcpp/CHANGELOG.rst | 5 +++++ rclcpp_action/CHANGELOG.rst | 5 +++++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 5 +++++ 4 files changed, 18 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 9842ec7e35..a6c295ad13 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* remove things that were deprecated during galactic (`#1913 `_) +* Contributors: William Woodall + 15.4.0 (2022-04-05) ------------------- * add take_data_by_entity_id API to waitable (`#1892 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 2b652ee089..06ef47367d 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* remove things that were deprecated during galactic (`#1913 `_) +* Contributors: William Woodall + 15.4.0 (2022-04-05) ------------------- * add take_data_by_entity_id API to waitable (`#1892 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 149c22fa29..0cdcdb9e10 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 15.4.0 (2022-04-05) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index fc1d83b05f..46cfab4b2b 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* remove things that were deprecated during galactic (`#1913 `_) +* Contributors: William Woodall + 15.4.0 (2022-04-05) ------------------- From 248d911ea58c834e0767d54ebb64cb4ead71cc25 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 8 Apr 2022 16:27:59 -0700 Subject: [PATCH 102/455] 16.0.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index a6c295ad13..f76581f6e4 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +16.0.0 (2022-04-08) +------------------- * remove things that were deprecated during galactic (`#1913 `_) * Contributors: William Woodall diff --git a/rclcpp/package.xml b/rclcpp/package.xml index b62bcddc33..15a01688f3 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 15.4.0 + 16.0.0 The ROS client library in C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 06ef47367d..ec221c0131 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +16.0.0 (2022-04-08) +------------------- * remove things that were deprecated during galactic (`#1913 `_) * Contributors: William Woodall diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 6e88ac86b6..08eaacc2a1 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 15.4.0 + 16.0.0 Adds action APIs for C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 0cdcdb9e10..7860711c9a 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +16.0.0 (2022-04-08) +------------------- 15.4.0 (2022-04-05) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 29f58c24e9..aef4f7251a 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 15.4.0 + 16.0.0 Package containing tools for dynamically loadable components Ivan Paunovic Jacob Perron diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 46cfab4b2b..7c9c339eaa 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +16.0.0 (2022-04-08) +------------------- * remove things that were deprecated during galactic (`#1913 `_) * Contributors: William Woodall diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index b85a973b63..3f01d21eee 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 15.4.0 + 16.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Jacob Perron From d99157d731c7c95376536f349fba6ae81523e332 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 11 Apr 2022 21:26:09 +0800 Subject: [PATCH 103/455] remove DEFINE_CONTENT_FILTER cmake option (#1914) * remove DEFINE_CONTENT_FILTER cmake option Signed-off-by: Chen Lihui * remove the macro CONTENT_FILTER_ENABLE as well Signed-off-by: Chen Lihui --- rclcpp/CMakeLists.txt | 5 --- rclcpp/include/rclcpp/subscription_base.hpp | 2 -- .../include/rclcpp/subscription_options.hpp | 3 +- rclcpp/src/rclcpp/subscription_base.cpp | 2 -- rclcpp/test/rclcpp/CMakeLists.txt | 34 +++++++++---------- 5 files changed, 17 insertions(+), 29 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index de424342ed..9572f422a8 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -2,11 +2,6 @@ cmake_minimum_required(VERSION 3.12) project(rclcpp) -option(DEFINE_CONTENT_FILTER "Content filter feature support" ON) -if(DEFINE_CONTENT_FILTER) - add_definitions(-DCONTENT_FILTER_ENABLE) -endif() - find_package(Threads REQUIRED) find_package(ament_cmake_ros REQUIRED) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 0ad098b027..f21f27e864 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -492,7 +492,6 @@ class SubscriptionBase : public std::enable_shared_from_this event_handlers_[event_type]->clear_on_ready_callback(); } -#ifdef CONTENT_FILTER_ENABLE /// Check if content filtered topic feature of the subscription instance is enabled. /** * \return boolean flag indicating if the content filtered topic of this subscription is enabled. @@ -526,7 +525,6 @@ class SubscriptionBase : public std::enable_shared_from_this RCLCPP_PUBLIC rclcpp::ContentFilterOptions get_content_filter() const; -#endif // CONTENT_FILTER_ENABLE protected: template diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index da81b8ffb0..b6914ce4b2 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -126,7 +126,6 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase rmw_implementation_payload->modify_rmw_subscription_options(result.rmw_subscription_options); } -#ifdef CONTENT_FILTER_ENABLE // Copy content_filter_options into rcl_subscription_options. if (!content_filter_options.filter_expression.empty()) { std::vector cstrings = @@ -141,7 +140,7 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase ret, "failed to set content_filter_options"); } } -#endif // CONTENT_FILTER_ENABLE + return result; } diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index dfe3c30ead..300f465a41 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -357,7 +357,6 @@ SubscriptionBase::set_on_new_message_callback( } } -#ifdef CONTENT_FILTER_ENABLE bool SubscriptionBase::is_cft_enabled() const { @@ -445,4 +444,3 @@ SubscriptionBase::get_content_filter() const return ret_options; } -#endif // CONTENT_FILTER_ENABLE diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 2d7e9a3001..6f915feef5 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -734,22 +734,20 @@ if(TARGET test_graph_listener) target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick) endif() -if(DEFINE_CONTENT_FILTER) - function(test_subscription_content_filter_for_rmw_implementation) - set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) - ament_add_gmock(test_subscription_content_filter${target_suffix} - test_subscription_content_filter.cpp - ENV ${rmw_implementation_env_var} - TIMEOUT 120 +function(test_subscription_content_filter_for_rmw_implementation) + set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) + ament_add_gmock(test_subscription_content_filter${target_suffix} + test_subscription_content_filter.cpp + ENV ${rmw_implementation_env_var} + TIMEOUT 120 + ) + if(TARGET test_subscription_content_filter${target_suffix}) + target_link_libraries(test_subscription_content_filter${target_suffix} ${PROJECT_NAME} mimick) + ament_target_dependencies(test_subscription_content_filter${target_suffix} + "rcpputils" + "rosidl_typesupport_cpp" + "test_msgs" ) - if(TARGET test_subscription_content_filter${target_suffix}) - target_link_libraries(test_subscription_content_filter${target_suffix} ${PROJECT_NAME} mimick) - ament_target_dependencies(test_subscription_content_filter${target_suffix} - "rcpputils" - "rosidl_typesupport_cpp" - "test_msgs" - ) - endif() - endfunction() - call_for_each_rmw_implementation(test_subscription_content_filter_for_rmw_implementation) -endif() + endif() +endfunction() +call_for_each_rmw_implementation(test_subscription_content_filter_for_rmw_implementation) From c24e485084f421292734ebf6b63f1a105a2c7602 Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Wed, 13 Apr 2022 18:17:29 -0700 Subject: [PATCH 104/455] 16.0.1 Signed-off-by: Audrow Nash --- rclcpp/CHANGELOG.rst | 5 +++++ rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 18 insertions(+), 4 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index f76581f6e4..dd4eadc225 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +16.0.1 (2022-04-13) +------------------- +* remove DEFINE_CONTENT_FILTER cmake option (`#1914 `_) +* Contributors: Chen Lihui + 16.0.0 (2022-04-08) ------------------- * remove things that were deprecated during galactic (`#1913 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 15a01688f3..9a9e358be0 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 16.0.0 + 16.0.1 The ROS client library in C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index ec221c0131..0e3bdc155e 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +16.0.1 (2022-04-13) +------------------- + 16.0.0 (2022-04-08) ------------------- * remove things that were deprecated during galactic (`#1913 `_) diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 08eaacc2a1..76e9cfcddf 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 16.0.0 + 16.0.1 Adds action APIs for C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 7860711c9a..b35089782f 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +16.0.1 (2022-04-13) +------------------- + 16.0.0 (2022-04-08) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index aef4f7251a..834e9a7527 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 16.0.0 + 16.0.1 Package containing tools for dynamically loadable components Ivan Paunovic Jacob Perron diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 7c9c339eaa..c8052cd9c8 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +16.0.1 (2022-04-13) +------------------- + 16.0.0 (2022-04-08) ------------------- * remove things that were deprecated during galactic (`#1913 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 3f01d21eee..44bd945e3c 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 16.0.0 + 16.0.1 Package containing a prototype for lifecycle implementation Ivan Paunovic Jacob Perron From a1980678aecbfd9c5a388a943d5077889b2001ee Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Tue, 26 Apr 2022 13:39:00 -0700 Subject: [PATCH 105/455] use reinterpret_cast for function pointer conversion. (#1919) Signed-off-by: Tomoya Fujita --- rclcpp/include/rclcpp/timer.hpp | 8 ++++---- rclcpp/test/rclcpp/test_create_timer.cpp | 17 +++++++++++++++++ 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index d55167e3ad..e31c9fe4bf 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -189,10 +189,10 @@ class GenericTimer : public TimerBase TRACEPOINT( rclcpp_timer_callback_added, static_cast(get_timer_handle().get()), - static_cast(&callback_)); + reinterpret_cast(&callback_)); TRACEPOINT( rclcpp_callback_register, - static_cast(&callback_), + reinterpret_cast(&callback_), tracetools::get_symbol(callback_)); } @@ -226,9 +226,9 @@ class GenericTimer : public TimerBase void execute_callback() override { - TRACEPOINT(callback_start, static_cast(&callback_), false); + TRACEPOINT(callback_start, reinterpret_cast(&callback_), false); execute_callback_delegate<>(); - TRACEPOINT(callback_end, static_cast(&callback_)); + TRACEPOINT(callback_end, reinterpret_cast(&callback_)); } // void specialization diff --git a/rclcpp/test/rclcpp/test_create_timer.cpp b/rclcpp/test/rclcpp/test_create_timer.cpp index f6acd79b24..13c3564544 100644 --- a/rclcpp/test/rclcpp/test_create_timer.cpp +++ b/rclcpp/test/rclcpp/test_create_timer.cpp @@ -116,3 +116,20 @@ TEST(TestCreateTimer, call_wall_timer_with_bad_arguments) std::invalid_argument); rclcpp::shutdown(); } + +static void test_timer_callback(void) {} + +TEST(TestCreateTimer, timer_function_pointer) +{ + rclcpp::init(0, nullptr); + auto node = std::make_shared("timer_function_pointer_node"); + + // make sure build succeeds with function pointer instead of lambda + auto some_timer = rclcpp::create_timer( + node, + node->get_clock(), + rclcpp::Duration(0ms), + test_timer_callback); + + rclcpp::shutdown(); +} From 82ddd441405a5122f1a2e445840cc2b93d713166 Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Fri, 29 Apr 2022 17:40:56 -0700 Subject: [PATCH 106/455] 16.1.0 Signed-off-by: Audrow Nash --- rclcpp/CHANGELOG.rst | 5 +++++ rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 18 insertions(+), 4 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index dd4eadc225..c621e8359d 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +16.1.0 (2022-04-29) +------------------- +* use reinterpret_cast for function pointer conversion. (`#1919 `_) +* Contributors: Tomoya Fujita + 16.0.1 (2022-04-13) ------------------- * remove DEFINE_CONTENT_FILTER cmake option (`#1914 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 9a9e358be0..c4e288b1af 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 16.0.1 + 16.1.0 The ROS client library in C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 0e3bdc155e..5c6491739d 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +16.1.0 (2022-04-29) +------------------- + 16.0.1 (2022-04-13) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 76e9cfcddf..bd2597d3ff 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 16.0.1 + 16.1.0 Adds action APIs for C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index b35089782f..29d087cc0c 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +16.1.0 (2022-04-29) +------------------- + 16.0.1 (2022-04-13) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 834e9a7527..2b379f3040 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 16.0.1 + 16.1.0 Package containing tools for dynamically loadable components Ivan Paunovic Jacob Perron diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index c8052cd9c8..63c2748c92 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +16.1.0 (2022-04-29) +------------------- + 16.0.1 (2022-04-13) ------------------- diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 44bd945e3c..278ea41517 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 16.0.1 + 16.1.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Jacob Perron From ee67c211c52b6f03a78b55a6cff377c65a519bdc Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 3 May 2022 10:35:10 -0700 Subject: [PATCH 107/455] Add 'best available' QoS enum values and methods (#1920) If users set a policy as 'best available', then the middleware will pick a policy that is most compatible with the current set of discovered endpoints while maintaining the highest level of service possible. For details about the expected behavior, see connected changes: - https://github.com/ros2/rmw/pull/320 - https://github.com/ros2/rmw_dds_common/pull/60 Signed-off-by: Jacob Perron --- rclcpp/include/rclcpp/qos.hpp | 41 +++++++++++++++++++++++++++++++++ rclcpp/src/rclcpp/qos.cpp | 16 +++++++++++++ rclcpp/test/rclcpp/test_qos.cpp | 9 ++++++++ 3 files changed, 66 insertions(+) diff --git a/rclcpp/include/rclcpp/qos.hpp b/rclcpp/include/rclcpp/qos.hpp index 99d5acbd33..4946d37889 100644 --- a/rclcpp/include/rclcpp/qos.hpp +++ b/rclcpp/include/rclcpp/qos.hpp @@ -44,6 +44,7 @@ enum class ReliabilityPolicy BestEffort = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, Reliable = RMW_QOS_POLICY_RELIABILITY_RELIABLE, SystemDefault = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT, + BestAvailable = RMW_QOS_POLICY_RELIABILITY_BEST_AVAILABLE, Unknown = RMW_QOS_POLICY_RELIABILITY_UNKNOWN, }; @@ -52,6 +53,7 @@ enum class DurabilityPolicy Volatile = RMW_QOS_POLICY_DURABILITY_VOLATILE, TransientLocal = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, SystemDefault = RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT, + BestAvailable = RMW_QOS_POLICY_DURABILITY_BEST_AVAILABLE, Unknown = RMW_QOS_POLICY_DURABILITY_UNKNOWN, }; @@ -60,6 +62,7 @@ enum class LivelinessPolicy Automatic = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC, ManualByTopic = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, SystemDefault = RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + BestAvailable = RMW_QOS_POLICY_LIVELINESS_BEST_AVAILABLE, Unknown = RMW_QOS_POLICY_LIVELINESS_UNKNOWN, }; @@ -180,6 +183,10 @@ class RCLCPP_PUBLIC QoS QoS & best_effort(); + /// Set the reliability setting to best available. + QoS & + reliability_best_available(); + /// Set the durability setting. QoS & durability(rmw_qos_durability_policy_t durability); @@ -199,6 +206,10 @@ class RCLCPP_PUBLIC QoS QoS & transient_local(); + /// Set the durability setting to best available. + QoS & + durability_best_available(); + /// Set the deadline setting. QoS & deadline(rmw_time_t deadline); @@ -488,6 +499,36 @@ class RCLCPP_PUBLIC SystemDefaultsQoS : public QoS )); }; +/** + * Best available QoS class + * + * Match majority of endpoints currently available while maintaining the highest level of service. + * Policies are chosen at the time of creating a subscription or publisher. + * The middleware is not expected to update policies after creating a subscription or publisher, + * even if one or more policies are incompatible with newly discovered endpoints. + * Therefore, this profile should be used with care since non-deterministic behavior can occur due + * to races with discovery. + * + * - History: Keep last, + * - Depth: 10, + * - Reliability: Best available, + * - Durability: Best available, + * - Deadline: Best available, + * - Lifespan: Default, + * - Liveliness: Best available, + * - Liveliness lease duration: Best available, + * - avoid ros namespace conventions: false + */ +class RCLCPP_PUBLIC BestAvailableQoS : public QoS +{ +public: + explicit + BestAvailableQoS( + const QoSInitialization & qos_initialization = ( + QoSInitialization::from_rmw(rmw_qos_profile_best_available) + )); +}; + } // namespace rclcpp #endif // RCLCPP__QOS_HPP_ diff --git a/rclcpp/src/rclcpp/qos.cpp b/rclcpp/src/rclcpp/qos.cpp index 8b912de07f..70b9976db6 100644 --- a/rclcpp/src/rclcpp/qos.cpp +++ b/rclcpp/src/rclcpp/qos.cpp @@ -150,6 +150,12 @@ QoS::best_effort() return this->reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); } +QoS & +QoS::reliability_best_available() +{ + return this->reliability(RMW_QOS_POLICY_RELIABILITY_BEST_AVAILABLE); +} + QoS & QoS::durability(rmw_qos_durability_policy_t durability) { @@ -176,6 +182,12 @@ QoS::transient_local() return this->durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); } +QoS & +QoS::durability_best_available() +{ + return this->durability(RMW_QOS_POLICY_DURABILITY_BEST_AVAILABLE); +} + QoS & QoS::deadline(rmw_time_t deadline) { @@ -380,4 +392,8 @@ SystemDefaultsQoS::SystemDefaultsQoS(const QoSInitialization & qos_initializatio : QoS(qos_initialization, rmw_qos_profile_system_default) {} +BestAvailableQoS::BestAvailableQoS(const QoSInitialization & qos_initialization) +: QoS(qos_initialization, rmw_qos_profile_best_available) +{} + } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/test_qos.cpp b/rclcpp/test/rclcpp/test_qos.cpp index 47d1d10b07..b2446beed3 100644 --- a/rclcpp/test/rclcpp/test_qos.cpp +++ b/rclcpp/test/rclcpp/test_qos.cpp @@ -93,6 +93,9 @@ TEST(TestQoS, setters_and_getters) { qos.reliable(); EXPECT_EQ(rclcpp::ReliabilityPolicy::Reliable, qos.reliability()); + qos.reliability_best_available(); + EXPECT_EQ(rclcpp::ReliabilityPolicy::BestAvailable, qos.reliability()); + qos.reliability(rclcpp::ReliabilityPolicy::BestEffort); EXPECT_EQ(rclcpp::ReliabilityPolicy::BestEffort, qos.reliability()); @@ -102,6 +105,9 @@ TEST(TestQoS, setters_and_getters) { qos.transient_local(); EXPECT_EQ(rclcpp::DurabilityPolicy::TransientLocal, qos.durability()); + qos.durability_best_available(); + EXPECT_EQ(rclcpp::DurabilityPolicy::BestAvailable, qos.durability()); + qos.durability(rclcpp::DurabilityPolicy::Volatile); EXPECT_EQ(rclcpp::DurabilityPolicy::Volatile, qos.durability()); @@ -183,6 +189,9 @@ TEST(TestQoS, DerivedTypes) { const rclcpp::KeepLast expected_initialization(RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT); const rclcpp::QoS expected_default(expected_initialization); EXPECT_EQ(expected_default.get_rmw_qos_profile(), system_default_qos.get_rmw_qos_profile()); + + rclcpp::BestAvailableQoS best_available_qos; + EXPECT_EQ(rmw_qos_profile_best_available, best_available_qos.get_rmw_qos_profile()); } TEST(TestQoS, policy_name_from_kind) { From b8b64b4c08ed8afedf8f776c8fdd89dc3479d134 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Wed, 4 May 2022 02:21:20 +0800 Subject: [PATCH 108/455] Update get_parameter_from_event to follow the function description (#1922) Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/parameter_event_handler.hpp | 1 + rclcpp/src/rclcpp/parameter_event_handler.cpp | 10 +++++++--- rclcpp/test/rclcpp/test_parameter_event_handler.cpp | 12 ++++++++++-- 3 files changed, 18 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter_event_handler.hpp b/rclcpp/include/rclcpp/parameter_event_handler.hpp index c2ee55a540..52a0233966 100644 --- a/rclcpp/include/rclcpp/parameter_event_handler.hpp +++ b/rclcpp/include/rclcpp/parameter_event_handler.hpp @@ -268,6 +268,7 @@ class ParameterEventHandler * \param[in] parameter_name Name of parameter. * \param[in] node_name Name of node which hosts the parameter. * \returns The resultant rclcpp::Parameter from the event. + * \throws std::runtime_error if input node name doesn't match the node name in parameter event. */ RCLCPP_PUBLIC static rclcpp::Parameter diff --git a/rclcpp/src/rclcpp/parameter_event_handler.cpp b/rclcpp/src/rclcpp/parameter_event_handler.cpp index 6ba5266809..b1b36b663e 100644 --- a/rclcpp/src/rclcpp/parameter_event_handler.cpp +++ b/rclcpp/src/rclcpp/parameter_event_handler.cpp @@ -133,9 +133,13 @@ ParameterEventHandler::get_parameter_from_event( { rclcpp::Parameter p; if (!get_parameter_from_event(event, p, parameter_name, node_name)) { - throw std::runtime_error( - "Parameter '" + parameter_name + "' of node '" + node_name + - "' is not part of parameter event"); + if (event.node == node_name) { + return rclcpp::Parameter(parameter_name, rclcpp::PARAMETER_NOT_SET); + } else { + throw std::runtime_error( + "The node name '" + node_name + "' of parameter '" + parameter_name + + +"' doesn't match the node name '" + event.node + "' in parameter event"); + } } return p; } diff --git a/rclcpp/test/rclcpp/test_parameter_event_handler.cpp b/rclcpp/test/rclcpp/test_parameter_event_handler.cpp index 9bdd96eab4..c80ca2f6d5 100644 --- a/rclcpp/test/rclcpp/test_parameter_event_handler.cpp +++ b/rclcpp/test/rclcpp/test_parameter_event_handler.cpp @@ -202,9 +202,17 @@ TEST_F(TestNode, GetParameterFromEvent) EXPECT_THROW( ParameterEventHandler::get_parameter_from_event(*multiple, "my_int", wrong_name), std::runtime_error); - // Throws if parameter not part of event + + // Parameter not part of event + // with correct node + rclcpp::Parameter expect_notset_ret("my_notset", rclcpp::PARAMETER_NOT_SET); + rclcpp::Parameter ret; + EXPECT_NO_THROW( + ret = ParameterEventHandler::get_parameter_from_event(*multiple, "my_notset", node_name);); + EXPECT_EQ(ret, expect_notset_ret); + // with incorrect node EXPECT_THROW( - ParameterEventHandler::get_parameter_from_event(*diff_ns_bool, "my_int", node_name), + ParameterEventHandler::get_parameter_from_event(*multiple, "my_notset", wrong_name), std::runtime_error); } From 491475f2324bb9dfcc943f4b9f3d4528d6e81716 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 3 May 2022 12:14:26 -0700 Subject: [PATCH 109/455] 16.2.0 --- rclcpp/CHANGELOG.rst | 6 ++++++ rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 19 insertions(+), 4 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index c621e8359d..76ba0e5c4c 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +16.2.0 (2022-05-03) +------------------- +* Update get_parameter_from_event to follow the function description (`#1922 `_) +* Add 'best available' QoS enum values and methods (`#1920 `_) +* Contributors: Barry Xu, Jacob Perron + 16.1.0 (2022-04-29) ------------------- * use reinterpret_cast for function pointer conversion. (`#1919 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index c4e288b1af..6de7d25d07 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 16.1.0 + 16.2.0 The ROS client library in C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 5c6491739d..9838922b49 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +16.2.0 (2022-05-03) +------------------- + 16.1.0 (2022-04-29) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index bd2597d3ff..dd6e61338c 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 16.1.0 + 16.2.0 Adds action APIs for C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 29d087cc0c..805592a07f 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +16.2.0 (2022-05-03) +------------------- + 16.1.0 (2022-04-29) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 2b379f3040..18275306ff 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 16.1.0 + 16.2.0 Package containing tools for dynamically loadable components Ivan Paunovic Jacob Perron diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 63c2748c92..420252b66a 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +16.2.0 (2022-05-03) +------------------- + 16.1.0 (2022-04-29) ------------------- diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 278ea41517..3ff98f3069 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 16.1.0 + 16.2.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Jacob Perron From 02802bcc385c3d6d814add93618f3388d44adec7 Mon Sep 17 00:00:00 2001 From: Jochen Sprickerhof Date: Fri, 13 May 2022 04:08:34 +0200 Subject: [PATCH 110/455] Drop wrong template specialization (#1926) This fails with g++ -std=gnu++20. Signed-off-by: Jochen Sprickerhof --- rclcpp/include/rclcpp/publisher_options.hpp | 2 +- rclcpp/include/rclcpp/subscription_options.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index 48250307e9..3c88ebccd1 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -72,7 +72,7 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase /// Optional custom allocator. std::shared_ptr allocator = nullptr; - PublisherOptionsWithAllocator() {} + PublisherOptionsWithAllocator() {} /// Constructor using base class as input. explicit PublisherOptionsWithAllocator(const PublisherOptionsBase & publisher_options_base) diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index b6914ce4b2..2b819da399 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -97,7 +97,7 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase /// Optional custom allocator. std::shared_ptr allocator = nullptr; - SubscriptionOptionsWithAllocator() {} + SubscriptionOptionsWithAllocator() {} /// Constructor using base class as input. explicit SubscriptionOptionsWithAllocator( From 5c688303b3cb994969f448979f64c12971243295 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Thu, 19 May 2022 23:10:17 +0800 Subject: [PATCH 111/455] Add statistics for handle_loaned_message (#1927) * Add statistics for handle_loaned_message Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/subscription.hpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 69b6031405..11bf9c6e43 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -363,11 +363,31 @@ class Subscription : public SubscriptionBase void * loaned_message, const rclcpp::MessageInfo & message_info) override { + if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid)) { + // In this case, the message will be delivered via intra process and + // we should ignore this copy of the message. + return; + } + auto typed_message = static_cast(loaned_message); // message is loaned, so we have to make sure that the deleter does not deallocate the message auto sptr = std::shared_ptr( typed_message, [](ROSMessageType * msg) {(void) msg;}); + + std::chrono::time_point now; + if (subscription_topic_statistics_) { + // get current time before executing callback to + // exclude callback duration from topic statistics result. + now = std::chrono::system_clock::now(); + } + any_callback_.dispatch(sptr, message_info); + + if (subscription_topic_statistics_) { + const auto nanos = std::chrono::time_point_cast(now); + const auto time = rclcpp::Time(nanos.time_since_epoch().count()); + subscription_topic_statistics_->handle_message(*typed_message, time); + } } /// Return the borrowed message. From 790e529ba359b83100ed4f7ca64a773aaa608968 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Thu, 2 Jun 2022 18:58:19 +0100 Subject: [PATCH 112/455] Always trigger guard condition waitset (#1923) * trigger guard condition waitset regardless of whether a trigger callback is present Signed-off-by: Alberto Soragna * restore mutex in guard_condition.cpp Signed-off-by: Alberto Soragna * remove whitespace Signed-off-by: Alberto Soragna * add unit-test Signed-off-by: Alberto Soragna * add documentation for trigger and set_on_trigger_callback Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/guard_condition.hpp | 18 +++++++++++++++- rclcpp/src/rclcpp/guard_condition.cpp | 24 +++++++++++---------- rclcpp/test/rclcpp/test_guard_condition.cpp | 18 ++++++++++++++++ 3 files changed, 48 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp index f6f5af9586..350f306010 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -72,7 +72,7 @@ class GuardCondition const rcl_guard_condition_t & get_rcl_guard_condition() const; - /// Notify the wait set waiting on this condition, if any, that the condition had been met. + /// Signal that the condition has been met, notifying both the wait set and listeners, if any. /** * This function is thread-safe, and may be called concurrently with waiting * on this guard condition in a wait set. @@ -107,6 +107,22 @@ class GuardCondition void add_to_wait_set(rcl_wait_set_t * wait_set); + /// Set a callback to be called whenever the guard condition is triggered. + /** + * The callback receives a size_t which is the number of times the guard condition was triggered + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if the guard condition was triggered before any + * callback was set. + * + * Calling it again will clear any previously set callback. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the guard condition + * or other information, you may use a lambda with captures or std::bind. + * + * \param[in] callback functor to be called when the guard condition is triggered + */ RCLCPP_PUBLIC void set_on_trigger_callback(std::function callback); diff --git a/rclcpp/src/rclcpp/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp index ea68c78d73..627644e602 100644 --- a/rclcpp/src/rclcpp/guard_condition.cpp +++ b/rclcpp/src/rclcpp/guard_condition.cpp @@ -74,16 +74,19 @@ GuardCondition::get_rcl_guard_condition() const void GuardCondition::trigger() { - std::lock_guard lock(reentrant_mutex_); + rcl_ret_t ret = rcl_trigger_guard_condition(&rcl_guard_condition_); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } - if (on_trigger_callback_) { - on_trigger_callback_(1); - } else { - rcl_ret_t ret = rcl_trigger_guard_condition(&rcl_guard_condition_); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + { + std::lock_guard lock(reentrant_mutex_); + + if (on_trigger_callback_) { + on_trigger_callback_(1); + } else { + unread_count_++; } - unread_count_++; } } @@ -125,10 +128,9 @@ GuardCondition::set_on_trigger_callback(std::function callback) callback(unread_count_); unread_count_ = 0; } - return; + } else { + on_trigger_callback_ = nullptr; } - - on_trigger_callback_ = nullptr; } } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/test_guard_condition.cpp b/rclcpp/test/rclcpp/test_guard_condition.cpp index 481051ccf9..1e72264869 100644 --- a/rclcpp/test/rclcpp/test_guard_condition.cpp +++ b/rclcpp/test/rclcpp/test_guard_condition.cpp @@ -164,3 +164,21 @@ TEST_F(TestGuardCondition, set_on_trigger_callback) { EXPECT_EQ(c1.load(), 2u); } } + +/* + * Testing that callback and waitset are both notified by triggering gc + */ +TEST_F(TestGuardCondition, callback_and_waitset) { + auto gc = std::make_shared(); + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; + gc->set_on_trigger_callback(increase_c1_cb); + + rclcpp::WaitSet wait_set; + wait_set.add_guard_condition(gc); + + gc->trigger(); + + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_set.wait(std::chrono::seconds(1)).kind()); + EXPECT_EQ(c1.load(), 1u); +} From 38581cc860fc98509c8c6654ab89a79c7dc045c0 Mon Sep 17 00:00:00 2001 From: Daniel Reuter Date: Tue, 7 Jun 2022 17:56:57 +0200 Subject: [PATCH 113/455] Fix documentation of `RCLCPP_[INFO,WARN,...]` (#1943) In the pull request https://github.com/ros2/rclcpp/pull/1442 the ability to use `std::string` as the first argument to the logging functions was (rightfully) removed. This commit just corrects the documentation of the macro, since it confused me a bit ;) Signed-off-by: Daniel Reuter --- rclcpp/resource/logging.hpp.em | 1 - 1 file changed, 1 deletion(-) diff --git a/rclcpp/resource/logging.hpp.em b/rclcpp/resource/logging.hpp.em index 7b5b0f349f..63581ceb54 100644 --- a/rclcpp/resource/logging.hpp.em +++ b/rclcpp/resource/logging.hpp.em @@ -114,7 +114,6 @@ def get_rclcpp_suffix_from_features(features): @[ end for]@ @[ if 'stream' not in feature_combination]@ * \param ... The format string, followed by the variable arguments for the format string. - * It also accepts a single argument of type std::string. @[ end if]@ */ @{params = rclcpp_feature_combinations[feature_combination].params.keys()}@ From b3f57033a9b0a1fc5b0929cc31690048b7aea54f Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Thu, 9 Jun 2022 12:14:12 -0700 Subject: [PATCH 114/455] wait for subscriptions on another thread. (#1940) Signed-off-by: Tomoya Fujita --- rclcpp/test/rclcpp/test_generic_pubsub.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/test/rclcpp/test_generic_pubsub.cpp b/rclcpp/test/rclcpp/test_generic_pubsub.cpp index 0d2bec588a..f4cef0b757 100644 --- a/rclcpp/test/rclcpp/test_generic_pubsub.cpp +++ b/rclcpp/test/rclcpp/test_generic_pubsub.cpp @@ -190,7 +190,7 @@ TEST_F(RclcppGenericNodeFixture, publish_loaned_msg_work) if (publisher->can_loan_messages()) { auto subscriber_future_ = std::async( - std::launch::deferred, [this, topic_name, type] { + std::launch::async, [this, topic_name, type] { return subscribe_raw_messages( 1, topic_name, type); }); From 8e6a6fb32d8d6a818b483660e326f2c5313b64ae Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 10 Jun 2022 17:18:25 -0300 Subject: [PATCH 115/455] Fix subscription.is_serialized() for callbacks with message info (#1950) * Fix subscription.is_serialized() for callbacks with message info argument Signed-off-by: Ivan Santiago Paunovic * Add tests + please linters Signed-off-by: Ivan Santiago Paunovic --- .../rclcpp/any_subscription_callback.hpp | 8 +- .../rclcpp/test_any_subscription_callback.cpp | 105 ++++++++++++++++++ 2 files changed, 112 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index ea26fad397..d4f5fc309b 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -950,7 +950,13 @@ class AnySubscriptionCallback std::holds_alternative(callback_variant_) || std::holds_alternative(callback_variant_) || std::holds_alternative(callback_variant_) || - std::holds_alternative(callback_variant_); + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_) || + std::holds_alternative( + callback_variant_) || + std::holds_alternative(callback_variant_); } void diff --git a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp index 4fd3f32626..45fe091f07 100644 --- a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp +++ b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp @@ -93,6 +93,111 @@ TEST_F(TestAnySubscriptionCallback, construct_destruct) { rclcpp::AnySubscriptionCallback asc2(allocator); } +TEST_F(TestAnySubscriptionCallback, is_serialized_message_callback) { + { + rclcpp::AnySubscriptionCallback asc; + asc.set([](const rclcpp::SerializedMessage &) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } + { + rclcpp::AnySubscriptionCallback asc; + asc.set([](const rclcpp::SerializedMessage &, const rclcpp::MessageInfo &) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } + { + rclcpp::AnySubscriptionCallback asc; + asc.set([](const rclcpp::SerializedMessage &, const rclcpp::MessageInfo &) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } + { + rclcpp::AnySubscriptionCallback asc; + asc.set([](std::unique_ptr) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } + { + rclcpp::AnySubscriptionCallback asc; + asc.set([](std::unique_ptr, const rclcpp::MessageInfo &) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } + { + rclcpp::AnySubscriptionCallback asc; + asc.set([](std::shared_ptr) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } + { + rclcpp::AnySubscriptionCallback asc; + asc.set([](std::shared_ptr, const rclcpp::MessageInfo &) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } + { + rclcpp::AnySubscriptionCallback asc; + asc.set([](const std::shared_ptr &) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } + { + rclcpp::AnySubscriptionCallback asc; + asc.set( + []( + const std::shared_ptr &, + const rclcpp::MessageInfo &) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } + { + rclcpp::AnySubscriptionCallback asc; + asc.set([](std::shared_ptr) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } + { + rclcpp::AnySubscriptionCallback asc; + asc.set([](std::shared_ptr, const rclcpp::MessageInfo &) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } +} + TEST_F(TestAnySubscriptionCallback, unset_dispatch_throw) { EXPECT_THROW( any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_), From 86a9d5882ed31d1fa6197a1f5d4b388f167bd23e Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Tue, 14 Jun 2022 18:22:53 +0200 Subject: [PATCH 116/455] Remove unused on_parameters_set_callback_ (#1945) Signed-off-by: Nikolai Morin --- .../node_interfaces/node_parameters.hpp | 2 -- .../node_interfaces/node_parameters.cpp | 28 +++++-------------- 2 files changed, 7 insertions(+), 23 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index 6f0d5b0ed2..3ef7ed4c24 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -196,8 +196,6 @@ class NodeParameters : public NodeParametersInterface // declare_parameter, etc). In those cases, this will be set to false. bool parameter_modification_enabled_{true}; - OnParametersSetCallbackType on_parameters_set_callback_ = nullptr; - CallbacksContainerType on_parameters_set_callback_container_; std::map parameters_; diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index aeaaa9a439..67bf370719 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -268,8 +268,7 @@ RCLCPP_LOCAL rcl_interfaces::msg::SetParametersResult __call_on_parameters_set_callbacks( const std::vector & parameters, - CallbacksContainerType & callback_container, - const OnParametersSetCallbackType & callback) + CallbacksContainerType & callback_container) { rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -286,9 +285,6 @@ __call_on_parameters_set_callbacks( it = callback_container.erase(it); } } - if (callback) { - result = callback(parameters); - } return result; } @@ -298,7 +294,6 @@ __set_parameters_atomically_common( const std::vector & parameters, std::map & parameter_infos, CallbacksContainerType & callback_container, - const OnParametersSetCallbackType & callback, bool allow_undeclared = false) { // Check if the value being set complies with the descriptor. @@ -307,9 +302,9 @@ __set_parameters_atomically_common( if (!result.successful) { return result; } - // Call the user callback to see if the new value(s) are allowed. + // Call the user callbacks to see if the new value(s) are allowed. result = - __call_on_parameters_set_callbacks(parameters, callback_container, callback); + __call_on_parameters_set_callbacks(parameters, callback_container); if (!result.successful) { return result; } @@ -336,7 +331,6 @@ __declare_parameter_common( std::map & parameters_out, const std::map & overrides, CallbacksContainerType & callback_container, - const OnParametersSetCallbackType & callback, rcl_interfaces::msg::ParameterEvent * parameter_event_out, bool ignore_override = false) { @@ -366,14 +360,13 @@ __declare_parameter_common( return result; } - // Check with the user's callback to see if the initial value can be set. + // Check with the user's callbacks to see if the initial value can be set. std::vector parameter_wrappers {rclcpp::Parameter(name, *initial_value)}; // This function also takes care of default vs initial value. auto result = __set_parameters_atomically_common( parameter_wrappers, parameter_infos, - callback_container, - callback); + callback_container); if (!result.successful) { return result; @@ -401,7 +394,6 @@ declare_parameter_helper( std::map & parameters, const std::map & overrides, CallbacksContainerType & callback_container, - const OnParametersSetCallbackType & callback, rclcpp::Publisher * events_publisher, const std::string & combined_name, rclcpp::node_interfaces::NodeClockInterface & node_clock) @@ -438,7 +430,6 @@ declare_parameter_helper( parameters, overrides, callback_container, - callback, ¶meter_event, ignore_override); @@ -486,7 +477,6 @@ NodeParameters::declare_parameter( parameters_, parameter_overrides_, on_parameters_set_callback_container_, - on_parameters_set_callback_, events_publisher_.get(), combined_name_, *node_clock_); @@ -522,7 +512,6 @@ NodeParameters::declare_parameter( parameters_, parameter_overrides_, on_parameters_set_callback_container_, - on_parameters_set_callback_, events_publisher_.get(), combined_name_, *node_clock_); @@ -636,7 +625,7 @@ NodeParameters::set_parameters_atomically(const std::vector & // Declare parameters into a temporary "staging area", incase one of the declares fail. // We will use the staged changes as input to the "set atomically" action. - // We explicitly avoid calling the user callback here, so that it may be called once, with + // We explicitly avoid calling the user callbacks here, so that it may be called once, with // all the other parameters to be set (already declared parameters). std::map staged_parameter_changes; rcl_interfaces::msg::ParameterEvent parameter_event_msg; @@ -657,7 +646,6 @@ NodeParameters::set_parameters_atomically(const std::vector & parameter_overrides_, // Only call callbacks once below empty_callback_container, // callback_container is explicitly empty - nullptr, // callback is explicitly null. ¶meter_event_msg, true); if (!result.successful) { @@ -717,11 +705,9 @@ NodeParameters::set_parameters_atomically(const std::vector & *parameters_to_be_set, // they are actually set on the official parameter storage parameters_, - // this will get called once, with all the parameters to be set - on_parameters_set_callback_container_, // These callbacks are called once. When a callback returns an unsuccessful result, // the remaining aren't called. - on_parameters_set_callback_, + on_parameters_set_callback_container_, allow_undeclared_); // allow undeclared // If not successful, then stop here. From dbded5c0d64623d93a94ee987582bec7d711b9f5 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 21 Jun 2022 18:59:25 -0700 Subject: [PATCH 117/455] fix virtual dispatch issues identified by clang-tidy (#1816) * fix virtual dispatch issues identified by clang-tidy Signed-off-by: William Woodall * fix up to reduce code duplication Signed-off-by: William Woodall * uncrustify Signed-off-by: William Woodall * avoid returning temporaries Signed-off-by: William Woodall * uncrustify Signed-off-by: William Woodall * add docs about overriding methods used in constructors Signed-off-by: William Woodall --- rclcpp/include/rclcpp/context.hpp | 7 +- rclcpp/include/rclcpp/graph_listener.hpp | 5 ++ .../rclcpp/node_interfaces/node_base.hpp | 10 ++- .../node_logging_interface.hpp | 8 ++- .../node_interfaces/node_parameters.hpp | 15 ++++ rclcpp/src/rclcpp/context.cpp | 4 +- rclcpp/src/rclcpp/graph_listener.cpp | 2 +- .../src/rclcpp/node_interfaces/node_base.cpp | 14 ++-- .../rclcpp/node_interfaces/node_logging.cpp | 2 +- .../node_interfaces/node_parameters.cpp | 68 ++++++++++++++++--- 10 files changed, 112 insertions(+), 23 deletions(-) diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 2d8db29d21..3add3f6d46 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -183,6 +183,11 @@ class Context : public std::enable_shared_from_this * * This function is thread-safe. * + * Note that if you override this method, but leave shutdown to be called in + * the destruction of this base class, it will not call the overridden + * version from your base class. + * So you need to ensure you call your class's shutdown() in its destructor. + * * \param[in] reason the description of why shutdown happened * \return true if shutdown was successful, false if context was already shutdown * \throw various exceptions derived from rclcpp::exceptions::RCLError, if rcl_shutdown fails @@ -318,7 +323,6 @@ class Context : public std::enable_shared_from_this /// Interrupt any blocking sleep_for calls, causing them to return immediately and return true. RCLCPP_PUBLIC - virtual void interrupt_all_sleep_for(); @@ -351,7 +355,6 @@ class Context : public std::enable_shared_from_this // Called by constructor and destructor to clean up by finalizing the // shutdown rcl context and preparing for a new init cycle. RCLCPP_PUBLIC - virtual void clean_up(); diff --git a/rclcpp/include/rclcpp/graph_listener.hpp b/rclcpp/include/rclcpp/graph_listener.hpp index 79357b952e..7d7f787fe8 100644 --- a/rclcpp/include/rclcpp/graph_listener.hpp +++ b/rclcpp/include/rclcpp/graph_listener.hpp @@ -128,6 +128,11 @@ class GraphListener : public std::enable_shared_from_this * If start_if_not_started() was never called, this function still succeeds, * but start_if_not_started() still cannot be called after this function. * + * Note that if you override this method, but leave shutdown to be called in + * the destruction of this base class, it will not call the overridden + * version from your base class. + * So you need to ensure you call your class's shutdown() in its destructor. + * * \throws rclcpp::execptions::RCLError from rcl_guard_condition_fini() * \throws rclcpp::execptions::RCLError from rcl_wait_set_fini() * \throws std::system_error anything std::mutex::lock() throws diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index 675aff0483..90011a26e0 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -39,6 +39,13 @@ class NodeBase : public NodeBaseInterface, public std::enable_shared_from_this; +protected: + RCLCPP_PUBLIC + void + perform_automatically_declare_parameters_from_overrides(); + private: RCLCPP_DISABLE_COPY(NodeParameters) diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index bea4eeb583..1599172e2e 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -154,7 +154,9 @@ Context::~Context() // this will not prevent errors, but will maybe make them easier to reproduce std::lock_guard lock(init_mutex_); try { - this->shutdown("context destructor was called while still not shutdown"); + // Cannot rely on virtual dispatch in a destructor, so explicitly use the + // shutdown() provided by this base class. + Context::shutdown("context destructor was called while still not shutdown"); // at this point it is shutdown and cannot reinit // clean_up will finalize the rcl context this->clean_up(); diff --git a/rclcpp/src/rclcpp/graph_listener.cpp b/rclcpp/src/rclcpp/graph_listener.cpp index e0b516b595..d7ea42117a 100644 --- a/rclcpp/src/rclcpp/graph_listener.cpp +++ b/rclcpp/src/rclcpp/graph_listener.cpp @@ -48,7 +48,7 @@ GraphListener::GraphListener(const std::shared_ptr & parent_context) GraphListener::~GraphListener() { - this->shutdown(std::nothrow); + GraphListener::shutdown(std::nothrow); } void GraphListener::init_wait_set() diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 8be8364dc6..89d8acf6fa 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -37,12 +37,13 @@ NodeBase::NodeBase( rclcpp::Context::SharedPtr context, const rcl_node_options_t & rcl_node_options, bool use_intra_process_default, - bool enable_topic_statistics_default) + bool enable_topic_statistics_default, + rclcpp::CallbackGroup::SharedPtr default_callback_group) : context_(context), use_intra_process_default_(use_intra_process_default), enable_topic_statistics_default_(enable_topic_statistics_default), node_handle_(nullptr), - default_callback_group_(nullptr), + default_callback_group_(default_callback_group), associated_with_executor_(false), notify_guard_condition_(context), notify_guard_condition_is_valid_(false) @@ -128,9 +129,12 @@ NodeBase::NodeBase( delete node; }); - // Create the default callback group. - using rclcpp::CallbackGroupType; - default_callback_group_ = create_callback_group(CallbackGroupType::MutuallyExclusive); + // Create the default callback group, if needed. + if (nullptr == default_callback_group_) { + using rclcpp::CallbackGroupType; + default_callback_group_ = + NodeBase::create_callback_group(CallbackGroupType::MutuallyExclusive); + } // Indicate the notify_guard_condition is now valid. notify_guard_condition_is_valid_ = true; diff --git a/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp b/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp index 269f14dac1..3c2266d403 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp @@ -19,7 +19,7 @@ using rclcpp::node_interfaces::NodeLogging; NodeLogging::NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base) : node_base_(node_base) { - logger_ = rclcpp::get_logger(this->get_logger_name()); + logger_ = rclcpp::get_logger(NodeLogging::get_logger_name()); } NodeLogging::~NodeLogging() diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 67bf370719..0f366792b7 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -39,6 +39,31 @@ using rclcpp::node_interfaces::NodeParameters; +RCLCPP_LOCAL +void +local_perform_automatically_declare_parameters_from_overrides( + const std::map & parameter_overrides, + std::function has_parameter, + std::function + declare_parameter) +{ + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + for (const auto & pair : parameter_overrides) { + if (!has_parameter(pair.first)) { + declare_parameter( + pair.first, + pair.second, + descriptor, + true); + } + } +} + NodeParameters::NodeParameters( const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, @@ -101,20 +126,43 @@ NodeParameters::NodeParameters( // If asked, initialize any parameters that ended up in the initial parameter values, // but did not get declared explcitily by this point. if (automatically_declare_parameters_from_overrides) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.dynamic_typing = true; - for (const auto & pair : this->get_parameter_overrides()) { - if (!this->has_parameter(pair.first)) { - this->declare_parameter( - pair.first, - pair.second, - descriptor, - true); + using namespace std::placeholders; + local_perform_automatically_declare_parameters_from_overrides( + this->get_parameter_overrides(), + std::bind(&NodeParameters::has_parameter, this, _1), + [this]( + const std::string & name, + const rclcpp::ParameterValue & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, + bool ignore_override) + { + NodeParameters::declare_parameter( + name, default_value, parameter_descriptor, ignore_override); } - } + ); } } +void +NodeParameters::perform_automatically_declare_parameters_from_overrides() +{ + local_perform_automatically_declare_parameters_from_overrides( + this->get_parameter_overrides(), + [this](const std::string & name) { + return this->has_parameter(name); + }, + [this]( + const std::string & name, + const rclcpp::ParameterValue & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, + bool ignore_override) + { + this->declare_parameter( + name, default_value, parameter_descriptor, ignore_override); + } + ); +} + NodeParameters::~NodeParameters() {} From 546ddf87fed71c772b7dc1eeb98b6f889ee1c612 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Wed, 22 Jun 2022 15:01:43 -0700 Subject: [PATCH 118/455] test adjustment for LoanedMessage. (#1951) Signed-off-by: Tomoya Fujita --- rclcpp/test/rclcpp/test_publisher.cpp | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 1679ca76f0..35605230d9 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -25,6 +25,8 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/rclcpp.hpp" +#include "rcutils/env.h" + #include "../mocking_utils/patch.hpp" #include "../utils/rclcpp_gtest_macros.hpp" @@ -413,10 +415,18 @@ TEST_F(TestPublisher, intra_process_publish_failures) { { rclcpp::LoanedMessage loaned_msg(*publisher, allocator); - loaned_msg.release(); + auto msg = loaned_msg.release(); // this will unmanage the ownership of the message RCLCPP_EXPECT_THROW_EQ( publisher->publish(std::move(loaned_msg)), std::runtime_error("loaned message is not valid")); + // if the message is actually loaned from the middleware but not be published, + // it is user responsibility to return the message to the middleware manually + if (publisher->can_loan_messages()) { + ASSERT_EQ( + RCL_RET_OK, + rcl_return_loaned_message_from_publisher( + publisher->get_publisher_handle().get(), msg.get())); + } } RCLCPP_EXPECT_THROW_EQ( node->create_publisher( @@ -485,6 +495,11 @@ class TestPublisherProtectedMethods : public rclcpp::Publisher>; + // This test only passes when message is allocated on heap, not middleware. + // Since `do_loaned_message_publish()` will fail, there is no way to return the message + // to the middleware. + // This eventually fails to destroy publisher handle in the implementation. + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "1")); auto publisher = node->create_publisher, PublisherT>("topic", 10); From 3c8e89d17c5638450f55162a959307014385cbd2 Mon Sep 17 00:00:00 2001 From: Hubert Liberacki Date: Fri, 24 Jun 2022 09:33:29 +0200 Subject: [PATCH 119/455] Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (#1821) (#1874) --- rclcpp/include/rclcpp/client.hpp | 6 +- rclcpp/include/rclcpp/executor.hpp | 143 ++++++++++++------ rclcpp/include/rclcpp/executors.hpp | 95 ++++++++++-- rclcpp/include/rclcpp/future_return_code.hpp | 2 +- rclcpp/include/rclcpp/rclcpp.hpp | 2 +- rclcpp/src/rclcpp/parameter_client.cpp | 32 ++-- rclcpp/src/rclcpp/time_source.cpp | 2 +- rclcpp/test/benchmark/benchmark_client.cpp | 2 +- rclcpp/test/benchmark/benchmark_executor.cpp | 28 ++-- rclcpp/test/benchmark/benchmark_service.cpp | 2 +- .../test/rclcpp/executors/test_executors.cpp | 75 +++++---- .../test_static_single_threaded_executor.cpp | 4 +- rclcpp/test/rclcpp/test_executor.cpp | 62 ++++++-- ..._intra_process_manager_with_allocators.cpp | 4 +- rclcpp/test/rclcpp/test_parameter_client.cpp | 38 ++--- rclcpp/test/rclcpp/test_qos_event.cpp | 2 +- .../test_subscription_topic_statistics.cpp | 6 +- .../benchmark/benchmark_action_client.cpp | 16 +- .../benchmark/benchmark_action_server.cpp | 12 +- rclcpp_action/test/test_client.cpp | 82 +++++----- rclcpp_action/test/test_server.cpp | 16 +- .../test/test_component_manager_api.cpp | 30 ++-- 22 files changed, 421 insertions(+), 240 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index e88fa8a949..9b1522399c 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -579,11 +579,11 @@ class Client : public ClientBase /// Send a request to the service server. /** * This method returns a `FutureAndRequestId` instance - * that can be passed to Executor::spin_until_future_complete() to + * that can be passed to Executor::spin_until_complete() to * wait until it has been completed. * * If the future never completes, - * e.g. the call to Executor::spin_until_future_complete() times out, + * e.g. the call to Executor::spin_until_complete() times out, * Client::remove_pending_request() must be called to clean the client internal state. * Not doing so will make the `Client` instance to use more memory each time a response is not * received from the service server. @@ -592,7 +592,7 @@ class Client : public ClientBase * auto future = client->async_send_request(my_request); * if ( * rclcpp::FutureReturnCode::TIMEOUT == - * executor->spin_until_future_complete(future, timeout)) + * executor->spin_until_complete(future, timeout)) * { * client->remove_pending_request(future); * // handle timeout diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index ed2ddc4a0a..a700b8f454 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #include "rcl/guard_condition.h" #include "rcl/wait.h" @@ -319,6 +320,51 @@ class Executor virtual void spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); + /// Spin (blocking) until the condition is complete, it times out waiting, + /// or rclcpp is interrupted. + /** + * \param[in] future The condition which can be callable or future type to wait on. + * If this function returns SUCCESS, the future can be + * accessed without blocking (though it may still throw an exception). + * \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once. + * `-1` is block forever, `0` is non-blocking. + * If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return + * code. + * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. + */ + template + FutureReturnCode + spin_until_complete( + const ConditionT & condition, + DurationT timeout = DurationT(-1)) + { + if constexpr (std::is_invocable_v) { + using RetT = std::invoke_result_t; + static_assert( + std::is_same_v, + "Conditional callable has to return boolean type"); + return spin_until_complete_impl(condition, timeout); + } else { + auto check_future = [&condition]() { + return condition.wait_for(std::chrono::seconds(0)) == + std::future_status::ready; + }; + return spin_until_complete_impl(check_future, timeout); + } + } + + /// Spin (blocking) for at least the given amount of duration. + /** + * \param[in] duration gets passed to Executor::spin_node_once, + * spins the executor for given duration. + */ + template + void + spin_for(DurationT duration) + { + (void)spin_until_complete([]() {return false;}, duration); + } + /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted. /** * \param[in] future The future to wait on. If this function returns SUCCESS, the future can be @@ -330,57 +376,13 @@ class Executor * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ template + [[deprecated("use spin_until_complete(const ConditionT & condition, DurationT timeout) instead")]] FutureReturnCode spin_until_future_complete( const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { - // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete - // inside a callback executed by an executor. - - // Check the future before entering the while loop. - // If the future is already complete, don't try to spin. - std::future_status status = future.wait_for(std::chrono::seconds(0)); - if (status == std::future_status::ready) { - return FutureReturnCode::SUCCESS; - } - - auto end_time = std::chrono::steady_clock::now(); - std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast( - timeout); - if (timeout_ns > std::chrono::nanoseconds::zero()) { - end_time += timeout_ns; - } - std::chrono::nanoseconds timeout_left = timeout_ns; - - if (spinning.exchange(true)) { - throw std::runtime_error("spin_until_future_complete() called while already spinning"); - } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); - while (rclcpp::ok(this->context_) && spinning.load()) { - // Do one item of work. - spin_once_impl(timeout_left); - - // Check if the future is set, return SUCCESS if it is. - status = future.wait_for(std::chrono::seconds(0)); - if (status == std::future_status::ready) { - return FutureReturnCode::SUCCESS; - } - // If the original timeout is < 0, then this is blocking, never TIMEOUT. - if (timeout_ns < std::chrono::nanoseconds::zero()) { - continue; - } - // Otherwise check if we still have time to wait, return TIMEOUT if not. - auto now = std::chrono::steady_clock::now(); - if (now >= end_time) { - return FutureReturnCode::TIMEOUT; - } - // Subtract the elapsed time from the original timeout. - timeout_left = std::chrono::duration_cast(end_time - now); - } - - // The future did not complete before ok() returned false, return INTERRUPTED. - return FutureReturnCode::INTERRUPTED; + return spin_until_complete(future, timeout); } /// Cancel any running spin* function, causing it to return. @@ -560,6 +562,55 @@ class Executor virtual void spin_once_impl(std::chrono::nanoseconds timeout); +protected: + // Implementation details, used by spin_until_complete and spin_for. + // Previouse implementation of spin_until_future_complete. + template + FutureReturnCode + spin_until_complete_impl(ConditionT condition, DurationT timeout) + { + auto end_time = std::chrono::steady_clock::now(); + std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast( + timeout); + if (timeout_ns > std::chrono::nanoseconds::zero()) { + end_time += timeout_ns; + } + std::chrono::nanoseconds timeout_left = timeout_ns; + + // Preliminary check, finish if conditon is done already. + if (condition()) { + return FutureReturnCode::SUCCESS; + } + + if (spinning.exchange(true)) { + throw std::runtime_error("spin_until_complete() called while already spinning"); + } + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + while (rclcpp::ok(this->context_) && spinning.load()) { + // Do one item of work. + spin_once_impl(timeout_left); + + if (condition()) { + return FutureReturnCode::SUCCESS; + } + // If the original timeout is < 0, then this is blocking, never TIMEOUT. + if (timeout_ns < std::chrono::nanoseconds::zero()) { + continue; + } + // Otherwise check if we still have time to wait, return TIMEOUT if not. + auto now = std::chrono::steady_clock::now(); + if (now >= end_time) { + return FutureReturnCode::TIMEOUT; + } + // Subtract the elapsed time from the original timeout. + timeout_left = std::chrono::duration_cast(end_time - now); + } + + // The condition did not pass before ok() returned false, return INTERRUPTED. + return FutureReturnCode::INTERRUPTED; + } + +public: typedef std::map> diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 36fb0d63cf..44db27686f 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -54,6 +54,50 @@ namespace executors using rclcpp::executors::MultiThreadedExecutor; using rclcpp::executors::SingleThreadedExecutor; +/// Spin (blocking) until the conditon is complete, it times out waiting, or rclcpp is interrupted. +/** + * \param[in] executor The executor which will spin the node. + * \param[in] node_ptr The node to spin. + * \param[in] condition The callable or future to wait on. If `SUCCESS`, the condition is safe to + * access after this function + * \param[in] timeout Optional timeout parameter, which gets passed to + * Executor::spin_node_once. + * `-1` is block forever, `0` is non-blocking. + * If the time spent inside the blocking loop exceeds this timeout, return a `TIMEOUT` return code. + * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. + */ +template +rclcpp::FutureReturnCode +spin_node_until_complete( + rclcpp::Executor & executor, + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + const ConditionT & condition, + DurationT timeout = DurationT(-1)) +{ + // TODO(wjwwood): does not work recursively; can't call spin_node_until_complete + // inside a callback executed by an executor. + executor.add_node(node_ptr); + auto retcode = executor.spin_until_complete(condition, timeout); + executor.remove_node(node_ptr); + return retcode; +} + +template +rclcpp::FutureReturnCode +spin_node_until_complete( + rclcpp::Executor & executor, + std::shared_ptr node_ptr, + const ConditionT & condition, + DurationT timeout = DurationT(-1)) +{ + return rclcpp::executors::spin_node_until_complete( + executor, + node_ptr->get_node_base_interface(), + condition, + timeout); +} + /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted. /** * \param[in] executor The executor which will spin the node. @@ -67,6 +111,10 @@ using rclcpp::executors::SingleThreadedExecutor; * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ template +[[deprecated( + "use spin_node_until_complete(Executor &, node_interfaces::NodeBaseInterface::SharedPtr, " + "const ConditionT &, DurationT) instead" +)]] rclcpp::FutureReturnCode spin_node_until_future_complete( rclcpp::Executor & executor, @@ -74,16 +122,15 @@ spin_node_until_future_complete( const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { - // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete - // inside a callback executed by an executor. - executor.add_node(node_ptr); - auto retcode = executor.spin_until_future_complete(future, timeout); - executor.remove_node(node_ptr); - return retcode; + return spin_until_complete(executor, node_ptr, future, timeout); } template +[[deprecated( + "use spin_node_until_complete(Executor &, std::shared_ptr, " + "const ConditionT &, DurationT) instead" +)]] rclcpp::FutureReturnCode spin_node_until_future_complete( rclcpp::Executor & executor, @@ -91,7 +138,7 @@ spin_node_until_future_complete( const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { - return rclcpp::executors::spin_node_until_future_complete( + return rclcpp::executors::spin_node_until_complete( executor, node_ptr->get_node_base_interface(), future, @@ -100,7 +147,33 @@ spin_node_until_future_complete( } // namespace executors +template +rclcpp::FutureReturnCode +spin_until_complete( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + const ConditionT & condition, + DurationT timeout = DurationT(-1)) +{ + rclcpp::executors::SingleThreadedExecutor executor; + return executors::spin_node_until_complete(executor, node_ptr, condition, timeout); +} + +template +rclcpp::FutureReturnCode +spin_until_complete( + std::shared_ptr node_ptr, + const ConditionT & condition, + DurationT timeout = DurationT(-1)) +{ + return rclcpp::spin_until_complete(node_ptr->get_node_base_interface(), condition, timeout); +} + template +[[deprecated( + "use spin_until_complete(node_interfaces::NodeBaseInterface::SharedPtr, " + "const ConditionT &,DurationT) instead" +)]] rclcpp::FutureReturnCode spin_until_future_complete( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, @@ -108,18 +181,22 @@ spin_until_future_complete( std::chrono::duration timeout = std::chrono::duration(-1)) { rclcpp::executors::SingleThreadedExecutor executor; - return executors::spin_node_until_future_complete(executor, node_ptr, future, timeout); + return executors::spin_node_until_complete(executor, node_ptr, future, timeout); } template +[[deprecated( + "use spin_until_complete(std::shared_ptr, const ConditionT &, " + "DurationT) instead" +)]] rclcpp::FutureReturnCode spin_until_future_complete( std::shared_ptr node_ptr, const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { - return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout); + return rclcpp::spin_until_complete(node_ptr->get_node_base_interface(), future, timeout); } } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/future_return_code.hpp b/rclcpp/include/rclcpp/future_return_code.hpp index 0da67d7f7b..505f68431b 100644 --- a/rclcpp/include/rclcpp/future_return_code.hpp +++ b/rclcpp/include/rclcpp/future_return_code.hpp @@ -23,7 +23,7 @@ namespace rclcpp { -/// Return codes to be used with spin_until_future_complete. +/// Return codes to be used with spin_until_complete. /** * SUCCESS: The future is complete and can be accessed with "get" without blocking. * This does not indicate that the operation succeeded; "get" may still throw an exception. diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index f1d751ff3f..380a766f5d 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -68,7 +68,7 @@ * - Executors (responsible for execution of callbacks through a blocking spin): * - rclcpp::spin() * - rclcpp::spin_some() - * - rclcpp::spin_until_future_complete() + * - rclcpp::spin_until_complete() * - rclcpp::executors::SingleThreadedExecutor * - rclcpp::executors::SingleThreadedExecutor::add_node() * - rclcpp::executors::SingleThreadedExecutor::spin() diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index 38ced0e1a5..c8c4d50f35 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -396,9 +396,9 @@ SyncParametersClient::get_parameters( std::chrono::nanoseconds timeout) { auto f = async_parameters_client_->get_parameters(parameter_names); - using rclcpp::executors::spin_node_until_future_complete; + using rclcpp::executors::spin_node_until_complete; if ( - spin_node_until_future_complete( + spin_node_until_complete( *executor_, node_base_interface_, f, timeout) == rclcpp::FutureReturnCode::SUCCESS) { @@ -424,9 +424,9 @@ SyncParametersClient::describe_parameters( { auto f = async_parameters_client_->describe_parameters(parameter_names); - using rclcpp::executors::spin_node_until_future_complete; + using rclcpp::executors::spin_node_until_complete; rclcpp::FutureReturnCode future = - spin_node_until_future_complete(*executor_, node_base_interface_, f, timeout); + spin_node_until_complete(*executor_, node_base_interface_, f, timeout); if (future == rclcpp::FutureReturnCode::SUCCESS) { return f.get(); } @@ -440,9 +440,9 @@ SyncParametersClient::get_parameter_types( { auto f = async_parameters_client_->get_parameter_types(parameter_names); - using rclcpp::executors::spin_node_until_future_complete; + using rclcpp::executors::spin_node_until_complete; if ( - spin_node_until_future_complete( + spin_node_until_complete( *executor_, node_base_interface_, f, timeout) == rclcpp::FutureReturnCode::SUCCESS) { @@ -458,9 +458,9 @@ SyncParametersClient::set_parameters( { auto f = async_parameters_client_->set_parameters(parameters); - using rclcpp::executors::spin_node_until_future_complete; + using rclcpp::executors::spin_node_until_complete; if ( - spin_node_until_future_complete( + spin_node_until_complete( *executor_, node_base_interface_, f, timeout) == rclcpp::FutureReturnCode::SUCCESS) { @@ -476,9 +476,9 @@ SyncParametersClient::delete_parameters( { auto f = async_parameters_client_->delete_parameters(parameters_names); - using rclcpp::executors::spin_node_until_future_complete; + using rclcpp::executors::spin_node_until_complete; if ( - spin_node_until_future_complete( + spin_node_until_complete( *executor_, node_base_interface_, f, timeout) == rclcpp::FutureReturnCode::SUCCESS) { @@ -494,9 +494,9 @@ SyncParametersClient::load_parameters( { auto f = async_parameters_client_->load_parameters(yaml_filename); - using rclcpp::executors::spin_node_until_future_complete; + using rclcpp::executors::spin_node_until_complete; if ( - spin_node_until_future_complete( + spin_node_until_complete( *executor_, node_base_interface_, f, timeout) == rclcpp::FutureReturnCode::SUCCESS) { @@ -512,9 +512,9 @@ SyncParametersClient::set_parameters_atomically( { auto f = async_parameters_client_->set_parameters_atomically(parameters); - using rclcpp::executors::spin_node_until_future_complete; + using rclcpp::executors::spin_node_until_complete; if ( - spin_node_until_future_complete( + spin_node_until_complete( *executor_, node_base_interface_, f, timeout) == rclcpp::FutureReturnCode::SUCCESS) { @@ -532,9 +532,9 @@ SyncParametersClient::list_parameters( { auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth); - using rclcpp::executors::spin_node_until_future_complete; + using rclcpp::executors::spin_node_until_complete; if ( - spin_node_until_future_complete( + spin_node_until_complete( *executor_, node_base_interface_, f, timeout) == rclcpp::FutureReturnCode::SUCCESS) { diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 42a9208251..21c42530a6 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -384,7 +384,7 @@ class TimeSource::NodeState final [this]() { auto future = cancel_clock_executor_promise_.get_future(); clock_executor_->add_callback_group(clock_callback_group_, node_base_); - clock_executor_->spin_until_future_complete(future); + clock_executor_->spin_until_complete(future); } ); } diff --git a/rclcpp/test/benchmark/benchmark_client.cpp b/rclcpp/test/benchmark/benchmark_client.cpp index 26ee58b633..2a964804e3 100644 --- a/rclcpp/test/benchmark/benchmark_client.cpp +++ b/rclcpp/test/benchmark/benchmark_client.cpp @@ -151,7 +151,7 @@ BENCHMARK_F(ClientPerformanceTest, async_send_request_and_response)(benchmark::S for (auto _ : state) { (void)_; auto future = client->async_send_request(shared_request); - rclcpp::spin_until_future_complete( + rclcpp::spin_until_complete( node->get_node_base_interface(), future, std::chrono::seconds(1)); benchmark::DoNotOptimize(future); benchmark::ClobberMemory(); diff --git a/rclcpp/test/benchmark/benchmark_executor.cpp b/rclcpp/test/benchmark/benchmark_executor.cpp index 652007b589..062f02da60 100644 --- a/rclcpp/test/benchmark/benchmark_executor.cpp +++ b/rclcpp/test/benchmark/benchmark_executor.cpp @@ -219,7 +219,7 @@ BENCHMARK_F( BENCHMARK_F( PerformanceTestExecutorSimple, - static_single_thread_executor_spin_until_future_complete)(benchmark::State & st) + static_single_thread_executor_spin_until_complete)(benchmark::State & st) { rclcpp::executors::StaticSingleThreadedExecutor executor; // test success of an immediately finishing future @@ -228,7 +228,7 @@ BENCHMARK_F( promise.set_value(true); auto shared_future = future.share(); - auto ret = executor.spin_until_future_complete(shared_future, 100ms); + auto ret = executor.spin_until_complete(shared_future, 100ms); if (ret != rclcpp::FutureReturnCode::SUCCESS) { st.SkipWithError(rcutils_get_error_string().str); } @@ -243,7 +243,7 @@ BENCHMARK_F( executor.add_node(node); st.ResumeTiming(); - ret = executor.spin_until_future_complete(shared_future, 100ms); + ret = executor.spin_until_complete(shared_future, 100ms); if (ret != rclcpp::FutureReturnCode::SUCCESS) { st.SkipWithError(rcutils_get_error_string().str); break; @@ -256,7 +256,7 @@ BENCHMARK_F( BENCHMARK_F( PerformanceTestExecutorSimple, - single_thread_executor_spin_node_until_future_complete)(benchmark::State & st) + single_thread_executor_spin_node_until_complete)(benchmark::State & st) { rclcpp::executors::SingleThreadedExecutor executor; // test success of an immediately finishing future @@ -265,7 +265,7 @@ BENCHMARK_F( promise.set_value(true); auto shared_future = future.share(); - auto ret = rclcpp::executors::spin_node_until_future_complete( + auto ret = rclcpp::executors::spin_node_until_complete( executor, node, shared_future, 1s); if (ret != rclcpp::FutureReturnCode::SUCCESS) { st.SkipWithError(rcutils_get_error_string().str); @@ -275,7 +275,7 @@ BENCHMARK_F( for (auto _ : st) { (void)_; - ret = rclcpp::executors::spin_node_until_future_complete( + ret = rclcpp::executors::spin_node_until_complete( executor, node, shared_future, 1s); if (ret != rclcpp::FutureReturnCode::SUCCESS) { st.SkipWithError(rcutils_get_error_string().str); @@ -286,7 +286,7 @@ BENCHMARK_F( BENCHMARK_F( PerformanceTestExecutorSimple, - multi_thread_executor_spin_node_until_future_complete)(benchmark::State & st) + multi_thread_executor_spin_node_until_complete)(benchmark::State & st) { rclcpp::executors::MultiThreadedExecutor executor; // test success of an immediately finishing future @@ -295,7 +295,7 @@ BENCHMARK_F( promise.set_value(true); auto shared_future = future.share(); - auto ret = rclcpp::executors::spin_node_until_future_complete( + auto ret = rclcpp::executors::spin_node_until_complete( executor, node, shared_future, 1s); if (ret != rclcpp::FutureReturnCode::SUCCESS) { st.SkipWithError(rcutils_get_error_string().str); @@ -305,7 +305,7 @@ BENCHMARK_F( for (auto _ : st) { (void)_; - ret = rclcpp::executors::spin_node_until_future_complete( + ret = rclcpp::executors::spin_node_until_complete( executor, node, shared_future, 1s); if (ret != rclcpp::FutureReturnCode::SUCCESS) { st.SkipWithError(rcutils_get_error_string().str); @@ -316,7 +316,7 @@ BENCHMARK_F( BENCHMARK_F( PerformanceTestExecutorSimple, - static_single_thread_executor_spin_node_until_future_complete)(benchmark::State & st) + static_single_thread_executor_spin_node_until_complete)(benchmark::State & st) { rclcpp::executors::StaticSingleThreadedExecutor executor; // test success of an immediately finishing future @@ -329,7 +329,7 @@ BENCHMARK_F( for (auto _ : st) { (void)_; - auto ret = rclcpp::executors::spin_node_until_future_complete( + auto ret = rclcpp::executors::spin_node_until_complete( executor, node, shared_future, 1s); if (ret != rclcpp::FutureReturnCode::SUCCESS) { st.SkipWithError(rcutils_get_error_string().str); @@ -338,7 +338,7 @@ BENCHMARK_F( } } -BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark::State & st) +BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_complete)(benchmark::State & st) { // test success of an immediately finishing future std::promise promise; @@ -346,7 +346,7 @@ BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark promise.set_value(true); auto shared_future = future.share(); - auto ret = rclcpp::spin_until_future_complete(node, shared_future, 1s); + auto ret = rclcpp::spin_until_complete(node, shared_future, 1s); if (ret != rclcpp::FutureReturnCode::SUCCESS) { st.SkipWithError(rcutils_get_error_string().str); } @@ -355,7 +355,7 @@ BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark for (auto _ : st) { (void)_; - ret = rclcpp::spin_until_future_complete(node, shared_future, 1s); + ret = rclcpp::spin_until_complete(node, shared_future, 1s); if (ret != rclcpp::FutureReturnCode::SUCCESS) { st.SkipWithError(rcutils_get_error_string().str); break; diff --git a/rclcpp/test/benchmark/benchmark_service.cpp b/rclcpp/test/benchmark/benchmark_service.cpp index a42723da90..43bc24f303 100644 --- a/rclcpp/test/benchmark/benchmark_service.cpp +++ b/rclcpp/test/benchmark/benchmark_service.cpp @@ -137,7 +137,7 @@ BENCHMARK_F(ServicePerformanceTest, async_send_response)(benchmark::State & stat benchmark::DoNotOptimize(service); benchmark::ClobberMemory(); - rclcpp::spin_until_future_complete(node->get_node_base_interface(), future); + rclcpp::spin_until_complete(node->get_node_base_interface(), future); } if (callback_count == 0) { state.SkipWithError("Service callback was not called"); diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 1fa2cbb4dd..929446c21a 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -222,7 +222,7 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { } // Check executor exits immediately if future is complete. -TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) { +TYPED_TEST(TestExecutors, testSpinUntilCompleteFuture) { using ExecutorType = TypeParam; ExecutorType executor; executor.add_node(this->node); @@ -232,11 +232,30 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) { std::future future = promise.get_future(); promise.set_value(true); - // spin_until_future_complete is expected to exit immediately, but would block up until its + // spin_until_complete is expected to exit immediately, but would block up until its // timeout if the future is not checked before spin_once_impl. auto start = std::chrono::steady_clock::now(); auto shared_future = future.share(); - auto ret = executor.spin_until_future_complete(shared_future, 1s); + auto ret = executor.spin_until_complete(shared_future, 1s); + executor.remove_node(this->node, true); + // Check it didn't reach timeout + EXPECT_GT(500ms, (std::chrono::steady_clock::now() - start)); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); +} + +// Check executor exits immediately if future is complete. +TYPED_TEST(TestExecutors, testSpinUntilCompleteCallable) { + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + // test success of an immediately completed condition + auto condition = []() {return true;}; + + // spin_until_complete is expected to exit immediately, but would block up until its + // timeout if the future is not checked before spin_once_impl. + auto start = std::chrono::steady_clock::now(); + auto ret = executor.spin_until_complete(condition, 1s); executor.remove_node(this->node, true); // Check it didn't reach timeout EXPECT_GT(500ms, (std::chrono::steady_clock::now() - start)); @@ -244,7 +263,7 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) { } // Same test, but uses a shared future. -TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) { +TYPED_TEST(TestExecutors, testSpinUntilCompleteSharedFuture) { using ExecutorType = TypeParam; ExecutorType executor; executor.add_node(this->node); @@ -254,11 +273,11 @@ TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) { std::future future = promise.get_future(); promise.set_value(true); - // spin_until_future_complete is expected to exit immediately, but would block up until its + // spin_until_complete is expected to exit immediately, but would block up until its // timeout if the future is not checked before spin_once_impl. auto shared_future = future.share(); auto start = std::chrono::steady_clock::now(); - auto ret = executor.spin_until_future_complete(shared_future, 1s); + auto ret = executor.spin_until_complete(shared_future, 1s); executor.remove_node(this->node, true); // Check it didn't reach timeout @@ -267,7 +286,7 @@ TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) { } // For a longer running future that should require several iterations of spin_once -TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) { +TYPED_TEST(TestExecutors, testSpinUntilCompleteNoTimeout) { using ExecutorType = TypeParam; ExecutorType executor; executor.add_node(this->node); @@ -286,7 +305,7 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) { // Timeout set to negative for no timeout. std::thread spinner([&]() { - auto ret = executor.spin_until_future_complete(future, -1s); + auto ret = executor.spin_until_complete(future, -1s); EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); executor.remove_node(this->node, true); executor.cancel(); @@ -312,15 +331,15 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) { spinner.join(); } -// Check spin_until_future_complete timeout works as expected -TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) { +// Check spin_until_complete timeout works as expected +TYPED_TEST(TestExecutors, testSpinUntilCompleteWithTimeout) { using ExecutorType = TypeParam; ExecutorType executor; executor.add_node(this->node); bool spin_exited = false; - // Needs to run longer than spin_until_future_complete's timeout. + // Needs to run longer than spin_until_complete's timeout. std::future future = std::async( std::launch::async, [&spin_exited]() { @@ -332,7 +351,7 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) { // Short timeout std::thread spinner([&]() { - auto ret = executor.spin_until_future_complete(future, 1ms); + auto ret = executor.spin_until_complete(future, 1ms); EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); executor.remove_node(this->node, true); spin_exited = true; @@ -482,8 +501,8 @@ TYPED_TEST(TestExecutors, spinSome) { spinner.join(); } -// Check spin_node_until_future_complete with node base pointer -TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) { +// Check spin_node_until_complete with node base pointer +TYPED_TEST(TestExecutors, testSpinNodeUntilCompleteNodeBasePtr) { using ExecutorType = TypeParam; ExecutorType executor; @@ -492,13 +511,13 @@ TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) { promise.set_value(true); auto shared_future = future.share(); - auto ret = rclcpp::executors::spin_node_until_future_complete( + auto ret = rclcpp::executors::spin_node_until_complete( executor, this->node->get_node_base_interface(), shared_future, 1s); EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); } -// Check spin_node_until_future_complete with node pointer -TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodePtr) { +// Check spin_node_until_complete with node pointer +TYPED_TEST(TestExecutors, testSpinNodeUntilCompleteNodePtr) { using ExecutorType = TypeParam; ExecutorType executor; @@ -507,13 +526,13 @@ TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodePtr) { promise.set_value(true); auto shared_future = future.share(); - auto ret = rclcpp::executors::spin_node_until_future_complete( + auto ret = rclcpp::executors::spin_node_until_complete( executor, this->node, shared_future, 1s); EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); } -// Check spin_until_future_complete can be properly interrupted. -TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) { +// Check spin_until_complete can be properly interrupted. +TYPED_TEST(TestExecutors, testSpinUntilCompleteInterrupted) { using ExecutorType = TypeParam; ExecutorType executor; executor.add_node(this->node); @@ -521,7 +540,7 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) { bool spin_exited = false; // This needs to block longer than it takes to get to the shutdown call below and for - // spin_until_future_complete to return + // spin_until_complete to return std::future future = std::async( std::launch::async, [&spin_exited]() { @@ -533,7 +552,7 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) { // Long timeout std::thread spinner([&spin_exited, &executor, &future]() { - auto ret = executor.spin_until_future_complete(future, 1s); + auto ret = executor.spin_until_complete(future, 1s); EXPECT_EQ(rclcpp::FutureReturnCode::INTERRUPTED, ret); spin_exited = true; }); @@ -555,8 +574,8 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) { spinner.join(); } -// Check spin_until_future_complete with node base pointer (instantiates its own executor) -TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) { +// Check spin_until_complete with node base pointer (instantiates its own executor) +TEST(TestExecutors, testSpinUntilCompleteNodeBasePtr) { rclcpp::init(0, nullptr); { @@ -567,7 +586,7 @@ TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) { promise.set_value(true); auto shared_future = future.share(); - auto ret = rclcpp::spin_until_future_complete( + auto ret = rclcpp::spin_until_complete( node->get_node_base_interface(), shared_future, 1s); EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); } @@ -575,8 +594,8 @@ TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) { rclcpp::shutdown(); } -// Check spin_until_future_complete with node pointer (instantiates its own executor) -TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) { +// Check spin_until_complete with node pointer (instantiates its own executor) +TEST(TestExecutors, testSpinUntilCompleteNodePtr) { rclcpp::init(0, nullptr); { @@ -587,7 +606,7 @@ TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) { promise.set_value(true); auto shared_future = future.share(); - auto ret = rclcpp::spin_until_future_complete(node, shared_future, 1s); + auto ret = rclcpp::spin_until_complete(node, shared_future, 1s); EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); } diff --git a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp index 5ca6c1c25a..4e440f7a5c 100644 --- a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp @@ -135,8 +135,8 @@ TEST_F(TestStaticSingleThreadedExecutor, execute_service) { std::future future = promise.get_future(); EXPECT_EQ( rclcpp::FutureReturnCode::TIMEOUT, - executor.spin_until_future_complete(future, std::chrono::milliseconds(1))); + executor.spin_until_complete(future, std::chrono::milliseconds(1))); executor.remove_node(node); - executor.spin_until_future_complete(future, std::chrono::milliseconds(1)); + executor.spin_until_complete(future, std::chrono::milliseconds(1)); } diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index bdbb0a1079..8796f4ea4d 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -294,6 +294,39 @@ TEST_F(TestExecutor, spin_some_elapsed) { ASSERT_TRUE(timer_called); } +TEST_F(TestExecutor, spin_for_duration) +{ + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool timer_called = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(0), [&]() { + timer_called = true; + }); + dummy.add_node(node); + // Wait for the wall timer to have expired. + dummy.spin_for(std::chrono::milliseconds(0)); + + ASSERT_TRUE(timer_called); +} + +TEST_F(TestExecutor, spin_for_longer_timer) +{ + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool timer_called = false; + auto timer = + node->create_wall_timer( + std::chrono::seconds(10), [&]() { + timer_called = true; + }); + dummy.add_node(node); + dummy.spin_for(std::chrono::milliseconds(5)); + + ASSERT_FALSE(timer_called); +} + TEST_F(TestExecutor, spin_once_in_spin_once) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); @@ -482,22 +515,20 @@ TEST_F(TestExecutor, get_group_by_timer_add_callback_group) { ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get()); } -TEST_F(TestExecutor, spin_until_future_complete_in_spin_until_future_complete) { +TEST_F(TestExecutor, spin_until_complete_in_spin_until_complete) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); - bool spin_until_future_complete_in_spin_until_future_complete = false; + bool spin_until_complete_in_spin_until_complete = false; auto timer = node->create_wall_timer( std::chrono::milliseconds(1), [&]() { try { - std::promise promise; - std::future future = promise.get_future(); - dummy.spin_until_future_complete(future, std::chrono::milliseconds(1)); + dummy.spin_for(std::chrono::milliseconds(1)); } catch (const std::runtime_error & err) { if (err.what() == std::string( - "spin_until_future_complete() called while already spinning")) + "spin_until_complete() called while already spinning")) { - spin_until_future_complete_in_spin_until_future_complete = true; + spin_until_complete_in_spin_until_complete = true; } } }); @@ -505,11 +536,9 @@ TEST_F(TestExecutor, spin_until_future_complete_in_spin_until_future_complete) { dummy.add_node(node); // Wait for the wall timer to have expired. std::this_thread::sleep_for(std::chrono::milliseconds(50)); - EXPECT_FALSE(spin_until_future_complete_in_spin_until_future_complete); - std::promise promise; - std::future future = promise.get_future(); - dummy.spin_until_future_complete(future, std::chrono::milliseconds(1)); - EXPECT_TRUE(spin_until_future_complete_in_spin_until_future_complete); + EXPECT_FALSE(spin_until_complete_in_spin_until_complete); + dummy.spin_for(std::chrono::milliseconds(2)); + EXPECT_TRUE(spin_until_complete_in_spin_until_complete); } TEST_F(TestExecutor, spin_node_once_base_interface) { @@ -546,7 +575,7 @@ TEST_F(TestExecutor, spin_node_once_node) { EXPECT_TRUE(spin_called); } -TEST_F(TestExecutor, spin_until_future_complete_future_already_complete) { +TEST_F(TestExecutor, spin_until_complete_condition_already_complete) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); std::promise promise; @@ -554,7 +583,12 @@ TEST_F(TestExecutor, spin_until_future_complete_future_already_complete) { promise.set_value(); EXPECT_EQ( rclcpp::FutureReturnCode::SUCCESS, - dummy.spin_until_future_complete(future, std::chrono::milliseconds(1))); + dummy.spin_until_complete(future, std::chrono::milliseconds(1))); + + auto condition = []() {return true;}; + EXPECT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + dummy.spin_until_complete(condition, std::chrono::milliseconds(1))); } TEST_F(TestExecutor, is_spinning) { diff --git a/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp b/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp index 6d192ca86b..35c43ef3d4 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp @@ -240,7 +240,7 @@ do_custom_allocator_test( EXPECT_NO_THROW( { publisher->publish(std::move(msg)); - executor.spin_until_future_complete(received_message_future, std::chrono::seconds(10)); + executor.spin_until_complete(received_message_future, std::chrono::seconds(10)); }); EXPECT_EQ(ptr, received_message_future.get().get()); EXPECT_EQ(1u, counter); @@ -249,7 +249,7 @@ do_custom_allocator_test( EXPECT_THROW( { publisher->publish(std::move(msg)); - executor.spin_until_future_complete(received_message_future, std::chrono::seconds(10)); + executor.spin_until_complete(received_message_future, std::chrono::seconds(10)); }, ExpectedExceptionT); } } diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index 4cd9e671be..cf69dd8ded 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -212,7 +212,7 @@ TEST_F(TestParameterClient, async_parameter_get_parameter_types) { std::vector names{"foo"}; std::shared_future> future = asynchronous_client->get_parameter_types(names, callback); - auto return_code = rclcpp::spin_until_future_complete( + auto return_code = rclcpp::spin_until_complete( node, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); ASSERT_TRUE(callback_called); @@ -236,7 +236,7 @@ TEST_F(TestParameterClient, async_parameter_get_parameter_types_allow_undeclared std::vector names{"foo"}; std::shared_future> future = asynchronous_client->get_parameter_types(names, callback); - auto return_code = rclcpp::spin_until_future_complete( + auto return_code = rclcpp::spin_until_complete( node_with_option, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); ASSERT_TRUE(callback_called); @@ -258,7 +258,7 @@ TEST_F(TestParameterClient, async_parameter_get_parameters) { std::vector names{"foo"}; std::shared_future> future = asynchronous_client->get_parameters( names, callback); - auto return_code = rclcpp::spin_until_future_complete( + auto return_code = rclcpp::spin_until_complete( node, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); ASSERT_TRUE(callback_called); @@ -280,7 +280,7 @@ TEST_F(TestParameterClient, async_parameter_get_parameters_allow_undeclared) { std::vector names{"foo"}; std::shared_future> future = asynchronous_client->get_parameters( names, callback); - auto return_code = rclcpp::spin_until_future_complete( + auto return_code = rclcpp::spin_until_complete( node_with_option, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); ASSERT_TRUE(callback_called); @@ -306,7 +306,7 @@ TEST_F(TestParameterClient, async_parameter_set_parameters_atomically) { parameters.emplace_back(rclcpp::Parameter("foo")); std::shared_future future = asynchronous_client->set_parameters_atomically(parameters, callback); - auto return_code = rclcpp::spin_until_future_complete( + auto return_code = rclcpp::spin_until_complete( node, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); ASSERT_TRUE(callback_called); @@ -328,7 +328,7 @@ TEST_F(TestParameterClient, async_parameter_list_parameters) { std::vector prefixes{"foo"}; std::shared_future future = asynchronous_client->list_parameters(prefixes, 0, callback); - auto return_code = rclcpp::spin_until_future_complete( + auto return_code = rclcpp::spin_until_complete( node, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); ASSERT_TRUE(callback_called); @@ -553,7 +553,7 @@ TEST_F(TestParameterClient, async_parameter_describe_parameters) { std::vector names{"none"}; std::shared_future> future = asynchronous_client->describe_parameters(names, callback); - auto return_code = rclcpp::spin_until_future_complete( + auto return_code = rclcpp::spin_until_complete( node, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); ASSERT_TRUE(callback_called); @@ -571,7 +571,7 @@ TEST_F(TestParameterClient, async_parameter_describe_parameters) { std::vector names{"foo"}; std::shared_future> future = asynchronous_client->describe_parameters(names, callback); - auto return_code = rclcpp::spin_until_future_complete( + auto return_code = rclcpp::spin_until_complete( node, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); std::vector parameter_descs = future.get(); @@ -597,7 +597,7 @@ TEST_F(TestParameterClient, async_parameter_describe_parameters) { std::vector names{"foo", "baz"}; std::shared_future> future = asynchronous_client->describe_parameters(names, callback); - auto return_code = rclcpp::spin_until_future_complete( + auto return_code = rclcpp::spin_until_complete( node, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); ASSERT_TRUE(callback_called); @@ -616,7 +616,7 @@ TEST_F(TestParameterClient, async_parameter_describe_parameters) { std::vector names{"baz", "foo"}; std::shared_future> future = asynchronous_client->describe_parameters(names, callback); - auto return_code = rclcpp::spin_until_future_complete( + auto return_code = rclcpp::spin_until_complete( node, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); ASSERT_TRUE(callback_called); @@ -634,7 +634,7 @@ TEST_F(TestParameterClient, async_parameter_describe_parameters) { std::vector names{"foo", "bar"}; std::shared_future> future = asynchronous_client->describe_parameters(names, callback); - auto return_code = rclcpp::spin_until_future_complete( + auto return_code = rclcpp::spin_until_complete( node, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); std::vector parameter_descs = future.get(); @@ -733,7 +733,7 @@ TEST_F(TestParameterClient, async_parameter_describe_parameters_allow_undeclared std::vector names{"none"}; std::shared_future> future = asynchronous_client->describe_parameters(names, callback); - auto return_code = rclcpp::spin_until_future_complete( + auto return_code = rclcpp::spin_until_complete( node_with_option, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); std::vector parameter_descs = future.get(); @@ -758,7 +758,7 @@ TEST_F(TestParameterClient, async_parameter_describe_parameters_allow_undeclared std::vector names{"foo", "baz"}; std::shared_future> future = asynchronous_client->describe_parameters(names, callback); - auto return_code = rclcpp::spin_until_future_complete( + auto return_code = rclcpp::spin_until_complete( node_with_option, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); std::vector parameter_descs = future.get(); @@ -788,7 +788,7 @@ TEST_F(TestParameterClient, async_parameter_describe_parameters_allow_undeclared std::vector names{"baz", "foo"}; std::shared_future> future = asynchronous_client->describe_parameters(names, callback); - auto return_code = rclcpp::spin_until_future_complete( + auto return_code = rclcpp::spin_until_complete( node_with_option, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); std::vector parameter_descs = future.get(); @@ -870,17 +870,17 @@ TEST_F(TestParameterClient, async_parameter_delete_parameters) { std::make_shared(node_with_option); // set parameter auto set_future = asynchronous_client->set_parameters({rclcpp::Parameter("foo", 4)}); - rclcpp::spin_until_future_complete( + rclcpp::spin_until_complete( node_with_option, set_future, std::chrono::milliseconds(100)); ASSERT_EQ(set_future.get()[0].successful, true); // delete one parameter auto delete_future = asynchronous_client->delete_parameters({"foo"}); - rclcpp::spin_until_future_complete( + rclcpp::spin_until_complete( node_with_option, delete_future, std::chrono::milliseconds(100)); ASSERT_EQ(delete_future.get()[0].successful, true); // check that deleted parameter isn't set auto get_future2 = asynchronous_client->get_parameters({"foo"}); - rclcpp::spin_until_future_complete( + rclcpp::spin_until_complete( node_with_option, get_future2, std::chrono::milliseconds(100)); ASSERT_EQ( get_future2.get()[0].get_type(), @@ -918,13 +918,13 @@ TEST_F(TestParameterClient, async_parameter_load_parameters) { const std::string parameters_filepath = ( test_resources_path / "test_node" / "load_parameters.yaml").string(); auto load_future = asynchronous_client->load_parameters(parameters_filepath); - auto result_code = rclcpp::spin_until_future_complete( + auto result_code = rclcpp::spin_until_complete( load_node, load_future, std::chrono::milliseconds(100)); ASSERT_EQ(result_code, rclcpp::FutureReturnCode::SUCCESS); ASSERT_EQ(load_future.get()[0].successful, true); // list parameters auto list_parameters = asynchronous_client->list_parameters({}, 3); - rclcpp::spin_until_future_complete( + rclcpp::spin_until_complete( load_node, list_parameters, std::chrono::milliseconds(100)); ASSERT_EQ(list_parameters.get().names.size(), static_cast(5)); } diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index 221e2bdf0a..26f47c0436 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -201,7 +201,7 @@ TEST_F(TestQosEvent, test_default_incompatible_qos_callbacks) // This future won't complete on fastrtps, so just timeout immediately const auto timeout = std::chrono::seconds(10); - ex.spin_until_future_complete(log_msgs_future, timeout); + ex.spin_until_complete(log_msgs_future, timeout); EXPECT_EQ( "New subscription discovered on topic '/ns/test_topic', requesting incompatible QoS. " diff --git a/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp index ce6887c631..99bf9e4a10 100644 --- a/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp @@ -432,7 +432,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no ex.add_node(empty_subscriber); // Spin and get future - ex.spin_until_future_complete( + ex.spin_until_complete( statistics_listener->GetFuture(), kTestTimeout); @@ -497,7 +497,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi ex.add_node(msg_with_header_subscriber); // Spin and get future - ex.spin_until_future_complete( + ex.spin_until_complete( statistics_listener->GetFuture(), kTestTimeout); @@ -550,7 +550,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_include_window ex.add_node(msg_with_header_subscriber); // Spin and get future - ex.spin_until_future_complete(statistics_listener->GetFuture(), kTestTimeout); + ex.spin_until_complete(statistics_listener->GetFuture(), kTestTimeout); const auto received_messages = statistics_listener->GetReceivedMessages(); EXPECT_EQ(kNumExpectedMessages, received_messages.size()); diff --git a/rclcpp_action/test/benchmark/benchmark_action_client.cpp b/rclcpp_action/test/benchmark/benchmark_action_client.cpp index 8b11935117..c3d85d4c0c 100644 --- a/rclcpp_action/test/benchmark/benchmark_action_client.cpp +++ b/rclcpp_action/test/benchmark/benchmark_action_client.cpp @@ -194,7 +194,7 @@ BENCHMARK_F(ActionClientPerformanceTest, async_send_goal_rejected)(benchmark::St for (auto _ : state) { (void)_; auto future_goal_handle = client->async_send_goal(goal); - rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1)); + rclcpp::spin_until_complete(node, future_goal_handle, std::chrono::seconds(1)); if (!future_goal_handle.valid()) { state.SkipWithError("Shared future was invalid"); return; @@ -223,7 +223,7 @@ BENCHMARK_F(ActionClientPerformanceTest, async_send_goal_get_accepted_response)( (void)_; // This server's execution is deferred auto future_goal_handle = client->async_send_goal(goal); - rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1)); + rclcpp::spin_until_complete(node, future_goal_handle, std::chrono::seconds(1)); if (!future_goal_handle.valid()) { state.SkipWithError("Shared future was invalid"); @@ -258,7 +258,7 @@ BENCHMARK_F(ActionClientPerformanceTest, async_get_result)(benchmark::State & st auto future_goal_handle = client->async_send_goal(goal); // Action server accepts and defers, so this spin doesn't include result - rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1)); + rclcpp::spin_until_complete(node, future_goal_handle, std::chrono::seconds(1)); if (!future_goal_handle.valid()) { state.SkipWithError("Shared future was invalid"); @@ -276,7 +276,7 @@ BENCHMARK_F(ActionClientPerformanceTest, async_get_result)(benchmark::State & st // Measure how long it takes client to receive the succeeded result auto future_result = client->async_get_result(goal_handle); - rclcpp::spin_until_future_complete(node, future_result, std::chrono::seconds(1)); + rclcpp::spin_until_complete(node, future_result, std::chrono::seconds(1)); const auto & wrapped_result = future_result.get(); if (rclcpp_action::ResultCode::SUCCEEDED != wrapped_result.code) { state.SkipWithError("Fibonacci action did not succeed"); @@ -310,12 +310,12 @@ BENCHMARK_F(ActionClientPerformanceTest, async_cancel_goal)(benchmark::State & s auto future_goal_handle = client->async_send_goal(goal); // Action server accepts and defers, so action can be canceled - rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1)); + rclcpp::spin_until_complete(node, future_goal_handle, std::chrono::seconds(1)); auto goal_handle = future_goal_handle.get(); state.ResumeTiming(); auto future_cancel = client->async_cancel_goal(goal_handle); - rclcpp::spin_until_future_complete(node, future_cancel, std::chrono::seconds(1)); + rclcpp::spin_until_complete(node, future_cancel, std::chrono::seconds(1)); auto cancel_response = future_cancel.get(); using CancelActionResponse = test_msgs::action::Fibonacci::Impl::CancelGoalService::Response; @@ -345,13 +345,13 @@ BENCHMARK_F(ActionClientPerformanceTest, async_cancel_all_goals)(benchmark::Stat state.PauseTiming(); for (int i = 0; i < num_concurrently_inflight_goals; ++i) { auto future_goal_handle = client->async_send_goal(goal); - rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1)); + rclcpp::spin_until_complete(node, future_goal_handle, std::chrono::seconds(1)); } // Action server accepts and defers, so action can be canceled state.ResumeTiming(); auto future_cancel_all = client->async_cancel_all_goals(); - rclcpp::spin_until_future_complete(node, future_cancel_all, std::chrono::seconds(1)); + rclcpp::spin_until_complete(node, future_cancel_all, std::chrono::seconds(1)); auto cancel_response = future_cancel_all.get(); using CancelActionResponse = test_msgs::action::Fibonacci::Impl::CancelGoalService::Response; diff --git a/rclcpp_action/test/benchmark/benchmark_action_server.cpp b/rclcpp_action/test/benchmark/benchmark_action_server.cpp index 6817f86b14..d0eb86c749 100644 --- a/rclcpp_action/test/benchmark/benchmark_action_server.cpp +++ b/rclcpp_action/test/benchmark/benchmark_action_server.cpp @@ -152,7 +152,7 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_accept_goal)(benchmark::S auto client_goal_handle_future = AsyncSendGoalOfOrder(1); state.ResumeTiming(); - rclcpp::spin_until_future_complete(node, client_goal_handle_future); + rclcpp::spin_until_complete(node, client_goal_handle_future); auto goal_handle = client_goal_handle_future.get(); if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) { state.SkipWithError("Valid goal was not accepted"); @@ -185,12 +185,12 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_cancel_goal)(benchmark::S auto client_goal_handle_future = AsyncSendGoalOfOrder(1); // This spin completes when the goal has been accepted, but not executed because server // responds with ACCEPT_AND_DEFER - rclcpp::spin_until_future_complete(node, client_goal_handle_future, std::chrono::seconds(1)); + rclcpp::spin_until_complete(node, client_goal_handle_future, std::chrono::seconds(1)); auto client_goal_handle = client_goal_handle_future.get(); auto future_cancel = action_client->async_cancel_goal(client_goal_handle); state.ResumeTiming(); - rclcpp::spin_until_future_complete(node, future_cancel, std::chrono::seconds(1)); + rclcpp::spin_until_complete(node, future_cancel, std::chrono::seconds(1)); auto cancel_response = future_cancel.get(); using CancelActionResponse = test_msgs::action::Fibonacci::Impl::CancelGoalService::Response; if (CancelActionResponse::ERROR_NONE != cancel_response->return_code) { @@ -221,7 +221,7 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_execute_goal)(benchmark:: state.PauseTiming(); auto client_goal_handle_future = AsyncSendGoalOfOrder(1); - rclcpp::spin_until_future_complete(node, client_goal_handle_future); + rclcpp::spin_until_complete(node, client_goal_handle_future); auto goal_handle = client_goal_handle_future.get(); if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) { state.SkipWithError("Valid goal was not accepted"); @@ -267,7 +267,7 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_set_success)(benchmark::S state.PauseTiming(); auto client_goal_handle_future = AsyncSendGoalOfOrder(goal_order); - rclcpp::spin_until_future_complete(node, client_goal_handle_future); + rclcpp::spin_until_complete(node, client_goal_handle_future); auto goal_handle = client_goal_handle_future.get(); if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) { state.SkipWithError("Valid goal was not accepted"); @@ -312,7 +312,7 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_abort)(benchmark::State & state.PauseTiming(); auto client_goal_handle_future = AsyncSendGoalOfOrder(goal_order); - rclcpp::spin_until_future_complete(node, client_goal_handle_future); + rclcpp::spin_until_complete(node, client_goal_handle_future); auto goal_handle = client_goal_handle_future.get(); if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) { state.SkipWithError("Valid goal was not accepted"); diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index b94a82d500..cf32abea70 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -247,7 +247,7 @@ class TestClient : public ::testing::Test } template - void dual_spin_until_future_complete(std::shared_future & future) + void dual_spin_until_complete(std::shared_future & future) { std::future_status status; do { @@ -416,13 +416,13 @@ TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks) ActionGoal bad_goal; bad_goal.order = -5; auto future_goal_handle = action_client->async_send_goal(bad_goal); - dual_spin_until_future_complete(future_goal_handle); + dual_spin_until_complete(future_goal_handle); EXPECT_EQ(nullptr, future_goal_handle.get().get()); ActionGoal good_goal; good_goal.order = 5; future_goal_handle = action_client->async_send_goal(good_goal); - dual_spin_until_future_complete(future_goal_handle); + dual_spin_until_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); EXPECT_FALSE(goal_handle->is_feedback_aware()); @@ -437,7 +437,7 @@ TEST_F(TestClientAgainstServer, bad_goal_handles) ActionGoal goal; goal.order = 0; auto future_goal_handle = action_client0->async_send_goal(goal); - dual_spin_until_future_complete(future_goal_handle); + dual_spin_until_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); auto action_client1 = rclcpp_action::create_client(client_node, action_name); @@ -454,14 +454,14 @@ TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks_wait_for_result) ActionGoal goal; goal.order = 5; auto future_goal_handle = action_client->async_send_goal(goal); - dual_spin_until_future_complete(future_goal_handle); + dual_spin_until_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); EXPECT_FALSE(goal_handle->is_feedback_aware()); EXPECT_FALSE(goal_handle->is_result_aware()); auto future_result = action_client->async_get_result(goal_handle); EXPECT_TRUE(goal_handle->is_result_aware()); - dual_spin_until_future_complete(future_result); + dual_spin_until_complete(future_result); auto wrapped_result = future_result.get(); ASSERT_EQ(6ul, wrapped_result.result->sequence.size()); EXPECT_EQ(0, wrapped_result.result->sequence[0]); @@ -477,7 +477,7 @@ TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks_then_invalidate) ActionGoal goal; goal.order = 5; auto future_goal_handle = action_client->async_send_goal(goal); - dual_spin_until_future_complete(future_goal_handle); + dual_spin_until_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); ASSERT_NE(nullptr, goal_handle); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); @@ -511,7 +511,7 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait ActionGoal bad_goal; bad_goal.order = -1; auto future_goal_handle = action_client->async_send_goal(bad_goal, send_goal_ops); - dual_spin_until_future_complete(future_goal_handle); + dual_spin_until_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_FALSE(goal_response_received); EXPECT_EQ(nullptr, goal_handle); @@ -521,7 +521,7 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait ActionGoal goal; goal.order = 4; auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); - dual_spin_until_future_complete(future_goal_handle); + dual_spin_until_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_TRUE(goal_response_received); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); @@ -529,7 +529,7 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait EXPECT_FALSE(goal_handle->is_result_aware()); auto future_result = action_client->async_get_result(goal_handle); EXPECT_TRUE(goal_handle->is_result_aware()); - dual_spin_until_future_complete(future_result); + dual_spin_until_complete(future_result); auto wrapped_result = future_result.get(); ASSERT_EQ(5u, wrapped_result.result->sequence.size()); EXPECT_EQ(3, wrapped_result.result->sequence.back()); @@ -555,14 +555,14 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_feedback_callback_wait_for_ feedback_count++; }; auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); - dual_spin_until_future_complete(future_goal_handle); + dual_spin_until_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); EXPECT_TRUE(goal_handle->is_feedback_aware()); EXPECT_FALSE(goal_handle->is_result_aware()); auto future_result = action_client->async_get_result(goal_handle); EXPECT_TRUE(goal_handle->is_result_aware()); - dual_spin_until_future_complete(future_result); + dual_spin_until_complete(future_result); auto wrapped_result = future_result.get(); ASSERT_EQ(5u, wrapped_result.result->sequence.size()); @@ -591,13 +591,13 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_result_callback_wait_for_re } }; auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); - dual_spin_until_future_complete(future_goal_handle); + dual_spin_until_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); EXPECT_FALSE(goal_handle->is_feedback_aware()); EXPECT_TRUE(goal_handle->is_result_aware()); auto future_result = action_client->async_get_result(goal_handle); - dual_spin_until_future_complete(future_result); + dual_spin_until_complete(future_result); auto wrapped_result = future_result.get(); EXPECT_TRUE(result_callback_received); @@ -613,7 +613,7 @@ TEST_F(TestClientAgainstServer, async_get_result_with_callback) ActionGoal goal; goal.order = 4; auto future_goal_handle = action_client->async_send_goal(goal); - dual_spin_until_future_complete(future_goal_handle); + dual_spin_until_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_NE(goal_handle, nullptr); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); @@ -632,7 +632,7 @@ TEST_F(TestClientAgainstServer, async_get_result_with_callback) result_callback_received = true; } }); - dual_spin_until_future_complete(future_result); + dual_spin_until_complete(future_result); auto wrapped_result = future_result.get(); EXPECT_TRUE(result_callback_received); @@ -648,12 +648,12 @@ TEST_F(TestClientAgainstServer, async_cancel_one_goal) ActionGoal goal; goal.order = 5; auto future_goal_handle = action_client->async_send_goal(goal); - dual_spin_until_future_complete(future_goal_handle); + dual_spin_until_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); auto future_cancel = action_client->async_cancel_goal(goal_handle); - dual_spin_until_future_complete(future_cancel); + dual_spin_until_complete(future_cancel); ActionCancelGoalResponse::SharedPtr cancel_response = future_cancel.get(); EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); } @@ -666,7 +666,7 @@ TEST_F(TestClientAgainstServer, async_cancel_one_goal_with_callback) ActionGoal goal; goal.order = 5; auto future_goal_handle = action_client->async_send_goal(goal); - dual_spin_until_future_complete(future_goal_handle); + dual_spin_until_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); @@ -684,7 +684,7 @@ TEST_F(TestClientAgainstServer, async_cancel_one_goal_with_callback) cancel_response_received = true; } }); - dual_spin_until_future_complete(future_cancel); + dual_spin_until_complete(future_cancel); auto cancel_response = future_cancel.get(); EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); ASSERT_EQ(1ul, cancel_response->goals_canceling.size()); @@ -700,14 +700,14 @@ TEST_F(TestClientAgainstServer, async_cancel_all_goals) ActionGoal goal; goal.order = 6; auto future_goal_handle0 = action_client->async_send_goal(goal); - dual_spin_until_future_complete(future_goal_handle0); + dual_spin_until_complete(future_goal_handle0); auto goal_handle0 = future_goal_handle0.get(); ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); goal.order = 8; auto future_goal_handle1 = action_client->async_send_goal(goal); - dual_spin_until_future_complete(future_goal_handle1); + dual_spin_until_complete(future_goal_handle1); auto goal_handle1 = future_goal_handle1.get(); if (goal_handle1->get_goal_id() < goal_handle0->get_goal_id()) { @@ -717,7 +717,7 @@ TEST_F(TestClientAgainstServer, async_cancel_all_goals) ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3))); auto future_cancel_all = action_client->async_cancel_all_goals(); - dual_spin_until_future_complete(future_cancel_all); + dual_spin_until_complete(future_cancel_all); auto cancel_response = future_cancel_all.get(); EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); @@ -736,14 +736,14 @@ TEST_F(TestClientAgainstServer, async_cancel_all_goals_with_callback) ActionGoal goal; goal.order = 6; auto future_goal_handle0 = action_client->async_send_goal(goal); - dual_spin_until_future_complete(future_goal_handle0); + dual_spin_until_complete(future_goal_handle0); auto goal_handle0 = future_goal_handle0.get(); ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); goal.order = 8; auto future_goal_handle1 = action_client->async_send_goal(goal); - dual_spin_until_future_complete(future_goal_handle1); + dual_spin_until_complete(future_goal_handle1); auto goal_handle1 = future_goal_handle1.get(); if (goal_handle1->get_goal_id() < goal_handle0->get_goal_id()) { @@ -766,7 +766,7 @@ TEST_F(TestClientAgainstServer, async_cancel_all_goals_with_callback) cancel_callback_received = true; } }); - dual_spin_until_future_complete(future_cancel_all); + dual_spin_until_complete(future_cancel_all); auto cancel_response = future_cancel_all.get(); EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); @@ -786,21 +786,21 @@ TEST_F(TestClientAgainstServer, async_cancel_some_goals) ActionGoal goal; goal.order = 6; auto future_goal_handle0 = action_client->async_send_goal(goal); - dual_spin_until_future_complete(future_goal_handle0); + dual_spin_until_complete(future_goal_handle0); auto goal_handle0 = future_goal_handle0.get(); ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); goal.order = 8; auto future_goal_handle1 = action_client->async_send_goal(goal); - dual_spin_until_future_complete(future_goal_handle1); + dual_spin_until_complete(future_goal_handle1); auto goal_handle1 = future_goal_handle1.get(); ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3))); auto future_cancel_some = action_client->async_cancel_goals_before(goal_handle1->get_goal_stamp()); - dual_spin_until_future_complete(future_cancel_some); + dual_spin_until_complete(future_cancel_some); auto cancel_response = future_cancel_some.get(); EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); @@ -817,14 +817,14 @@ TEST_F(TestClientAgainstServer, async_cancel_some_goals_with_callback) ActionGoal goal; goal.order = 6; auto future_goal_handle0 = action_client->async_send_goal(goal); - dual_spin_until_future_complete(future_goal_handle0); + dual_spin_until_complete(future_goal_handle0); auto goal_handle0 = future_goal_handle0.get(); ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); goal.order = 8; auto future_goal_handle1 = action_client->async_send_goal(goal); - dual_spin_until_future_complete(future_goal_handle1); + dual_spin_until_complete(future_goal_handle1); auto goal_handle1 = future_goal_handle1.get(); ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3))); @@ -842,7 +842,7 @@ TEST_F(TestClientAgainstServer, async_cancel_some_goals_with_callback) cancel_callback_received = true; } }); - dual_spin_until_future_complete(future_cancel_some); + dual_spin_until_complete(future_cancel_some); auto cancel_response = future_cancel_some.get(); EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); @@ -877,7 +877,7 @@ TEST_F(TestClientAgainstServer, send_rcl_errors) auto mock = mocking_utils::patch_and_return( "lib:rclcpp_action", rcl_action_send_result_request, RCL_RET_ERROR); auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); - dual_spin_until_future_complete(future_goal_handle); + dual_spin_until_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_UNKNOWN, goal_handle->get_status()); } @@ -886,7 +886,7 @@ TEST_F(TestClientAgainstServer, send_rcl_errors) auto mock = mocking_utils::patch_and_return( "lib:rclcpp_action", rcl_action_send_cancel_request, RCL_RET_ERROR); auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); - dual_spin_until_future_complete(future_goal_handle); + dual_spin_until_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_THROW( action_client->async_cancel_goals_before(goal_handle->get_goal_stamp()), @@ -913,11 +913,11 @@ TEST_F(TestClientAgainstServer, execute_rcl_errors) "lib:rclcpp_action", rcl_action_take_feedback, RCL_RET_ERROR); auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); - dual_spin_until_future_complete(future_goal_handle); + dual_spin_until_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); auto future_result = action_client->async_get_result(goal_handle); EXPECT_THROW( - dual_spin_until_future_complete(future_result), + dual_spin_until_complete(future_result), rclcpp::exceptions::RCLError); } { @@ -928,7 +928,7 @@ TEST_F(TestClientAgainstServer, execute_rcl_errors) auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); EXPECT_THROW( - dual_spin_until_future_complete(future_goal_handle), + dual_spin_until_complete(future_goal_handle), rclcpp::exceptions::RCLError); } { @@ -938,11 +938,11 @@ TEST_F(TestClientAgainstServer, execute_rcl_errors) "lib:rclcpp_action", rcl_action_take_result_response, RCL_RET_ERROR); auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); - dual_spin_until_future_complete(future_goal_handle); + dual_spin_until_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); auto future_result = action_client->async_get_result(goal_handle); EXPECT_THROW( - dual_spin_until_future_complete(future_result), + dual_spin_until_complete(future_result), rclcpp::exceptions::RCLError); } { @@ -952,12 +952,12 @@ TEST_F(TestClientAgainstServer, execute_rcl_errors) "lib:rclcpp_action", rcl_action_take_cancel_response, RCL_RET_ERROR); auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); - dual_spin_until_future_complete(future_goal_handle); + dual_spin_until_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); auto future_cancel_some = action_client->async_cancel_goals_before(goal_handle->get_goal_stamp()); EXPECT_THROW( - dual_spin_until_future_complete(future_cancel_some), + dual_spin_until_complete(future_cancel_some), rclcpp::exceptions::RCLError); } } diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 8687f90fbe..fc5539d8dd 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -59,7 +59,7 @@ class TestServer : public ::testing::Test auto request = std::make_shared(); request->goal_id.uuid = uuid; auto future = client->async_send_request(request); - auto return_code = rclcpp::spin_until_future_complete(node, future, timeout); + auto return_code = rclcpp::spin_until_complete(node, future, timeout); if (rclcpp::FutureReturnCode::SUCCESS == return_code) { return request; } else if (rclcpp::FutureReturnCode::TIMEOUT == return_code) { @@ -82,7 +82,7 @@ class TestServer : public ::testing::Test auto request = std::make_shared(); request->goal_info.goal_id.uuid = uuid; auto future = cancel_client->async_send_request(request); - auto return_code = rclcpp::spin_until_future_complete(node, future, timeout); + auto return_code = rclcpp::spin_until_complete(node, future, timeout); if (rclcpp::FutureReturnCode::SUCCESS == return_code) { return future.get(); } else if (rclcpp::FutureReturnCode::TIMEOUT == return_code) { @@ -264,7 +264,7 @@ TEST_F(TestServer, handle_goal_called) auto future = client->async_send_request(request); ASSERT_EQ( rclcpp::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future)); + rclcpp::spin_until_complete(node, future)); ASSERT_EQ(uuid, received_uuid); } @@ -881,7 +881,7 @@ TEST_F(TestServer, get_result) // Wait for the result request to be received ASSERT_EQ( rclcpp::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future)); + rclcpp::spin_until_complete(node, future)); auto response = future.get(); EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status); @@ -897,7 +897,7 @@ TEST_F(TestServer, get_result) future = result_client->async_send_request(request); ASSERT_EQ( rclcpp::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future)); + rclcpp::spin_until_complete(node, future)); response = future.get(); EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_UNKNOWN, response->status); @@ -958,7 +958,7 @@ TEST_F(TestServer, get_result_deferred) // Wait for the result request to be received ASSERT_EQ( rclcpp::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node, future)); + rclcpp::spin_until_complete(node, future)); auto response = future.get(); EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status); @@ -1045,7 +1045,7 @@ class TestBasicServer : public TestServer // Wait for the result request to be received ASSERT_EQ( rclcpp::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node_, future)); + rclcpp::spin_until_complete(node_, future)); auto response = future.get(); EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status); @@ -1061,7 +1061,7 @@ class TestBasicServer : public TestServer future = result_client->async_send_request(request); ASSERT_EQ( rclcpp::FutureReturnCode::SUCCESS, - rclcpp::spin_until_future_complete(node_, future)); + rclcpp::spin_until_complete(node_, future)); } protected: diff --git a/rclcpp_components/test/test_component_manager_api.cpp b/rclcpp_components/test/test_component_manager_api.cpp index dfb9db76a2..0d5c3771ed 100644 --- a/rclcpp_components/test/test_component_manager_api.cpp +++ b/rclcpp_components/test/test_component_manager_api.cpp @@ -66,7 +66,7 @@ void test_components_api(bool use_dedicated_executor) request->plugin_name = "test_rclcpp_components::TestComponentFoo"; auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, true); @@ -81,7 +81,7 @@ void test_components_api(bool use_dedicated_executor) request->plugin_name = "test_rclcpp_components::TestComponentBar"; auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, true); @@ -98,7 +98,7 @@ void test_components_api(bool use_dedicated_executor) request->node_name = "test_component_baz"; auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, true); @@ -116,7 +116,7 @@ void test_components_api(bool use_dedicated_executor) request->node_name = "test_component_bing"; auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, true); @@ -132,7 +132,7 @@ void test_components_api(bool use_dedicated_executor) request->plugin_name = "test_rclcpp_components::TestComponent"; auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); auto result = future.get(); EXPECT_EQ(result->success, false); @@ -148,7 +148,7 @@ void test_components_api(bool use_dedicated_executor) request->plugin_name = "test_rclcpp_components::TestComponentFoo"; auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); auto result = future.get(); EXPECT_EQ(result->success, false); @@ -166,7 +166,7 @@ void test_components_api(bool use_dedicated_executor) request->remap_rules.push_back("alice:=bob"); auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, true); @@ -186,7 +186,7 @@ void test_components_api(bool use_dedicated_executor) request->extra_arguments.push_back(use_intraprocess_comms.to_parameter_msg()); auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, true); @@ -208,7 +208,7 @@ void test_components_api(bool use_dedicated_executor) request->extra_arguments.push_back(use_intraprocess_comms.to_parameter_msg()); auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, false); @@ -230,7 +230,7 @@ void test_components_api(bool use_dedicated_executor) request->extra_arguments.push_back(forward_global_arguments.to_parameter_msg()); auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, true); @@ -251,7 +251,7 @@ void test_components_api(bool use_dedicated_executor) request->extra_arguments.push_back(forward_global_arguments.to_parameter_msg()); auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, false); @@ -284,7 +284,7 @@ void test_components_api(bool use_dedicated_executor) { auto request = std::make_shared(); auto future = client->async_send_request(request); - auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); auto result = future.get(); auto result_node_names = result->full_node_names; @@ -322,7 +322,7 @@ void test_components_api(bool use_dedicated_executor) request->unique_id = 1; auto future = client->async_send_request(request); - auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, true); @@ -334,7 +334,7 @@ void test_components_api(bool use_dedicated_executor) request->unique_id = 1; auto future = client->async_send_request(request); - auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, false); @@ -353,7 +353,7 @@ void test_components_api(bool use_dedicated_executor) { auto request = std::make_shared(); auto future = client->async_send_request(request); - auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); auto result = future.get(); auto result_node_names = result->full_node_names; From f43a9198ebb033b43b4bb7b0d8ea2d2798bd566e Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 24 Jun 2022 11:21:24 -0700 Subject: [PATCH 120/455] Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (#1821) (#1874)" (#1956) This reverts commit 3c8e89d17c5638450f55162a959307014385cbd2. --- rclcpp/include/rclcpp/client.hpp | 6 +- rclcpp/include/rclcpp/executor.hpp | 143 ++++++------------ rclcpp/include/rclcpp/executors.hpp | 95 ++---------- rclcpp/include/rclcpp/future_return_code.hpp | 2 +- rclcpp/include/rclcpp/rclcpp.hpp | 2 +- rclcpp/src/rclcpp/parameter_client.cpp | 32 ++-- rclcpp/src/rclcpp/time_source.cpp | 2 +- rclcpp/test/benchmark/benchmark_client.cpp | 2 +- rclcpp/test/benchmark/benchmark_executor.cpp | 28 ++-- rclcpp/test/benchmark/benchmark_service.cpp | 2 +- .../test/rclcpp/executors/test_executors.cpp | 75 ++++----- .../test_static_single_threaded_executor.cpp | 4 +- rclcpp/test/rclcpp/test_executor.cpp | 62 ++------ ..._intra_process_manager_with_allocators.cpp | 4 +- rclcpp/test/rclcpp/test_parameter_client.cpp | 38 ++--- rclcpp/test/rclcpp/test_qos_event.cpp | 2 +- .../test_subscription_topic_statistics.cpp | 6 +- .../benchmark/benchmark_action_client.cpp | 16 +- .../benchmark/benchmark_action_server.cpp | 12 +- rclcpp_action/test/test_client.cpp | 82 +++++----- rclcpp_action/test/test_server.cpp | 16 +- .../test/test_component_manager_api.cpp | 30 ++-- 22 files changed, 240 insertions(+), 421 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 9b1522399c..e88fa8a949 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -579,11 +579,11 @@ class Client : public ClientBase /// Send a request to the service server. /** * This method returns a `FutureAndRequestId` instance - * that can be passed to Executor::spin_until_complete() to + * that can be passed to Executor::spin_until_future_complete() to * wait until it has been completed. * * If the future never completes, - * e.g. the call to Executor::spin_until_complete() times out, + * e.g. the call to Executor::spin_until_future_complete() times out, * Client::remove_pending_request() must be called to clean the client internal state. * Not doing so will make the `Client` instance to use more memory each time a response is not * received from the service server. @@ -592,7 +592,7 @@ class Client : public ClientBase * auto future = client->async_send_request(my_request); * if ( * rclcpp::FutureReturnCode::TIMEOUT == - * executor->spin_until_complete(future, timeout)) + * executor->spin_until_future_complete(future, timeout)) * { * client->remove_pending_request(future); * // handle timeout diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index a700b8f454..ed2ddc4a0a 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -26,7 +26,6 @@ #include #include #include -#include #include "rcl/guard_condition.h" #include "rcl/wait.h" @@ -320,51 +319,6 @@ class Executor virtual void spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - /// Spin (blocking) until the condition is complete, it times out waiting, - /// or rclcpp is interrupted. - /** - * \param[in] future The condition which can be callable or future type to wait on. - * If this function returns SUCCESS, the future can be - * accessed without blocking (though it may still throw an exception). - * \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once. - * `-1` is block forever, `0` is non-blocking. - * If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return - * code. - * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. - */ - template - FutureReturnCode - spin_until_complete( - const ConditionT & condition, - DurationT timeout = DurationT(-1)) - { - if constexpr (std::is_invocable_v) { - using RetT = std::invoke_result_t; - static_assert( - std::is_same_v, - "Conditional callable has to return boolean type"); - return spin_until_complete_impl(condition, timeout); - } else { - auto check_future = [&condition]() { - return condition.wait_for(std::chrono::seconds(0)) == - std::future_status::ready; - }; - return spin_until_complete_impl(check_future, timeout); - } - } - - /// Spin (blocking) for at least the given amount of duration. - /** - * \param[in] duration gets passed to Executor::spin_node_once, - * spins the executor for given duration. - */ - template - void - spin_for(DurationT duration) - { - (void)spin_until_complete([]() {return false;}, duration); - } - /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted. /** * \param[in] future The future to wait on. If this function returns SUCCESS, the future can be @@ -376,13 +330,57 @@ class Executor * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ template - [[deprecated("use spin_until_complete(const ConditionT & condition, DurationT timeout) instead")]] FutureReturnCode spin_until_future_complete( const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { - return spin_until_complete(future, timeout); + // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete + // inside a callback executed by an executor. + + // Check the future before entering the while loop. + // If the future is already complete, don't try to spin. + std::future_status status = future.wait_for(std::chrono::seconds(0)); + if (status == std::future_status::ready) { + return FutureReturnCode::SUCCESS; + } + + auto end_time = std::chrono::steady_clock::now(); + std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast( + timeout); + if (timeout_ns > std::chrono::nanoseconds::zero()) { + end_time += timeout_ns; + } + std::chrono::nanoseconds timeout_left = timeout_ns; + + if (spinning.exchange(true)) { + throw std::runtime_error("spin_until_future_complete() called while already spinning"); + } + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + while (rclcpp::ok(this->context_) && spinning.load()) { + // Do one item of work. + spin_once_impl(timeout_left); + + // Check if the future is set, return SUCCESS if it is. + status = future.wait_for(std::chrono::seconds(0)); + if (status == std::future_status::ready) { + return FutureReturnCode::SUCCESS; + } + // If the original timeout is < 0, then this is blocking, never TIMEOUT. + if (timeout_ns < std::chrono::nanoseconds::zero()) { + continue; + } + // Otherwise check if we still have time to wait, return TIMEOUT if not. + auto now = std::chrono::steady_clock::now(); + if (now >= end_time) { + return FutureReturnCode::TIMEOUT; + } + // Subtract the elapsed time from the original timeout. + timeout_left = std::chrono::duration_cast(end_time - now); + } + + // The future did not complete before ok() returned false, return INTERRUPTED. + return FutureReturnCode::INTERRUPTED; } /// Cancel any running spin* function, causing it to return. @@ -562,55 +560,6 @@ class Executor virtual void spin_once_impl(std::chrono::nanoseconds timeout); -protected: - // Implementation details, used by spin_until_complete and spin_for. - // Previouse implementation of spin_until_future_complete. - template - FutureReturnCode - spin_until_complete_impl(ConditionT condition, DurationT timeout) - { - auto end_time = std::chrono::steady_clock::now(); - std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast( - timeout); - if (timeout_ns > std::chrono::nanoseconds::zero()) { - end_time += timeout_ns; - } - std::chrono::nanoseconds timeout_left = timeout_ns; - - // Preliminary check, finish if conditon is done already. - if (condition()) { - return FutureReturnCode::SUCCESS; - } - - if (spinning.exchange(true)) { - throw std::runtime_error("spin_until_complete() called while already spinning"); - } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); - while (rclcpp::ok(this->context_) && spinning.load()) { - // Do one item of work. - spin_once_impl(timeout_left); - - if (condition()) { - return FutureReturnCode::SUCCESS; - } - // If the original timeout is < 0, then this is blocking, never TIMEOUT. - if (timeout_ns < std::chrono::nanoseconds::zero()) { - continue; - } - // Otherwise check if we still have time to wait, return TIMEOUT if not. - auto now = std::chrono::steady_clock::now(); - if (now >= end_time) { - return FutureReturnCode::TIMEOUT; - } - // Subtract the elapsed time from the original timeout. - timeout_left = std::chrono::duration_cast(end_time - now); - } - - // The condition did not pass before ok() returned false, return INTERRUPTED. - return FutureReturnCode::INTERRUPTED; - } - -public: typedef std::map> diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 44db27686f..36fb0d63cf 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -54,50 +54,6 @@ namespace executors using rclcpp::executors::MultiThreadedExecutor; using rclcpp::executors::SingleThreadedExecutor; -/// Spin (blocking) until the conditon is complete, it times out waiting, or rclcpp is interrupted. -/** - * \param[in] executor The executor which will spin the node. - * \param[in] node_ptr The node to spin. - * \param[in] condition The callable or future to wait on. If `SUCCESS`, the condition is safe to - * access after this function - * \param[in] timeout Optional timeout parameter, which gets passed to - * Executor::spin_node_once. - * `-1` is block forever, `0` is non-blocking. - * If the time spent inside the blocking loop exceeds this timeout, return a `TIMEOUT` return code. - * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. - */ -template -rclcpp::FutureReturnCode -spin_node_until_complete( - rclcpp::Executor & executor, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const ConditionT & condition, - DurationT timeout = DurationT(-1)) -{ - // TODO(wjwwood): does not work recursively; can't call spin_node_until_complete - // inside a callback executed by an executor. - executor.add_node(node_ptr); - auto retcode = executor.spin_until_complete(condition, timeout); - executor.remove_node(node_ptr); - return retcode; -} - -template -rclcpp::FutureReturnCode -spin_node_until_complete( - rclcpp::Executor & executor, - std::shared_ptr node_ptr, - const ConditionT & condition, - DurationT timeout = DurationT(-1)) -{ - return rclcpp::executors::spin_node_until_complete( - executor, - node_ptr->get_node_base_interface(), - condition, - timeout); -} - /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted. /** * \param[in] executor The executor which will spin the node. @@ -111,10 +67,6 @@ spin_node_until_complete( * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ template -[[deprecated( - "use spin_node_until_complete(Executor &, node_interfaces::NodeBaseInterface::SharedPtr, " - "const ConditionT &, DurationT) instead" -)]] rclcpp::FutureReturnCode spin_node_until_future_complete( rclcpp::Executor & executor, @@ -122,15 +74,16 @@ spin_node_until_future_complete( const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { - return spin_until_complete(executor, node_ptr, future, timeout); + // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete + // inside a callback executed by an executor. + executor.add_node(node_ptr); + auto retcode = executor.spin_until_future_complete(future, timeout); + executor.remove_node(node_ptr); + return retcode; } template -[[deprecated( - "use spin_node_until_complete(Executor &, std::shared_ptr, " - "const ConditionT &, DurationT) instead" -)]] rclcpp::FutureReturnCode spin_node_until_future_complete( rclcpp::Executor & executor, @@ -138,7 +91,7 @@ spin_node_until_future_complete( const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { - return rclcpp::executors::spin_node_until_complete( + return rclcpp::executors::spin_node_until_future_complete( executor, node_ptr->get_node_base_interface(), future, @@ -147,33 +100,7 @@ spin_node_until_future_complete( } // namespace executors -template -rclcpp::FutureReturnCode -spin_until_complete( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const ConditionT & condition, - DurationT timeout = DurationT(-1)) -{ - rclcpp::executors::SingleThreadedExecutor executor; - return executors::spin_node_until_complete(executor, node_ptr, condition, timeout); -} - -template -rclcpp::FutureReturnCode -spin_until_complete( - std::shared_ptr node_ptr, - const ConditionT & condition, - DurationT timeout = DurationT(-1)) -{ - return rclcpp::spin_until_complete(node_ptr->get_node_base_interface(), condition, timeout); -} - template -[[deprecated( - "use spin_until_complete(node_interfaces::NodeBaseInterface::SharedPtr, " - "const ConditionT &,DurationT) instead" -)]] rclcpp::FutureReturnCode spin_until_future_complete( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, @@ -181,22 +108,18 @@ spin_until_future_complete( std::chrono::duration timeout = std::chrono::duration(-1)) { rclcpp::executors::SingleThreadedExecutor executor; - return executors::spin_node_until_complete(executor, node_ptr, future, timeout); + return executors::spin_node_until_future_complete(executor, node_ptr, future, timeout); } template -[[deprecated( - "use spin_until_complete(std::shared_ptr, const ConditionT &, " - "DurationT) instead" -)]] rclcpp::FutureReturnCode spin_until_future_complete( std::shared_ptr node_ptr, const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { - return rclcpp::spin_until_complete(node_ptr->get_node_base_interface(), future, timeout); + return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout); } } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/future_return_code.hpp b/rclcpp/include/rclcpp/future_return_code.hpp index 505f68431b..0da67d7f7b 100644 --- a/rclcpp/include/rclcpp/future_return_code.hpp +++ b/rclcpp/include/rclcpp/future_return_code.hpp @@ -23,7 +23,7 @@ namespace rclcpp { -/// Return codes to be used with spin_until_complete. +/// Return codes to be used with spin_until_future_complete. /** * SUCCESS: The future is complete and can be accessed with "get" without blocking. * This does not indicate that the operation succeeded; "get" may still throw an exception. diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 380a766f5d..f1d751ff3f 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -68,7 +68,7 @@ * - Executors (responsible for execution of callbacks through a blocking spin): * - rclcpp::spin() * - rclcpp::spin_some() - * - rclcpp::spin_until_complete() + * - rclcpp::spin_until_future_complete() * - rclcpp::executors::SingleThreadedExecutor * - rclcpp::executors::SingleThreadedExecutor::add_node() * - rclcpp::executors::SingleThreadedExecutor::spin() diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index c8c4d50f35..38ced0e1a5 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -396,9 +396,9 @@ SyncParametersClient::get_parameters( std::chrono::nanoseconds timeout) { auto f = async_parameters_client_->get_parameters(parameter_names); - using rclcpp::executors::spin_node_until_complete; + using rclcpp::executors::spin_node_until_future_complete; if ( - spin_node_until_complete( + spin_node_until_future_complete( *executor_, node_base_interface_, f, timeout) == rclcpp::FutureReturnCode::SUCCESS) { @@ -424,9 +424,9 @@ SyncParametersClient::describe_parameters( { auto f = async_parameters_client_->describe_parameters(parameter_names); - using rclcpp::executors::spin_node_until_complete; + using rclcpp::executors::spin_node_until_future_complete; rclcpp::FutureReturnCode future = - spin_node_until_complete(*executor_, node_base_interface_, f, timeout); + spin_node_until_future_complete(*executor_, node_base_interface_, f, timeout); if (future == rclcpp::FutureReturnCode::SUCCESS) { return f.get(); } @@ -440,9 +440,9 @@ SyncParametersClient::get_parameter_types( { auto f = async_parameters_client_->get_parameter_types(parameter_names); - using rclcpp::executors::spin_node_until_complete; + using rclcpp::executors::spin_node_until_future_complete; if ( - spin_node_until_complete( + spin_node_until_future_complete( *executor_, node_base_interface_, f, timeout) == rclcpp::FutureReturnCode::SUCCESS) { @@ -458,9 +458,9 @@ SyncParametersClient::set_parameters( { auto f = async_parameters_client_->set_parameters(parameters); - using rclcpp::executors::spin_node_until_complete; + using rclcpp::executors::spin_node_until_future_complete; if ( - spin_node_until_complete( + spin_node_until_future_complete( *executor_, node_base_interface_, f, timeout) == rclcpp::FutureReturnCode::SUCCESS) { @@ -476,9 +476,9 @@ SyncParametersClient::delete_parameters( { auto f = async_parameters_client_->delete_parameters(parameters_names); - using rclcpp::executors::spin_node_until_complete; + using rclcpp::executors::spin_node_until_future_complete; if ( - spin_node_until_complete( + spin_node_until_future_complete( *executor_, node_base_interface_, f, timeout) == rclcpp::FutureReturnCode::SUCCESS) { @@ -494,9 +494,9 @@ SyncParametersClient::load_parameters( { auto f = async_parameters_client_->load_parameters(yaml_filename); - using rclcpp::executors::spin_node_until_complete; + using rclcpp::executors::spin_node_until_future_complete; if ( - spin_node_until_complete( + spin_node_until_future_complete( *executor_, node_base_interface_, f, timeout) == rclcpp::FutureReturnCode::SUCCESS) { @@ -512,9 +512,9 @@ SyncParametersClient::set_parameters_atomically( { auto f = async_parameters_client_->set_parameters_atomically(parameters); - using rclcpp::executors::spin_node_until_complete; + using rclcpp::executors::spin_node_until_future_complete; if ( - spin_node_until_complete( + spin_node_until_future_complete( *executor_, node_base_interface_, f, timeout) == rclcpp::FutureReturnCode::SUCCESS) { @@ -532,9 +532,9 @@ SyncParametersClient::list_parameters( { auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth); - using rclcpp::executors::spin_node_until_complete; + using rclcpp::executors::spin_node_until_future_complete; if ( - spin_node_until_complete( + spin_node_until_future_complete( *executor_, node_base_interface_, f, timeout) == rclcpp::FutureReturnCode::SUCCESS) { diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 21c42530a6..42a9208251 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -384,7 +384,7 @@ class TimeSource::NodeState final [this]() { auto future = cancel_clock_executor_promise_.get_future(); clock_executor_->add_callback_group(clock_callback_group_, node_base_); - clock_executor_->spin_until_complete(future); + clock_executor_->spin_until_future_complete(future); } ); } diff --git a/rclcpp/test/benchmark/benchmark_client.cpp b/rclcpp/test/benchmark/benchmark_client.cpp index 2a964804e3..26ee58b633 100644 --- a/rclcpp/test/benchmark/benchmark_client.cpp +++ b/rclcpp/test/benchmark/benchmark_client.cpp @@ -151,7 +151,7 @@ BENCHMARK_F(ClientPerformanceTest, async_send_request_and_response)(benchmark::S for (auto _ : state) { (void)_; auto future = client->async_send_request(shared_request); - rclcpp::spin_until_complete( + rclcpp::spin_until_future_complete( node->get_node_base_interface(), future, std::chrono::seconds(1)); benchmark::DoNotOptimize(future); benchmark::ClobberMemory(); diff --git a/rclcpp/test/benchmark/benchmark_executor.cpp b/rclcpp/test/benchmark/benchmark_executor.cpp index 062f02da60..652007b589 100644 --- a/rclcpp/test/benchmark/benchmark_executor.cpp +++ b/rclcpp/test/benchmark/benchmark_executor.cpp @@ -219,7 +219,7 @@ BENCHMARK_F( BENCHMARK_F( PerformanceTestExecutorSimple, - static_single_thread_executor_spin_until_complete)(benchmark::State & st) + static_single_thread_executor_spin_until_future_complete)(benchmark::State & st) { rclcpp::executors::StaticSingleThreadedExecutor executor; // test success of an immediately finishing future @@ -228,7 +228,7 @@ BENCHMARK_F( promise.set_value(true); auto shared_future = future.share(); - auto ret = executor.spin_until_complete(shared_future, 100ms); + auto ret = executor.spin_until_future_complete(shared_future, 100ms); if (ret != rclcpp::FutureReturnCode::SUCCESS) { st.SkipWithError(rcutils_get_error_string().str); } @@ -243,7 +243,7 @@ BENCHMARK_F( executor.add_node(node); st.ResumeTiming(); - ret = executor.spin_until_complete(shared_future, 100ms); + ret = executor.spin_until_future_complete(shared_future, 100ms); if (ret != rclcpp::FutureReturnCode::SUCCESS) { st.SkipWithError(rcutils_get_error_string().str); break; @@ -256,7 +256,7 @@ BENCHMARK_F( BENCHMARK_F( PerformanceTestExecutorSimple, - single_thread_executor_spin_node_until_complete)(benchmark::State & st) + single_thread_executor_spin_node_until_future_complete)(benchmark::State & st) { rclcpp::executors::SingleThreadedExecutor executor; // test success of an immediately finishing future @@ -265,7 +265,7 @@ BENCHMARK_F( promise.set_value(true); auto shared_future = future.share(); - auto ret = rclcpp::executors::spin_node_until_complete( + auto ret = rclcpp::executors::spin_node_until_future_complete( executor, node, shared_future, 1s); if (ret != rclcpp::FutureReturnCode::SUCCESS) { st.SkipWithError(rcutils_get_error_string().str); @@ -275,7 +275,7 @@ BENCHMARK_F( for (auto _ : st) { (void)_; - ret = rclcpp::executors::spin_node_until_complete( + ret = rclcpp::executors::spin_node_until_future_complete( executor, node, shared_future, 1s); if (ret != rclcpp::FutureReturnCode::SUCCESS) { st.SkipWithError(rcutils_get_error_string().str); @@ -286,7 +286,7 @@ BENCHMARK_F( BENCHMARK_F( PerformanceTestExecutorSimple, - multi_thread_executor_spin_node_until_complete)(benchmark::State & st) + multi_thread_executor_spin_node_until_future_complete)(benchmark::State & st) { rclcpp::executors::MultiThreadedExecutor executor; // test success of an immediately finishing future @@ -295,7 +295,7 @@ BENCHMARK_F( promise.set_value(true); auto shared_future = future.share(); - auto ret = rclcpp::executors::spin_node_until_complete( + auto ret = rclcpp::executors::spin_node_until_future_complete( executor, node, shared_future, 1s); if (ret != rclcpp::FutureReturnCode::SUCCESS) { st.SkipWithError(rcutils_get_error_string().str); @@ -305,7 +305,7 @@ BENCHMARK_F( for (auto _ : st) { (void)_; - ret = rclcpp::executors::spin_node_until_complete( + ret = rclcpp::executors::spin_node_until_future_complete( executor, node, shared_future, 1s); if (ret != rclcpp::FutureReturnCode::SUCCESS) { st.SkipWithError(rcutils_get_error_string().str); @@ -316,7 +316,7 @@ BENCHMARK_F( BENCHMARK_F( PerformanceTestExecutorSimple, - static_single_thread_executor_spin_node_until_complete)(benchmark::State & st) + static_single_thread_executor_spin_node_until_future_complete)(benchmark::State & st) { rclcpp::executors::StaticSingleThreadedExecutor executor; // test success of an immediately finishing future @@ -329,7 +329,7 @@ BENCHMARK_F( for (auto _ : st) { (void)_; - auto ret = rclcpp::executors::spin_node_until_complete( + auto ret = rclcpp::executors::spin_node_until_future_complete( executor, node, shared_future, 1s); if (ret != rclcpp::FutureReturnCode::SUCCESS) { st.SkipWithError(rcutils_get_error_string().str); @@ -338,7 +338,7 @@ BENCHMARK_F( } } -BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_complete)(benchmark::State & st) +BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark::State & st) { // test success of an immediately finishing future std::promise promise; @@ -346,7 +346,7 @@ BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_complete)(benchmark::State promise.set_value(true); auto shared_future = future.share(); - auto ret = rclcpp::spin_until_complete(node, shared_future, 1s); + auto ret = rclcpp::spin_until_future_complete(node, shared_future, 1s); if (ret != rclcpp::FutureReturnCode::SUCCESS) { st.SkipWithError(rcutils_get_error_string().str); } @@ -355,7 +355,7 @@ BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_complete)(benchmark::State for (auto _ : st) { (void)_; - ret = rclcpp::spin_until_complete(node, shared_future, 1s); + ret = rclcpp::spin_until_future_complete(node, shared_future, 1s); if (ret != rclcpp::FutureReturnCode::SUCCESS) { st.SkipWithError(rcutils_get_error_string().str); break; diff --git a/rclcpp/test/benchmark/benchmark_service.cpp b/rclcpp/test/benchmark/benchmark_service.cpp index 43bc24f303..a42723da90 100644 --- a/rclcpp/test/benchmark/benchmark_service.cpp +++ b/rclcpp/test/benchmark/benchmark_service.cpp @@ -137,7 +137,7 @@ BENCHMARK_F(ServicePerformanceTest, async_send_response)(benchmark::State & stat benchmark::DoNotOptimize(service); benchmark::ClobberMemory(); - rclcpp::spin_until_complete(node->get_node_base_interface(), future); + rclcpp::spin_until_future_complete(node->get_node_base_interface(), future); } if (callback_count == 0) { state.SkipWithError("Service callback was not called"); diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 929446c21a..1fa2cbb4dd 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -222,7 +222,7 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { } // Check executor exits immediately if future is complete. -TYPED_TEST(TestExecutors, testSpinUntilCompleteFuture) { +TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) { using ExecutorType = TypeParam; ExecutorType executor; executor.add_node(this->node); @@ -232,30 +232,11 @@ TYPED_TEST(TestExecutors, testSpinUntilCompleteFuture) { std::future future = promise.get_future(); promise.set_value(true); - // spin_until_complete is expected to exit immediately, but would block up until its + // spin_until_future_complete is expected to exit immediately, but would block up until its // timeout if the future is not checked before spin_once_impl. auto start = std::chrono::steady_clock::now(); auto shared_future = future.share(); - auto ret = executor.spin_until_complete(shared_future, 1s); - executor.remove_node(this->node, true); - // Check it didn't reach timeout - EXPECT_GT(500ms, (std::chrono::steady_clock::now() - start)); - EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); -} - -// Check executor exits immediately if future is complete. -TYPED_TEST(TestExecutors, testSpinUntilCompleteCallable) { - using ExecutorType = TypeParam; - ExecutorType executor; - executor.add_node(this->node); - - // test success of an immediately completed condition - auto condition = []() {return true;}; - - // spin_until_complete is expected to exit immediately, but would block up until its - // timeout if the future is not checked before spin_once_impl. - auto start = std::chrono::steady_clock::now(); - auto ret = executor.spin_until_complete(condition, 1s); + auto ret = executor.spin_until_future_complete(shared_future, 1s); executor.remove_node(this->node, true); // Check it didn't reach timeout EXPECT_GT(500ms, (std::chrono::steady_clock::now() - start)); @@ -263,7 +244,7 @@ TYPED_TEST(TestExecutors, testSpinUntilCompleteCallable) { } // Same test, but uses a shared future. -TYPED_TEST(TestExecutors, testSpinUntilCompleteSharedFuture) { +TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) { using ExecutorType = TypeParam; ExecutorType executor; executor.add_node(this->node); @@ -273,11 +254,11 @@ TYPED_TEST(TestExecutors, testSpinUntilCompleteSharedFuture) { std::future future = promise.get_future(); promise.set_value(true); - // spin_until_complete is expected to exit immediately, but would block up until its + // spin_until_future_complete is expected to exit immediately, but would block up until its // timeout if the future is not checked before spin_once_impl. auto shared_future = future.share(); auto start = std::chrono::steady_clock::now(); - auto ret = executor.spin_until_complete(shared_future, 1s); + auto ret = executor.spin_until_future_complete(shared_future, 1s); executor.remove_node(this->node, true); // Check it didn't reach timeout @@ -286,7 +267,7 @@ TYPED_TEST(TestExecutors, testSpinUntilCompleteSharedFuture) { } // For a longer running future that should require several iterations of spin_once -TYPED_TEST(TestExecutors, testSpinUntilCompleteNoTimeout) { +TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) { using ExecutorType = TypeParam; ExecutorType executor; executor.add_node(this->node); @@ -305,7 +286,7 @@ TYPED_TEST(TestExecutors, testSpinUntilCompleteNoTimeout) { // Timeout set to negative for no timeout. std::thread spinner([&]() { - auto ret = executor.spin_until_complete(future, -1s); + auto ret = executor.spin_until_future_complete(future, -1s); EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); executor.remove_node(this->node, true); executor.cancel(); @@ -331,15 +312,15 @@ TYPED_TEST(TestExecutors, testSpinUntilCompleteNoTimeout) { spinner.join(); } -// Check spin_until_complete timeout works as expected -TYPED_TEST(TestExecutors, testSpinUntilCompleteWithTimeout) { +// Check spin_until_future_complete timeout works as expected +TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) { using ExecutorType = TypeParam; ExecutorType executor; executor.add_node(this->node); bool spin_exited = false; - // Needs to run longer than spin_until_complete's timeout. + // Needs to run longer than spin_until_future_complete's timeout. std::future future = std::async( std::launch::async, [&spin_exited]() { @@ -351,7 +332,7 @@ TYPED_TEST(TestExecutors, testSpinUntilCompleteWithTimeout) { // Short timeout std::thread spinner([&]() { - auto ret = executor.spin_until_complete(future, 1ms); + auto ret = executor.spin_until_future_complete(future, 1ms); EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); executor.remove_node(this->node, true); spin_exited = true; @@ -501,8 +482,8 @@ TYPED_TEST(TestExecutors, spinSome) { spinner.join(); } -// Check spin_node_until_complete with node base pointer -TYPED_TEST(TestExecutors, testSpinNodeUntilCompleteNodeBasePtr) { +// Check spin_node_until_future_complete with node base pointer +TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) { using ExecutorType = TypeParam; ExecutorType executor; @@ -511,13 +492,13 @@ TYPED_TEST(TestExecutors, testSpinNodeUntilCompleteNodeBasePtr) { promise.set_value(true); auto shared_future = future.share(); - auto ret = rclcpp::executors::spin_node_until_complete( + auto ret = rclcpp::executors::spin_node_until_future_complete( executor, this->node->get_node_base_interface(), shared_future, 1s); EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); } -// Check spin_node_until_complete with node pointer -TYPED_TEST(TestExecutors, testSpinNodeUntilCompleteNodePtr) { +// Check spin_node_until_future_complete with node pointer +TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodePtr) { using ExecutorType = TypeParam; ExecutorType executor; @@ -526,13 +507,13 @@ TYPED_TEST(TestExecutors, testSpinNodeUntilCompleteNodePtr) { promise.set_value(true); auto shared_future = future.share(); - auto ret = rclcpp::executors::spin_node_until_complete( + auto ret = rclcpp::executors::spin_node_until_future_complete( executor, this->node, shared_future, 1s); EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); } -// Check spin_until_complete can be properly interrupted. -TYPED_TEST(TestExecutors, testSpinUntilCompleteInterrupted) { +// Check spin_until_future_complete can be properly interrupted. +TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) { using ExecutorType = TypeParam; ExecutorType executor; executor.add_node(this->node); @@ -540,7 +521,7 @@ TYPED_TEST(TestExecutors, testSpinUntilCompleteInterrupted) { bool spin_exited = false; // This needs to block longer than it takes to get to the shutdown call below and for - // spin_until_complete to return + // spin_until_future_complete to return std::future future = std::async( std::launch::async, [&spin_exited]() { @@ -552,7 +533,7 @@ TYPED_TEST(TestExecutors, testSpinUntilCompleteInterrupted) { // Long timeout std::thread spinner([&spin_exited, &executor, &future]() { - auto ret = executor.spin_until_complete(future, 1s); + auto ret = executor.spin_until_future_complete(future, 1s); EXPECT_EQ(rclcpp::FutureReturnCode::INTERRUPTED, ret); spin_exited = true; }); @@ -574,8 +555,8 @@ TYPED_TEST(TestExecutors, testSpinUntilCompleteInterrupted) { spinner.join(); } -// Check spin_until_complete with node base pointer (instantiates its own executor) -TEST(TestExecutors, testSpinUntilCompleteNodeBasePtr) { +// Check spin_until_future_complete with node base pointer (instantiates its own executor) +TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) { rclcpp::init(0, nullptr); { @@ -586,7 +567,7 @@ TEST(TestExecutors, testSpinUntilCompleteNodeBasePtr) { promise.set_value(true); auto shared_future = future.share(); - auto ret = rclcpp::spin_until_complete( + auto ret = rclcpp::spin_until_future_complete( node->get_node_base_interface(), shared_future, 1s); EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); } @@ -594,8 +575,8 @@ TEST(TestExecutors, testSpinUntilCompleteNodeBasePtr) { rclcpp::shutdown(); } -// Check spin_until_complete with node pointer (instantiates its own executor) -TEST(TestExecutors, testSpinUntilCompleteNodePtr) { +// Check spin_until_future_complete with node pointer (instantiates its own executor) +TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) { rclcpp::init(0, nullptr); { @@ -606,7 +587,7 @@ TEST(TestExecutors, testSpinUntilCompleteNodePtr) { promise.set_value(true); auto shared_future = future.share(); - auto ret = rclcpp::spin_until_complete(node, shared_future, 1s); + auto ret = rclcpp::spin_until_future_complete(node, shared_future, 1s); EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); } diff --git a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp index 4e440f7a5c..5ca6c1c25a 100644 --- a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp @@ -135,8 +135,8 @@ TEST_F(TestStaticSingleThreadedExecutor, execute_service) { std::future future = promise.get_future(); EXPECT_EQ( rclcpp::FutureReturnCode::TIMEOUT, - executor.spin_until_complete(future, std::chrono::milliseconds(1))); + executor.spin_until_future_complete(future, std::chrono::milliseconds(1))); executor.remove_node(node); - executor.spin_until_complete(future, std::chrono::milliseconds(1)); + executor.spin_until_future_complete(future, std::chrono::milliseconds(1)); } diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index 8796f4ea4d..bdbb0a1079 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -294,39 +294,6 @@ TEST_F(TestExecutor, spin_some_elapsed) { ASSERT_TRUE(timer_called); } -TEST_F(TestExecutor, spin_for_duration) -{ - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - bool timer_called = false; - auto timer = - node->create_wall_timer( - std::chrono::milliseconds(0), [&]() { - timer_called = true; - }); - dummy.add_node(node); - // Wait for the wall timer to have expired. - dummy.spin_for(std::chrono::milliseconds(0)); - - ASSERT_TRUE(timer_called); -} - -TEST_F(TestExecutor, spin_for_longer_timer) -{ - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - bool timer_called = false; - auto timer = - node->create_wall_timer( - std::chrono::seconds(10), [&]() { - timer_called = true; - }); - dummy.add_node(node); - dummy.spin_for(std::chrono::milliseconds(5)); - - ASSERT_FALSE(timer_called); -} - TEST_F(TestExecutor, spin_once_in_spin_once) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); @@ -515,20 +482,22 @@ TEST_F(TestExecutor, get_group_by_timer_add_callback_group) { ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get()); } -TEST_F(TestExecutor, spin_until_complete_in_spin_until_complete) { +TEST_F(TestExecutor, spin_until_future_complete_in_spin_until_future_complete) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); - bool spin_until_complete_in_spin_until_complete = false; + bool spin_until_future_complete_in_spin_until_future_complete = false; auto timer = node->create_wall_timer( std::chrono::milliseconds(1), [&]() { try { - dummy.spin_for(std::chrono::milliseconds(1)); + std::promise promise; + std::future future = promise.get_future(); + dummy.spin_until_future_complete(future, std::chrono::milliseconds(1)); } catch (const std::runtime_error & err) { if (err.what() == std::string( - "spin_until_complete() called while already spinning")) + "spin_until_future_complete() called while already spinning")) { - spin_until_complete_in_spin_until_complete = true; + spin_until_future_complete_in_spin_until_future_complete = true; } } }); @@ -536,9 +505,11 @@ TEST_F(TestExecutor, spin_until_complete_in_spin_until_complete) { dummy.add_node(node); // Wait for the wall timer to have expired. std::this_thread::sleep_for(std::chrono::milliseconds(50)); - EXPECT_FALSE(spin_until_complete_in_spin_until_complete); - dummy.spin_for(std::chrono::milliseconds(2)); - EXPECT_TRUE(spin_until_complete_in_spin_until_complete); + EXPECT_FALSE(spin_until_future_complete_in_spin_until_future_complete); + std::promise promise; + std::future future = promise.get_future(); + dummy.spin_until_future_complete(future, std::chrono::milliseconds(1)); + EXPECT_TRUE(spin_until_future_complete_in_spin_until_future_complete); } TEST_F(TestExecutor, spin_node_once_base_interface) { @@ -575,7 +546,7 @@ TEST_F(TestExecutor, spin_node_once_node) { EXPECT_TRUE(spin_called); } -TEST_F(TestExecutor, spin_until_complete_condition_already_complete) { +TEST_F(TestExecutor, spin_until_future_complete_future_already_complete) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); std::promise promise; @@ -583,12 +554,7 @@ TEST_F(TestExecutor, spin_until_complete_condition_already_complete) { promise.set_value(); EXPECT_EQ( rclcpp::FutureReturnCode::SUCCESS, - dummy.spin_until_complete(future, std::chrono::milliseconds(1))); - - auto condition = []() {return true;}; - EXPECT_EQ( - rclcpp::FutureReturnCode::SUCCESS, - dummy.spin_until_complete(condition, std::chrono::milliseconds(1))); + dummy.spin_until_future_complete(future, std::chrono::milliseconds(1))); } TEST_F(TestExecutor, is_spinning) { diff --git a/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp b/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp index 35c43ef3d4..6d192ca86b 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp @@ -240,7 +240,7 @@ do_custom_allocator_test( EXPECT_NO_THROW( { publisher->publish(std::move(msg)); - executor.spin_until_complete(received_message_future, std::chrono::seconds(10)); + executor.spin_until_future_complete(received_message_future, std::chrono::seconds(10)); }); EXPECT_EQ(ptr, received_message_future.get().get()); EXPECT_EQ(1u, counter); @@ -249,7 +249,7 @@ do_custom_allocator_test( EXPECT_THROW( { publisher->publish(std::move(msg)); - executor.spin_until_complete(received_message_future, std::chrono::seconds(10)); + executor.spin_until_future_complete(received_message_future, std::chrono::seconds(10)); }, ExpectedExceptionT); } } diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index cf69dd8ded..4cd9e671be 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -212,7 +212,7 @@ TEST_F(TestParameterClient, async_parameter_get_parameter_types) { std::vector names{"foo"}; std::shared_future> future = asynchronous_client->get_parameter_types(names, callback); - auto return_code = rclcpp::spin_until_complete( + auto return_code = rclcpp::spin_until_future_complete( node, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); ASSERT_TRUE(callback_called); @@ -236,7 +236,7 @@ TEST_F(TestParameterClient, async_parameter_get_parameter_types_allow_undeclared std::vector names{"foo"}; std::shared_future> future = asynchronous_client->get_parameter_types(names, callback); - auto return_code = rclcpp::spin_until_complete( + auto return_code = rclcpp::spin_until_future_complete( node_with_option, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); ASSERT_TRUE(callback_called); @@ -258,7 +258,7 @@ TEST_F(TestParameterClient, async_parameter_get_parameters) { std::vector names{"foo"}; std::shared_future> future = asynchronous_client->get_parameters( names, callback); - auto return_code = rclcpp::spin_until_complete( + auto return_code = rclcpp::spin_until_future_complete( node, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); ASSERT_TRUE(callback_called); @@ -280,7 +280,7 @@ TEST_F(TestParameterClient, async_parameter_get_parameters_allow_undeclared) { std::vector names{"foo"}; std::shared_future> future = asynchronous_client->get_parameters( names, callback); - auto return_code = rclcpp::spin_until_complete( + auto return_code = rclcpp::spin_until_future_complete( node_with_option, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); ASSERT_TRUE(callback_called); @@ -306,7 +306,7 @@ TEST_F(TestParameterClient, async_parameter_set_parameters_atomically) { parameters.emplace_back(rclcpp::Parameter("foo")); std::shared_future future = asynchronous_client->set_parameters_atomically(parameters, callback); - auto return_code = rclcpp::spin_until_complete( + auto return_code = rclcpp::spin_until_future_complete( node, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); ASSERT_TRUE(callback_called); @@ -328,7 +328,7 @@ TEST_F(TestParameterClient, async_parameter_list_parameters) { std::vector prefixes{"foo"}; std::shared_future future = asynchronous_client->list_parameters(prefixes, 0, callback); - auto return_code = rclcpp::spin_until_complete( + auto return_code = rclcpp::spin_until_future_complete( node, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); ASSERT_TRUE(callback_called); @@ -553,7 +553,7 @@ TEST_F(TestParameterClient, async_parameter_describe_parameters) { std::vector names{"none"}; std::shared_future> future = asynchronous_client->describe_parameters(names, callback); - auto return_code = rclcpp::spin_until_complete( + auto return_code = rclcpp::spin_until_future_complete( node, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); ASSERT_TRUE(callback_called); @@ -571,7 +571,7 @@ TEST_F(TestParameterClient, async_parameter_describe_parameters) { std::vector names{"foo"}; std::shared_future> future = asynchronous_client->describe_parameters(names, callback); - auto return_code = rclcpp::spin_until_complete( + auto return_code = rclcpp::spin_until_future_complete( node, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); std::vector parameter_descs = future.get(); @@ -597,7 +597,7 @@ TEST_F(TestParameterClient, async_parameter_describe_parameters) { std::vector names{"foo", "baz"}; std::shared_future> future = asynchronous_client->describe_parameters(names, callback); - auto return_code = rclcpp::spin_until_complete( + auto return_code = rclcpp::spin_until_future_complete( node, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); ASSERT_TRUE(callback_called); @@ -616,7 +616,7 @@ TEST_F(TestParameterClient, async_parameter_describe_parameters) { std::vector names{"baz", "foo"}; std::shared_future> future = asynchronous_client->describe_parameters(names, callback); - auto return_code = rclcpp::spin_until_complete( + auto return_code = rclcpp::spin_until_future_complete( node, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); ASSERT_TRUE(callback_called); @@ -634,7 +634,7 @@ TEST_F(TestParameterClient, async_parameter_describe_parameters) { std::vector names{"foo", "bar"}; std::shared_future> future = asynchronous_client->describe_parameters(names, callback); - auto return_code = rclcpp::spin_until_complete( + auto return_code = rclcpp::spin_until_future_complete( node, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); std::vector parameter_descs = future.get(); @@ -733,7 +733,7 @@ TEST_F(TestParameterClient, async_parameter_describe_parameters_allow_undeclared std::vector names{"none"}; std::shared_future> future = asynchronous_client->describe_parameters(names, callback); - auto return_code = rclcpp::spin_until_complete( + auto return_code = rclcpp::spin_until_future_complete( node_with_option, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); std::vector parameter_descs = future.get(); @@ -758,7 +758,7 @@ TEST_F(TestParameterClient, async_parameter_describe_parameters_allow_undeclared std::vector names{"foo", "baz"}; std::shared_future> future = asynchronous_client->describe_parameters(names, callback); - auto return_code = rclcpp::spin_until_complete( + auto return_code = rclcpp::spin_until_future_complete( node_with_option, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); std::vector parameter_descs = future.get(); @@ -788,7 +788,7 @@ TEST_F(TestParameterClient, async_parameter_describe_parameters_allow_undeclared std::vector names{"baz", "foo"}; std::shared_future> future = asynchronous_client->describe_parameters(names, callback); - auto return_code = rclcpp::spin_until_complete( + auto return_code = rclcpp::spin_until_future_complete( node_with_option, future, std::chrono::milliseconds(100)); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); std::vector parameter_descs = future.get(); @@ -870,17 +870,17 @@ TEST_F(TestParameterClient, async_parameter_delete_parameters) { std::make_shared(node_with_option); // set parameter auto set_future = asynchronous_client->set_parameters({rclcpp::Parameter("foo", 4)}); - rclcpp::spin_until_complete( + rclcpp::spin_until_future_complete( node_with_option, set_future, std::chrono::milliseconds(100)); ASSERT_EQ(set_future.get()[0].successful, true); // delete one parameter auto delete_future = asynchronous_client->delete_parameters({"foo"}); - rclcpp::spin_until_complete( + rclcpp::spin_until_future_complete( node_with_option, delete_future, std::chrono::milliseconds(100)); ASSERT_EQ(delete_future.get()[0].successful, true); // check that deleted parameter isn't set auto get_future2 = asynchronous_client->get_parameters({"foo"}); - rclcpp::spin_until_complete( + rclcpp::spin_until_future_complete( node_with_option, get_future2, std::chrono::milliseconds(100)); ASSERT_EQ( get_future2.get()[0].get_type(), @@ -918,13 +918,13 @@ TEST_F(TestParameterClient, async_parameter_load_parameters) { const std::string parameters_filepath = ( test_resources_path / "test_node" / "load_parameters.yaml").string(); auto load_future = asynchronous_client->load_parameters(parameters_filepath); - auto result_code = rclcpp::spin_until_complete( + auto result_code = rclcpp::spin_until_future_complete( load_node, load_future, std::chrono::milliseconds(100)); ASSERT_EQ(result_code, rclcpp::FutureReturnCode::SUCCESS); ASSERT_EQ(load_future.get()[0].successful, true); // list parameters auto list_parameters = asynchronous_client->list_parameters({}, 3); - rclcpp::spin_until_complete( + rclcpp::spin_until_future_complete( load_node, list_parameters, std::chrono::milliseconds(100)); ASSERT_EQ(list_parameters.get().names.size(), static_cast(5)); } diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index 26f47c0436..221e2bdf0a 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -201,7 +201,7 @@ TEST_F(TestQosEvent, test_default_incompatible_qos_callbacks) // This future won't complete on fastrtps, so just timeout immediately const auto timeout = std::chrono::seconds(10); - ex.spin_until_complete(log_msgs_future, timeout); + ex.spin_until_future_complete(log_msgs_future, timeout); EXPECT_EQ( "New subscription discovered on topic '/ns/test_topic', requesting incompatible QoS. " diff --git a/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp index 99bf9e4a10..ce6887c631 100644 --- a/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp @@ -432,7 +432,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no ex.add_node(empty_subscriber); // Spin and get future - ex.spin_until_complete( + ex.spin_until_future_complete( statistics_listener->GetFuture(), kTestTimeout); @@ -497,7 +497,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi ex.add_node(msg_with_header_subscriber); // Spin and get future - ex.spin_until_complete( + ex.spin_until_future_complete( statistics_listener->GetFuture(), kTestTimeout); @@ -550,7 +550,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_include_window ex.add_node(msg_with_header_subscriber); // Spin and get future - ex.spin_until_complete(statistics_listener->GetFuture(), kTestTimeout); + ex.spin_until_future_complete(statistics_listener->GetFuture(), kTestTimeout); const auto received_messages = statistics_listener->GetReceivedMessages(); EXPECT_EQ(kNumExpectedMessages, received_messages.size()); diff --git a/rclcpp_action/test/benchmark/benchmark_action_client.cpp b/rclcpp_action/test/benchmark/benchmark_action_client.cpp index c3d85d4c0c..8b11935117 100644 --- a/rclcpp_action/test/benchmark/benchmark_action_client.cpp +++ b/rclcpp_action/test/benchmark/benchmark_action_client.cpp @@ -194,7 +194,7 @@ BENCHMARK_F(ActionClientPerformanceTest, async_send_goal_rejected)(benchmark::St for (auto _ : state) { (void)_; auto future_goal_handle = client->async_send_goal(goal); - rclcpp::spin_until_complete(node, future_goal_handle, std::chrono::seconds(1)); + rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1)); if (!future_goal_handle.valid()) { state.SkipWithError("Shared future was invalid"); return; @@ -223,7 +223,7 @@ BENCHMARK_F(ActionClientPerformanceTest, async_send_goal_get_accepted_response)( (void)_; // This server's execution is deferred auto future_goal_handle = client->async_send_goal(goal); - rclcpp::spin_until_complete(node, future_goal_handle, std::chrono::seconds(1)); + rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1)); if (!future_goal_handle.valid()) { state.SkipWithError("Shared future was invalid"); @@ -258,7 +258,7 @@ BENCHMARK_F(ActionClientPerformanceTest, async_get_result)(benchmark::State & st auto future_goal_handle = client->async_send_goal(goal); // Action server accepts and defers, so this spin doesn't include result - rclcpp::spin_until_complete(node, future_goal_handle, std::chrono::seconds(1)); + rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1)); if (!future_goal_handle.valid()) { state.SkipWithError("Shared future was invalid"); @@ -276,7 +276,7 @@ BENCHMARK_F(ActionClientPerformanceTest, async_get_result)(benchmark::State & st // Measure how long it takes client to receive the succeeded result auto future_result = client->async_get_result(goal_handle); - rclcpp::spin_until_complete(node, future_result, std::chrono::seconds(1)); + rclcpp::spin_until_future_complete(node, future_result, std::chrono::seconds(1)); const auto & wrapped_result = future_result.get(); if (rclcpp_action::ResultCode::SUCCEEDED != wrapped_result.code) { state.SkipWithError("Fibonacci action did not succeed"); @@ -310,12 +310,12 @@ BENCHMARK_F(ActionClientPerformanceTest, async_cancel_goal)(benchmark::State & s auto future_goal_handle = client->async_send_goal(goal); // Action server accepts and defers, so action can be canceled - rclcpp::spin_until_complete(node, future_goal_handle, std::chrono::seconds(1)); + rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1)); auto goal_handle = future_goal_handle.get(); state.ResumeTiming(); auto future_cancel = client->async_cancel_goal(goal_handle); - rclcpp::spin_until_complete(node, future_cancel, std::chrono::seconds(1)); + rclcpp::spin_until_future_complete(node, future_cancel, std::chrono::seconds(1)); auto cancel_response = future_cancel.get(); using CancelActionResponse = test_msgs::action::Fibonacci::Impl::CancelGoalService::Response; @@ -345,13 +345,13 @@ BENCHMARK_F(ActionClientPerformanceTest, async_cancel_all_goals)(benchmark::Stat state.PauseTiming(); for (int i = 0; i < num_concurrently_inflight_goals; ++i) { auto future_goal_handle = client->async_send_goal(goal); - rclcpp::spin_until_complete(node, future_goal_handle, std::chrono::seconds(1)); + rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1)); } // Action server accepts and defers, so action can be canceled state.ResumeTiming(); auto future_cancel_all = client->async_cancel_all_goals(); - rclcpp::spin_until_complete(node, future_cancel_all, std::chrono::seconds(1)); + rclcpp::spin_until_future_complete(node, future_cancel_all, std::chrono::seconds(1)); auto cancel_response = future_cancel_all.get(); using CancelActionResponse = test_msgs::action::Fibonacci::Impl::CancelGoalService::Response; diff --git a/rclcpp_action/test/benchmark/benchmark_action_server.cpp b/rclcpp_action/test/benchmark/benchmark_action_server.cpp index d0eb86c749..6817f86b14 100644 --- a/rclcpp_action/test/benchmark/benchmark_action_server.cpp +++ b/rclcpp_action/test/benchmark/benchmark_action_server.cpp @@ -152,7 +152,7 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_accept_goal)(benchmark::S auto client_goal_handle_future = AsyncSendGoalOfOrder(1); state.ResumeTiming(); - rclcpp::spin_until_complete(node, client_goal_handle_future); + rclcpp::spin_until_future_complete(node, client_goal_handle_future); auto goal_handle = client_goal_handle_future.get(); if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) { state.SkipWithError("Valid goal was not accepted"); @@ -185,12 +185,12 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_cancel_goal)(benchmark::S auto client_goal_handle_future = AsyncSendGoalOfOrder(1); // This spin completes when the goal has been accepted, but not executed because server // responds with ACCEPT_AND_DEFER - rclcpp::spin_until_complete(node, client_goal_handle_future, std::chrono::seconds(1)); + rclcpp::spin_until_future_complete(node, client_goal_handle_future, std::chrono::seconds(1)); auto client_goal_handle = client_goal_handle_future.get(); auto future_cancel = action_client->async_cancel_goal(client_goal_handle); state.ResumeTiming(); - rclcpp::spin_until_complete(node, future_cancel, std::chrono::seconds(1)); + rclcpp::spin_until_future_complete(node, future_cancel, std::chrono::seconds(1)); auto cancel_response = future_cancel.get(); using CancelActionResponse = test_msgs::action::Fibonacci::Impl::CancelGoalService::Response; if (CancelActionResponse::ERROR_NONE != cancel_response->return_code) { @@ -221,7 +221,7 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_execute_goal)(benchmark:: state.PauseTiming(); auto client_goal_handle_future = AsyncSendGoalOfOrder(1); - rclcpp::spin_until_complete(node, client_goal_handle_future); + rclcpp::spin_until_future_complete(node, client_goal_handle_future); auto goal_handle = client_goal_handle_future.get(); if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) { state.SkipWithError("Valid goal was not accepted"); @@ -267,7 +267,7 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_set_success)(benchmark::S state.PauseTiming(); auto client_goal_handle_future = AsyncSendGoalOfOrder(goal_order); - rclcpp::spin_until_complete(node, client_goal_handle_future); + rclcpp::spin_until_future_complete(node, client_goal_handle_future); auto goal_handle = client_goal_handle_future.get(); if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) { state.SkipWithError("Valid goal was not accepted"); @@ -312,7 +312,7 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_abort)(benchmark::State & state.PauseTiming(); auto client_goal_handle_future = AsyncSendGoalOfOrder(goal_order); - rclcpp::spin_until_complete(node, client_goal_handle_future); + rclcpp::spin_until_future_complete(node, client_goal_handle_future); auto goal_handle = client_goal_handle_future.get(); if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) { state.SkipWithError("Valid goal was not accepted"); diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index cf32abea70..b94a82d500 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -247,7 +247,7 @@ class TestClient : public ::testing::Test } template - void dual_spin_until_complete(std::shared_future & future) + void dual_spin_until_future_complete(std::shared_future & future) { std::future_status status; do { @@ -416,13 +416,13 @@ TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks) ActionGoal bad_goal; bad_goal.order = -5; auto future_goal_handle = action_client->async_send_goal(bad_goal); - dual_spin_until_complete(future_goal_handle); + dual_spin_until_future_complete(future_goal_handle); EXPECT_EQ(nullptr, future_goal_handle.get().get()); ActionGoal good_goal; good_goal.order = 5; future_goal_handle = action_client->async_send_goal(good_goal); - dual_spin_until_complete(future_goal_handle); + dual_spin_until_future_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); EXPECT_FALSE(goal_handle->is_feedback_aware()); @@ -437,7 +437,7 @@ TEST_F(TestClientAgainstServer, bad_goal_handles) ActionGoal goal; goal.order = 0; auto future_goal_handle = action_client0->async_send_goal(goal); - dual_spin_until_complete(future_goal_handle); + dual_spin_until_future_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); auto action_client1 = rclcpp_action::create_client(client_node, action_name); @@ -454,14 +454,14 @@ TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks_wait_for_result) ActionGoal goal; goal.order = 5; auto future_goal_handle = action_client->async_send_goal(goal); - dual_spin_until_complete(future_goal_handle); + dual_spin_until_future_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); EXPECT_FALSE(goal_handle->is_feedback_aware()); EXPECT_FALSE(goal_handle->is_result_aware()); auto future_result = action_client->async_get_result(goal_handle); EXPECT_TRUE(goal_handle->is_result_aware()); - dual_spin_until_complete(future_result); + dual_spin_until_future_complete(future_result); auto wrapped_result = future_result.get(); ASSERT_EQ(6ul, wrapped_result.result->sequence.size()); EXPECT_EQ(0, wrapped_result.result->sequence[0]); @@ -477,7 +477,7 @@ TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks_then_invalidate) ActionGoal goal; goal.order = 5; auto future_goal_handle = action_client->async_send_goal(goal); - dual_spin_until_complete(future_goal_handle); + dual_spin_until_future_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); ASSERT_NE(nullptr, goal_handle); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); @@ -511,7 +511,7 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait ActionGoal bad_goal; bad_goal.order = -1; auto future_goal_handle = action_client->async_send_goal(bad_goal, send_goal_ops); - dual_spin_until_complete(future_goal_handle); + dual_spin_until_future_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_FALSE(goal_response_received); EXPECT_EQ(nullptr, goal_handle); @@ -521,7 +521,7 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait ActionGoal goal; goal.order = 4; auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); - dual_spin_until_complete(future_goal_handle); + dual_spin_until_future_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_TRUE(goal_response_received); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); @@ -529,7 +529,7 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait EXPECT_FALSE(goal_handle->is_result_aware()); auto future_result = action_client->async_get_result(goal_handle); EXPECT_TRUE(goal_handle->is_result_aware()); - dual_spin_until_complete(future_result); + dual_spin_until_future_complete(future_result); auto wrapped_result = future_result.get(); ASSERT_EQ(5u, wrapped_result.result->sequence.size()); EXPECT_EQ(3, wrapped_result.result->sequence.back()); @@ -555,14 +555,14 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_feedback_callback_wait_for_ feedback_count++; }; auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); - dual_spin_until_complete(future_goal_handle); + dual_spin_until_future_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); EXPECT_TRUE(goal_handle->is_feedback_aware()); EXPECT_FALSE(goal_handle->is_result_aware()); auto future_result = action_client->async_get_result(goal_handle); EXPECT_TRUE(goal_handle->is_result_aware()); - dual_spin_until_complete(future_result); + dual_spin_until_future_complete(future_result); auto wrapped_result = future_result.get(); ASSERT_EQ(5u, wrapped_result.result->sequence.size()); @@ -591,13 +591,13 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_result_callback_wait_for_re } }; auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); - dual_spin_until_complete(future_goal_handle); + dual_spin_until_future_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); EXPECT_FALSE(goal_handle->is_feedback_aware()); EXPECT_TRUE(goal_handle->is_result_aware()); auto future_result = action_client->async_get_result(goal_handle); - dual_spin_until_complete(future_result); + dual_spin_until_future_complete(future_result); auto wrapped_result = future_result.get(); EXPECT_TRUE(result_callback_received); @@ -613,7 +613,7 @@ TEST_F(TestClientAgainstServer, async_get_result_with_callback) ActionGoal goal; goal.order = 4; auto future_goal_handle = action_client->async_send_goal(goal); - dual_spin_until_complete(future_goal_handle); + dual_spin_until_future_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_NE(goal_handle, nullptr); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); @@ -632,7 +632,7 @@ TEST_F(TestClientAgainstServer, async_get_result_with_callback) result_callback_received = true; } }); - dual_spin_until_complete(future_result); + dual_spin_until_future_complete(future_result); auto wrapped_result = future_result.get(); EXPECT_TRUE(result_callback_received); @@ -648,12 +648,12 @@ TEST_F(TestClientAgainstServer, async_cancel_one_goal) ActionGoal goal; goal.order = 5; auto future_goal_handle = action_client->async_send_goal(goal); - dual_spin_until_complete(future_goal_handle); + dual_spin_until_future_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); auto future_cancel = action_client->async_cancel_goal(goal_handle); - dual_spin_until_complete(future_cancel); + dual_spin_until_future_complete(future_cancel); ActionCancelGoalResponse::SharedPtr cancel_response = future_cancel.get(); EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); } @@ -666,7 +666,7 @@ TEST_F(TestClientAgainstServer, async_cancel_one_goal_with_callback) ActionGoal goal; goal.order = 5; auto future_goal_handle = action_client->async_send_goal(goal); - dual_spin_until_complete(future_goal_handle); + dual_spin_until_future_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); @@ -684,7 +684,7 @@ TEST_F(TestClientAgainstServer, async_cancel_one_goal_with_callback) cancel_response_received = true; } }); - dual_spin_until_complete(future_cancel); + dual_spin_until_future_complete(future_cancel); auto cancel_response = future_cancel.get(); EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); ASSERT_EQ(1ul, cancel_response->goals_canceling.size()); @@ -700,14 +700,14 @@ TEST_F(TestClientAgainstServer, async_cancel_all_goals) ActionGoal goal; goal.order = 6; auto future_goal_handle0 = action_client->async_send_goal(goal); - dual_spin_until_complete(future_goal_handle0); + dual_spin_until_future_complete(future_goal_handle0); auto goal_handle0 = future_goal_handle0.get(); ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); goal.order = 8; auto future_goal_handle1 = action_client->async_send_goal(goal); - dual_spin_until_complete(future_goal_handle1); + dual_spin_until_future_complete(future_goal_handle1); auto goal_handle1 = future_goal_handle1.get(); if (goal_handle1->get_goal_id() < goal_handle0->get_goal_id()) { @@ -717,7 +717,7 @@ TEST_F(TestClientAgainstServer, async_cancel_all_goals) ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3))); auto future_cancel_all = action_client->async_cancel_all_goals(); - dual_spin_until_complete(future_cancel_all); + dual_spin_until_future_complete(future_cancel_all); auto cancel_response = future_cancel_all.get(); EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); @@ -736,14 +736,14 @@ TEST_F(TestClientAgainstServer, async_cancel_all_goals_with_callback) ActionGoal goal; goal.order = 6; auto future_goal_handle0 = action_client->async_send_goal(goal); - dual_spin_until_complete(future_goal_handle0); + dual_spin_until_future_complete(future_goal_handle0); auto goal_handle0 = future_goal_handle0.get(); ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); goal.order = 8; auto future_goal_handle1 = action_client->async_send_goal(goal); - dual_spin_until_complete(future_goal_handle1); + dual_spin_until_future_complete(future_goal_handle1); auto goal_handle1 = future_goal_handle1.get(); if (goal_handle1->get_goal_id() < goal_handle0->get_goal_id()) { @@ -766,7 +766,7 @@ TEST_F(TestClientAgainstServer, async_cancel_all_goals_with_callback) cancel_callback_received = true; } }); - dual_spin_until_complete(future_cancel_all); + dual_spin_until_future_complete(future_cancel_all); auto cancel_response = future_cancel_all.get(); EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); @@ -786,21 +786,21 @@ TEST_F(TestClientAgainstServer, async_cancel_some_goals) ActionGoal goal; goal.order = 6; auto future_goal_handle0 = action_client->async_send_goal(goal); - dual_spin_until_complete(future_goal_handle0); + dual_spin_until_future_complete(future_goal_handle0); auto goal_handle0 = future_goal_handle0.get(); ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); goal.order = 8; auto future_goal_handle1 = action_client->async_send_goal(goal); - dual_spin_until_complete(future_goal_handle1); + dual_spin_until_future_complete(future_goal_handle1); auto goal_handle1 = future_goal_handle1.get(); ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3))); auto future_cancel_some = action_client->async_cancel_goals_before(goal_handle1->get_goal_stamp()); - dual_spin_until_complete(future_cancel_some); + dual_spin_until_future_complete(future_cancel_some); auto cancel_response = future_cancel_some.get(); EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); @@ -817,14 +817,14 @@ TEST_F(TestClientAgainstServer, async_cancel_some_goals_with_callback) ActionGoal goal; goal.order = 6; auto future_goal_handle0 = action_client->async_send_goal(goal); - dual_spin_until_complete(future_goal_handle0); + dual_spin_until_future_complete(future_goal_handle0); auto goal_handle0 = future_goal_handle0.get(); ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); goal.order = 8; auto future_goal_handle1 = action_client->async_send_goal(goal); - dual_spin_until_complete(future_goal_handle1); + dual_spin_until_future_complete(future_goal_handle1); auto goal_handle1 = future_goal_handle1.get(); ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3))); @@ -842,7 +842,7 @@ TEST_F(TestClientAgainstServer, async_cancel_some_goals_with_callback) cancel_callback_received = true; } }); - dual_spin_until_complete(future_cancel_some); + dual_spin_until_future_complete(future_cancel_some); auto cancel_response = future_cancel_some.get(); EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); @@ -877,7 +877,7 @@ TEST_F(TestClientAgainstServer, send_rcl_errors) auto mock = mocking_utils::patch_and_return( "lib:rclcpp_action", rcl_action_send_result_request, RCL_RET_ERROR); auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); - dual_spin_until_complete(future_goal_handle); + dual_spin_until_future_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_UNKNOWN, goal_handle->get_status()); } @@ -886,7 +886,7 @@ TEST_F(TestClientAgainstServer, send_rcl_errors) auto mock = mocking_utils::patch_and_return( "lib:rclcpp_action", rcl_action_send_cancel_request, RCL_RET_ERROR); auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); - dual_spin_until_complete(future_goal_handle); + dual_spin_until_future_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); EXPECT_THROW( action_client->async_cancel_goals_before(goal_handle->get_goal_stamp()), @@ -913,11 +913,11 @@ TEST_F(TestClientAgainstServer, execute_rcl_errors) "lib:rclcpp_action", rcl_action_take_feedback, RCL_RET_ERROR); auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); - dual_spin_until_complete(future_goal_handle); + dual_spin_until_future_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); auto future_result = action_client->async_get_result(goal_handle); EXPECT_THROW( - dual_spin_until_complete(future_result), + dual_spin_until_future_complete(future_result), rclcpp::exceptions::RCLError); } { @@ -928,7 +928,7 @@ TEST_F(TestClientAgainstServer, execute_rcl_errors) auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); EXPECT_THROW( - dual_spin_until_complete(future_goal_handle), + dual_spin_until_future_complete(future_goal_handle), rclcpp::exceptions::RCLError); } { @@ -938,11 +938,11 @@ TEST_F(TestClientAgainstServer, execute_rcl_errors) "lib:rclcpp_action", rcl_action_take_result_response, RCL_RET_ERROR); auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); - dual_spin_until_complete(future_goal_handle); + dual_spin_until_future_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); auto future_result = action_client->async_get_result(goal_handle); EXPECT_THROW( - dual_spin_until_complete(future_result), + dual_spin_until_future_complete(future_result), rclcpp::exceptions::RCLError); } { @@ -952,12 +952,12 @@ TEST_F(TestClientAgainstServer, execute_rcl_errors) "lib:rclcpp_action", rcl_action_take_cancel_response, RCL_RET_ERROR); auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); - dual_spin_until_complete(future_goal_handle); + dual_spin_until_future_complete(future_goal_handle); auto goal_handle = future_goal_handle.get(); auto future_cancel_some = action_client->async_cancel_goals_before(goal_handle->get_goal_stamp()); EXPECT_THROW( - dual_spin_until_complete(future_cancel_some), + dual_spin_until_future_complete(future_cancel_some), rclcpp::exceptions::RCLError); } } diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index fc5539d8dd..8687f90fbe 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -59,7 +59,7 @@ class TestServer : public ::testing::Test auto request = std::make_shared(); request->goal_id.uuid = uuid; auto future = client->async_send_request(request); - auto return_code = rclcpp::spin_until_complete(node, future, timeout); + auto return_code = rclcpp::spin_until_future_complete(node, future, timeout); if (rclcpp::FutureReturnCode::SUCCESS == return_code) { return request; } else if (rclcpp::FutureReturnCode::TIMEOUT == return_code) { @@ -82,7 +82,7 @@ class TestServer : public ::testing::Test auto request = std::make_shared(); request->goal_info.goal_id.uuid = uuid; auto future = cancel_client->async_send_request(request); - auto return_code = rclcpp::spin_until_complete(node, future, timeout); + auto return_code = rclcpp::spin_until_future_complete(node, future, timeout); if (rclcpp::FutureReturnCode::SUCCESS == return_code) { return future.get(); } else if (rclcpp::FutureReturnCode::TIMEOUT == return_code) { @@ -264,7 +264,7 @@ TEST_F(TestServer, handle_goal_called) auto future = client->async_send_request(request); ASSERT_EQ( rclcpp::FutureReturnCode::SUCCESS, - rclcpp::spin_until_complete(node, future)); + rclcpp::spin_until_future_complete(node, future)); ASSERT_EQ(uuid, received_uuid); } @@ -881,7 +881,7 @@ TEST_F(TestServer, get_result) // Wait for the result request to be received ASSERT_EQ( rclcpp::FutureReturnCode::SUCCESS, - rclcpp::spin_until_complete(node, future)); + rclcpp::spin_until_future_complete(node, future)); auto response = future.get(); EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status); @@ -897,7 +897,7 @@ TEST_F(TestServer, get_result) future = result_client->async_send_request(request); ASSERT_EQ( rclcpp::FutureReturnCode::SUCCESS, - rclcpp::spin_until_complete(node, future)); + rclcpp::spin_until_future_complete(node, future)); response = future.get(); EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_UNKNOWN, response->status); @@ -958,7 +958,7 @@ TEST_F(TestServer, get_result_deferred) // Wait for the result request to be received ASSERT_EQ( rclcpp::FutureReturnCode::SUCCESS, - rclcpp::spin_until_complete(node, future)); + rclcpp::spin_until_future_complete(node, future)); auto response = future.get(); EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status); @@ -1045,7 +1045,7 @@ class TestBasicServer : public TestServer // Wait for the result request to be received ASSERT_EQ( rclcpp::FutureReturnCode::SUCCESS, - rclcpp::spin_until_complete(node_, future)); + rclcpp::spin_until_future_complete(node_, future)); auto response = future.get(); EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status); @@ -1061,7 +1061,7 @@ class TestBasicServer : public TestServer future = result_client->async_send_request(request); ASSERT_EQ( rclcpp::FutureReturnCode::SUCCESS, - rclcpp::spin_until_complete(node_, future)); + rclcpp::spin_until_future_complete(node_, future)); } protected: diff --git a/rclcpp_components/test/test_component_manager_api.cpp b/rclcpp_components/test/test_component_manager_api.cpp index 0d5c3771ed..dfb9db76a2 100644 --- a/rclcpp_components/test/test_component_manager_api.cpp +++ b/rclcpp_components/test/test_component_manager_api.cpp @@ -66,7 +66,7 @@ void test_components_api(bool use_dedicated_executor) request->plugin_name = "test_rclcpp_components::TestComponentFoo"; auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, true); @@ -81,7 +81,7 @@ void test_components_api(bool use_dedicated_executor) request->plugin_name = "test_rclcpp_components::TestComponentBar"; auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, true); @@ -98,7 +98,7 @@ void test_components_api(bool use_dedicated_executor) request->node_name = "test_component_baz"; auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, true); @@ -116,7 +116,7 @@ void test_components_api(bool use_dedicated_executor) request->node_name = "test_component_bing"; auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, true); @@ -132,7 +132,7 @@ void test_components_api(bool use_dedicated_executor) request->plugin_name = "test_rclcpp_components::TestComponent"; auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); auto result = future.get(); EXPECT_EQ(result->success, false); @@ -148,7 +148,7 @@ void test_components_api(bool use_dedicated_executor) request->plugin_name = "test_rclcpp_components::TestComponentFoo"; auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); auto result = future.get(); EXPECT_EQ(result->success, false); @@ -166,7 +166,7 @@ void test_components_api(bool use_dedicated_executor) request->remap_rules.push_back("alice:=bob"); auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, true); @@ -186,7 +186,7 @@ void test_components_api(bool use_dedicated_executor) request->extra_arguments.push_back(use_intraprocess_comms.to_parameter_msg()); auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, true); @@ -208,7 +208,7 @@ void test_components_api(bool use_dedicated_executor) request->extra_arguments.push_back(use_intraprocess_comms.to_parameter_msg()); auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, false); @@ -230,7 +230,7 @@ void test_components_api(bool use_dedicated_executor) request->extra_arguments.push_back(forward_global_arguments.to_parameter_msg()); auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, true); @@ -251,7 +251,7 @@ void test_components_api(bool use_dedicated_executor) request->extra_arguments.push_back(forward_global_arguments.to_parameter_msg()); auto future = composition_client->async_send_request(request); - auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, false); @@ -284,7 +284,7 @@ void test_components_api(bool use_dedicated_executor) { auto request = std::make_shared(); auto future = client->async_send_request(request); - auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); auto result = future.get(); auto result_node_names = result->full_node_names; @@ -322,7 +322,7 @@ void test_components_api(bool use_dedicated_executor) request->unique_id = 1; auto future = client->async_send_request(request); - auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, true); @@ -334,7 +334,7 @@ void test_components_api(bool use_dedicated_executor) request->unique_id = 1; auto future = client->async_send_request(request); - auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. auto result = future.get(); EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); EXPECT_EQ(result->success, false); @@ -353,7 +353,7 @@ void test_components_api(bool use_dedicated_executor) { auto request = std::make_shared(); auto future = client->async_send_request(request); - auto ret = exec->spin_until_complete(future, 5s); // Wait for the result. + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); auto result = future.get(); auto result_node_names = result->full_node_names; From 33dae5d679751b603205008fcb31755986bcee1c Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Tue, 28 Jun 2022 09:20:48 -0500 Subject: [PATCH 121/455] Mirror rolling to master --- .github/workflows/mirror-rolling-to-master.yaml | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 .github/workflows/mirror-rolling-to-master.yaml diff --git a/.github/workflows/mirror-rolling-to-master.yaml b/.github/workflows/mirror-rolling-to-master.yaml new file mode 100644 index 0000000000..2885eb4a4f --- /dev/null +++ b/.github/workflows/mirror-rolling-to-master.yaml @@ -0,0 +1,13 @@ +name: Mirror rolling to master + +on: + push: + branches: [ rolling ] + +jobs: + mirror-to-master: + runs-on: ubuntu-latest + steps: + - uses: zofrex/mirror-branch@v1 + with: + target-branch: master From 6dd3a0377bbacd07fa6ed3c9e70c6de70931b45f Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 7 Jul 2022 00:41:16 +0800 Subject: [PATCH 122/455] use regex for wildcard matching (#1839) * use regex for wildcard matching Co-authored-by: Aaron Lipinski Signed-off-by: Chen Lihui * use map to process the content of parameter file by order Signed-off-by: Chen Lihui * add more test cases Signed-off-by: Chen Lihui * try to not decrease the performance and make the param win last Signed-off-by: Chen Lihui * update node name Signed-off-by: Chen Lihui * update document comment Signed-off-by: Chen Lihui * add more test for parameter_map_from Signed-off-by: Chen Lihui Co-authored-by: Aaron Lipinski --- rclcpp/include/rclcpp/parameter_map.hpp | 4 +- .../detail/resolve_parameter_overrides.cpp | 17 +-- rclcpp/src/rclcpp/parameter_map.cpp | 16 ++- .../node_interfaces/test_node_parameters.cpp | 132 ++++++++++++++++++ rclcpp/test/rclcpp/test_parameter_map.cpp | 130 +++++++++++++++++ .../complicated_wildcards.yaml | 5 + .../test_node_parameters/params_by_order.yaml | 16 +++ .../test_node_parameters/wildcards.yaml | 57 ++++++++ 8 files changed, 364 insertions(+), 13 deletions(-) create mode 100644 rclcpp/test/resources/test_node_parameters/complicated_wildcards.yaml create mode 100644 rclcpp/test/resources/test_node_parameters/params_by_order.yaml create mode 100644 rclcpp/test/resources/test_node_parameters/wildcards.yaml diff --git a/rclcpp/include/rclcpp/parameter_map.hpp b/rclcpp/include/rclcpp/parameter_map.hpp index 9cfcdaaacf..303eac4a95 100644 --- a/rclcpp/include/rclcpp/parameter_map.hpp +++ b/rclcpp/include/rclcpp/parameter_map.hpp @@ -35,11 +35,13 @@ using ParameterMap = std::unordered_map>; /// Convert parameters from rcl_yaml_param_parser into C++ class instances. /// \param[in] c_params C structures containing parameters for multiple nodes. +/// \param[in] node_fqn a Fully Qualified Name of node, default value is nullptr. +/// If it's not nullptr, return the relative node parameters belonging to this node_fqn. /// \returns a map where the keys are fully qualified node names and values a list of parameters. /// \throws InvalidParametersException if the `rcl_params_t` is inconsistent or invalid. RCLCPP_PUBLIC ParameterMap -parameter_map_from(const rcl_params_t * const c_params); +parameter_map_from(const rcl_params_t * const c_params, const char * node_fqn = nullptr); /// Convert parameter value from rcl_yaml_param_parser into a C++ class instance. /// \param[in] c_value C structure containing a value of a parameter. diff --git a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp index a62121b37f..3959e64882 100644 --- a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp +++ b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp @@ -51,18 +51,13 @@ rclcpp::detail::resolve_parameter_overrides( [params]() { rcl_yaml_node_struct_fini(params); }); - rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params); + rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params, node_fqn.c_str()); - // Enforce wildcard matching precedence - // TODO(cottsay) implement further wildcard matching - const std::array node_matching_names{"/**", node_fqn}; - for (const auto & node_name : node_matching_names) { - if (initial_map.count(node_name) > 0) { - // Combine parameter yaml files, overwriting values in older ones - for (const rclcpp::Parameter & param : initial_map.at(node_name)) { - result[param.get_name()] = - rclcpp::ParameterValue(param.get_value_message()); - } + if (initial_map.count(node_fqn) > 0) { + // Combine parameter yaml files, overwriting values in older ones + for (const rclcpp::Parameter & param : initial_map.at(node_fqn)) { + result[param.get_name()] = + rclcpp::ParameterValue(param.get_value_message()); } } } diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index e5e3da019c..8f7bc0655c 100644 --- a/rclcpp/src/rclcpp/parameter_map.cpp +++ b/rclcpp/src/rclcpp/parameter_map.cpp @@ -13,8 +13,10 @@ // limitations under the License. #include +#include #include +#include "rcpputils/find_and_replace.hpp" #include "rclcpp/parameter_map.hpp" using rclcpp::exceptions::InvalidParametersException; @@ -23,7 +25,7 @@ using rclcpp::ParameterMap; using rclcpp::ParameterValue; ParameterMap -rclcpp::parameter_map_from(const rcl_params_t * const c_params) +rclcpp::parameter_map_from(const rcl_params_t * const c_params, const char * node_fqn) { if (NULL == c_params) { throw InvalidParametersException("parameters struct is NULL"); @@ -49,6 +51,17 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params) node_name = c_node_name; } + if (node_fqn) { + // Update the regular expression ["/*" -> "(/\\w+)" and "/**" -> "(/\\w+)*"] + std::string regex = rcpputils::find_and_replace(node_name, "/*", "(/\\w+)"); + if (!std::regex_match(node_fqn, std::regex(regex))) { + // No need to parse the items because the user just care about node_fqn + continue; + } + + node_name = node_fqn; + } + const rcl_node_params_t * const c_params_node = &(c_params->params[n]); std::vector & params_node = parameters[node_name]; @@ -65,6 +78,7 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params) params_node.emplace_back(c_param_name, parameter_value_from(c_param_value)); } } + return parameters; } diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp index 31b755b4a7..97e3a3188d 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp @@ -31,6 +31,8 @@ #include "../../mocking_utils/patch.hpp" #include "../../utils/rclcpp_gtest_macros.hpp" +#include "rcpputils/filesystem_helper.hpp" + class TestNodeParameters : public ::testing::Test { public: @@ -47,6 +49,7 @@ class TestNodeParameters : public ::testing::Test dynamic_cast( node->get_node_parameters_interface().get()); ASSERT_NE(nullptr, node_parameters); + test_resources_path /= "test_node_parameters"; } void TearDown() @@ -57,6 +60,8 @@ class TestNodeParameters : public ::testing::Test protected: std::shared_ptr node; rclcpp::node_interfaces::NodeParameters * node_parameters; + + rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY}; }; TEST_F(TestNodeParameters, construct_destruct_rcl_errors) { @@ -199,3 +204,130 @@ TEST_F(TestNodeParameters, add_remove_parameters_callback) { node_parameters->remove_on_set_parameters_callback(handle.get()), std::runtime_error("Callback doesn't exist")); } + +TEST_F(TestNodeParameters, wildcard_with_namespace) +{ + rclcpp::NodeOptions opts; + opts.arguments( + { + "--ros-args", + "--params-file", (test_resources_path / "wildcards.yaml").string() + }); + + std::shared_ptr node = std::make_shared("node2", "ns", opts); + + auto * node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); + + const auto & parameter_overrides = node_parameters->get_parameter_overrides(); + EXPECT_EQ(7u, parameter_overrides.size()); + EXPECT_EQ(parameter_overrides.at("full_wild").get(), "full_wild"); + EXPECT_EQ(parameter_overrides.at("namespace_wild").get(), "namespace_wild"); + EXPECT_EQ( + parameter_overrides.at("namespace_wild_another").get(), + "namespace_wild_another"); + EXPECT_EQ( + parameter_overrides.at("namespace_wild_one_star").get(), + "namespace_wild_one_star"); + EXPECT_EQ(parameter_overrides.at("node_wild_in_ns").get(), "node_wild_in_ns"); + EXPECT_EQ( + parameter_overrides.at("node_wild_in_ns_another").get(), + "node_wild_in_ns_another"); + EXPECT_EQ(parameter_overrides.at("explicit_in_ns").get(), "explicit_in_ns"); + EXPECT_EQ(parameter_overrides.count("should_not_appear"), 0u); +} + +TEST_F(TestNodeParameters, wildcard_no_namespace) +{ + rclcpp::NodeOptions opts; + opts.arguments( + { + "--ros-args", + "--params-file", (test_resources_path / "wildcards.yaml").string() + }); + + std::shared_ptr node = std::make_shared("node2", opts); + + auto * node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); + + const auto & parameter_overrides = node_parameters->get_parameter_overrides(); + EXPECT_EQ(5u, parameter_overrides.size()); + EXPECT_EQ(parameter_overrides.at("full_wild").get(), "full_wild"); + EXPECT_EQ(parameter_overrides.at("namespace_wild").get(), "namespace_wild"); + EXPECT_EQ( + parameter_overrides.at("namespace_wild_another").get(), + "namespace_wild_another"); + EXPECT_EQ(parameter_overrides.at("node_wild_no_ns").get(), "node_wild_no_ns"); + EXPECT_EQ(parameter_overrides.at("explicit_no_ns").get(), "explicit_no_ns"); + EXPECT_EQ(parameter_overrides.count("should_not_appear"), 0u); + // "/*" match exactly one token, not expect to get `namespace_wild_one_star` + EXPECT_EQ(parameter_overrides.count("namespace_wild_one_star"), 0u); +} + +TEST_F(TestNodeParameters, params_by_order) +{ + rclcpp::NodeOptions opts; + opts.arguments( + { + "--ros-args", + "--params-file", (test_resources_path / "params_by_order.yaml").string() + }); + + std::shared_ptr node = std::make_shared("node2", "ns", opts); + + auto * node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); + + const auto & parameter_overrides = node_parameters->get_parameter_overrides(); + EXPECT_EQ(3u, parameter_overrides.size()); + EXPECT_EQ(parameter_overrides.at("a_value").get(), "last_one_win"); + EXPECT_EQ(parameter_overrides.at("foo").get(), "foo"); + EXPECT_EQ(parameter_overrides.at("bar").get(), "bar"); +} + +TEST_F(TestNodeParameters, complicated_wildcards) +{ + rclcpp::NodeOptions opts; + opts.arguments( + { + "--ros-args", + "--params-file", (test_resources_path / "complicated_wildcards.yaml").string() + }); + + { + // regex matched: /**/foo/*/bar + std::shared_ptr node = + std::make_shared("node2", "/a/b/c/foo/d/bar", opts); + + auto * node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); + + const auto & parameter_overrides = node_parameters->get_parameter_overrides(); + EXPECT_EQ(2u, parameter_overrides.size()); + EXPECT_EQ(parameter_overrides.at("foo").get(), "foo"); + EXPECT_EQ(parameter_overrides.at("bar").get(), "bar"); + } + + { + // regex not matched: /**/foo/*/bar + std::shared_ptr node = + std::make_shared("node2", "/a/b/c/foo/bar", opts); + + auto * node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); + + const auto & parameter_overrides = node_parameters->get_parameter_overrides(); + EXPECT_EQ(0u, parameter_overrides.size()); + } +} diff --git a/rclcpp/test/rclcpp/test_parameter_map.cpp b/rclcpp/test/rclcpp/test_parameter_map.cpp index 3f54e4c879..0158b7c7e7 100644 --- a/rclcpp/test/rclcpp/test_parameter_map.cpp +++ b/rclcpp/test/rclcpp/test_parameter_map.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include "rclcpp/parameter_map.hpp" @@ -353,3 +354,132 @@ TEST(Test_parameter_map_from, string_array_param_value) c_params->params[0].parameter_values[0].string_array_value = NULL; rcl_yaml_node_struct_fini(c_params); } + +TEST(Test_parameter_map_from, one_node_one_param_by_node_fqn) +{ + rcl_params_t * c_params = make_params({"foo"}); + make_node_params(c_params, 0, {"string_param"}); + + std::string hello_world = "hello world"; + char * c_hello_world = new char[hello_world.length() + 1]; + std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str()); + c_params->params[0].parameter_values[0].string_value = c_hello_world; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params, "/foo"); + const std::vector & params = map.at("/foo"); + EXPECT_STREQ("string_param", params.at(0).get_name().c_str()); + EXPECT_STREQ(hello_world.c_str(), params.at(0).get_value().c_str()); + + c_params->params[0].parameter_values[0].string_value = NULL; + delete[] c_hello_world; + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, multi_nodes_same_param_name_by_node_fqn) +{ + std::vector node_names_keys = { + "/**", // index: 0 + "/*", // index: 1 + "/**/node", // index: 2 + "/*/node", // index: 3 + "/ns/node" // index: 4 + }; + + rcl_params_t * c_params = make_params(node_names_keys); + + std::vector param_values; + for (size_t i = 0; i < node_names_keys.size(); ++i) { + make_node_params(c_params, i, {"string_param"}); + std::string hello_world = "hello world" + std::to_string(i); + char * c_hello_world = new char[hello_world.length() + 1]; + std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str()); + c_params->params[i].parameter_values[0].string_value = c_hello_world; + param_values.push_back(c_hello_world); + } + + std::unordered_map> node_fqn_expected = { + {"/ns/foo/another_node", {0}}, + {"/another", {0, 1}}, + {"/node", {0, 1, 2}}, + {"/another_ns/node", {0, 2, 3}}, + {"/ns/node", {0, 2, 3, 4}}, + }; + + for (auto & kv : node_fqn_expected) { + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params, kv.first.c_str()); + const std::vector & params = map.at(kv.first); + + EXPECT_EQ(kv.second.size(), params.size()); + for (size_t i = 0; i < params.size(); ++i) { + std::string param_value = "hello world" + std::to_string(kv.second[i]); + EXPECT_STREQ("string_param", params.at(i).get_name().c_str()); + EXPECT_STREQ(param_value.c_str(), params.at(i).get_value().c_str()); + } + } + + for (size_t i = 0; i < node_names_keys.size(); ++i) { + c_params->params[i].parameter_values[0].string_value = NULL; + } + for (auto c_hello_world : param_values) { + delete[] c_hello_world; + } + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, multi_nodes_diff_param_name_by_node_fqn) +{ + std::vector node_names_keys = { + "/**", // index: 0 + "/*", // index: 1 + "/**/node", // index: 2 + "/*/node", // index: 3 + "/ns/**", // index: 4 + "/ns/*", // index: 5 + "/ns/**/node", // index: 6 + "/ns/*/node", // index: 7 + "/ns/**/a/*/node", // index: 8 + "/ns/node" // index: 9 + }; + + rcl_params_t * c_params = make_params(node_names_keys); + + for (size_t i = 0; i < node_names_keys.size(); ++i) { + std::string param_name = "string_param" + std::to_string(i); + make_node_params(c_params, i, {param_name}); + } + + std::string hello_world = "hello world"; + char * c_hello_world = new char[hello_world.length() + 1]; + std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str()); + + for (size_t i = 0; i < node_names_keys.size(); ++i) { + c_params->params[i].parameter_values[0].string_value = c_hello_world; + } + + std::unordered_map> node_fqn_expected = { + {"/ns/node", {0, 2, 3, 4, 5, 6, 9}}, + {"/node", {0, 1, 2}}, + {"/ns/foo/node", {0, 2, 4, 6, 7}}, + {"/ns/foo/a/node", {0, 2, 4, 6}}, + {"/ns/foo/a/bar/node", {0, 2, 4, 6, 8}}, + {"/ns/a/bar/node", {0, 2, 4, 6, 8}}, + {"/ns/foo/zoo/a/bar/node", {0, 2, 4, 6, 8}}, + }; + + for (auto & kv : node_fqn_expected) { + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params, kv.first.c_str()); + const std::vector & params = map.at(kv.first); + EXPECT_EQ(kv.second.size(), params.size()); + for (size_t i = 0; i < params.size(); ++i) { + std::string param_name = "string_param" + std::to_string(kv.second[i]); + EXPECT_STREQ(param_name.c_str(), params.at(i).get_name().c_str()); + EXPECT_STREQ(hello_world.c_str(), params.at(i).get_value().c_str()); + } + } + + for (size_t i = 0; i < node_names_keys.size(); ++i) { + c_params->params[i].parameter_values[0].string_value = NULL; + } + delete[] c_hello_world; + rcl_yaml_node_struct_fini(c_params); +} diff --git a/rclcpp/test/resources/test_node_parameters/complicated_wildcards.yaml b/rclcpp/test/resources/test_node_parameters/complicated_wildcards.yaml new file mode 100644 index 0000000000..53da409135 --- /dev/null +++ b/rclcpp/test/resources/test_node_parameters/complicated_wildcards.yaml @@ -0,0 +1,5 @@ +/**/foo/*/bar: + node2: + ros__parameters: + foo: "foo" + bar: "bar" diff --git a/rclcpp/test/resources/test_node_parameters/params_by_order.yaml b/rclcpp/test/resources/test_node_parameters/params_by_order.yaml new file mode 100644 index 0000000000..680d96beaf --- /dev/null +++ b/rclcpp/test/resources/test_node_parameters/params_by_order.yaml @@ -0,0 +1,16 @@ +/**: + node2: + ros__parameters: + a_value: "first" + foo: "foo" + +/ns: + node2: + ros__parameters: + a_value: "second" + bar: "bar" + +/*: + node2: + ros__parameters: + a_value: "last_one_win" diff --git a/rclcpp/test/resources/test_node_parameters/wildcards.yaml b/rclcpp/test/resources/test_node_parameters/wildcards.yaml new file mode 100644 index 0000000000..b89b0d8cd0 --- /dev/null +++ b/rclcpp/test/resources/test_node_parameters/wildcards.yaml @@ -0,0 +1,57 @@ +/**: + ros__parameters: + full_wild: "full_wild" + +/**: + node2: + ros__parameters: + namespace_wild: "namespace_wild" + +/**/node2: + ros__parameters: + namespace_wild_another: "namespace_wild_another" + +/*: + node2: + ros__parameters: + namespace_wild_one_star: "namespace_wild_one_star" + +ns: + "*": + ros__parameters: + node_wild_in_ns: "node_wild_in_ns" + +/ns/*: + ros__parameters: + node_wild_in_ns_another: "node_wild_in_ns_another" + +ns: + node2: + ros__parameters: + explicit_in_ns: "explicit_in_ns" + +"*": + ros__parameters: + node_wild_no_ns: "node_wild_no_ns" + +node2: + ros__parameters: + explicit_no_ns: "explicit_no_ns" + +ns: + nodeX: + ros__parameters: + should_not_appear: "incorrect_node_name" + +/**/nodeX: + ros__parameters: + should_not_appear: "incorrect_node_name" + +nsX: + node2: + ros__parameters: + should_not_appear: "incorrect_namespace" + +/nsX/*: + ros__parameters: + should_not_appear: "incorrect_namespace" From 37b589dc85a2311289d80238bd13055eda25f5e6 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 7 Jul 2022 12:07:12 -0400 Subject: [PATCH 123/455] Fix the documentation for rclcpp::ok to be accurate. (#1965) That is, it returns false if shutdown has been called, and true in all other circumstances. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/utilities.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 5274cba33a..5b4ea86a6d 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -169,7 +169,7 @@ remove_ros_arguments(int argc, char const * const * argv); * the context initialized by rclcpp::init(). * * \param[in] context Optional check for shutdown of this Context. - * \return true if shutdown has been called, false otherwise + * \return false if shutdown has been called, true otherwise */ RCLCPP_PUBLIC bool From f6056beaa09ec2c19a6cc3b74405e2b83948f161 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 11 Jul 2022 10:09:44 -0700 Subject: [PATCH 124/455] Make create_client accept rclcpp::QoS (#1964) * Make create_client accept rclcpp::QoS Signed-off-by: Shane Loretz * Deprecate passing rmw_qos_profile_t to create_client Signed-off-by: Shane Loretz --- rclcpp/include/rclcpp/create_client.hpp | 30 ++++ rclcpp/include/rclcpp/node.hpp | 18 ++- rclcpp/include/rclcpp/node_impl.hpp | 16 ++ .../test_allocator_memory_strategy.cpp | 4 +- rclcpp/test/rclcpp/test_client.cpp | 81 +++++++--- rclcpp/test/rclcpp/test_memory_strategy.cpp | 4 +- rclcpp/test/rclcpp/test_service.cpp | 8 +- rclcpp_lifecycle/CMakeLists.txt | 8 + .../rclcpp_lifecycle/lifecycle_node.hpp | 18 ++- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 29 ++-- rclcpp_lifecycle/test/test_client.cpp | 143 ++++++++++++++++++ 11 files changed, 316 insertions(+), 43 deletions(-) create mode 100644 rclcpp_lifecycle/test/test_client.cpp diff --git a/rclcpp/include/rclcpp/create_client.hpp b/rclcpp/include/rclcpp/create_client.hpp index 1a960b8d8f..00e6ff3c0e 100644 --- a/rclcpp/include/rclcpp/create_client.hpp +++ b/rclcpp/include/rclcpp/create_client.hpp @@ -20,10 +20,40 @@ #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/qos.hpp" #include "rmw/rmw.h" namespace rclcpp { +/// Create a service client with a given type. +/** + * \param[in] node_base NodeBaseInterface implementation of the node on which + * to create the client. + * \param[in] node_graph NodeGraphInterface implementation of the node on which + * to create the client. + * \param[in] node_services NodeServicesInterface implementation of the node on + * which to create the client. + * \param[in] service_name The name on which the service is accessible. + * \param[in] qos Quality of service profile for client. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created client. + */ +template +typename rclcpp::Client::SharedPtr +create_client( + std::shared_ptr node_base, + std::shared_ptr node_graph, + std::shared_ptr node_services, + const std::string & service_name, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr) +{ + return create_client( + node_base, node_graph, node_services, + service_name, + qos.get_rmw_qos_profile(), + group); +} /// Create a service client with a given type. /// \internal diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 65b8797716..f9ebd4e3ee 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -246,12 +246,28 @@ class Node : public std::enable_shared_from_this * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client. * \param[in] group Callback group to call the service. * \return Shared pointer to the created client. + * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t */ template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] typename rclcpp::Client::SharedPtr create_client( const std::string & service_name, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, + const rmw_qos_profile_t & qos_profile, + rclcpp::CallbackGroup::SharedPtr group = nullptr); + + /// Create and return a Client. + /** + * \param[in] service_name The name on which the service is accessible. + * \param[in] qos Quality of service profile for client. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created client. + */ + template + typename rclcpp::Client::SharedPtr + create_client( + const std::string & service_name, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); /// Create and return a Service. diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 086c2bb17c..b957bd2439 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -120,6 +120,22 @@ Node::create_wall_timer( this->node_timers_.get()); } +template +typename Client::SharedPtr +Node::create_client( + const std::string & service_name, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_client( + node_base_, + node_graph_, + node_services_, + extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), + qos, + group); +} + template typename Client::SharedPtr Node::create_client( diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index e1b916e9c0..cd28949f3d 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -177,7 +177,7 @@ class TestAllocatorMemoryStrategy : public ::testing::Test clients_.push_back( node_with_client->create_client( - "service", rmw_qos_profile_services_default, callback_group)); + "service", rclcpp::ServicesQoS(), callback_group)); return node_with_client; } @@ -831,7 +831,7 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_client_out_of_scope) { node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); auto client = node->create_client( - "service", rmw_qos_profile_services_default, callback_group); + "service", rclcpp::ServicesQoS(), callback_group); weak_groups_to_nodes.insert( std::paircreate_client("service"); } + { + // suppress deprecated function warning + #if !defined(_WIN32) + # pragma GCC diagnostic push + # pragma GCC diagnostic ignored "-Wdeprecated-declarations" + #else // !defined(_WIN32) + # pragma warning(push) + # pragma warning(disable: 4996) + #endif + + auto client = node->create_client( + "service", rmw_qos_profile_services_default); + + // remove warning suppression + #if !defined(_WIN32) + # pragma GCC diagnostic pop + #else // !defined(_WIN32) + # pragma warning(pop) + #endif + } + { + auto client = node->create_client( + "service", rclcpp::ServicesQoS()); + } { ASSERT_THROW( @@ -123,6 +147,27 @@ TEST_F(TestClient, construction_with_free_function) { nullptr); }, rclcpp::exceptions::InvalidServiceNameError); } + { + auto client = rclcpp::create_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + "service", + rclcpp::ServicesQoS(), + nullptr); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + "invalid_?service", + rclcpp::ServicesQoS(), + nullptr); + }, rclcpp::exceptions::InvalidServiceNameError); + } } TEST_F(TestClient, construct_with_rcl_error) { @@ -351,15 +396,15 @@ 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"); - rmw_qos_profile_t client_qos = rmw_qos_profile_services_default; - client_qos.depth = 3; + rclcpp::ServicesQoS client_qos; + client_qos.keep_last(3); auto client = client_node->create_client("test_service", client_qos); 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, client_qos); + "test_service", server_callback, client_qos.get_rmw_qos_profile()); auto request = std::make_shared(); std::atomic c1 {0}; @@ -455,32 +500,30 @@ TEST_F(TestClient, rcl_client_response_subscription_get_actual_qos_error) { } TEST_F(TestClient, client_qos) { - rmw_qos_profile_t qos_profile = rmw_qos_profile_services_default; - qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; - uint64_t duration = 1; - qos_profile.deadline = {duration, duration}; - qos_profile.lifespan = {duration, duration}; - qos_profile.liveliness_lease_duration = {duration, duration}; + rclcpp::ServicesQoS qos_profile; + qos_profile.liveliness(rclcpp::LivelinessPolicy::Automatic); + rclcpp::Duration duration(std::chrono::nanoseconds(1)); + qos_profile.deadline(duration); + qos_profile.lifespan(duration); + qos_profile.liveliness_lease_duration(duration); auto client = node->create_client("client", qos_profile); - auto init_qos = - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile), qos_profile); auto rp_qos = client->get_request_publisher_actual_qos(); auto rs_qos = client->get_response_subscription_actual_qos(); - EXPECT_EQ(init_qos, rp_qos); + EXPECT_EQ(qos_profile, rp_qos); // Lifespan has no meaning for subscription/readers - rs_qos.lifespan(qos_profile.lifespan); - EXPECT_EQ(init_qos, rs_qos); + rs_qos.lifespan(qos_profile.lifespan()); + EXPECT_EQ(qos_profile, rs_qos); } TEST_F(TestClient, client_qos_depth) { using namespace std::literals::chrono_literals; - rmw_qos_profile_t client_qos_profile = rmw_qos_profile_default; - client_qos_profile.depth = 2; + rclcpp::ServicesQoS client_qos_profile; + client_qos_profile.keep_last(2); auto client = node->create_client("test_qos_depth", client_qos_profile); @@ -522,10 +565,10 @@ TEST_F(TestClient, client_qos_depth) { std::this_thread::sleep_for(2ms); } - EXPECT_GT(server_cb_count_, client_qos_profile.depth); + EXPECT_GT(server_cb_count_, client_qos_profile.depth()); start = std::chrono::steady_clock::now(); - while ((client_cb_count_ < client_qos_profile.depth) && + while ((client_cb_count_ < client_qos_profile.depth()) && (std::chrono::steady_clock::now() - start) < 1s) { rclcpp::spin_some(node); @@ -535,5 +578,5 @@ TEST_F(TestClient, client_qos_depth) { // so more client callbacks might be called than expected. rclcpp::spin_some(node); - EXPECT_EQ(client_cb_count_, client_qos_profile.depth); + EXPECT_EQ(client_cb_count_, client_qos_profile.depth()); } diff --git a/rclcpp/test/rclcpp/test_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp index b6fff57a92..732e4475fc 100644 --- a/rclcpp/test/rclcpp/test_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/test_memory_strategy.cpp @@ -197,7 +197,7 @@ TEST_F(TestMemoryStrategy, get_client_by_handle) { node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); { auto client = node->create_client( - "service", rmw_qos_profile_services_default, callback_group); + "service", rclcpp::ServicesQoS(), callback_group); client_handle = client->get_client_handle(); weak_groups_to_nodes.insert( @@ -435,7 +435,7 @@ TEST_F(TestMemoryStrategy, get_group_by_client) { node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); client = node->create_client( - "service", rmw_qos_profile_services_default, callback_group); + "service", rclcpp::ServicesQoS(), callback_group); weak_groups_to_nodes.insert( std::pair( diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index 1dbb8ca9e4..14cead0864 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -246,10 +246,10 @@ TEST_F(TestService, on_new_request_callback) { auto server_callback = [](const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {FAIL();}; - rmw_qos_profile_t service_qos = rmw_qos_profile_services_default; - service_qos.depth = 3; + rclcpp::ServicesQoS service_qos; + service_qos.keep_last(3); auto server = node->create_service( - "~/test_service", server_callback, service_qos); + "~/test_service", server_callback, service_qos.get_rmw_qos_profile()); std::atomic c1 {0}; auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; @@ -378,7 +378,7 @@ TEST_F(TestService, server_qos_depth) { auto server = server_node->create_service( "test_qos_depth", std::move(server_callback), server_qos_profile); - rmw_qos_profile_t client_qos_profile = rmw_qos_profile_default; + rclcpp::QoS client_qos_profile(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); auto client = node->create_client("test_qos_depth", client_qos_profile); ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index a823d44a68..8a210e09cc 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -109,6 +109,14 @@ if(BUILD_TESTING) ) target_link_libraries(test_lifecycle_service_client ${PROJECT_NAME} mimick) endif() + ament_add_gtest(test_client test/test_client.cpp TIMEOUT 120) + if(TARGET test_client) + target_link_libraries(test_client + ${PROJECT_NAME} + mimick + ${rcl_interfaces_TARGETS} + rclcpp::rclcpp) + endif() ament_add_gtest(test_state_machine_info test/test_state_machine_info.cpp) if(TARGET test_state_machine_info) ament_target_dependencies(test_state_machine_info diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 6048d0d691..d0f26c4a80 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -258,12 +258,28 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, /// Create and return a Client. /** * \sa rclcpp::Node::create_client + * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t */ template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] typename rclcpp::Client::SharedPtr create_client( const std::string & service_name, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, + const rmw_qos_profile_t & qos_profile, + rclcpp::CallbackGroup::SharedPtr group = nullptr); + + /// Create and return a Client. + /** + * \param[in] service_name The name on which the service is accessible. + * \param[in] qos Quality of service profile for client. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created client. + */ + template + typename rclcpp::Client::SharedPtr + create_client( + const std::string & service_name, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); /// Create and return a Service. diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 22fd7f9c08..1e8fc5f662 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -22,6 +22,7 @@ #include #include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/create_client.hpp" #include "rclcpp/create_generic_publisher.hpp" #include "rclcpp/create_generic_subscription.hpp" #include "rclcpp/create_publisher.hpp" @@ -103,21 +104,21 @@ LifecycleNode::create_client( const rmw_qos_profile_t & qos_profile, rclcpp::CallbackGroup::SharedPtr group) { - rcl_client_options_t options = rcl_client_get_default_options(); - options.qos = qos_profile; - - using rclcpp::Client; - using rclcpp::ClientBase; - - auto cli = Client::make_shared( - node_base_.get(), - node_graph_, - service_name, - options); + return rclcpp::create_client( + node_base_, node_graph_, node_services_, + service_name, qos_profile, group); +} - auto cli_base_ptr = std::dynamic_pointer_cast(cli); - node_services_->add_client(cli_base_ptr, group); - return cli; +template +typename rclcpp::Client::SharedPtr +LifecycleNode::create_client( + const std::string & service_name, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_client( + node_base_, node_graph_, node_services_, + service_name, qos, group); } template diff --git a/rclcpp_lifecycle/test/test_client.cpp b/rclcpp_lifecycle/test/test_client.cpp new file mode 100644 index 0000000000..89e1f42fa6 --- /dev/null +++ b/rclcpp_lifecycle/test/test_client.cpp @@ -0,0 +1,143 @@ +// Copyright 2022 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. + +#include + +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "rcl_interfaces/srv/list_parameters.hpp" + + +class TestClient : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node_ = std::make_shared("my_lifecycle_node", "/ns"); + } + + void TearDown() + { + node_.reset(); + } + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; +}; + +/* + Testing client construction and destruction. + */ +TEST_F(TestClient, construction_and_destruction) { + using rcl_interfaces::srv::ListParameters; + { + auto client = node_->create_client("service"); + EXPECT_TRUE(client); + } + { + // suppress deprecated function warning + #if !defined(_WIN32) + # pragma GCC diagnostic push + # pragma GCC diagnostic ignored "-Wdeprecated-declarations" + #else // !defined(_WIN32) + # pragma warning(push) + # pragma warning(disable: 4996) + #endif + + auto client = node_->create_client( + "service", rmw_qos_profile_services_default); + EXPECT_TRUE(client); + + // remove warning suppression + #if !defined(_WIN32) + # pragma GCC diagnostic pop + #else // !defined(_WIN32) + # pragma warning(pop) + #endif + } + { + auto client = node_->create_client( + "service", rclcpp::ServicesQoS()); + EXPECT_TRUE(client); + } + + { + ASSERT_THROW( + { + auto client = node_->create_client("invalid_service?"); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} + +TEST_F(TestClient, construction_with_free_function) { + { + auto client = rclcpp::create_client( + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_services_interface(), + "service", + rmw_qos_profile_services_default, + nullptr); + EXPECT_TRUE(client); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_client( + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_services_interface(), + "invalid_?service", + rmw_qos_profile_services_default, + nullptr); + }, rclcpp::exceptions::InvalidServiceNameError); + } + { + auto client = rclcpp::create_client( + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_services_interface(), + "service", + rclcpp::ServicesQoS(), + nullptr); + EXPECT_TRUE(client); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_client( + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_services_interface(), + "invalid_?service", + rclcpp::ServicesQoS(), + nullptr); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} From 64e4c72791811e25ee7780fc84b07dc70f36a4c9 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 11 Jul 2022 17:41:40 -0700 Subject: [PATCH 125/455] Make create_service accept rclcpp::QoS (#1969) Signed-off-by: Shane Loretz --- rclcpp/include/rclcpp/create_service.hpp | 26 +++++ rclcpp/include/rclcpp/node.hpp | 20 +++- rclcpp/include/rclcpp/node_impl.hpp | 17 +++ .../test_allocator_memory_strategy.cpp | 4 +- rclcpp/test/rclcpp/test_client.cpp | 6 +- rclcpp/test/rclcpp/test_memory_strategy.cpp | 4 +- rclcpp/test/rclcpp/test_service.cpp | 32 +++--- rclcpp_lifecycle/CMakeLists.txt | 8 ++ .../rclcpp_lifecycle/lifecycle_node.hpp | 16 ++- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 13 +++ rclcpp_lifecycle/test/test_service.cpp | 100 ++++++++++++++++++ 11 files changed, 219 insertions(+), 27 deletions(-) create mode 100644 rclcpp_lifecycle/test/test_service.cpp diff --git a/rclcpp/include/rclcpp/create_service.hpp b/rclcpp/include/rclcpp/create_service.hpp index 9aaa02a1ed..42c253a526 100644 --- a/rclcpp/include/rclcpp/create_service.hpp +++ b/rclcpp/include/rclcpp/create_service.hpp @@ -26,6 +26,32 @@ namespace rclcpp { +/// Create a service with a given type. +/** + * \param[in] node_base NodeBaseInterface implementation of the node on which + * to create the service. + * \param[in] node_services NodeServicesInterface implementation of the node on + * which to create the service. + * \param[in] service_name The name on which the service is accessible. + * \param[in] callback The callback to call when the service gets a request. + * \param[in] qos Quality of service profile for the service. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created service. + */ +template +typename rclcpp::Service::SharedPtr +create_service( + std::shared_ptr node_base, + std::shared_ptr node_services, + const std::string & service_name, + CallbackT && callback, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return create_service( + node_base, node_services, service_name, + std::forward(callback), qos.get_rmw_qos_profile(), group); +} /// Create a service with a given type. /// \internal diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index f9ebd4e3ee..54a4b922ab 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -277,13 +277,31 @@ class Node : public std::enable_shared_from_this * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client. * \param[in] group Callback group to call the service. * \return Shared pointer to the created service. + * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t + */ + template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + typename rclcpp::Service::SharedPtr + create_service( + const std::string & service_name, + CallbackT && callback, + const rmw_qos_profile_t & qos_profile, + rclcpp::CallbackGroup::SharedPtr group = nullptr); + + /// Create and return a Service. + /** + * \param[in] service_name The topic to service on. + * \param[in] callback User-defined callback function. + * \param[in] qos Quality of service profile for the service. + * \param[in] group Callback group to call the service. + * \return Shared pointer to the created service. */ template typename rclcpp::Service::SharedPtr create_service( const std::string & service_name, CallbackT && callback, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); /// Create and return a GenericPublisher. diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index b957bd2439..5b427b5b25 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -152,6 +152,23 @@ Node::create_client( group); } +template +typename rclcpp::Service::SharedPtr +Node::create_service( + const std::string & service_name, + CallbackT && callback, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_service( + node_base_, + node_services_, + extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), + std::forward(callback), + qos, + group); +} + template typename rclcpp::Service::SharedPtr Node::create_service( diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index cd28949f3d..1779d5d88e 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -162,7 +162,7 @@ class TestAllocatorMemoryStrategy : public ::testing::Test services_.push_back( node_with_service->create_service( - "service", std::move(service_callback), rmw_qos_profile_services_default, callback_group)); + "service", std::move(service_callback), rclcpp::ServicesQoS(), callback_group)); return node_with_service; } @@ -793,7 +793,7 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_service_out_of_scope) { [](const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; auto service = node->create_service( - "service", std::move(service_callback), rmw_qos_profile_services_default, callback_group); + "service", std::move(service_callback), rclcpp::ServicesQoS(), callback_group); node->for_each_callback_group( [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp index 01b7d1c85c..a9d81d9176 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -404,7 +404,7 @@ TEST_F(TestClient, on_new_response_callback) { 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, client_qos.get_rmw_qos_profile()); + "test_service", server_callback, client_qos); auto request = std::make_shared(); std::atomic c1 {0}; @@ -534,10 +534,10 @@ TEST_F(TestClient, client_qos_depth) { auto server_node = std::make_shared("server_node", "/ns"); - rmw_qos_profile_t server_qos_profile = rmw_qos_profile_default; + rclcpp::QoS server_qos(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); auto server = server_node->create_service( - "test_qos_depth", std::move(server_callback), server_qos_profile); + "test_qos_depth", std::move(server_callback), server_qos); auto request = std::make_shared(); ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); diff --git a/rclcpp/test/rclcpp/test_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp index 732e4475fc..e96729a2a1 100644 --- a/rclcpp/test/rclcpp/test_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/test_memory_strategy.cpp @@ -155,7 +155,7 @@ TEST_F(TestMemoryStrategy, get_service_by_handle) { { auto service = node->create_service( "service", std::move(service_callback), - rmw_qos_profile_services_default, callback_group); + rclcpp::ServicesQoS(), callback_group); service_handle = service->get_service_handle(); @@ -396,7 +396,7 @@ TEST_F(TestMemoryStrategy, get_group_by_service) { service = node->create_service( "service", std::move(service_callback), - rmw_qos_profile_services_default, callback_group); + rclcpp::ServicesQoS(), callback_group); weak_groups_to_nodes.insert( std::pair( diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index 14cead0864..a3c361cfde 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -249,7 +249,7 @@ TEST_F(TestService, on_new_request_callback) { rclcpp::ServicesQoS service_qos; service_qos.keep_last(3); auto server = node->create_service( - "~/test_service", server_callback, service_qos.get_rmw_qos_profile()); + "~/test_service", server_callback, service_qos); std::atomic c1 {0}; auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; @@ -338,28 +338,25 @@ TEST_F(TestService, rcl_service_request_subscription_get_actual_qos_error) { TEST_F(TestService, server_qos) { - rmw_qos_profile_t qos_profile = rmw_qos_profile_services_default; - qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; - uint64_t duration = 1; - qos_profile.deadline = {duration, duration}; - qos_profile.lifespan = {duration, duration}; - qos_profile.liveliness_lease_duration = {duration, duration}; + rclcpp::ServicesQoS qos_profile; + qos_profile.liveliness(rclcpp::LivelinessPolicy::Automatic); + rclcpp::Duration duration(std::chrono::nanoseconds(1)); + qos_profile.deadline(duration); + qos_profile.lifespan(duration); + qos_profile.liveliness_lease_duration(duration); auto callback = [](const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; auto server = node->create_service( - "service", callback, - qos_profile); - auto init_qos = - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile), qos_profile); + "service", callback, qos_profile); auto rs_qos = server->get_request_subscription_actual_qos(); auto rp_qos = server->get_response_publisher_actual_qos(); - EXPECT_EQ(init_qos, rp_qos); + EXPECT_EQ(qos_profile, rp_qos); // Lifespan has no meaning for subscription/readers - rs_qos.lifespan(qos_profile.lifespan); - EXPECT_EQ(init_qos, rs_qos); + rs_qos.lifespan(qos_profile.lifespan()); + EXPECT_EQ(qos_profile, rs_qos); } TEST_F(TestService, server_qos_depth) { @@ -372,8 +369,7 @@ TEST_F(TestService, server_qos_depth) { auto server_node = std::make_shared("server_node", "/ns"); - rmw_qos_profile_t server_qos_profile = rmw_qos_profile_default; - server_qos_profile.depth = 2; + rclcpp::QoS server_qos_profile(2); auto server = server_node->create_service( "test_qos_depth", std::move(server_callback), server_qos_profile); @@ -398,7 +394,7 @@ TEST_F(TestService, server_qos_depth) { } auto start = std::chrono::steady_clock::now(); - while ((server_cb_count_ < server_qos_profile.depth) && + while ((server_cb_count_ < server_qos_profile.depth()) && (std::chrono::steady_clock::now() - start) < 1s) { rclcpp::spin_some(server_node); @@ -409,5 +405,5 @@ TEST_F(TestService, server_qos_depth) { // so more server responses might be processed than expected. rclcpp::spin_some(server_node); - EXPECT_EQ(server_cb_count_, server_qos_profile.depth); + EXPECT_EQ(server_cb_count_, server_qos_profile.depth()); } diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 8a210e09cc..7a5b4cf2f0 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -117,6 +117,14 @@ if(BUILD_TESTING) ${rcl_interfaces_TARGETS} rclcpp::rclcpp) endif() + ament_add_gtest(test_service test/test_service.cpp TIMEOUT 120) + if(TARGET test_service) + target_link_libraries(test_service + ${PROJECT_NAME} + mimick + ${test_msgs_TARGETS} + rclcpp::rclcpp) + endif() ament_add_gtest(test_state_machine_info test/test_state_machine_info.cpp) if(TARGET test_state_machine_info) ament_target_dependencies(test_state_machine_info diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index d0f26c4a80..2978a5ecaa 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -285,13 +285,27 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, /// Create and return a Service. /** * \sa rclcpp::Node::create_service + * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t */ template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] typename rclcpp::Service::SharedPtr create_service( const std::string & service_name, CallbackT && callback, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, + const rmw_qos_profile_t & qos_profile, + rclcpp::CallbackGroup::SharedPtr group = nullptr); + + /// Create and return a Service. + /** + * \sa rclcpp::Node::create_service + */ + template + typename rclcpp::Service::SharedPtr + create_service( + const std::string & service_name, + CallbackT && callback, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); /// Create and return a GenericPublisher. diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 1e8fc5f662..88d981e051 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -134,6 +134,19 @@ LifecycleNode::create_service( service_name, std::forward(callback), qos_profile, group); } +template +typename rclcpp::Service::SharedPtr +LifecycleNode::create_service( + const std::string & service_name, + CallbackT && callback, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_service( + node_base_, node_services_, + service_name, std::forward(callback), qos, group); +} + template std::shared_ptr LifecycleNode::create_generic_publisher( diff --git a/rclcpp_lifecycle/test/test_service.cpp b/rclcpp_lifecycle/test/test_service.cpp new file mode 100644 index 0000000000..4084b0da0c --- /dev/null +++ b/rclcpp_lifecycle/test/test_service.cpp @@ -0,0 +1,100 @@ +// Copyright 2022 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. + +#include + +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "test_msgs/srv/empty.hpp" + +using namespace std::chrono_literals; + +class TestService : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("my_lifecycle_node", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp_lifecycle::LifecycleNode::SharedPtr node; +}; + +/* + Testing service construction and destruction. + */ +TEST_F(TestService, construction_and_destruction) { + using test_msgs::srv::Empty; + auto callback = + [](const Empty::Request::SharedPtr, Empty::Response::SharedPtr) { + }; + { + auto service = node->create_service("service", callback); + EXPECT_NE(nullptr, service->get_service_handle()); + const rclcpp::ServiceBase * const_service_base = service.get(); + EXPECT_NE(nullptr, const_service_base->get_service_handle()); + } + { + // suppress deprecated function warning + #if !defined(_WIN32) + # pragma GCC diagnostic push + # pragma GCC diagnostic ignored "-Wdeprecated-declarations" + #else // !defined(_WIN32) + # pragma warning(push) + # pragma warning(disable: 4996) + #endif + + auto service = node->create_service( + "service", callback, rmw_qos_profile_services_default); + + // remove warning suppression + #if !defined(_WIN32) + # pragma GCC diagnostic pop + #else // !defined(_WIN32) + # pragma warning(pop) + #endif + + EXPECT_NE(nullptr, service->get_service_handle()); + const rclcpp::ServiceBase * const_service_base = service.get(); + EXPECT_NE(nullptr, const_service_base->get_service_handle()); + } + + { + ASSERT_THROW( + { + auto service = node->create_service("invalid_service?", callback); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} From b953bdddf8de213b4a051ecf2d668dad65ff9f89 Mon Sep 17 00:00:00 2001 From: Deepanshu Bansal <36300643+deepanshubansal01@users.noreply.github.com> Date: Thu, 14 Jul 2022 18:22:48 -0400 Subject: [PATCH 126/455] Support pre-set and post-set parameter callbacks in addition to on-set-parameter-callback. (#1947) * Support add_pre_set_parameter and add_post_set_parameter callbacks in addition to add_on_set_parameter_callback in Node API Signed-off-by: deepanshu --- rclcpp/doc/param_callback_design.png | Bin 0 -> 167646 bytes .../doc/proposed_node_parameter_callbacks.md | 29 +++ rclcpp/include/rclcpp/node.hpp | 241 +++++++++++++++++- .../node_interfaces/node_parameters.hpp | 33 ++- .../node_parameters_interface.hpp | 71 +++++- rclcpp/src/rclcpp/node.cpp | 32 ++- .../node_interfaces/node_parameters.cpp | 213 +++++++++++++--- .../node_interfaces/test_node_parameters.cpp | 120 ++++++++- rclcpp/test/rclcpp/test_node.cpp | 228 +++++++++++++++++ .../rclcpp_lifecycle/lifecycle_node.hpp | 56 +++- rclcpp_lifecycle/src/lifecycle_node.cpp | 28 +- 11 files changed, 991 insertions(+), 60 deletions(-) create mode 100644 rclcpp/doc/param_callback_design.png create mode 100644 rclcpp/doc/proposed_node_parameter_callbacks.md diff --git a/rclcpp/doc/param_callback_design.png b/rclcpp/doc/param_callback_design.png new file mode 100644 index 0000000000000000000000000000000000000000..485b5b9532a75c7c0a0a73ca0492efc77b732314 GIT binary patch literal 167646 zcma%iXIPU<*Df6qX`+IHR8f&ms7kMa0HFth^b!K3Kqw(dvw?*sAc_bU5S1pq7ey3B zstPC~D7`Da=gb3pf8YC_^XKfjwh1YdnKf%xx$m`Zn41~09XxT6hK7dC2(D*IL&KOs zLqqrS026qn!Ro6sb3d%FCwMUQboV7dU(xjrAmG3&26D3U(z4J1FIu^J zxdveWV+v^Ho`J5I{Z*p_VESI}!O8(vUPka>lo`g-({6vY!JYvmECIj&F*#K*9rz~n z<-ky)=l-i6p21jm@PqPVFnu7ne92Dw6L%?Fu)o5BCUu%y1w2Rm@m=;ho;!t>*JKna7G69J}9D{x2c7N zy%{k;6>Ei1@(XlTG*t>F;X`#1axkJVQqIQ+siJ~`wiAFy8zcQyY!Ox#cwfA>8^O&C zWkuHUhNIoKAruw#_I(jJkU{9i>D$iPxo zMN!oVhE&qW_=FnRQsivN_GC{vtgQu3-`&@T;P0;+gt1j1$SGnKFxI-BHa3PfSabIf zHx)NMUp;Rnd0%fl9Q?e2VIbB-S>H}Uo@7Mu_CU&ML#t5m@WdG*!P`V*4{sEB#Xufm zgqD{j7{TSRHWqM_8%oF1T-yMlPqHA86%awF0Am<}9O&h$Yi?yga+AXl{1q+nWQ?kO zfC&x~V2FaY1;*Z5SJ}WHM$tF&KpO`^2d88LM;POXp0+rgg;%JptDFhh-!~Kot;|oG zVnu+KV5>^;!IE(LFiWB(n8_HUgK_iq@RcQ4tJ((H+klspv=Isvte=@LLEqNQ3Qy5- z$68}BK6-XIZ?q4=#tUz)g0u-yG!C?oBkIZqxa;W?;lV@-0;c2>Y8I><8ldQ|Y#SPY z3bry+4D|B!Q-B$Tg!sZk0^Q{0@jA+OcpEGCU<-u+D_v+oNDoDG@S{ed;7%e9zzvtz zAsMKe%0ddP=nH+q$Ir?bE=zDF>X{=E7P4d=UzE4E57yY)#G7Ih>Zan2B`A<6Xj>Ii zPhI;!?OF!%#>k6oWr{NPSGM&vHBkyw^7n;>g2@$CLyZ-{to}eF3=F(vamL7C zMOPCGCEHNjP&nEQDX&kqRJ2rkn89siJ&2yVa!5rTe;gd)p=WB2$GPK? z3VvjHJ!2I;g0E4iz9kB)kFs<(K>1rLDF$Ql_9#qdc(Mt1rhSdxK- zj}94Q73vY{X66pV7$RW`2v;+hmm@W z7y=pMi?cPsm;@k@R!AKstg4QUzO{9b5>^LNE)!WVJsUjA*VG)GsT^ES$=ps3E~|jI z^nfcU__+mA(4izU&~`8#$<#t6NRJX^LJ76i4R%N2gVBlzNX>B=e{EG1T2~kELv|zS zDSPSKDPl4BV7#%ikB5&b%-jmD6O1s0tGbg-+;Mj9R$g#j52RtR4c5aR;f=Hhve5C< z^>%}VZj8_kRFWfLv9gM;p(ItZA_@ny^3_FH;qZaxCjfdI0fe-7U{QbNF zP0V5Xs%RY_WmS|4v~62LAe?9IV z;dzz4%CHNqRl+LkfcB& z3?u7lZDtNxAQhYi4m_ac@L;`u7&5e%AcTs$ygtSN++aCnJjN@~Gms+Zi#Jf!(RTGx zR!~9tfYrm`2z?z>lz}H%#}0hS8?S@ZM(a>KOyF{I_O4{KGDcY&W~-y1r>lyV2fq+N zP;j@S*!kP27-EPfa!9^ z+(SM6^u7FjC;>QSBX3Jnd0$1>V3lCBt4*+>o1UyP_^7>`0txte16$zM4T60=l!N@0 zK`f%91X+F+JrF8@$A6=Rz0d*t{ZH5h(`VcCB+}3b(HQAzTa%q;A2QzKZ&|IB;5?-z zboJ`6z^SU*+S<03U%%&}|0w<;Av-z;1nzFLByyf=y>_Y7VXgP)&&n7sO48% zR&i!0dDn;g!8NdeMRd@C7Sk?RJi+K2maeG~>Al;v z-F=oQP8IoQJc)XnebhS~?dO!Z{qEvM5a0R0$;pVF&9RPiwv0C%6U=sos5@C!)bH#G zHXV1I<(ltqpK6T%No=JarLVb@HHvvCJBw#NXp}wbyX?eDk(vy8lvPGI zqxKX`7cfV|b72iD4SN+?1o!excIcm^A3-6(jNcnpk=Z&p74G0QHC zX_M;DcXpd+cDVQ6hdvXpT;#KZ@^ebvKBY-JF+FqmTg>d!((f@*rTZf-D(Fc`FZZi+ zN`H+a?l_CiTCO^n6rIxiP*f^C88m#1^fs zNl1ZR{i$p^d==z)3qE$7O4+Zj>$>|-V!=;`+wA}JNnRW0v^{0u_-6RDcWzO}y)cq% ze;2Mg#xE?y`AvD<=dV7dzfT^9Gt0NkLW&ajD2Kkl0C#)LdGwzV=u$|(MBWnkd&Oc-OOw!Ofzs=;ypZh?C#uzlqTQHq+tmW|PXEnKkGwSnUGlH)gvHL5WlxjDeeshzGT}|gCYe<|7aWqc!Hv5L zdD_Fqo*!IVahjno;3(7R+SJ;AvaZ=zl$9y#lOS+* z_+|h3C$Nvu^|WtNJ!7T|9=_E?$2%6m%y{Xf>7=nIV++=LS3LyVD}_=clfc-hmgXijA<%PjLbNB>rM06yEze5 zk^QZHbeU0pJE{yFQ&Y~Sju9}oi9oI=%99fJRtBxk!Z>zywr$^V?g?vbySpGK$1`xK zA)J(;ozJcitbSh6uU|HIxBR3)UWQns8+R5Ec+fIS^|o^k`$y{0Qp7*H5B3^KU7&^v ze=+{$vclI(xwsS~7(uKLTbnQ2;#euV7Y+w1`Bm_3;Tfu>-$T(2qllCB)UVqX^V{8x zk<^#I#k{*fs=9TyuMfhHg|vSEbBuHrvcW5;eKieQzHGgV+?AU`{Dypk2aAVZi}-%=PeniJX6*iZ4qAvaO{{cRDR zjkaO89EU4MGtKA1dz4m#9EpGW)sm*&^R%*N6Old5hF^Zs*h6`0V*$+UT zFFUe2-;h}zE!e6#aSsmTC!dNY{k+~>(_P%Fn=fi`n^;d-nJ#Ox-;rPfO9qAq=tpv- zIQzCoW64l|Fcybgb%@_V_eI(k-a!Gi`NttI_A&1n$Dkd|U>b`G6_x9DK(ZjS zo%7dW2kG8bhC|S}Nly>S@XS`~&PO1)^X8Ljc?`unh2`pbkrjBA{73K6NGb{#;98?k>yv{s_Vi^MpCQs>lT%mHF_i z|DF}hEq?8e?W2vS{dGb>+qCKK9tfVbkE&#u+|?YkkkGW z_ryl7{IMiP7ke_He?_6;gfnf}KVz$db+&+yea7Guy2xJ1uXhWW|Hc3H!h7C&@#;-t zaL4wY^RyEGG4eWC^W6lk?FT;Z8>O2?91IzGZCxdPz4wopH)I+2ka+T2T@(!V0P8PX~oQ)Puz z|B5U5aC*MifzY1Qr#u*#T>JZ`#E$_&<6Okf3QBq9i&d>>nq%)t@^CahUUl4i2VbzY zzP;2rcpKI+IkU92{2966b3|`S3x=&(J|WLLFDo zkCM%_hKQ=;C#!5;>J|JojnD<2BY*R7ZDtGA;(g6-={z6F%IA=P8hKfd+cagy8|;ig zT`PWfA`ir+vlCg7!PCwT#SZUv7u#%6AByf>1eZq!uC-hJKXS@>GW(SD?fvlUD}$J< z@NuuK@L=UO$x`c+Rozbww;F?HdZj`HT?!0E8+TS8@I`L_eOiX8FD%d8OjDNxfn>FU zPDKA4hvxS1=IY!)jq|&T>+C0|tDLel!heuD)hB2^%gwEUoriR4QtKSt;}m~r?*2_M zdF?SMLk#J23<>Jg*vfwpHebE7mLE{uTiU+>?A(m@#^y}XLSfDH%a;+}S>dbxeNJHo z)Gd{f+ati_O^un}ce%{+16MgXe)nR4(dY0>8)In#r>rzKCq>O(>YwY`83E!~xl3(RN39}Sn9$VCcm1etEwEA#ip<(FqhX&1*8 z1Fy;GV#B7gv<6K7`@xW*aDnv0s;uO&wZRZ|6|wWj8|#;Q5VcDk7dqR|&*T?0r7W|@ zoD9AvDw1}fA0bo!rJ$_GSfhj|v7PFf^M$;#(l^*`C^+X7Hm69vDnBI}{LPc666nK5 zB=^Y-Nml8qM8KY!7;?ggCD`p7oI*o^DxkItxK1qx?O4D zlV;N2e)oh8jpP$o&W??SgnY-I>C5Yj4A`cl1`ptr*NP%{+EymJoBt{|yjNVZ^pULm z5ioxrlj)C;Do+X9oXQ=DYO{E1P+d^*{bR1wH`kbxmjYFXH~utptd%e>C*!Zcr@x|{ z2iKko469MMmujudR9j9Gn`rS}kF1{w-3mnN{=8uw1lNC7V`te)`I|?w zs*A?ZeUCdv=oz)eR?!BIZ2HFu%RHgiV&4TCvpo^uaU#2}eMV|-T#2?pIes;46AT?q zimzzcS#=PUYJAPaw;p^ZgQ!D0cqru5@I~<>1spfq9g4>=j?@;rA&EDt?z07#kG?ur z@ft)_nzVaE-8H3*uD!8koN+I*dBvb-2OVTQI7n=9TrN0B`>3Ux5`~kQmllmG!GYD@ zlCEi`_jq#CFrI%(Y3LG(L!9=mm5JQ7EwF!ewNWnL9u1c^R>@aU%(Yssi-(pX*AES2 zPN;D3DSguQ)tb5MfS-DNCMqpOo#!GkI;M{KWME>6W6 zGo`vN;IjP**0~(Xe0g*iD}Le)dRq|gl|nPWD?)72<%!`{G|A1 zmCy~F9*hHq`!k!M#t*}DDEbaHfq1p5VgawYXJBzE=-!`P4U>b^^DW;{p~I1pF&fumiWz9aX(*NLGs zJ+Dqh!#3%J&@^+vs6@uS52H6AA-AHM2wXJo;F-^P)rK$Pm| z$k9@WaTK%zmn;05NCq`;Nsi&Jl3?3qjnyFF*Cvk*kU`*-6;<``XbTUl=8CZV$31sj z@IC48E3d!|zOo;<<%k41Z3lfvBXzg7b|N!O?F%FNXHcJ`#`n;n@U@jkcf38HToI@3 z&kSGvwi`ktV@__0qSZKex2|P7j+$#t-=Wt`r+njJ#Po~YOO7HDwDsUvpHun7vNI2e zE5_a%6T38vyK#sKf7pVR_>lbj#?FH7^+VY3Zvw>hut%feJXN0rPPTqTaAUKM9OuK8 zB}M)n@+)m0A1GkTpuMhVZKab<5U|s?t$XBO_xIh>+NZE0a>bRQ-U^59K`V& zJ(OqyFvuNOT!)o$^vgH2&Me*^I_{t)KXULk(3>@I}{AvY~735}G0sXJY z@`&!umYQF$@J9+(KydLt5zbf~0sg|GeSrQH?TtEF&Ocl4_z2`b{50~iN4=|7zx!UW z#O9ZLJg0GvLfu&l!DRSfCsgF*wCK7TD-LhJW)@sO>vS+NC77o0mP|ucOv>ZzXjo`s zwNg~d7fxEI!#Gy+oJNpC;2Arj2LJq4ZwrZT%Vv8_UN`biQ zeEpgeRYz08PVC9O@^degVwKnv>jZ{Pj>xZwUre*+t=s16x&(ssisSfsu5Z`UWa*yM zc4~!e>YO|DPPDCqp2%dDeDE6F|4C1D8x4kCk~geL(_;LWx$Cu~wtuFKbf}BAe2X$I z+tI)T2-D^X*iAS?;pDrznguDb~3c{a`<9N+bniOLTEz>?-E zm#=C4E7>5Wph<{2KYC)!f8D1Aisei3vHNlD(Scz^#MlQJ8cdJWRsIjzjxHT4>%&*V zPx-#$ctX(xKKxTh(#y*VtfTA^3-xR0rvWca;ypz|u5`8* zUnJ)&dz$XYI$AneOX&SvbX?Sq3B47TW7mLCx4cMGe`A}Skr=l4;S35$7Tt51#;H?v zf}bPqbJGThNtU^WwGFZl3AZqW!5Rb)Zag>Ivfv<|e#r7d;q1kb!JxjC0EU$0w)?GE zr%MN-&SV9aB>ueW?L91aEtuwGXw!WPzi}6VT^K&b72v|n#iqt=eod!_OXQc)ge&7; zkft%~bE43zHV0%Xg<)%Cz(9bGtD~5WB;h zUdn*v2qZ*3jXz%4Fke3YyLeN0BQ0S278Iq&Lby&Io6O-?(}-D0dv>I~3>(8HUAj@I zmc~z-Cm%WyHyLSUUYxpfjprj5i^)gE9@w8Z8cr__h5O`&?^#N{Q7vn+<4Qf0&2WIE z&Rz;H(l(->NW6K4dgje}N17s<1)7ZLZ=;#dM#Iy*z2=#l%mQV_j{m9~iN8=IQB)_t zmJ_Y##I4Wv=k%sBiwTzX5Boi_A-V1zJ%P_iElJgkg!9v9vud){n?ix7B&hQb*VI)d z)_tg}OE?g;%YF!x5qtjh!H3)JN1|ZN4B4k1t8M>j-W`j)%_c5S^F>dKK1KH`467XL za=PH)h`vG)&6cZ)O2(-)n0z(xJ`PWNrVvg^JI9vRK$?r{yo7x?ls}_6Q{%|hLH+uf zmkyCv7e&tDe)aJ_0WzllC?#^(_MgBE64^c~5t2YaJAWq{T%2&LDM6S&90As8`^+r4BnLVE4}H%XIwXIhUHOEZu1MLz9Gt&QOkVn)t9a zoz6KyJpKFFtLpDQdF6CYtK_;b>{Z8~5N{mtQm5%+cyfA4Hs*luZbX|@<@y14OJDOd ztSS@XX%VC>EqhBHS%b>K;0FWbkvR20#=<+NpP<<>^D8&bWd5Pa)M*$2vhQdP8CBRE~pmJF6fyjC!fz|5$;KA zrgN%kEf*iUv=oRn#KRD@6-9{NX?tz*X~O2Dw@6(wP@s!X>G|3D!)A*G?$OT@ z8aBKl$=WHTvtLBx4$`ETiHGKh&RvOUxtU+_(@=diZ`ac|UZh`Q?63n!TpM|}D)CZM zQcpt>Sf?BQk3F_}SJ&Bd1E-w_y8pjpV^+TS1kK$lT}z+a{Hi62bVcdU(8_kjGu?tK zMg7-NixP$rP45>PsXq;8?)(%=i4mZ*>Y5)EB7>+uB>TIN8}D!ph@QiUp}--K&mwBB zr=py`<$f00p?h&O>Y-i{l<)}{Tdki|F-=hry3^WU+Ht<^+6-6tp|^~e=~6+Y?U~g> z*N?al@`i9tn!8OGU6i<*(wrzSVzOt78XID=X+7vlUTA{6Xnldc7$_+6u z9!dI_^klIjWD6k~i&MWtKy+Erk4H&pd1)LGQnGm6%C|w2mBjZ)Ig0Y+ zGHq#VM&sv5ONTC|l{eu;-XKZoId$u19w4U-l z_rALQk`_a$0f{w57Jg*Lflxn2d)A^Y`-jjNBjX>0hP#NfE){;wAM)3#eyqCI3z|4$*bBdw_mKR}}66yJmqX*ZJ5 z@qooJrGV{FtCSKS$T3Ady>Ri?#je|YBlHJy36r*ldYlli;;@6ArNA4GFBe%4s|8MW zb$|Rj(Ca<(jZ<7qFQ++q`zG#)*nYaMzuN*KUVF~gc^{sVP~t$3VZyUZxcHRRSVvGjxGk-ry6sas~(o4p02+l_u1D6ulrJ0Rpb+Ix8tVr^%%>|Lt zZ!rMca;%&%01%0$-&WqT($ExzP`&?!&M~LD_qd~>5zhz z(X25rE)+$^k5L&ixZU#&DU5Fm_$C3sFWh(iBEajBO-0Txn6b8+(o{(>F8HRA62h6_2`}!k`@whD9WE>sX740 z5oT!!L`AMj>>+~}HnrMn03lSs+XS&P6qvp zq*g)yJ3DB)Q;eq|#Jvw7?)qTlZ0y_u6S{}XfUE`@vtpdhkF)i~U1VGY_qJ}AN}r$q zZ)6=C+V(QH{YhjmTg*{wyBQYm17Ijktb?1dzo9dA(};)R*7Gx z?$6~1?(wPtYqU=d6ILS^dJ7y)zG^)T^=|@U{Pq*a-G3 zw{3a?3yS~dnHIFOwN^xFmRlw4QL=7l@30@qnE6>BRv2fhQM>oW4GFJ;4>N;`8NfA6VB#(VO9pzF~pJJ9+#k zKFByY769xkdG}~?>3+lw!O7r;1~Klq5BB9E1(hKv0XVhl;-=8auE$TeR;=P*uhLoN z96DR^7{WgL`Eb5ra3^rDOZTFj#@|g&K=0xp96r4zLXa{kyavoc6@+-2!rB#Vl=p(7q*^l8{qDyBh|E_M0r8(CK>t3%CyKDER z0Hir|9dy5OK`tf(oc0UK!S!C>%-`L;Rj&R4sl~=(w;~p-zMoLFEd~zX6&A=7^f^Y= zF^)^n*lcCvm2HBMAGSeaHeX+K68r7^)nm0zrTqlRH6?GCHS}y6&*TA~eCSm#0QuiH zrd02KM$ni4;^h2zA66#ZbG`Mv_PfGdmdXqk<<%;H76aHG%18>HD*JijRTwfxa-2TX zZfKwQw-#WJ*p3-bh|ko_pBV(IM}@}Ctw#C2NU)K;;Y5e#Jo2CkSll9k3q6OHFgd(8AXeT1WxV}(6RP~}-wqF48W5Zvryy(r{SeTR` z?VQ=WVHqRIsg_SIReIx!k6l9l!M@IA%sO_7B2MZ#s1SMN0gqc-(I>W;C=#xVH|$aY z#2TzP%Uu9mnr1;(c|`A~{mg_xVOHBk>c8lD7>jqag%Nh4xcAtoBRB7ss;E zi9XurPs_mFJM(&16{vS%)VtNc_IU_f<2cDquZKMOD+-Y*ut>Z2qQUBMMek5sOTV|4 zz)1|E(P)FUxdG+MzT%VD%Kw9q1IYD1)a*u@u_MIpc?f<@(}RG1*f(arcmYGy~XhH2l0T) zdwSNAp+a1vbL39>g|#1~L4cKx86*hIjXw-=T5IOetODFf8?OATEgk}C09^KPrmx6( z{*)EY=~4Sqr{>V0|NE2pJ4UDd8@Cs4ZH+%5{D2Tp>8jz&FLC4TFqCt6dF|hKe1qIb zwO`}+F#D{LoGS7iA}D55HsdnsJM&TtRT{p|>oaer-sxUH_Mj3n3M>Cs zBm|goUq5{)?@9uo??Gw9qaJP=edPls2?58_ZB^SEkMJdUCH-TOCd%0Dc5G%T4_hK8 z(tV}-@=!7mdlS~p*s5bIoKV3?H%(%vf{upXM;E6_<;oP7%J|7IanC($&$;NCYQ zsU^ymG1YfaB7gX2#1@XatQ{tr5&dAPT{)|Ed$Fzl4&w&xW561;rAH+jZYQXY^D4W& zJrL5NI#Je(q{nQo&6ajjh={kVfYJE+Y_5OA3s=;;&JJ8L#5O@Zfxx=5NX~0E1iYl=VOE7g<}`LAE7^!nSm=l_lZ(-IAp&NyL;ha`th-pabf$9ByW)^?l>U+&h3 zluk@3V?jBE<~VrW_uQs70!&oHcopPpK>&Bi;XPtn+h13@eZ{mlS+Wi zY&6yb97zM$oMb34_4-`+7!^LGMfG|ZWE&~fqsHla%MVa!%7Wo+N6wVfizA|{Kt5j6 zIIhp*IQm&DdyLD&xK>X@2GBPeL2n`Irq>>G(vShRBS5J8yB)Ny26$fq7nnio^}6@= zG%Df473;rm)qXgW^&<|}p)wCPx1^d5IN69u8QbByX7&pz8n@NwV5>ix6+WAc3<#Gq zZ^8xxKUagwjxR$T@+{Z*y8w4}C*dN8#fKjf%m=Z+e$e?tW6Fp#Fvi&naITJ2cfW_Zo`PfjF+pnI z9b{Nl=#N9((PwK;?L*cMZa2h!0APIf*B=ONb7p7mzXXiv4{&2=sH?4_3G1=;!>5PD zoMDfSPF8*Mda&_@2UMovj`DNfSBgq5qrIBrC?wdGd|+zqu0x^M)usBipEH1ySLb+t zW0LU@zPRPsE?SSXQ)$4jAxn5uaVCQ1=V-nYKy||#oQ3|FU6}iFkSq6|+rdgepY2hm zYe5j%3}Fz+4O*A+TCHwSZZp*EesE_ZeKh~{G*6ZB=o;X6CTXREV%Ye4PyEUdOAxZM zbP;Og&{%JJ2pCTdQx>eg#_qa12MKv$P9f6t9WD7_6~lMwl2(!Vsobm%>)!m7qx_QQ ziK42N$2o2uRY`6>J~;KQG&hY!>tr~2EETnlWIxh%O*}WFN08~0e4Au}yY5@X;nY~a zp66YfmUXg9fON6o8{U2|7`ZOTG!FTuqmPtWRY-!%wEb~r0nZN~j5*wz&LcusCsR8s z&oS8D43T0p!coVYOcfM~>8)WjihB_4+NNPmng_PX(bnM`D8z}xvr{uF-w>{^AofoF z5blny`1skqnt?~h`AclFK)K)LoO>rh#m_%QK2Ty`_$U(Chn^FUSG05DS{sin@j04C z7!8{YMVCh*MJ$Y(Iiq0R@3u9ElPP)}+)~ENGK{iSBn(@r%>-AU{(bw^A)lhzN#u%Ek3yB+~#b?i^NrwoOCL^+=KS1yG`4lGCvi z*~*;QNlTXlsFv*D#1Y4k!Lj!-x(rVJX=NPjqP#`2K+{U&ikQjlGd#n)0MeY~pwYPs zTNEt`1N7!v_e@Hl2T^g@L_P4BH;2mU%bD;%p`CW+q1i2Fq5-wL**sPRb20i4Kkp+} zfJx>sPaJ8|SmUv5y}Dq(qdiN}j+isOk!;JmBJ=rbSwrVBMBYws4-_a3GI0o~rE}-h zz4;(?&G6#K{P*Se@e)8EOUJjNI2{_Z$t*~T#L#Ml6hF(~wy)zC?A z_35#^O6SL@)ZtRVMj2i(vSnPp6)we&NlI5jvQDr$KOc;c0@#BJsF+ z+gj8R%^Ihw2|D3n$j@(cppv7YPw3!1o})~|CXI>o)ycGfXsj)ju13)28u95+7&V|O z8a@@`qXb(f0p|P9#-dF&_(tCFc1N6raArb`22Iut+0&PKoZdX95n2X7;V0|f(aqz} z5vP7?sH zve+A?Bk$%wVZ+LcL!gG{L{_FqnRdqy5A}##b;MQD`OEQ>c3Ps?Q?0N0L^5tTiKWtu z0UX_54p}7z)Hhp*bXtug`uO2=3hX;)E$A^s0Jp7V-K=>#__wVddtyMjh z&xTJT#pmLPOmLa9E*${{?$_=8L?Nx4H#vXtzhhGAAYYH2%})`eakRYqNO$rJt4!VS zLP6#M%sg||r>YohiS6u%K3p9jjj2gv8Da2dRw3D$OI-duT?FV%YUxH}-O)$A%yG_w z0Du6DdslGYNRn=nt~Q-*_zg$M?IrJyNue0>OS0WpbRm3lQKo%i&-dlLVDlVNPT5QC z#yJMIyMf7Q8#QFIT>PzMd}19}J$6@vqR3(rU0SEYv0l|<|UB$OB+aGV^1-Q&ubrilc+?FK_b>tz-R-|RZng-vu;E^ z{_!5fFB2p_`hX9lPvxzq52Y{hG<6`BY38$^A81lPMm+7v6fP8yes+kH(JYX1LWKYl zAps%o75L3$$>C4xT%PT(>8c;xQ2OaxR6otsp?8CO@KgO~0gDb0G+6p= z6S)t^_r$qqhKfNYs%NN}rN~fskQ$HlF?$vQgcVli$ewfW>0Y%qttoF@mMnzuWFE6_ znY{=C?H;~ud1W)e#0efgOKMKKTh+^q7JTcJW;ZY+xm#40tsV7Pvgw|Q_?ySakiCs=nYkHg*Pus;-#x(g% z)CB7bHn$`r!f;|~QRn-ITf}EW+1ZaGN7uyE>2^-%7=oyMN@hxcCU=>o%Sbc=& z>Ryf4s>g@6Unc6qY8z=rkKY7^vbflBEuRwwE$M!Oyw6tmczZn-yaDCJZ008ap9mr961-o5&N-?(dYw> z%PFed)H#}(G9IUO!w1WrAURqKin$nI@mPz*} z6&gB>Mx3sBaIp(Qi8*<{an;Aq()@0XPdgCM_(jx{MOJ{9^~-nz&B|?NM);>jENf`u z`xDrzjn-r)`Urs%JF4#}Ct{oYg*KziNDF#nXKq1do!%(_u4qZRF&$)o2*Bw|Q+ZGA6Y zSX;kKYf`YQ0bpeNtbbrss7iSCaH6g><}p5%RBGbAUq&qPtP`kpUu@`WpNXEvfaN{x z6ZDmY(bj$?*v;%EAN%w}B`IF!g*^OLK)Yh!dWxW-K&Z!q_$SPC9dpbdY%kmi4>a3) zo_bJhb^U*FTX(*z<@Ep^P8>^Qycl5So)Hi@!=SmVo%%IlzYmIMZ9fxc%wWu*&gq-8 zN_vA1mEPM+d@>T-^R)}x2-Xj@4?hL9?`SN2f@x^3KGME&sn93DHd*jO-q|m=Zs99MbAO;ZkfR}I0UXDj4i48If@2`q@?^bh zGBufcYpySU&VEObk6mf4jwU~nx@)sW*E@rrvERbrWP-t20NgKOn_jA&kUr$L1z3i~ zPH433>_LeWxBuR0mKbl{uLEYal!EVd;7Gx5Z3u%Zz|WT()-P*7RvlA51n@*G)+hYY zN;DMsmwq>#ok0&+Q5AsQ!R>DB^=KpmGEAEe82uE>r$WSD(UUF5K0#(DOr*SGuc<&Y zuL1Ny>_{{WCxhyE_ko=9davRkdBGPZzIgz)^DVFJUjYQg9w7s|st|_s{!qIZM0eGD z{f*pnCyW`OcVL3k2B=+GKX)pa;eJ>C8eeVZXf9illHLZF4%91@3c<{U4{Q6*bNKAH z4J|n&M(ry_Yh!DpLSv-(znD72Te&#K+Rb1AqKXh*Hr_<-^?8BPXsEay!qb6Hp_%iS zEI_AP*0Bf70M13uY>hMD*(>aa>T&i`B#=BzKC<}7*=f=~o@P}AZ5L3--+{w>Bv-np zt~GQOphlXFAt#2x+K|p%n;R(v2wuDZlw^>R=i;!^HxF)VsQ`5i0(O#-8sF--=4%Tb zD)IB6Wby?CmzU*tjEbpuMyS7=0=45IJdS}DEh@q?yAhzsxmJC(zqzfkp1C1Gef3`B zI|y5By)u-G%$nR<7ujjEl)kk+P1`>=Hzb;%_FKAs*)jx_;?`zGY<@6R=+U`*GnMe?5AQw>USZ~7KCc2N`W8^%F6TjRI-2roZpzp z3W2KUF8}~z@I`Lscj2auZroWzTMmf`EDSPSi%6n zZ3k(Y`4h!;H!CNf+^PXsOkKMQn}AwB4%F++=a+T_+;;EY9gL*z;6cE7j5yyXld$Hw z7zc30W*jI=e8{dsuLeLHgSEHZNU}P>NB;hRYS<$+r=LIlago$+!+kY4J6x8toW1Ac zyr6tN0QLqUkcNZ&EGvRSNCS|k|Fxf&R=;6DLR3KlE30#$Oy!VPUOwM>##i~V1>O1m zUkID7Llv*jCxVOJ>*<`i%C%L~yZWWkjn5sraM6al=}t?8kqPL!(RDl|=nA}hdwu~h z7!i4d2>Lgm)a@o7GyoBH+`d!Gc~oRTzXJY6&fNg0`0<5SY#168MDM86KGE3zqYKqV zR|5yy1xlU$*qA%OzOIjy0?dntX?Ajl(6}>R@go3H2~gYGIHm8chcX(s<}UzjV5u6U z)a7j6MM{laEbX+k-FLsv7L?LWTv(|4jH~)}{b)(?W*mAY*4QADU3D}u*f2K4cKCLe z&QPqIGd3qx?Z^;-eg{Ea<7AfkJ$%j7G}G+YhOWt#ugrp%h9Ocwd26<`GWap5INvg+ z3Pf)E?=hNyvav3&^m()efEyB6Wf6g1u(ccm@HiP{re5ngEe!kZOv<#l@T{>Sp;!ljuy1ZMEHuJrEp3;vE+i4BNw~Fg8_Csci1<*Gi%GmxGfmPh;J` zKz%(?do5WYW(SS7Yp3UazB+oJ2lSO~NxhF~+k>lS(mu-F4L|aEtMF{&ujZIqQ1<;T z92Bw@qKcdf=->m)LLLB(F1+yOeg$ogp3&W-(Y*6msIEKH&9P6uA?woO4W#fQQ0lPO zhPQ_i)(;Oz&3&znAEw-rEP>CcaD4+s9_DMqS2)t6zJi{t5rf;jzW^+p@p5JYDrqVJ z)WpMcZ#JN6b0DsSu$h9&bEA&d9eQa{C2V{OZ|!W!E#|1Xhnh8qi3W*k$D3ZagX=Ok z_5hV1AnhJr4z#ouh;px+q`8+8u? z&5Ak3Da%>D*Tc}DpJeb5?9;HB+Fv~?Rqt32KFWMl=(K(dC`X@qt?+22iJoJ5N+foE z`@QC_MK^~)f?+&%U4b~P70K3M$RL@}`MC&`lzo^3@Me4?{ovXFL9?$EgOm~7)OU`$ z|5)ZWTR3fcOoNMH_UXsr!|Jm4-l#yh`n)rBGfnV}2O`je_sl%AR!c~7Qtw4~0CB7& ztWp8lhX3;1LO^nHT{C=Q6I8`lrm;;K{tTTssp$o^MLh6%`GVg9RI2X;N)Wwn_g*BK zMxGRTCBDs`50q-+$EjqK-`Y}1-A(Aiy*@!h!SF9_Q5WA`tOc>jZbX;3kaN`nr|U;r zx*cBCfNVmt?&0qa#Mg}V7rVu@Mtt%L#)CnXtnGcTIGM$<3w;+!aSMHOl|%DTjdGEp za8Fjs;V9dASKx8hXp$l<;?)-6MM%)8a9L17=r<^7m|CGi93cdBSF%3wQ@vCLKy&q~ zcDjyFSNcV3n#27CCk2v&$H2p)3*9yQKLc;M5Id`HsH)QMa)s@Z zvvcU!QJLiYzpng7Y1&4HDoc^eY#W09Z@CELP0jz-0%#1ozYGKlJ>b3qrs zlO}MT{Ri*M7EUd6MP@2!;ep|LCAekpb-!gsm~ve}7_m&Z@pPb7wqJTl87axIqc2mf z>0#y!;g*qq6m8b)L-dk%6&NhDw zD1?ICm2kNWceZQpZoiTnZb=l@RrA;ex}&~MbCvEJyt|2_pr?|IIWEMoPCsfMC3h+6 z{E;RO&ZEPuhv;r{)mOebC_#SCeUKBIS5UFYd6xeky&P>t6z{=5yoov~=Mn)ctg*Nw zrcWot{Rg7HLJyp9I)A`KbTOsXv5D^*RkTF*2X|6Ejanfqzq=*}XhE>>eqi`D`gdK| z9|%?Iquop_IzdjGWtOL765G;#2uQs4zX5t$oRNqDW-VSjmi|*VjMsL%dfQ|gXMt77 z=xv~RKly*ydh58Ty0-m$0vNhex(P_Squr3IBP5hO;C zR8m?bWN7KGncv#reLv6peBR4HuaAm*?R~Db&f_?~Ck3tirXcR-MZAVSmf`6`CE$_? zsAm&C>~Rk zxiumii8K^^97Q+jSYL|6lQ6xIwpw3K>&-&G;|*SKAKUOlSUQJ_^ru!Pk)!Qjt_QJv zt;|EG_Q_kPl}5!@6AlDk>xm4kO;?w{R0(Pxl}Trt(?L54-|Qyd*Q#@&CiyJs{^y;m z3K=%Rj)KvXnf3*B`p@M1pAtj+Fpq52*C(H*z^uqo2<(++lv~FnQK;k`ajE&8xaVI$puQ?G!h4G-8M{0GW`L9Gm{7mDroSW(EsSe8r_Y zqIH`(EDSa?0=3r%E3yqfcWpk7)MVA6HpcwLI#7QrhQp-oFsZl3=fneFUSnePg(aP!9TI%Kb#(6z}I-~dkzT;F#qj+0$2BQZ)Tofscpz(mF{n;B@b;i znSsv5R))k7l}>HxDH&#STewZ=lS2jB2`pv>6qi&c-_ZN3^38Eb@sT{!{Kbr+t+|M& zuK&7e^ov+4RNsmH{S!s;vC*`F=v$Y0RKfpv5#A8TT?gTUpNGZ zYx#k?+s5Gm<}F#Ks_8-+BnhZ&y+UE27Ov8Jvu7q&{gQS2RA~4T=CIG134bn4{D9#M zVkhJAV-L9g>RFt=Nj(b&X?h=Hdm#Xzac z7CM@?#qeuVK%wrifVV!^LU#Q5fRGcR?P0&GA|c)P@QexHs}e-i0WV3CjGL z5$$At6kpyvgAzRsfz7!;VS7qJ=F_N|zP^4ktR)?ac(`F&GE_ALu#D1|HCHgbuv_FP zILx2g)A5U?UWC;t1&_!C{|E z!Q*|z&C!LF`Zqhs846uzI6Ae35g&9vXu@6ALXbUvup8-gJy}q(cV^_Z zWu7ArAu^yd*8hmg{P>eNvR(D*`z6rw4tN@SYO?9^E>F6`Q(ylGUAMx^-|R}c;R+1K z2I?ZM%#OqgxGyjrjPXc4?0wh^fh6`sS~h)djJ1Yzx?F%>9Zr0c_%mlSCd`(H$>{X~ z{4^eFzoK)|tr4Gsu46A{##eFk8ER<;mS6T<2R@bM4EcFokyrYkNtWQ$@H?Z1B1-{I zJZF)myi%SBw@XA~Js@qsrw<={g<7RCG900#W-hGTzeghi56=)-dVScy@gC+Uy2Lr2 zj>2kZ-h{uB0;*wDu2rr?hFBUJw%VovsUmb&a5C1+NgPb*q-^ z$jS@~G$Ao5E5;~NHgek-tt8Lb2kTtDM-%~#Ibp3c!IIKfUQrEd>O@iac2$%Jdy-k@ zsG5Avz7xI0Y2u%bVziasf5mB}+l3q@QD9)|^y+aKnRdm=&G0;Ve}Df%w%n(16_ zmTgvczR9(gw#`$wR8|`?+n$9E)Srt3QblumG>%rsY3g!LSl7-VFQEL=6f`H75wwgX`OTR1|1(6hLl9o%|xq zV+~mz>Ygte%skgIWpD*M77hN)yD>ZkGH{1p$5mZ*n2Z?rM`8t(<6DwWm{I2Cpmyd7 zj(k#1I4zyiC89H;^wCaF!rX{05usHi#S+@m^mnK<43V?lEGB$LtiiD0Mg_Vcj&+&b zAemx1U7d0vT3L(MCW>@o#`VYcWPh(99nu9cG>IX-))NK9?nh!vmcrM#t8umcZULrG^7lWivxr}#X{0)9AJ~lQi8s6FvIfm;_hMD)JuSXalMtTSU>ihk^!~zrS(-2Aob^z)gfD@ z$Ws(=@boGMaQ9vzvkB>3ZoqJF&@A8m z_^+D9c|Q%$edI(=djJoF|wn#p!i zugLL=Y?H72+h+B@DPW`+`BHm`u_E2{VeOL$ACI5d#FPr{+?a5l>^>1h%5x>+&+yVJ zbY`9wZxfv;N7%>}ux{K=Bw@5P-SYGDz1XVSDfUgCf~&Yu!OIB6XKWEb&%%l2N-DLp zzWb%GqY!$)*X29lM_J03(j)3_#s9K9@N9!3$3>WUqdrHOsq_}Y!9;<_kH^0_5yTyS zckW-Io8b5x-RkGrhDd>b)|N#E6<`x%IkjUL6vNZt%fk8K)sF)QKRZg7KPcGE3H+Jn7rT%N@;W@LHXI zRAs((4d`h4lG>nwXu>U9f4yBB|1M!X@=z)M@YqCh12jyJ!w+J9M=e(DdYWY2VeKENM;+9%Fulgqm!YZrEP!c4?M zR1Tw!}ZeAp@B)YnZ9=4CNfxTTpS%v>b>6aVipZ_iLA+)LtS zJ5v$rV>*xM$v6hS;IJ1+IG*ruH>)#ZR6fXr9SY3K9`QUy zh?0JmrzUlzV555)ak#6@!-45i=pBcV1RE@}+3ZJGatW_* z{70uyx&3RJYKGq4 zkh<$H-GoNs4~Id|DJ7zqBqiE|Gkw*O<-{(=I(^QOexS(Q2G3ZEcnH`tR}w*sSxno(_$7&YSG zEiLP^+NHIyt;QINU4h#RW8(4r2ggq z&bKrHX^IUb83TwKy5rF4#ksf{pgAyE1p-@2(!<8P5TOK@_Hx*GcpWLukb(Na*PE#F zS-TKX2+Hdzx+pRcegq5lyyPF?X>4dzgYovnV)b~a=eh5}9k6^-dw90GPb#Oc1e$A5VHsQX*fIBe*&+wv35^tWb2L)){3FbcD3$3oUo4BQ z_$b2?a<&!u<9nU%)vtDISY3>000IxY5|pmZhzRojz&dEK9X!_xK^y90B{uo37(Svx zc$G6-p5rc$+?k`;(X|N^p$@oJGl$yIx5RKh9_?woXv+noV3WQAv%sDI@`@p<50D1l zp~<-Cna)otV{FqOc&|-@4B;Fv$GoW{J84H3=m33P)P%h{R3GQBW}KrOB5FXSM3L=W zB*P~9mtp&G{+;6ru*nM^Z4IB-=rysz(vge;9NMr#QP?#NNgvJ|Yk%2Jg(S~FH~`_kZld#-)}q@1m)Li8|4CuG+1%Nkuz<5UuV~K2ywMwYTr9YmSeF&? zO7&AyZ4dL{L~0ZTnx9)SK(JD!`c=nzOd~IK%^c1C3)gg7UdnEv+uSxfY2DWk>69Xt zdHnV@*f)dLGN^3NczvvuKKsgJtmXyhnSpY>LN8oc0ACu^LgOBr^e8d*=ML2_XIan7 zVrXLtIQQQ-)Zord);G1bC9LK$r$SCpLXQfFYyW2XRd_v4etI`w*BS@|{8d5E-2l3)k$N~#?JR>n1l5- zR7(DU`apf3UXEk;?xWXhm$#$WrYC~^SF%&v#5$zw{s|^b@Z>4uq^}v~!mA+U#8V-q zoA)7ov&dP}e9lV(CI%eiq^yjnoMY!l$L;!Kd#7_3gxybntHevp7DC%ppdnK_5V`$6s=M)yew$G%&|MxlHPGsW1#S)&CD zje8(e2d*=aKy}qn75ZFiN^{<3PIka~@F1bQQ;3&MgmbEEwU%3Cqq`?UCul?(wi4WZ>_<1RXI{p`n0 z02cFa7{{Ed`SyawjsFn0vpRX_NzLCE)oopn@v{cp>tGj7A2%WDVkK`&;3Of z|FZ0dNVf05bEI?nXKlsiKYW){JsQ4`2hR6p(AC-GVKo1N!nB8uw>f`b>6>~CJ4=}P za;QvY81(VbCW1@V@|GBZn!t?_w~ZeUD&#ehfQ<@_PVaCB?tZ;|rirmYI)R{%w6m4% ze)DOad>`eRLlsD3%Na=Wz9Zn-Ipi)itqNHSd=Bb}FAkSEh#AZhb^~|YR)ET|Et@^N z0^5S6@4a2qcy`#?wYsvZ?6nsF2nvrFt|(*>9#rrh^y4K?Vi}`>1dcjU5T^L28=F$k zrd-eFDCH>if-^!U7fU0u_e`jJB}=^{P%_~c)8ESDfKJVgggbjInf{OgHW0t(z)hzD z+$#iSw}iAwqyjZJk63Z^%J$-5msZEBp4Jr&NrHTjHUns-!JI73pa@=(_+G`1+U+eV zn18b<<_LCxJfu-qBu%$gAes;^ep^Dx-7M-48_#Bez1Wx}!22~0j36o8T6|A#L9I`{ zfTp@(|KtV}5NpT%uTnr#cI|&?eC|S&i{9S~bJzNJ&o#K7oXvoZOg-{A7BuREk?E%M zJ_JXTeHtmbDdEVdG>yA38TFtr_-T^Wp**u9(^XcO0D5XCl3)U7kJ}#ZpON2Z=kZ*} z1dWGMvi;$&?ul8|ThKRg_1#%s+HwRri#6`e&_-hT@4%`nqY(PQDd62!9Ed{=njJsD z{=k39B^}i3|06!8F%wzR$`YPn|0kL06!_10zyaLpr(ua*Kq;I>BM^IJ_A`zC@J$LB zZTgQD9OaC#n>4Hu_t|2Wt%KWc`H+tKbhl#fn-EeT>az{JD0RgI4b)z`^V}GuN??8- zc81P9p0`n{=RJ_P8-Hx8>83<|{O=R=*pBU(Fv@4J|LZ&A#+aM{iN)BWy&xNxdXP2v zr}zVPIvg1eYX_&)T6S5-NEXrhP#R3cXsyN<+Uw+>7k^a(<0!V4j|Z>~p#v~pxZX_~ z;Xh+5fOVq2xcipIr@F^ZLza6pJ`E@!RfI&6-nO0|e1VDCJ9O#zT|^Fa11ho@==$%E z1uZOa_FIIRPmhW-Btby{5GtOBVt#VBt&2yhHj6lp#Gt^J3#t?Q`BvkzJ(iX=1|F8c z^-8sXeQ#FzsaG<-tIhRVo+3^X=Sz;mZsYEMk27~-&i;D8;oj7!(;D`HG(86}2hTKQ z+-l4?ycd@(6zXkV0>Ps0v`Gc53>+8{?CUKF)78|2lqk!8KVMex#mkOOLZ_kqfXs92 zx%F}IqE(HkuX=XnS&Gd4FmV1SRj4HHL?3O%1EB4D?%Y)}!bMXJ88vwNS`5uRiYVko zM=;niS|dC;Ilc+dI4ERKYNfhU+ZXdONR;7HH`9RA3 zt%~wLZ0wo{Si0MW*&wVod9R@#@N4%k25CY>(!KaU$MJSby8?o_41^Qol0-HD@)kQm zSv|mS%veaqE%2`uyq>%cVshvqxt>;K!Jkj@6| zT*oUtQwgrM9iMOMFEV|Qs>AO)6P&<*M`WW(K=K&{H<2YYqH?#K#YOHVzUh;`vtP7L ziBECf_lQO=wMiaQ($}>@_Zh9G8+jwI7v*nNS06svh7>*3HrF4YZ02nS{Td2>0W6%3 z@J7I$53As{T>^N#lIC_zkItsTmZsM6Y98zf8_3o82qLB4Mobu{hy`Q;!gfa3b{N3a zYVSsJ@7SIo2yO#-+B8e5n(m0~2iFETT~znix|gfbcjIg-$h5(Ie+;_hr|$JG*Z`M; zXoI3wXo2uU|Lb(;;}C$k!kGmD)<8S?AD0HQn*htY`n-*ow8Vi(iyGtPjhm>LMZZ7- zup1uELK_jcyC^b!H&P6Uhq+#=e%n7>j5c(RQ-AfoGv5<7TA(df4FqnusXAgPhSaLE za+4n@5fTIU=Z`|m6--rM57+zRGC@>A=hQg#7q^3S)k%tPgUIRadjOy`YJUjSf|Iz- z{cq=oKIR^CNengq5h|0KK<0H)*V)d2YKT?tUM!G^w?ni<{Ix6iz@f9kjPP+j z7C2}u>QB_$|NdwY^Y++pMKX8XHc`I!=3l&UkEskgv}qy0DN#wx4G&E9oKmd(6{PBV zF=u`n$W%ee8z8`r6V%!SV(d@9I%a`_4RWAtEE3Q3ljY@M;NhCi4#;*LG@iDVtqaNJ zUggdf`!frw>%a&d6mNWKE!WT~hi2SBb@2ViH_@Y+)gN^3qscPIY zEmSc#?o!ot((eC+O+1+=e!M&i(jckn$kF#y%NfBzr7x) zH`xj>5iJ9LfWCM8|7_Ud0|2hcQF>C;lRO$Th&^_0wBN7bhp?}78v_YBP$;9Fs584| zVr-YT!$5#HGEw)g^|H>D8OsYAS|>n*pNHq}{&0nKVs%Q$Fu!vh7uI67_SLKjLjLvE zpV19I-jr)>SuQTV+n)KEzmD3!E|D$lBYRfQzIrLO*qGuV7Vj#<4xuBa~(O z@zuqhqO$LXM+a#{<@Nb}B*Qu-URSelIzbu3TN{_b1N;w}fC2&X^76->bkHpa zSd1LDLz~2iYdLbC-W=-2L)enfkQ1db_9?Svs{}|lCBATlILjvv`>MYt2bEVEXh3?F zYG)y@Xg^nt*}|T#68KgQHrXvM;ENJ$VnE88C&+R!z7XWBp@#ml04N`fS1D$0hclgv zqWO;(z!);G-HNv?W;`~pIp+|k9cP>lO@D*~JP!DP#IbuI{No{2e^`p@fa*r5u)h$9 ztK8TZJPH8nW;cr*tqxdhrzxI5P+T!+r|A z@`XJImpcF|IpHGQ03G(@Oev6L6=GucfZ*!^Wxb*J=ctLk#58bz{T=vt?3kRlQQUX< zS0cuJXEq8yqM>8mPLV%Q^Eqfz!H>0;6x9q8{VfT$-1EktMtsVKbpCCdAfcsG(0=Bv zXyTa;UaMo*QDXOPh~a_Dh3xH$F<>@9hihjQc%wBZG$M0TecM%6vB(EGiFe%rbnC`s zO4fkSSId8q_$2iw#}M%%{zD4IHDKG)OY`#s=sP#CX8y^IwfV@e(CCEIK~QmBB;D(u zqUlrb;NF_k*BSorj|4U?si1Io@8dY}NrZmqe`M=RY(^g_N|r!&xMi*<>o>SDEvp0L z-pgb_*YXdXsksf6oN_gh6T19jUMJaKD8h@LX!$89R!|;I@LU`y$Sg}jv#S%DQ!4>b zA8Z8tLI1-0U}xm8KBEIkQV7X@mW5;1x7Ie@$yX@%{aHai6)M709oQ-#JY&1-mRDH) zF&WtM{IhSGg{31~Ro1>SJZNGv+bmYG==Hr`P`FuC&{Yi*d81);{G(veCJD5QcW#?1 zKK44`ygGfH%!W8kuO-$)zR7f{T-_j)-o2bx*LRghJ!;u*pdy$65t*!PgNR2V>Jmr? zgxnyQ+@C4RMSmTuJ6zXaiD}!Z0SVlyck$NMmp5)Usn6az+HMuR=y&sXy<+pQ_6|n(g=G*xgcHO56OG zeb)!nk2ZfYiAv^gn`V!J*2>?2O+Xr34%mbusZ6DLyR)pA&tZojZ`!OgsD~Ap0q5HG zuemK76J&KodHJj5UzM)QA4hzH0j=x1UW1!Q$#n%H~JOTTkS=r-&_ZF*Z*9wrhFp~y@yhU1dJ7^ydLd{#LMxL{xbbt?`XTLTO6!R^Z}?%J$QF*EY{Aj?9fi9^yyi z0gsPi4uFc5n`x;!IQ{*?W~|yo8D!9`T&`a9g-PJ~@Fv%aMy6=;7vs+^@!wCWn29JX zCOG!(OMRyyQ*ii!FeQGYN$TiH9D2Kdc4+Ga&V4;_iqK6-@n$Hn5q!eJVEZx|t(U#s z2$gSk#x3IL!<;{fRM}^sMxK5YH;mZGy4NzHh2s^f)C1&*b&|Y9a_;qQeTeLqIOz3 zT}n*5MgTs|FarAyBW4#$ia|y{XoK-}`!-Ikx@klcy+G``c?3Mk$2jWaGdXlCvf2I& zEI|j0!W;EMMfw8Ez&_kRQw>PJ*(+B|VQIj1yM5!%m3FCCCs2w!f!FvUyW$csDlUTr z>OJIG!;r_W=A_%x5@0CVvkJ15^Fg$Tf<3luFwK))6FtW*O4g9jT?AOJEsLNf+A-sG z58|c)GQ5V2E|k&@;ze4!{vG^2jgPJ^;z+ zy@TxI!ydZ*7GMr~0MlauyK{bte|_ZmYx%LcqMMhS`|?;6Har^u1!I<4VFIS34#4}< z-E__BifHrZ_(`xi z3TDFXw^HEuJlN^gm-4ZQxyN7ht){R7ouO`eMB!^Q6W}JXRb=wV6mYOgE95=_CpkXv zuIr@_QD<_tDBp7>ip&^++ep_v*OEnEA84M$$-n?68>6=WnNc7=!yl)uhj`seMsA>= z)74tKUHTWIEIfjk!l*2bSg1$XC~V78WfP>vn0^`-cfL9lidRGC-UlB)%Sz-#^fw!-?~V3qFxjFPbk9IK&m zj1#;(?A=t3xdKHLM5x%GX*e%nlC+_@(Z&ru-~&%YD*KmmCmq zM@vu>Ifk-S8pHB&%ddIs%MaOhAT0QI?(}L9srH>n$pxAbR>`gw_?CAAGaTkiKJBZ! zvt?!RQjtDd2ZlFgYo2Og5k5ekY6n+%i>u&0k$*>GXd3y6*uo!tT4RMztlfC9pDkLT zQ%1gzRC#>o`YE&?0_PtF_1sOq zZ@Pxlp38^wH~IT4Cl+~^$C6-{lD^Ac(+$Ey2Oo%f%z7|{yaySF6>DxklK>lF1&5=_ z`O@**`%wE~Ybg29;~1)@E3q-&`&U(N|A zS?f37D6^jU@io+39PK9oa${fR5zDd4)Z_2o;gAI%1QO^;qy z;LGwoKw{K|ZyRF?@T*-P-A({4F9vY?w<6bma4Q-fQuyQmkk3aQB*q zlt`Xd-+a=?d27@u?L0d8u;ePtt-|kJpnLvv4Wa#%xxe`)&UnEW>^I+~fF4zWlFGfl z_ol6NSOmpxF5*c_NyfdiId6SIj7d3+VQ)@7{u{hEMXw|A%O-Tx1DS3R4`m~8Q9 zCOb8k4C-PeOPr%wUYht%f+{$IaXpF}gH0re!<7)%!$fi-nL~8PU^l|f2LMXBWspbd zgVvT)Fu&>tpWj@NgcB74&)_nkpx$4ak*FN~<56c4Dc_TxA^AZJla!$X%4hh>EK_E< zUXkt1%9z_hEXi=B)~$R+Tb}-JU{D5bq3QwP*A$N8fCmN_E5EJH!BBGPgOFkKYex^0 zP~5yBy#kWHO7dwik}pD3`%83D65@y81#%3_ocXoD8$|=3$H8|I_~YYmu_Hddq;d%# zJ5JAZu;|6nT8U30c#6C#&i@8-;ZSb%hr3z+mDUp}2EmOxuGRBGY0JVx^mJWs)|$BV zs7w~EqkXZr^gpvua~6Zt6>)YR_EFejp{aOa@Je%`PIBAYom8Gv>_=oDOiw+$xfqar zFgQG&i$BsqoEmqzTnxpG?r~ojM9+U|TZDWPLoHlJ&}vyQNkUxt59lV22yg)HmkF|3 z>a{#ctY7~vyszFj5_tkr>|X)N!L6d!XGqL>uF=)jK-GPhrL?WUyncQ&>3ND{o$8LR z{YD7Gl{tRNc{HpOmqdKmxEHI4qoiIvZ6QyU^3!XPZ5)OGT{oY!c7DdfwFvZKBO%7o zW#an#WRiP&ZhGWlw6{)KcV1-2dTMx;_C?DK3ce$;jd2`>3Fzry2|eR5l`2V}_uxWk zCJ!i4<<>$sz$bJiCu@SwNs&%5nZvK}`%sKWQb`x0)5(Wk(O^F={dl7j(6lS1>XoN` ze)+3uBo(od;WA*?jdJY?26^lTy8FY^I4lKd#Cms5{bhk&+LYtj2IcZ2>teco#Y|9C8oZAz33feJyt8Qg~9XLxgha>uJX54)=F(_3`{ zw)ba?X_q*gs4bh?ZJ~)2-jaV?SYAWZaeB6hBlPxvx@QvRAEm%pt>YVAmVs@g;L`R3 z1*m`Z7({~)bUW-H7%nPhiW5+{Nnh%s zL%^h8LK00N88>>W?Ymc7j}?{UH$-Ozj%d{&JU|x(D|YxRflWN1JgbfFy$}A+{KGu6 z!0b@cs6I`+o}Xd3tK9FlqzG1LKC2kJ4ppna@;-)8yu(mcYmzvm=10`|SS8~bpQ%`b z023*+onDcUrb_^ zG4WCXHcw91sTm(RZQ<~C`e$!9!T0c@fx3OqF_rJ51HM4VCg}oUF9&&DNn7AI)enxuwo2{RS?^ClWaHLq@zB62s~8R*=H z-xs{s*y-NJ-+Yvq4CaB1Q0mAXAy3gWL8HHv`MxxM?|??^e3d7jpky?#jt~14#joQ# zoKvLFWtZs}biXwj-;ey}+$M@Mx)Pz9o*2(o2yoYdHP0KUr6lr&x>lcjIr(@PU%TZT zWv=U^xvS{ky*gA>fNO8Kf3ImI*O0fnJi=P1#XGIzn_qw@)_qAG!tyZlv0hljGHgBO zoaNUP;vz4%E_0aAG`GStc)J;|j-s{Hj_^bre3(g7E`(tyV|2v_I>!2f9x?m&yI^iEc65YqNt+Ql7s2B_+PLUCCfD-QRA~%7H z1w_$Rk8`zA@^Xt&pTy@O`t(ciOaCFEHXQorv+fNlrrCPZJHi;Ifwn1Ot#p3VDUs4Uwco zy>x8`RPfh+YisW7KW_BX#ihw;P>ToB@| z-U3&s)u>L5`xs>>=H5YK6EK_Q7t`PcbSv3q%?MpB1h({nePgXmq2`R9B0TB2<>d&~ zo;L0ByRTLEo(7f8%Q9rg!emK~J`9jdxPzqQhEbiWsNCnB<0VSxqQ3Ikfk)l8IB8jW z;FXkJv$@PNW4aqypbe?22)(tSL(K2&sPq7gI!civuutX+1c`e3s%%*}`xEaU1=wAz zRCgl1XLjluarr=X{5=#6NW^DPwUOZ>q%hyR$?`(Nsi{%J^`NcieD6Q=lwu8b@eYuM+@(D3R+*)LE+GJP64z#DefrFO)1swIe2$u$?QJNr#Y6?-1ebm>S!a>PzeVa zGt(ozPjs81VYb#6Te|GowTn6d770J#Z7~I=f6b%S=hA~!c7;vU64x`{AAWqTY#aGd zN7APnd8q9Qdr{FmQXNR*`%?(T08QhHctmn;BeN%x=G1Us^qDE5KEv+{i-F!HoT1_> zg4h_=)&l^%^n~)8{0*tW@wV{;Yfb8I-3W!kM^bYXPUm|A1W*bnJry?E%_-2f+uUiS zU@8gX!}tBM$q#H*s(%Z*NNQ4QfhBM1SHkf99`y@0ii=8lDc^5!3#|AXEZWpA*7H4* z1pE;@yQBulIHsx?dz;!CG}X(~+ka;*U`;lX$u?yh!XW@RdxqJ7SHg9|!f!X&_2;R? z&CGCtffKt~_?!*QLE8kW<;*0un)Q~Cy{YpPZx=GhRCO+Ft)PYbJ-S03U0z=9ZL8m> zvRW0UUICduFAx+cr!FLLgv-UC+cv(OnKp}1FSZ`E>^fVwV6(9zwRn@jRO0cjPvkT2 zAU{JKBEMx4np6nIiT*zz^}3AM|G4%6gIG1<8a=Nrjvtd@SpxBu;vd&%H<22*X=`6C z&3w6s?lqv@(%)2|i!=o7A&Ifk!`AagSPYV7H}4+pS3P%4Fq%Qcveklz#O=#ahZ{-68;dNCHZ7j#7Gbl)T0-#}`(f@l#ulN01U~%XHem=_B~|!!ZUi9VW3Cz;<>{m)Hxax@fHK!e%xsRW$3z@cw%JcG^B||(^(=Ukw#KN)(lX39U2HiUd|%w2*Lw zcofuhf9=BKs3t)LqQ_M9qpGx{dd0M)^rI4F%(7(kq+caIHzL3PDPDDW7)te*;3GCJ zQ=ghz_qg+I;jglv2Onmux;Q^r-3(Yrxq998t)G&Y>Q>RI;LEBH#o|VwW$8m%_2#BQ5+Sj=P}6ldeRpgvFyDY5Oej5PMfjms82$-gzU*r*YFU` zI*XuhL=WpR(bIl1o_YS&qOn3z@THrE6f^GOkYWanHMTPta*MqgX0Owu+|_zKS~qS7 zUErd?I5~QK%o-h6{f}tuLx>-dtX=AF3S^obHAgks_^R0qG^y3#dAwAM4lOMRaldCp z-(3tUTBWXetWmr44m|09BxywSPr2()`T_@kos8-Q!+Kp0W=+p=v`>z1P?bNnUL81@ zSPgh5W2x!@#=7JNh@7IK9zmzc6?XjV`VLJy4hN3^8FZbO9d0P9D5|WG2zi0I+WiKs zA6hzoK_dq8sEW?j)YeXJ?dSDvE&L%^4!hj|W}jPNhy?}UfBI@Q?X*`(u5XOFE`Zs0 zCy&d|4_~|V=~n}In2bQVPtFPE1&_`KJUSaO{MZ*Y$2S)w`!?jaK=UTF{%FY+wD2B~YOe=?E5a?%(xef~FFrp6;1`YO zDI^$8=KH}IH4boxs127~wS~g7XT$;ba}vyKqk$AJmPE1i?7UOHEsZndV4FP~z5<3# zF&7h041>5k?HfRAEe7nq5OBtlwa#X$-3O=J29O%34?J4535BLP@*W%jG80O9VvX(UZ6M}n8CjqJDYJz94-Z73c<#*V zs~D>29)R#X9hA;oT|pDR{9J(!+~$DSoT)zK)<`9`h8_sbbpFF2S-1OV6}0_BK~PsT zk-L2?L5C2oj^8Z?4XdZ1Bp89I#qfy38vJQ5)g|@z0^vozdeZ@PL1(ZJS%i%L?rM+0 zs}6Dox;voxQBcmsVi$dlp!D*$G8gbj0W;I|v6)E-416y9XJ$GDnVIa~1WB~Cc#O73}iyRzaQ{p21wXlXdx%k z?l7<&$$qK_zsKu|1=M%{3?!HaUqk~ zZact`TDleh-d!3718L2I?b+6+q2q8jMK{~OrS%&GFyimi6yY^V0n^dq1OJVHR%-^B z(-atfB>!m6;q;ZMNfbxxWIT96<8Qz@G5Ct8dg0W4DKFKC!@l~Qt6l|%`lr8aE_^T} zSl$HLc$#T9$?nH1di31=WTs|tU_-moC&uCmrg18s41HQ1v06A8`c$;{OlvW3yQs+W zwVMwZ2i6Ek8Cl}uqyO;&utQ{%hOZm@ZKg-#0ENH+?*Qa}wbcTu(it}HrWnW!Vh8Up2WSIU)0d?I3Q7-;hfbyvYE6&5di z%xiWugE@Yx1)~D_tSNX%F%UFZsk--L0IUlfTMtd9-uU{1ISm=4er4+-3+c1fqiOfU zzOY9C(y;*76`1*RH*27)E8W}%iYE=wh)ukK6wv@G)Tyt|WzkFHKyqQpiU%@!lQftm z6L}N>K#k<_U_7cuJ#1|6{n1}JOV07TM_>{vcv&X_d4X&*!?@&NLIWCiQIfV<|33R~ znuEVpqLkH^6vqth^daCfG+Rwa@*XIMUI7bYlk3X6dChRi<9AzxnhKED*M2cw*r((W zMx^P^f@)YOsNVNBz#F#{s(#2DxB=k4Z`=;*mkSsBk%k1z9+nlaT2T3J)9~-b#pvJL ztEk3{+0`RDiS5#Sj|;zWs99$*WSymWljA4O`Pm8YhU5T4{Ei1BRGj7Z8I)2TcB5U= z*(c9b*l5bj9m8ArX>UYH#lF|b6x(=;huJ+J5F&TXd@e)|CQBL-MP|!Qh|Q^NM#jRm zsqyoMX!7iNo}{Rv5N^Pd4%#F)g<%0f0!>tV!B`atc>#ZQwgaEb3h=!NvjB+n*u8ee zo{7F+Ko4jDH}CO>RLHK)6%3L(Y$DNFil{XYH3nVU1nZOJ| zis*KT`mkR*0SHhw2Jc{>@q_jF{xu&Q43jMmIKY!T^@IQv(X4AWtHxOCG9bcGLogy2 zB=&-=O4RGoF3@db-3ApqVL-qN0GoQvD|Na0Ao1@uQ6*Chs($uP6@Yy|1*5?vt=0X| zuaKq4ob`H83*???@-@KSwIgCk?)r_H7AF_Nwr&yPpWz;Tmcj_OyF0?p^I2 zxYWzFCEq2#gaC8E=^R_l$EEdI$sDJ7Ze>jxmK-LC)J{p?eTQ}f%o``9I5CS+b& z1YQ2{qw?+DXK^(-NY!pJy7riWW*wGx1Q1}xz z_w&X6gMb83?i$Al_?Mdoj7+}8&3UVimm zk@r$OJ;IIeS|y8zKu zbmAqc3s!Pa@GPL!W@f+(0TIG0BuM*G^hY?1gc9;6W^oYKK(>eKH>zet zAA6E(!k1Wh3}Y2AFVBHVsYwacf54XJqX1}Ld=rAcet-q=a!}|kgy2On~QD!LzNE}@O;0;^^PIn(~(23wTC%%oChwrq$CH3Md zc#OO$ARnX5{utN!nHPz>*mq9wKzaW>;WNv^Phne``$%PpQ0X+}UBr{JsaS4@5iNDS z4MZ`?0v|t*B#H@!CQm;#)LzLTGKU1k%;hx0&sk9EWu6>DhcEb$;T~|RPHB?&U?I#J z%58E5YYrgZ;c9NI@a*Zgqb*pyP1Z+?=G`IWcIzbZcIPe1qX`~zA2*3*7OM+sJ-mMt#uaUg$9oL-!&Keg z0tN8Q`M?n(W`++B=vHC3>hAQ+`Hs>o^*qMNPFeLmUPQW2G#zFB1ZOdqkiIwVDE0F% z7?krb5r3L<;7@-R(PWdMHDoqs!!#W8`1gFlU_N?y(eIZ&7#1tG{m~lihW)Kz+yVcr zQVNeZmAIEGn~NMtvhQfkzv*6iQXl?Bb#8MO*WWwpy{=}hE%>hI~)#iC~1U zrBo=DQ)}-b)_I5k{{awE~c$76m+3wi0w1m$+ zM;qlSz{n$SoTt5Nl6HQ9wD&6QQz{jmI*cJkXx~_XhR;|O_6VlBWc;_x$6_YSxoiiu zTIO&B%Zs8Z#nCp9cSreY2vg$d-?f>`iVd$xS2;}Ibq7V`G!MN9#gB_DL7T(l@X%Kj z#cTm(NRs{U=Ie@oUm$d+)j%nkvZpCS(t7CLCKh=muIN9>HHTDc{R(?@Uf`@7vvM2r z7n}6cN-wy~V`aCz&3Qu4&ee3dmJ^qs^MRU^bU37zM3*#AAN9lClwr8>2FEp6o@gI( zWF4#Gscw^x)^OTP>q?S9)&*5^Jb_IFeb=T#Cc(Q*Kcihz9qkKSXiy$VET z!Y8-CybIv1PISDl9NWOsGYC4*eCNGK><)FmHdd0qC~OJH)Y*J{23H;HKVCqyd)9@l zwrS?Mch^i8x1zg!A3IhKr2^)Aw8pyHPs}Q0Z|ooR0bA;T*-h1WGhp^F8RY(j8~nLt zlciXecnJLSJQXFECu??+t5Z2rIk@O`p^MIm$Np#SC+1B{?pAgAGf_$|-rrpm!X^Zg zB3ds6cP=HMmllt-g+|N>Y|K2IJ$c#zi2c?|1NE?H#NrMcQZenYU2`|~T7(^ZO1l$z>$x<^0L#&p zx@B>qcM`o=n4HAYr6_b;gbX&pS%z!)a1>^s%vz#G8`o~$Q}@okb-Z$(+jv;J&gwKx zUg#LoT#x0+5xnbp(rhx3_OMAex9u6bge{cDZ5O7r#&P%?|6t5o>!96V5Kq`(wMdcZ z(~1C1?wka?pkKOn?cV_T^)BrY4+R8IPTd8MnAB&Dg3TnVW#i9>W-uW)*4`*Rb)K|g z^}t8axRj^%F&*oTeqs@A>8 zzwGOJ7gTL%*!phaFH5k1)`Cq*3wTq^?jD&5Xu@X+F)z%bYPRf)&kkLrcx6g;rat}8 z&N4Igaqyw2=iC|^J@}+N&mVw1H`qZp%YpuVFk1A>But56UgsKrPw`SQ4f6bjB%N|z zMT-x}A9sEeg&=8V)R)hgHbLaNJE#0< zl^ZXsVZ63f6Jd_#FTP?grU{-;a)y5s)i}Ex_eodp>);054I^W3$aQ01>zdp=CCs$^ z)?(Y=S$GSkF|&)^e(F?fKY3J@HZhX3pqqO_bP4_!6Y{$)I2Z{#0M~EUt3+;StilS| z!(cVT96wqy9A7vTco=Y$(x?!t3<7Ko=%ZB(tInSupuCeuU*hi08)2%o4@Md#B=+SA z%J*ST1u#gqpgkbRIiz3;vwfUouV{mL)QW(fLee!Jx)r2r@%PDlo6KA6zom9A{Tiaf zC|ijakj~!L8(p1L$A?CtM(x(t_| zJ?#SzjV3s0C@%+$@#YHH9V679)p>l5Chi1>T$){WzE35$`0Ca?EBO&IjF4@%CR6#$a(X!5*&ZLn>oRY@#x45m$qjW#cdMe5!CtmKKX3z zc;6Oo6}K9xaj$m$(}{Ebg%=k8kvPC?ueClFycpvNf?!|~HI~|lg7sFE>+hfl(-|8W z7Z1(u9Z^zPn}(=+1AS9czgW6(BvY9EQ(0OVS0*`p>H>`bKfT<6Mgt!Kr7PH;7z@*o zU!ZNG*yVF|F@~R_BfAtLEXrL9aNw?3MR!VuC`y5MZm!xgce#s6e(;}XZ#4|Gz%og* z4u!nJ-0+`%ix4QQ>1utk){5pbBGZ`J&`ykY9Z@s%)23j!bFVu!rA+&3bTr?Ao3h$V ze?0;$+fF4C)l+@W+MGKIN1U(J;eF^MDNjJQW*wZQw_@{%nq?FS({vUOJ z9u{-|{*S}6wC`!vzLBKG(!Ns)A(2R0kfbOP?M>2x_C+C;N=i~_(LRb)v=Ob^_q6X* zQ@`^$@&3HOpYQKJ?)%^Sn7^*;I<98sHP7WdALnDa^Zi~@tZv$OeX$Oy5v+Wehs*=s zy2YPjSAD&-|yzeZzhx&$zQp#~eq35i5!-{& z5IZKNtuXsJGVzxpVWr`m@Wh<NvG9)nLZqkOMLc*OkQv$x-tf&B3_CZafN@;-h& zr&$%TE7-W|_?c>NMPc$2OQkk)o8QXoFuif<@1ksPnK@~0pSw!?wdq{yNrF?4RAG>c zk!Va%jfR}k!1cn<8G-6sZ5gx`S688R;#vJzIrPpy=%V(vh{h55e#4ys$O|&X%sc*m z`ZJqHXZu8H67z5l`dWpi<9J%y5nQ6q?P`L-u$V^Pj%zFq%KNIhlW$`)TDcqJ?}&Y# zu=-^DB>deyKTdwuF`SLth=j`rr8gH(eCu-Ay%6+fn27zYd+hWyyY?*>l@Xq$JJ7;? zu{V3`sES!^Vq~FFW)kzKp_RJrv#6$0U>9ZHtiABMyN3<;Tl-h4f3JJ$P8i&~sE!S0 z2Znu;9Z?I^yDZ#Rb`FAof^`q1c&x1Ec~qR1$-aHZyG7czy-G;tod=lZjYemdKQ4vx zcB!b#{a5+rb$`{aMPrn0l}pqD;7(!oNZ$I~vfmAWV|J^qTKVr3@NdZ(WehbZkE`#D zv{X3&Nts$vkR59JH$w2UYGW|=&f*fYkP&Me zxZrYsUN3-iowkJLky&Ypdb&QRBzuAG^>)`*+qriVVKOJ)Vt)T2WBXrYvJZiG&e@~7 zyHAvf!zokq-Yodrq8R==A-BsIkhb`)|Ld1y_gdorp5VCq4Lmwl~NBhBz1PY$|JY zSo+0*;PY%}4U3}xR7%u^8r-QE9u+4&k<*=>6|2LGL{?Huk95;Z#i7u$ZkC?1niGyo&swM(O2QW! zGD!MQ6IKq8?f0!7_STD>89Q=*UT%<-t)|8U0OW_i1+c|=CdEASl8Hm!pE)uHOpnP@ z>X^p6Wto)F9hY}uw;hx*+lgns+ha`lxWary`=aP<3CG^&VlT%M^}h3A?irMa%jnvF zSLzWV27Azko?yk;$ltrgLXab}H9a6`Yjb;v9>q^n=g?F26=9ULcNuZ8k%0%N*ILy= zDipHgPuD+;7h}I4psT3sj}ycm;y%>*t(@(KK@slFkpUU@Zry!9lzP}$Z>0#E$7Yw| zJ$T}I+6CtBoJf;#U#e~dFg`poNYTWI}&#O;VNDo_Z&N* zH=A@qTg1zXi7>l}(b0SwyUk9C;L`lo-q}Z8ppoHXRqKuOB64b9%IW%=vXVcyPOV>XN= z{ZRX{Bw9UxKc-7ZPpLINp|h55RiizlZ9GsR%BbphtnrQB-6(HrH{7?k`VPUKKWUQr z+f>Q;+?ykKHhojVP-@!8fa+4FV8tt8vQAE)N|S6c7FK)ukLPM8ekDDGcu1`Z=Q-iq zXFkT=(AL?>+Fo&N$B4wW_66lst6kIA^jgGCSn`i|V<$b;#JZtjCtb*i-%Wjej_rp7 zAG^cD16Y=`$`XgLw^knc33i^HTt2ATSu6yDwbgFtae0cwcH+!No4dH2HB7A=RkAM* zq#WMbXhMLqiz4=cfduH%v$YH$-1D4@mIRED3`FwIyLZfuSr(TWA8OvteupSYJl2F? zRiAr0s7!OpM*PuSyNxr}nSG+hba0M6GL;W=9UEw1hxvm|#xWk!vi8ppx;$u;p>80D zmEW$)QB9kaJ3~#trn$14Fc2W12&0>u@)N%c_bNzPPm!?GY97V{Tg8PmNc&GoIng)j z3u9&gf#SEE8orb4dS9LT!WRF>zcH7!Sij-iXWYahv12O@Q4fDHSa||pW0dpIA$R;6 zz4@2t8Qq+;N-*}YnvN!^%eM|-B?=mbYz}EW6v%^>)VAKVoxpwNz9+BGz*pYAm)i_3 zkKtgVqizt^`NLBqv7}YofraH=zcmC`J&k!s2)=_D1iK}s0 zc}6>rebB@ub}b8>lB!T}`?Py7N0DyszER~IR_z^4Z#?T+7+$mL&VHA~N9ty689gF9 z?7*p2#Cp$m*T{hr)}4(yR@uI$nSl-l;`@a$1qAy6hV;+%IpWgQk<6m{pWOKbxo9th1@?YH{EI9W}{y=HUH5wZZN+}Hb<*xkGtcxryNf_ z+ZcWByFqq*>2G?6 z!|YDzIhqq8IuzIk0(X^)dq?Vp4K`{=9|Acv?}FoO!yqL*JY2h2NdO~>k!woxToAr; zL65)djq1oVTF7hfK0S0K+rn)OJALS!&asg{2YBwAhAUbBWDF8;W8d1)-> zVF8?+zdXtTf$}zEyP{*XL9KJOyq98~0h#0L>n~n0)3QeQkN2eNiqwW(`KjvATC%>+ zg%V}bHQkWvBr3O?opeMd@6=1WGscfv{Uq;)PlT-7o3UImoTvt1Ovkd`7l7JCMS%s{ z(-uBYV5m(Yow;oo4+i*|=MEem(o5Z&le**QK{Xvh3u_I;OjMVz9j7%IcLMbNvuCTi zNEqkN0nU2-Z-g+WeHY1M8nu-upItAED8FE6Fuea2^X%<&GZ@?Nxzk&n-$scA&1>ns zm?V}xn+qOdtrPb8xM70yt0)cS`n&Co_c~0J+jC-`8o->SK|%zSQ+*4x8RK>bPSvyi z)blpEu58#x+~S-)t8ksjc_KpHFe^RvmxrT=%T~)NavBh`e|{+k>a_IkhG{gybW&PK7eeVI|ZvFEl(?JSvpiSXK&GUU}-!lXs7nBw$tN(p_ z+XJc8H%Wm}$fkNCZmHDUoMalZZc^A=-OaA;w?pmwwO~Glov(8$7_1>9Yn)1_gcMnn zFrUpwKJt&?@YdS9--&gj_Mt!n%r6|yK<+~E(Naz7Iw#@<=YY>x63btiI;WGDR6+j!OK#EBh-frIS&<3I}ypttmbmX#6w zDhxeQQh5z|vQ@FHYxOA5C>A5kWV;q6wCWsfhLmRaAdTfrUe8^|gN?}Dzd#H56V#4a zrd&%|vs5WPEK|Od5;1;tlD;}~q|5zDW37r19Q8l$Cebrz*POgpN?FGgyAjfA>`rj> zAiO7|%A3GV^|>dYSbV|~0Dsm8b+{<=63(K~b=+-Wse(~X*L4=GM?TXs$wE%*?pTKtGZro98h(_Y#ymrH z`|#^sYWhR3Q)ubSPD`0h$K1?? zT1rx+ZJFu#U!NF5oP|$#k;QrLZyAGq@R4RSohu^>+^2k-5Z-t4+B9L8vnKu-w$an1 z%}H-W(c?;i^9uYk8%u7pgf?Lt+M8=|}ml1Z8*kxVCq$5bXY0BQEyu~$py zg3;2;bSA%y=G*4`KB2GOgpj4o2|wW6*pV@J0}Etcxe0g<@|;3UqLwb>mduCb(+ej z{YF+VLlh}poZC^A!RVTk7Y}cw`jj%qd%V-Ua;sfz!+h7{&2?2@Be^r%+J66!dujAp zcJg-#AJ%{;!@<#X_Y|?MxXS=m>^NFgQiPlN%sqSA0yACw4Tcb3*k#K>b~zKX;HW+15J7%KF#)mvZ28m18=ePb(3PGkfBxjQzfW^0tW4gAvWU5ze%BF1^zrXoKx>dK)(q}%XYQ%4 zv3MrfzbWjq2kOqQQ@Y4W-t|ECZ=gSGe+_<9S06 zmp6T3+PX)=5CmY)zLPl*sV$TY!d;GD+3V}swG<1Ho6e2t^jV#{=qTHceWpE5)jrN@ z-0~wJC0;lZ>>J8*tYP8oP6PY)rfkdd(8+11H=qU9z=?K2YWr{bF9J93L5BEFG*6$g z?FlD@Ysu@NcaU2yj~-8J(ArQJx`2I*ggPTfE<^1(qNUu}=YJeb40l)0g$BY+{hRAYYK4GJ=Dgn+lG_;RVx-@fslEEi49@!y}N(vz*HxUhqLqBdWj!Syq^O z$KTdWp39Fdvas+bK8FKR4VMPgD#>+qNR#wOB&vhEBLn=FfX9_#QhrUd!QD#~%c_f?(1N&MEJPOQ(Q7#Nvuw4XpCqrE-mKcNUH z)E?ev9lcj~6Q%n=_raB{Gv;n)6azMokl_{@653J^(NiP9lsr(?b&dEH+y}%6+o@?q z7)10W_VqlbWp7tAYR_Eia(@?g^-ram5L7yz9=4ut>B6@|j&`VBe8@b9lYM%gXQ45Z zman^OYxgUbA;f_!%kh5m@60aSa*`z-ks9y#J}MH5jD?fkcs!;6C_*dL8Q{-1o>u_8 zzYFCi-Y@oKXSw8B3K`Jl?s2l2g3J`JdyU%CU0}8puw2VSDHSMlt^uXn4dnuCZK6#O z6T;?-jeD4AG`PCtfX{xm$mG1_`LXS0f6k=w)x zE-nRBPImp9y_O1MpEEyIbbvChil)bW;Afm#kT=a$FCf;j-MCul|R3#L04uj!7J{{>0(4e z7-yVCp=~qhGK8nPsQ>ruq>g{OYg_>=(2i-^-IIRwYx}5ccA6HRd55Q(sf3F-(%R;S>JxWaqanv_$Tw))XDzO)}f_ybD#m? zxtV!UO<<&zu~8jdTlA<8m%f?Hmjs8X!*G`ErFSI<6eP5i|L35T?&*BwAKTLFCU|Ac zeJqjN(`?%AV@n5PS+h4#Ho+uScuTr7xYh~q9W)vuH^OS}piho?g^#OJ+YsO44tH0W z_BD_VhL+E1{ycTk|IqfwsRx=-wSemPBM*R+Xz9fTr&H84%rf+U(@Mbr^c4pRL>f)^zna%IoMYq8b)>tJHRd z{(f+p+UaEDsReiJqm_l;D7EGw*VBovkdJ%ilAi+!ua<7W zRnmlZicsXxM0(NbDYcR(t2NK-vWuh6U4D>W6pmQAH@rk9~7?JeKT3d2q$u)u&(h3>4Ji zw3GIUOao^zbc(niA4rqn+ko=dJhAkLen)uC-0?0LanMV5S!gl|aa^zM8Xp>;QOn^i z?a^3u2in?2*-P`EEJi{`kDPYg)!xOT+?WWcW6q$<%O%aRzQj^AxQ66G@SHqaaBt=@ zqnC!w@y#7;3z?Q+gaaN^7R&ZJuc$M)Qx`15)4HnNJED9ts$ju_r=!qK!h0SAWI@5M ziRbfupncVQ`vX-*VQl0gpN9Lbd*3}DF|UB;w0TDCLJW}fewO=eEVHE3XwN>ex=UZQ zqrAn@bv#67eGv5cVXO@oU%N{FW|#^VmYWsmP}AZ!Ph;s#xxOjwA+TvE7+grb(^;}m^=L*x$78S$9=yrk4fnqV5D)#|ZT0WrUw89MqJGG| z3$5Mu>BWgaV`#qEoF+YpZH#H}7YOmhT68^B*l4Hu9`$=lqmf}N61}liWZNCNo;C}D z-8xPD&~J-HxID0GZo@ZAIIdljOTMx4_>M)YM;y?8`)yrkl)8LhFRp{>O;xvD=2DGc&WWd?B2UGO{H*r_wQ5{o!V(%bDU(m1t~lzMEaFs zXK$3l2y@fo`FvPnJH^ds`JS&FoqKAUc(P@#U4OBN=BZ3%n^=t9!$he^|9dNZS!Ry7g9C^XMZO-rGANnbe0vnmLuoj? zYrT)+EXsSK#7A%lL|kg`$;MlF966u;nrwFJ{o0Z4V^Yy(*(DP(Kg(}-M37#h-)zBB z%k7X9VFbw(@JG9mFP&ZGa$3}D zbc3Z``mVp@x`qj4kur?@5*|rgv*;RRFkUX7dQ6j_+Wuef7=~SsY@A z=Y0>Gv^f@&jn4m~?V14)cQ*5~&)H451X4%o_coe#q4wbIm5TJs71N>mWA1#GA=&V4 z;QMsfTbogyGO_S?(wE|TsbX(-%J~k!P(F@Ri%n~~UO!{pZMfiXV~95R3Fs-NDOGHX z=q4A8qw!kcd3Q?aLe%=t4ejS}5m4S>&L@SA1R9~vLMrTe?C<&2Jy|e!9Y^jf5=Mb| z|9|Z!SWmqZaDFE64y`PXX=z)1)RTY-mPlu?hBtreDSo3zhhjvBwRF96`TjElh$2? zgtEcNdzPq2YB33bxL^0pZs9*D__&|5ce?EC#me0dtvQJHEPtswJir4B^lwmAjBP7l zC$9#cNIB@z8C_Li(SMFQ=R4{6czLsTWrxN2UvitZ7yt**&YH{XyK}vl-6SD^tr_D! z3Lu{`@x3Eq9u>9r?x(ZyH?y*iK%qY=HGQ88y(zyp$nGaY^6w-_srk?; z+xti$9liq%_y5n!(#ZaQmSqfW5jsX1?8_;20J?!U`1Km03mmHd>}D0+?&#VvM}mGk z12ku5LS7BLq_fpi=CQkKLTZ`x=9u%T)1 z6&;i7VlkWZWWaY}(ekb1=ZBkZ*8ia_J_bqu z$5FLC*O?>yZzDb(^S>MMGFn^_t_ZC}_=Ps3gjL98Z+EWNg;6jy8}dVv^utW z885J}Rx}omg^g*ur7^&q-C1%p3^L3&rQ2RW_8S+YS^K~ z<8TlCXW4aGgx5k}`eO5V4rBh3V_jL%`1}BpHStYSm$WSWnpus~)0P}t=;{f%{U zCD>7_-jg6pzm!8=Vy*kp^}=}MIf$x$v{938cHKWtBkgz1=I;DhOu^sXS*_ovMYTo+ z?+tT-(ZsczmeUQed}jGX!5=do9Zk~XiPZ66W%;D{)%WtXyi#~`X?x+ZL}bETEM@zj zy}CttTzYDdl4DH)0{#d0JWetdGC=1%<(89OJU3{%bFO_SY~Hl|BaR+66Q)uMx!$sj z%>&@_h8{xcyY=h`cNZ2*O*YDL9Q5I9z4d z3}mhp7%5w54J^n})_+VTocLmSUi+?sv*)Tf7A~WwSyrD|Ke3j~ofvb!*hj(J&k`Xd z;c!xcfW_E23cI&13d ze>XNYI{klJRS;u1bD%Mi-A7ZOq(|sEqV{o=5q280zJ)l|E$Ukc!iH~pmc9OglkAh; z5_Aa8wk!hfdz}1(^g`~nWeF)BkXq zbvftw{uq81tnG{DD@r@&+TGd~d^fK_l4I3roAQap1OI!`?}8IYU|Y|#kwqB3UD;V!dGV1H$Ps z@@yH!z^~ptu=rocon4a9PSYI%*}{>O-1GnIc-r)#yaomWq@>Og6ngK%(E2~ah)ZoR z>E!hYZalmhs>rSKdR?sypuk{si~oFG_8*~#k1`9oKATs!^;9g0|cb(69c&;Hwrb;5&dsqh;v zDCV@RHLb7s{qK!9koa{4Uz2SyJKkOH1BTP5z8tml99Jk?DQ<$~|H&8P6 zvid9^Xa&(%CAjHrMe?>BD4heEQuG5Mxq4S8NI84Ke~}(c{SK~mzbT&N^B+5O)|#{g zfUQ5s{w0B*poXN5Q~P#=OqvqUVy*S|8vIv&dLr#||<@_ae{_rejxJA?vaNZ2!0tV1^-I$+Y(#%#!MvU=4? zVR%y9v2^{G!`rZ35V$1cyaEq~0n`=8koA^ohRJd8U0&PJZRJ=|Xxne+lR>tLCLk$r z754`GhjRRW`#07KEf`T%sRsz~ve%A~B4vfgTK`j0p#?LJA(NkKaGaFw`+jSP}!!=z)siU}< z((m}ej2AUJsOEW01;YZ^7RP7pKPO&Ymx22TIA#D!i}sky^)O8f0$S7xx#yKI4&4fR z)Q1#ITE~yZAIYV{Z(jg^IR<3lsXUSUqXOi&Jm6_vPZtQziyd^&e2KI|u2x8;+uvQ& zVmLn=ytkegNf4bt<@?!`TbUx$gYU`&-ip`t@6Ew%trHvX2yydqUsoQTyMDiA=+3N0 z)NTBkqnab&PrN4{#zyRC@K2`-hlNnP5x`FaC43#K&tERwyeQOkZt=w5W&`BVabN-# zXRNlz(E~>_$jwW+YH@Y;v0Z=FlIygOD9774eLS`}wIFF1M44HGq>m1UW|xDF@M2nE zfY}SG$58o|SD9}fl%G?9xv99iH>9Ri9G1@F6lMZlm}Hd?nk88SiaoP83p0#p0*k#6 z7d-i)9;9Df9h$S%;^Go7wNONQ@Vl~^Dt_^Mr&(-$AP#^vSF@3nv_6iX@)m_rf9%z@ zBY$xj^zC+Fq5Bdj`CBv@{qLeFkZ@DOcMjA}T)i?jpweMN<}ijQu})GKGw~t8%rzD7{Tp#7)hm+A$i^mR}VI$%xK8M<1ZOf1(>1m z56`3}2;8!vs@K(&gN7ehFY^b#Zv&kd(mf7a8cDC>wMJxC{?6`?vGA1!q);DGp+)@* zLNi}dCkpM1%)`}{4-WaiJ&FeRRPperAvj`G`!FNE9tJ*_?^Wv&R{(_$QAx})2T2W5 zY=;#-s+nU-?*bXE>be$u_&`S44@uj45cfj=N-1o&+c#)n|G68E&w%BoKN^wXY)R$t ztr<}HYHs8{Ozo&K(Op~j11$`);|kgF-(wx#u9z9kciumR+NAJ4vhgs)p&1VaIaPER zTr)Zk=Rb7%ZhvU%)z_vTNWU40HuhVqV^yElMw9A`sw`ruGB0SPWUB`~0c9{7i+{iM3rZXNrttsfLOC|P_|l+faxiF%!|-^3L# zehFgb8nE^PZKR9q&|}*Rlf&S@xcv{z_h+7Q@&nYhiCR@sp+n1Q<}(j`Xg34ofG%|X zLvKJ1m=^mZA@A#gF83MHyU0!Y8`p2>MV#RJ352zv^#{;+c6aH(_ddcs3vwCcCj|&H z;K{NaEBBru`5{vQOUkev$)+#G!ykfzspc>Km&SXn#MeV%=uL|E0C^AMus z9I@G1-2BH>4T*j5E^PN#O?01Fz#%6OBe=M$NhfAPpB5zidO0|6WfFBc2#g-6x@XtA(RtS)5ks@luX7iCX0M6xD6s!*`)-hk?Ze!XwkTct#m`YyFdAUF zI=6r3is#oA1vqGbkP3?yZo+beCM-B}N#b7En6%`1>K!XwYK+lt> zJq@zVe~r_wp6{O|?GL1GVqV#@9A;MwU16e)3@;Awvs_btqIK#$0!(g}8eiSFB)h;; z&F=JOX_+C&-t*zw(?#LIMJ|d$;cs8Q3Bh#TC!T{ipw3Uop%kiduzV+7oICYKb3H{F(2%Z?B5NCe7m5_OIxI4We?T`QC~86Hql;J|hHBir2d@ zC|vhM7WF6b2U{dwnEj{B6`E2_{Z-(m^F z*9D)9?D1=;e@K#h&5HVmgg+^K|3&P%)Fug&+E{n71wcFsp3t{!P6rjr+MV{<%0BVw z(Aqdn0k(zqxucY)QZ=6)70{yS@$FcZu|*+H&>NGoEjEK$HgA+E-Ki|-;tT!v79Mr_ z#|1zmoxDm}sKP1T#8@t5+FdWfGzS!r`7J2V^AG9p<^e`7YqB4zk z`LS*adon+I71lymIj?za$)|MxK05%@OF{4B5qC=d_oAKm#|zyAM6@7#@O~ zNckr1j&qzqHbDdc3Zf~**UFI23j>%Qg~<>7+q+*zpBLUlaF(u9z5FF-R-W3W1ek!D zPwpPp%j0R2mYbL0@Bm|BGfgizP5@~wdXtPozl>gwDUdNr^8!j@$tKFFej76kMnGCf z&faUeK{9oPTdy)T>KGr4Y=U!u#i7sT3iVGVoU=U0uS^~i-TYAz1eKtQ^sVj@q7{J1clk^26Unfh z4#c7y$Zh80JDIX#01LPf?n`7rgFGXe7r@R!<#q97QB#^Me3t2gaN=bjNN#^ ztR!#b__7x`V>3e~U_8>2ZMmh;b?U(Q$*kk66{Ot%pafBN29oKjIt#M$Kn%I7$M~o$ z$LbJbO>G)v65g}qre$)bx~`$(SRgSG(|q2!W?LovBEl?SP%#b0%E;$?59$Hf*&v*! z$@)_eyLFvH3llsbkJotR4v@}Nf;BRCsQPTR{O?;o{L9w{<$A%-neNVS7t=WSl`3Hl zD=69u*Pk%tDz4=q;{`PQtgx}#YxOu}-!y$V(+8SMi09P{KT-&}5NsL5f{^)NzYD=m z&*;@};^ReMn`#DmR{1sH^;|f$#C^_z-Lmd*nUC(hM3CV3f^bU<8dvtd^j^Xbu|#nY^B4Fm(MBx@cG(svdu&F!yQ2^Fyx?sn<}R=Y42$P#94 z1-v9e-|3b=^PmP&B80DNmA|!VD_fL+@U~6%qStOyPa^?%(C} zFVHOxBSPPWYO9CmADGH^xjXOJERwQz$s-2dri}8*J#Ndk!=h$Sf^6PUJFSD6(AFQfoP<+(Da`;0%P<{7XSu!vKfu9o64=G* z861^R$cC8=kLnye@v_2mo-R z_~Dr#Wl26^r$l(Tdqd`1ReRCXCE=W^ZO-$Df>U z_`%s#quan$ut30Bfu-$VWs&8OgD^!sC~M5#78EU09w9S?3$3W$F#M%OkaW5U-T64? zDkRD>l&{n|Ry07uMBH89_*%>;baqm}|4wcr?6tfvd7+6RB5A;mdV3R#d~`0=*&f3O zp-`F^;Fa-qzAe#!@7(!*OdT5P-CN@9w;b|<8vb;G@kMwpAx(9i9w$Widp!5&W`It> zbeTNmBRqKMNdQ<(?p23hXSbH3ic8J4qvtqh*CLnr(#!w6`+9rd)5LCrN+NG~AC(&V zyW%5~rL^pQU=FD#AX#-8ajgyd!$Yj}-e|9OuP+wtz8kY1>A~-|CE}PFo3@hOv14GW z8P`65P!@G7Ol@Rh+&qJBB~&!>6~>ozYB`81F4e#5HX!UzV6uE;-IYzJh~|4vqv<-$ z6?O2q64Op&Q>o7jXLsNKN%6$z)Qh5C3@XL3XD-~2dxhKTl&#ysQ-gQAxkKBFMdrhH z=j3OW4SpB(F+lB`TKFn05d;oYgx;TJEG=SPxrG1lX{jTn>nV;8MeIiIf7S zQ|(XA0re}-&sQaQ4ufM$yY4>=LQiCj(E;hDdFPm z-RxsDy_=M(&nGj$N+EF|kZP0OPMI$j2^&-8|MJ-_^AT>)Ey4+kE#@~BPVvHUA27d2 zVd7S8fXBY*^%%~J8nL?St1=}c6zum7qk)Rb4MJoHVQ&)KG+mIGbJ&QHw#{+EUJ-se zWpL}#-=Iy$UBNwvl&lC@zs`C(+u7sUK(J>Y;o>4GAs!_pHmD!GGe=Msk$1-J!e{z^ zTnnMQ#1PKa7H)wZp<9+@-M4a0syb|5FjWh)2RG{~B3O>k@y&TClYi!3+HXB&JTvwg zK}oFrP~{AW(LR@9hX~nf6S*g;TFqnk?La(wfg=MN-JWxwcUbmhy;d`s&{*AK$5|5BQ5C* zFXl+oP0YS2qWQq54c}aKR}QD(b*n@`k{z3)AlgfG(a%v>e!Bp}fbU-U#8qTMtM+Rt z3uimdnva0z&vN{m|RRe1m}J?nkjXM-ekc`MU5|NkT!?P8%vFSklKKeXJ=+e!UsgG z^28k`(Fpa>?!!I8(HsiXx%Zk2v$H3h?r0AuaSPo+U8!r>(<@ANf{j`|D2__#B%e{x zWkouB)?RTEM>W;*z~Ef;H<`=JZ}2iaQfU!Ewa?Xw3{+4-mft?Rhas$BXpxvOoQ=>lH2G% zS22Hp^m>0gwQ~lRm5eJXUsv{f2n00>-(t`h4&RnJtQ(awOuUf%rI3YGyKIE1H;>(M zW%kp_Mh&6c23Ps2>gI98<27UcFBC^*x%^w|?JQTnzG;_R;qstDKt;*sRG`r6&q>yF zR6dt5#BPeaoI~~D@_ENtc8YP^e_#ZHqCmS7g<9LSpaTU}iDI?pn4K`uC|Uph8Hoz# zM`thJZ;VJk%OE9&IY4{|CdA_VMV)_d`TPNv{vB*R@pSUj`$ZBAkE@M^LTLwZ^)q?i za!v<_kpUSmmQh*adz8U3qT>lW9;eHtG;36~xXy}Dsu5*Ze2c} zjrLfitY5mRScS?dG18;%Lz!0o`Ph$`lFP)cjkM-N)SOh|?UnRcU&8fERU2D{wdI^1 zt=8dei)G~><@TFb(XTvrZ1VAqzxJi4*{Rcr&> z$7i(l)!6X!60F|~VXKn+rtghdiKR8UBztW=GAZbKT%rK`r;n>?gCIf6)2SsN+i=4u zvUDQ>XG_q?zhEW!fuT`PC{?u(u0@=k$pLB9`@`IWsJ|#8lfi_&VN60(TDI^kNCJ!Vm_Xy1>QW}XTWfjMOpmh560vS zsk5aym8U!On@sU*di{aSqVF&3&7IR2E>O*y0{#%PIxU!%U0b;#==_1gE&?}OKW0!tlZpu$_TU>r3@&?XZnS->lEVypN}#kdxt)fJvbTkJ_`{V5Gfa zaMEhU+@pvE|0k4pcXEI4eYa}}=M>u~@g#bl`;jqz2mPNHn011a?mOck?z|nQ_F)@Q zuL$#W1P>M}WvU$yULW-Cisn1JN_{=5_%WW!+2!Pi?M-mwQ1H5xNB9sO#4Cf!t) zrkA>NxNIriI$WfTu#y%>&6B#HN9GkLKxABxaT@q+J4dojY)EUy2z_5V7%@aVN_er* zL^`%_uaEe5?hoJ8*ymFZiQ?jK@e-Sp_sd=tiw2XV_4@)t>xE%|$OVzcewyvADXb%O zWHGIM?z#_HlZx~>qtd9hvb!8(akvyKqgV#tWjR#6=51YxPVT$5bJ9`@)eG0iz2W!Q zF$trr@ACa(UMGl4iY2Ioi!7;u389e~bl$6nhpUiiFunY+VPn*X@Wa_!0?%KWFRWPFQbX%V8TU$lI) zaHQcEP=3l5u;Hv$hPU>d__nhD)Q$`>gKY#u4dUYm zU#C#DzF>0vAr4&b_Q)skwc2OC-yQ(_DftFJ5#QgPT3#NuU(KC;!19e`(sHJ&);AGrabEr@43&cLOuMRgXjr{ZWo`hCi}_^zou2@ zIAM58uJBG)IM3twFS@~7s1M6tFy;v#yW9CeyBwnUG=o95J0q?;61jfY{j@V?fze&EfMUM_BXGt{x)kdr>!b>!?&KWt7Z)gBh+t3Dvv0%M1`H~!I5(Wk}Y-;;F|xqYWXoYvvut5}8jraa|JD!-SMr;JuDCymdESdF!$ZYk?w6GRl3z|hd#CsS4hZ0RaXdLh zq_8FLW>Z8nS`y)iGX~7jrgCWSRv_biQnl(`n6tZO%$jj(bI&nf+teZqUDS`QyoVo9 zJFm9m$J6!8&N@#-YTmtaO6TQA*2q=u%T)uCG zUmqc5zp&EJ{jD(thMrZ>w)CD8k17+w$vM`5xx6ci;P)oCr2HK zqvhC>FTggtDcQiQj*rd&h14{4lq^4FGi?1 zJ+5T#kz2x*P#Z@b#FygDIsEJKDu?Xf0_OL{9l=@-!e?(FHj!De)RRb{@XT+HpT3#JjM zi_z@sKUCdllR-FF#VL zRq)#ae3(MZBGAeifQDF5C2W+%&27{HIQLf+{h+l?z29#K3SM!o2(d8+FGj< z6TjjvCPCGGnntf`@k;0k7)&SuJ^H-xv{4yisWK%YiLA|P%k`Y>^_aOOw96OlPR{RG zHx)@}-V_oIGte|=ia=>Vk$j>f0CLFVA%`4@H)|O~-Ok`oZzBG0`$LM7MmD0p!2{!B zJ3a6`^ShSd4g+9=jS3HG=C`5IFL+JR$1lXZbY{Ga<`1gNVE0!7)Rb7n-3Bo;UIFr& zThJK~qTNhXi=0D-0Eik0Oz)k6!0NW~O$1p^BAVQo{@O6P(F4qt@_(|r(=5XmCfPT3 z2DW$yZq93FxTX)_D6k9gYNOT_9(yWma3h5#qHv?}+ESs%$uNquK?#Xc`qnP~zjEKY zR#B|83RJiI{L_m{9Wr`q!@%(z8bt=xn@9>D`na7PBfF{b7K*w@VO z2l}!A=bnT)C`SdjDDjLe0k+=~5L)*m!Ry1M_5`wihMv@$7pABc;$iAr9E*zsHA3lU zQ6^}GBlx9G!?og%crgOt=qxk(wRKO~S@CHY)>@q1_-z@XyWay8Hnb%kZlo zXE*$Tbo53l<`B|`UMdID`419!|AScwg%*n*Yd7X}T}ePoD2R?jCJPStIP{~e)xZ~z z%wE~_ctX_}0D3lgbC;hc6a?I7xl-7jS{c|&WNY$53oA^(?au%pt+wzn)3nPyW8N{l zbn!t>Ay<#*_d}BY&`rpdKz(ct{{Usw?TIk&nTMA5ex5!j1EsbsVtufbul}B*ieFu@ zcT02z{Qt9G0(hU_gM3a=@N5!e0yJq2;4D^z`p$he`KXi&_-@%Waz?MHK70A`wr@bY zn)&F8q&a zqfgQ5gn;GV+FYPQi~0L=J>PnJU?jn%*nTAiv4%osJpV4LIbKIL3)+|>i&xY~iUr=0 zB$KwI*G4ivEJ48YZ*7j41}{5!zj}MLc%Q`Eiu~D``FabQiG=ch#n(X7VO0k+1|F3C za>=SH#i#QJ0tH=6-r0JNmu&PnT|%zx8!HVy6_D@9Gq?;E z>U!ZDXmDe=w{D+o&o`apw+*#OBPn!QD$Bhhm~bzElRhVcB7MlUF1b%o$+48PpV549 z9eHF92-J`Q9j19w4u*_Pp9kPcA91THMFR!4gsKe1?RW#N3K|r(LdT>Uz}S&uiOjK- z0ePERR*3|Y7@WZ&UQUCFff{2CI7hN^pfzi#{&Z|Cdi5yu*s z-sxNHpLj5Be-KyV{Afe^WrIts<>4O0m=M|n)(@o8M%X;NG#{Be*G$7G>^11g z+qaK}1`$tLr4`TQdGLX9y5!}lmW9?Sh->0L8B~Jw_wVE&gPZyM{yP&3RLUNhGZx;u zj*cF`L048}5}JL!L(K;jue;yz(gpqI zjwvx`Q5{L*twB?r+poM~kLVNIcRByoBo1Sf+jiICZQp!-7-VNP2S3}$n5DS?8)wh^ z36g!wpcXsMa{B1T1H#@7@jf&mWJO*GoO;Cza>BjArG5N}Nf`Qk`K#snmTPeau4>Bj zNDgALnnj^NTjh26F0oLsfo3h4xc{!n`6h4nX_YVz?c_eC2LhR*!+Pu4@s?3*RlCrY zSKVC*!+Z)|-SqbQ1au_EQz&i#ah)iB@`$S!WHvNDPn1gTr&tg~ue2{iEG94Vv8Q7L z;Iq#ORCR%t>$9;iUdoQ)P*?+licjgk5RC=-DC{;S!8neIs81XRU{QdEnU52WmIS&y zRPMGasvN92u?BpTPXaH@V}m|FKM+_|ts$lYo1kGDMhp7W2l;g;oS7M{Vx#ep6Ht0mV1rapNU;DOGKCR58xFfUWdw;#nDNaX7 zFB1mam7CEi3ze^fvb%0FYwCCP0UV^T$hchBGPg1RQp^9`^}=ljg(-uDQKq275`%OT zCt}l&CW$y>aQO`Jz7G#yhv3!uG+^v$JEQhNjFh`dvy@Db>d9rFmVW|ha%Q<6Qm znv%XKzs~)BcqVfdjTp^^CxL6$3&T`;I7YqVZd7fpga8iqRr6a~BaN`e_=9q`+t8o* zpz9m?tliiwcuN6Q*|qA%A^T@!X!YdPFEd%PcM-ekon zl2#zyoF>^%s+y~3gNSFnCwd^@Q~5P8+MYS5P)+Zppdrri>`Gs6WuU&Gn5k^}UfEU# z=jU3w!5kB$OiS|4U({>PIoh|By#_73H&(m#d+JM_dweIwIQ~4m`}>SAFD>aUIGa!= z&g~5T<0*lyv|f9So-G{+xl|dqrOwf-V7Cme-aKrcuT@ecsv_DH$HRq!qnR~8t+w~ zW`%9G1WOld$Yp*&3OawI6#37r2YbswW=T^(lgh1P1JmVI#2x(-Ty3?+tQnfSEqL2v zL+j-)k>d)u`d|g`9-vJI&*GGoewZ{&s|1*$9M-{~2H$?M4{S1RYu2pM4u+ ze4T}gh_0=J!&VNKC4=*qwjry62B-@VHAeQ#>PsObN|DSZi;84mbSNf3Q3kL*;+auo zLDIjltC<8d(-l{Uq&uQOp~bIV55F$E92jjyvq%*sjrk@PMs@w`8WhjAf`< zu_Hv5VUJ;j!QsM?DkR$TFf(TR;@3)yZ#6|-{~2Se9l=GI0)yxdsfw-P@1`pl^^*=_ zb1~G|ECR>>L)Lr8Q`yJ=<7eQAtdhM_83`p@IHHJ95+S3IJwmqQNTo6&E1415r%1{= zr)BTG&nbKF?Kt1}<-YIF=llEq?*6#zzTJ=ex~}(ljpulgSjpLLXh*cD<%uabm>C`E zh0_7Rac8x7;lF4m2bJMp&i&wga|ui^^hUH2xL}-qE-^E}SB49>)~?QUMPwZuQm^IV-f8B-|fB%k2BS4dSlICQXjkdasav*c4Z`c zSq^*AS^)q^wfJ{a)4hPYz;ViOCyDZq!wQ||1KR{)o{z~4);le*oD}xJ;|+p?CIyh# zq{o-xBK4ts*Nov`QvhB0uiZ7#Pz(4S(a-4%p#u!d(R-QaCF;5Q>4(8Q74fk*9sG12 z>b)&Cp)SWml6kuXNfG9-L?7@7wAP}s7W-8IGyj!qG9NJ~#TcR5&Stp^rqd|Ro=;c9 zi}b4s^wn}Gql7%=e<_nLE*}qE3u%K-H7K|CKXiX`S0Uw-y0{me-&!TF${*E_SHj(| z@CYe0LC+4 z?TYmqb}!F|hDwuyPm!EA>E<8si12(l&lZ8)wL9Fkl1mgt#kjq9P(tkaUF7}$vvYq- zpjX9&*+GU698>MR{&P&V-&s&4fWFV6nqzjgZMAK5M+6c94V`tTv0#SN!jRJs@CXa4 z6!?vC%$dg>+WMaIiSX7}st3MQBdnRHUkFk$vW#d-^T%Xgdf!Tic7m{5xLu}x!^BZA zcL};nish_{C=dP(bO5obdO*)B{Y5=S72CfA~LnddCS*}AX-geG+(Jm2wHe} zJ@&aL!P|%iIJj<1bF=iK-RMYc1f94!5SjUGqxhI3hjOJ$ERn4=Te+Gx63e_Ij?~NK zWa7Ti0h0(F`ix=%GQnAmT@_@7+bu9U@>PjpZvQTA%JVqkOdbx#C*ub()c&=;u!H1= zT4V}5dz#}syGZMsDSH3VS60mo^0E3{0@p8?Puj&^l>J>!?;H`s9yH-;^p!1oq&8K= zMnFn7iUE;0!;em$^Wd#g7Pa~4&zjfSMR%PUG=V#PdJwxp-8}M7m8_e&H(s`d&UbWBxZ`;lPPX*nA zmKHlI^*cp3s{#nLc=hanq0_M~$Sv536Kz^ZB;f)V~{@G zgs}J{>W?%WS8OEEP5`fuLQ6A|?b{ZQWN3}^_m%AlB93v;)C>B@=FWm#Gb{9>0DbuM z_}TylbWNB|RkxW-&e$TD6TM$(P)j{nkRvjtBQqYF*L=YP%>ZkKN~Im&kEscP?h$YY zDb>cdvFrf5y%}KzXt^})OAzi3H(-h-gW`n#R_ zcNQIOo;7zU3rGpPoP&8eaC0(~Q>blgnVuU6Wg|SwF2AMq0&Nb}7sEmmh>oV=@+)vV z>On32q~PZs6Y$t+-i_xkg)R?u>Woj+DQDu*-lH+c!{k|O`7*A>DLiH6hi!w2tYV1n zv{pBQ)jHLAI{7n=Z|wg*D1jGy+_-PuShkY`+q`Tjf3tj85}(B_Bsc%WsS3>7>)?0) z`Xk8w&QsnCdRXfWr(TLQmo+2qx$o6FE%SR%!?ZKS*kaQ9L^Hz{&~*kcNm=dp1Y2H2 zU#*L#-wMG?4M}vKuz8wxVRAC;ZRD9v9abq9kLdeh5)G2sYP$ z(jVK(Vb;8RdGM5C+MZPExP?6PPqrN)7`4gY^7dPFW7pTE!+=Ib!+-x`y1seae7O?F z6ky#ft<e8f?o#fYYu0 zhWuyM-;1j}M@rJ&cwujby~xrGrvC+|rcoX)IIC%HYpaN|W3|v1ns`&(!rDxJ3ALvN zR$s|BXKB3@$hdrkRs4ns2v6I>t6U%TUPkY$cf1_GObHiE39%yFv^u#=6im66N>~E! z;8Lh#1CRLxxzT(x7BA_fKJW_SUKmo_OGyw2`~Cgf4=5I_0Hc-@80tK6BR%f)0PRD9 z(u`#LMQ6H<#y`|7InyY`D>kY@b?AvoOqf%Ebi@Mj(_W@WbyvR~?t;)bE9Vm9SC zsCHmrVHnjiUS(;OWs5k2qNN%S{8V0D+=h3Z6@nFU6Q zBNHDKIkd`<3i+Z0SZKDjF`(1hZSGWxg&gbf7?=5)xA{kc^kFRDrdh zRMlyTPGsrG2uwR*d8+eS8Y{hPR?06+6rYFpBuXc%%7-?p=B@F61M>Q!+#U ztu8q}kn;TRnbkzvG3S$9Npmb0!WraZb@f9oDwKQ@4bRzt*>PKA#lbx+^pYD(27UZW z?8R>czw=h7%taBcvI#kJOzde?xHG%Ir#s(k%*PIEko*|B^U7;BPO%NzM2~?b1g@^71_|Xyfaa{3=xE$X< z*{464)Clx=struQ-s4Zb&fU3yqNVBsl|#YlZPN9()SX6mW@!Mz`uH;qTCmeLTD^zQ z7xfvo8Dj*ybJfC7knx)2J(d@NkCFp_YRocloEP}vq|#>h`>#WMs(X#a;+FpWB=VCmbtRK@j zx=AHJ6ojnVZdCDbqDl!d($GYd!z&}iDrgAKc8pVjPcAxKj)pt1`o7_N`iUxmMb~wP z?Rrg-hI}mBlloh=FLTWe=-u1c$aHUn6=fum2d^&4vkH*uJAO`c23_XgNir0l=E5d+ z3u&Zvs%$tUdeK8eydt!-Z=_=~=6Gt>spLUF>Cc5!dZvm~1}DsEQasH`=T`ro58Ip( z+h{*i8~b>~W)C}(k%ji@`U#pO%8fh<@FLM^X$SS^h1n|1rAf}uO@&)@yswAzacK<= zAgK+8n6rc5v`3u^yn6KPAaWb!}bM|4H!AUhYV0GEIFJah*H;nwK0|hJ3-Y{+)n| zPozw*{cbyY{zSEK_0i2n@|dQ=FnX%!Fv$Po>j3n%{ht4CW;eJj2*=gDFaFhG4BWxna{2zs~L)Y zJUV^lz!-Ln5LK^~!6)#_kaYxZmmzxd^@mh%)9WUmzSD63qSb)$_}yzbMTVGt0+nD! z;8paM3RtHYYzd|T^B9D6!iZXr;8+AtC%?zo>sK7-ui6L+prZA5;g!OJ!gw*i1bCIk zg=6F+&U|3x%=~eyH=mDaJdQO8iQ{-*MS44cfpue@u+npyS+^s0*ggIto@*%q?wV)+ zD)%YJl2^g0Z02D*0H_>zNnhrLQrc8i`CytEXNj@q8PvIx}(vMT^!a za7j(6z0xQg^KU1k6-U4JrsBu8%TI>LQ;HhNUGUZ^{}3= z&%QotKK~P324Nu~ex!&fcLpy8g?GWtn)T0rau#2EqOeQb()-ejl#IKW@qT`JOtHiAQQV$~;CdTg!Oaj&R4L##6az zqeYJ1K3z8Yq4&9klS$iG7&)+ns#~yM>q%33?&+UL?9a9PG5)FNX0WPF;{-&oe!(yC z8&4zX`~L7PCvMv_9xR)!$AqE-1vj;oo$KaL?Af04ZO_4a;w4=Fk$d49X4SX_U7j1b zc>xS$gn2@@z;dNUjd9rvXtd55o`c2>+Tg9z$(cSp{SR;0pl(5kLmR-r{xOS$ zY$dX&{G0w9hBln~CUD0C7O9)!cU~^a|Dy^99-tQ_A)n;ij{i2#u*KM7NOuFEgCp{< zf|#;}&5<3$Eibe2BL4Mn_bsQoS!{bNBj*!ooeeIX0|P#J_e=$!zL*sz$~)X@yxTVy z*0`G($PalB+yeG0Ke-xSxe1(?W9B8Y_qP3~o(a9GP~{PI{V>Q5Mmwo59$NgWnL=?D zG9%U3GSo(G0oQ0tlNGXlZV zIe?f2!R`NdNL?5fj0T1fNJ!EKkp8lphk*w)Lo6V39=4BXkrGY7i+pP(s4V!>``0KM zM8c<=9@)ru4JY*&=dapcUy}sxd<&NKXY^`ZR+s4IDf-EEko${$8uL)$dsz*)fb|zo z7J$iNN6da>2xPQ+5!eC54J|`H%zf!O#UD1*6jUX^gK5D?%|A1IwfBLS#MzN($ayZt z-(vX-0H9Ww&+!f0{s!Brx`bSSxC4D7iy-h)o9{O0|JP&;+6VN`-p;q=A#-A5_RxRk z7NrZ_)%~j5rv1n&Z%3EDQ%FeAO27&rnWrROfImZ9noVb9 zg$7>1ryeL-9oAy$9`cF9(Q{%U|Jg`nfB`t;z!v7vJ{PFI4+vVoLumsVPWtE*aCot= zvW09F9LDA14AkeQN(hk7I)> zt8|-gi-$YVJ`k-uK~njR_!)z zIn*I+RC&VwZUK-Xw?KvwU#=f8s8I$`V7VkPsR8B;Q_PTGPa&%i$4p0(V;wQVm>$+d0V zok~&IO7dLveb8IcXYM)qcA#`kDPVJ>Vli+^$#M(8QNf{_iN)pK_#O@XLt)>t83RQN zhFw5U#?NBbv-lsCe`|3u4OF=wtFx8mjpy1+mUQ?wS*}IF!CM*Ji;L^EsqKnrooD== zww%_J8Mf`$oz|VOi}J?Rw&xjcelUC`NA6s2a%`6jKPt~f8iq~zA^pNT+ZfzO`&E7C zt@($7cDvGDv7T}!t$PRG)%kbZ7l!~t`z0a;z=o>cIRBo43MR~Q=m>->Exzrmdf)@5 zU_XL}Hdb&ClW0Z&J~HnLz&2e$^SQ~|%yFMlfBQI!U68(iXKck1c*zkUgzhCz%Hd}i zft~dt7?ir3@pJukV%7V-27SzH#?w`~pV?BAg9rC-vEF%4>4Qzt$jcG@j!t$KdpTcE zDO+T29%6A=Pi|?K+m+d+rhbcY-Th|B-@RCFg!poWh6j8FRCD2pg97oy?6DSYd=IUB z@U%MrtSflNSIV}0SMbmvQ(F?BNBzk;Rbn|_jKv+yaW1Na@Uf9dSFfz^j}lj#zG5I+ z#a2>VU`{24ykyCvkE09|@#+lPPYJOZ_BavCd#(>v;OM zhEMgJIS!iTDoYCMoSNM#NvtlcoKjvIFyH>c&L^g$ejo~?ywDH4?*q6RUcf6r*9V|7 zKJ57I-<7X|_s2%=h4ISDkN)I8SY8B!p65r`bL7q_D$#CFYpM>8%vo&Bwt~+u>ASw$v74MJ}!S}b5(Fr_P= zS(aHu>D`QmCFlLlX}Rn8!>O3ELTJwY0?%1HnuKAQ= z-wfZ%b4{=EstIvTM615BiQSGyN@0w8`8MWlQwRdcD(_f>Y$utP2^wqczUdGk4 zw-e`yxL-%7ZelMgHo9VJFQ#>nm%~)&CHxw6is=YDz=>db6TlUVefWb%kXwQHk=)-X z!o7X3ohHi~&-xMoOERwZGLw#8!n6g9cBT5y=FME(-G*dDyNj5uN+L-fC`SCt_ZBOC zAOVZd7&lQKPQm%LICaYVd}}hV*c;udnS-wzVA5_=TFhRR@3c%VC3L#Q99SY#xJh@N z1$0iBIN%LJM0j!Dcv<{!k*dKxBSniL;uk!_GBz@FQ~{pABmT(jf1xuXW1BN}K)GfaaV#4D3iNw`#VkUz~%zH?CQYA7NCQ zk8WhE7ueVXaxr4{Qqa;I-$t(H8Il7S;5}EfS%!NAwxMNZ!0;H~h>qoMC)GB2(FZ@r zw~R%3v415{cTTkd>BS-dR2)&xnf125w+H={L+3?O2mOYay4*~5n|qMG_LXZ@Z?DwX$<(x{1kQCHzV#E z4Kf9~sqX7NfJ9-76@AsNT*up-RJXq~DWdqNABVa5{^a)7$E?QQBGz4+yfC7vvKq6P zBUkP4T4|v6yV>R#c;$d{pnUX_4Blxv-d07Tmek5nq12BmSiC*SiqN!wiT&$Q4(fTXDM%}8D#lz%EHs}i zpbSaEW&i>(xftmdXuVZvr!2OxbVb4=X8pn*^$k=fqI(4^Dc(!%bz73t{J!~gw#M@| zxP;7@>a-e@DHALh+-X|qj}Kv)B{m(GIZ?0=EBAw!ukg^0?`2t(2Fb})O~OuLuak<% zR!Xp1Mz%nQ-o1}u%I6lc4yGSaLJDjI?C)RCD6oU2EHts#ECY_HM8I%Ll*;b3;n+6V zVmdyh$s_0BO>RMRW?dI;yNKY@SsL9a_Bn&)#WKlj(n}-co9tzu8A$oggEQ&!`kj0s z?dZ#(-(s6gZVrskc&bRK66+|qeBG#+*xxlq>v^cXoq8a1R^_s_$Nl2zlck)g^>kLe zOS9Gt9qkvTvKll-r|BrSJdilpTwO@t$^aYQ<#f&OBX7nw<@n3MoHol!%b9()8HxEWip6PlP?rl)AZd5NFnT^wZ zU33&N%C?*^xHd?B5KnY~+YRJN8Nk)iL$F1gT|PjSIuaQ7?i{x~*ZG@8cd)5#i1W7h zt|pvX=JGufN7dLcY?y4c`C(_me9z@{e;ItB1 z?)1gpCJ1J5ZckO(-6KHVyt`ZISKnF< zbbd8z4on&6-McRBVm%A#(M9ic3h6VBxPRFqbZ3;Rhc#JjmK=G*NZ`bB!bC=Y+D%U1 zur{D452I2RsL!t(l~>$u+>YG&?MpD+-v+`^6;oOO~~uvHrqXRstW zjUl{Sx-c6TpRX`yuvhbP$$y*~lDl+TjK_SKZkdO3wInw(Oagm<@5vWGPc)*P0#cmz z1)$#!H4A%ybwr^VkwRq;g5`2<;MjE-k0csh#KoKUtQ)`pM3 zo^-6ob}TZZIgwa_U5y<7btJH@%ji#YoSdC_sx8wWc9J|Sna46`pnYWynF7M)#&MFf z#3#mpP}yljE_J)obU#Qy4EQ-KCZFW^J~Wu?b5FRy$Qp05u#9{q&#`=l`>q3Hz!%q% zinw^3%~68UixUGR^fv63vKc?@BAO39f1WVP!R*V z{al2ZDv5X!q43EK8xSU@;Pz)NT6B*uX|k7VY-<15-U=jPT1hAyQbz~c2y88I4`|Sv zOqJ}lurFn0l_AH2_(pamk%52cE#GKAjgC3ceP8v9-hG&_{52Q1htC?*v+SV5dOTwe#!e6FZnVe4;bseidR%OOC-N0x@cMkYSG^?lN`w zv6ALLYF0a}URmtGqy)vn8OFb~-7HoQwkBQ6J`Y>koVo_8v8&b{N(U0*CSL;U^YO@i zUN(k?y06Nvo8<#UaOfD^`J!tL>YR!c3}f|vX+w+9;nvS8z1wm%5tJ18lVO;+vD=N3 zWqUGYIqq~>P>Q`nyL>yx>sAEdD`eC|xL;QrYiFVrV1&wz#%uJ=>nm`EP{eOY3ogh<|nBkoal8A%FF`M1OGSr<+wlt^|wJmSqow}yO@80Uyq!!luVU3wIkG*z;eV_3Te~$;+c)>mF zm`}}G>havPt~Pkgv9nUsJta}cM0UR3)K4A#&a{qD)-XbxbKA6*)|4zK-^tq^6eQzm zB$%UY|m?T}d?i^@E z4jPjKrFTRorUuCgfARI_*FDZl7KcyHR_+!%7BT|4CS&ESWb!e>=`0guU?cDdBhWYP zTTXuT|3ok-d!xFMr=MLD3$x1*7SPWTS=LryD7zctFj>6+1DfK|@hHd|xexmTe+K_% zdmZs9q{Wd}Jsm81S0wHoz27=ac)AXbSV| z3Y^s~>IUrsd$%2tD1ie!We@+tRqA6rGwh(DByzBEI$!#68Iu3haqeXl+Bi(Xf=i^p zBu7!8_nn$3i6_9xb@H9pPZ0vAxJ^l8#O7lqfdc<3m@dJG-#zNlpCUVzwWPe$nyr%L zqjsNTTq1+xaMQ@vanze7zfDFymc6m;5V+~&KUF}7D~R{&RPTq3*CNWIn%reA!`N<0 zsZZiNIZJKi$8MLkE_@56_COo0Xjh%<2tTuMbJ==1!Hfzb;AQI6pY~^hw=&+#Y_5}M z`shZCTl?sm?eTYQyju z(%uLHc|F|k;I37+DIQj6Pq_TZGsJB0>o37{R4csP=4z7-)$Cb*IF@P@p%juY6&Iop zvo>A<)lr)dlV82M!PQ*FrbbU5Fo!Mg7b=63nNgma1c<>FqDZsqyI|g#&Dsy8ftV60 z_+7t?63n+O+Y-;vp=pGi(3;s2-NKtZSWW>}tYb0yE=h@WLsrOL2Br&>Vm9QamJg^= z>_)p`okwA%*giMRTJnolU|wM6zw_1<&jo9{81Hp{3u5ZUE5I``AgR^~3R&tE2sIJ6 zE^}SrZ67L)m%ZWti+(Y#`h3o@vy-7|XZBIFRP$mNPR_AsEL;iYy1~`bqe#Ow7%y*k^2hhs8`q%T7wH}Z z6QiIijgL(7W7llYV&6@F7!PG%es|QETsHdxKCJN#UYIdOjRd;(Kt!>Mh130%HON&6~&+xzRBLrIB#%Q(SC^U; zN|J4x5*E!I@POh{iqAUdKT{LGnfxdi-fBTJZR@FyxdUrO&xn*tMdD@W_IJI`W$QTL zbsaDkfw?@de!l<8c|k1QReL@8*^->hf!V(;JY%0YS(y-V=+QDxQIUnd^``slP2jGgk$=^OO}&~fyK^TZaDC%;+62!)OUX^04by)gsubZD zK1h6*Nb@$prk|974~(q_=fZMh6ON0b{@3EX@P?rD*jwO;koMO>TM zNY<~Ht76&#_MpY;KBc{JdjTV}DQDT>(E|EU#0PV|MN0a*`45X^Piog)6fXNTxSt?o z6^SWD#YIwNbMD0w2e$LUGD?cHuj_P4X76Tr<0tu>bHYH4L&Y;|o&RTMa54AaM>kVi zkDVw~Spu#Isg4VdF2Q}R2aH8FXBKdYs`-*dX=Q^Ah*H1hTiOBb zE;lYzi6Jh26D;C3$kMa|CCFI{&1*hybf6b?vJGYjb=bFe(4}?qn^)vrZ_9&;2@9x| zTmgDI#F>DD4M?RKE}4EI2U9a;X<;}CT4wO5^8Wkb>!k-fi3-1#Fi{B0L# zF!`G}U5Wo~yuMPRD7n0L#_2-MoXm1b&afX}V#p(v*m@<}tRG=#r81WpTrDoeh zWbD37ryOkIKVWK*(d0He@+e9_B>zCD;3Deo*b=-R;Y~j3wV{l1_jK=TD?tJ4r@jrV zkN6F^wUu;x@0~HOZ&ZrhKeu3oV*%K~UQTP8WnKYa#wcVvAgFi8sih)kRhG73!yRYQ zn<`p3CbTllUUi5`dcAaMC1Af0moDk`j{XtknYcTj+dJ;#(&$#0zu-1RGzbQWfdTMG z_tdE~6I=VAlZns&ly~G1GB6XP!^xU!*Fl(p4f1qdZTkYDWCzuv%5h52>7;-qxG-Q- zJ;tH85|$uF{vv3ZJ2ld(tTkWkY9rhuj?ugGSfY)rvfCDc1m{S11Wu*?Nc~26QTeE1 zNCrIm%aatvBkFL{&jHvKRC@?l5q#&9AYnIE#*p-o;H zB)<(N?(adu}(&vjd8Hy1D8K=m2=SJ^h-*g|(@ z1$*M8vW3etzS*1BhX>f0k#y9HiVyO?nwbMFXDwn#L=7?t-duxZK^h(=5G ze(>;X`@C7H+c$HNU&RaZk6r4{0jF1Qh&b?faB%& z;F54Lhj z6E>krp3xrxV~wJ!}># zQy>(4FMRv6lPAFd1-38S$Ck-{X;5wG()lJLPA4|{L1kb;2!)GuX%K!_kHW+utTYfl z7Kp0D6yd4yKR8RvpJWs#efUClJpHmhbRi~|=Ap)c#t&Zs8n~}s zw2g^jG{rKKaIp5kt?YetDSrz_daonfgO~CC>QBy$&ytTCdBNmUf@k0Ykj3O}dSzLb z;?+V7@YSC!yV`VBaWwxv3cdTZ&I!W>jP4(qr{!F%7A`fCyg2Cc72+2s*~2xPU`^Rp zw@RU;QT8^0Qy+pBbVyPH^T2NWn(7|o{wnp+HS+`^?DZM!HJ4kc`LOBIyaqQYr7&YS zSc4EUXjr>Wn8L&8u}T~S)--UNtgRh3GJ^~!XLGdhIhL}$=d%Mpa*by}m(D-u73;at z+?x|^hg^k%>SIL9Nqa`MJeAxLoI-Qy%sZ%}>~YU1tH)a3f~AyGP{V@|_s6U$ z(n^}wo!%|mA@{vsBY3M9{Bi2pxazWF{+?1k08j^C3yUQut;YudaQ&bPiPEg83%ymD zZsGG<+|)}FLF(l8#+;k{AWP@p7Wh=8-(eUN;?FYwEyORTqoN09Pc9nccW?U;;3zL3 zXTkoH^--r`+jxjkwg$?6#tUYnYY|U=pcq2`JPay9PR(unC=_ zegGns4goJhniF1Q_}~3S28y^7JBR66@-cEN5WFk`EuYJ4KlJA%M@`5be8vgbg6UZ{ z{*CMOyOo(tUTguF!Tr*`(CqzpNE?6Ex~*) z`DQ<^hD-Jp2DY<2gk4eH&P+yPy{&(pWaYh(UzCF^zv(1WrU^O`6farTOz<=S3s>f| zr}MLZCvtmO_nq|9PFU!Lj&?BA42;5Cb}em@<=(p?d7(pSqiYJtVKse#cNf0VH@i)I zp>I;IN*ttL9oZdjkGht+(ht_}1oiAE4(z`k1VPcW4_tl0vsG5t9axC*4gGF-as{v! zc&7wR&br!w^Y8+xHhxhP@$jAPQ?D*q>g=OUGG!4d}p zRUIzLWXznjRNd`(Rgvm5`b`Pg^T+b71_9##DE6UHBzr1gMyw8vmp3f=vRaF8qd*Qg z3B+3jTXmr#7h13mG+CSxJ@?N$B2F}k^2_vfKtJjjLwt+kY!b!G^nE&fS_XFu(ba-W z&#W2~ogXAUK|e4asvdST5E{x6!Ru0rRCdJU%R%xI*uA13-{Q%!xt313G;>zw@61_C zuiFbjV!XHow^Z_mV8g>AFIC4ar@f&~c4{tC%F1_Bd9~ak5%3&z*`jehh&w%0XA^Fz zI2LC$xWy8C1o1KiB~L5w5^w`>QK( z#-5Sz{uG{#+aL7rk}MtQfzD(GT6af$qM#E~&~wmPxBf;_vE}3dKt)#;4H7H5``MYH zb4_&2wzO2#=`*W@PPjia9VNf@q3hun!k$3ffNC78V&dLjdrH+2WScLA2! z2P^AA(Z;WOg6hd**ZA+z$|Fj7UW_sgaIWgfPAKVL1{!KBu5`PBk9*b z)x4dCPZ0}V^k~%7RY(up|FQ#pMyOxKMQI_bE=%3AmE+0SY1}ZQj7Ycy+YP+}?{mm) zD9faP>&FFZZLdM&J?`o4;qeKsSPYO`+HOGwMx!r{0+@?X@pi3X;YQhx_=uX*o>KB- zOP%$CJUQ)+o=ellp1f;sEWtMHGd}F2Er2cACI1!ry&K;?*y6KYk_g5P4O>J&Oi!*+ zqj5Glp)nZ!fK(!xEhP~&qaz$Wupi&>#_+85=$hHgy3^WTFsuE_Q0GRE!9@8Av2(la z15fcz=3d-r#$F&CT+)ZNY>TS7(}6>HFc_BpPuI0wRQF_Cjy?Bbn9ODj(#^Q9u!Eaa z1SRS}cc@I5-)YR7gPZy3I=mY>lmtdn++a~B1nAadv-DF94wS&1dc56Z{!^Vdh;#RS z2AV9sR5sg6#9}Ayp{v1I8{fGCw%nT2X3;?T@4<2dr8}K&MmyO|wyG3}nZXFe?(gZD zz+`$uZTq`oJ?f>cI90%7N!SGc4RlKlfbxfoH!`rc!sp!dEqd0|i9DC?45HO3HyPZ0 z$q)OzcR|iEz?+|A`4%JEgDTr|dAT~YPVAg;`L^vAV|;e6JL!FTeB*YjPfb@4r~~z_ z8iAeX&Z0+R<#5KmI}*D+7}pu!H&S)M5daFGv$oK?_UZUg_694}~JK^Yh*I!g=6W#h`@*@Z5j zSc+q@b@xI3&}Pi5!1tBp1vk}rq{(42^OR!KUH$h2PHR9wu6jmSQ_u_AVC5paP$VCL zkVvnd&fNqjOmRSdWq$Q~w&ViePQ4~$XGQJ4&ARW;JqWO01nd>Ge=A@$RV?0P3b?at zBHsT-=X&P)x02_@AmjXycnYG#EJ?VB&_|3X=`IigS$1*y^PNEjBW^cWE4w!mTHV0K z5-dKht~-efT5!6SRt(5L-XJ&Ysa#843Y@F#9W4h{`^0JbTo2lK6_$cE53{P_M@VL;68i=k?89>|{4gFkv0q=)sorHQk?+XfmkwV{{ zp|aNqK)TqfZg5is8r|NuZ=3sU?{eFBkl^`@G|{>&#r}+_Kx5f^19b z@3T;WsUnNk**VZA4kyQdr@7-k;yHcgXj!_M;;7>o{${dsH53EiTedtpx_~QN0Ih}* zpy!^t4VO;`=kJv|x*1Z2TiFoVgS>^q$|{!3|eC2CO)zoWYaHbJG*G6C5&>>4=6EzH>q z?dV7PevP*}+dxZHhqExnbahb(8NE*}>8pk~j7wh7R282Y9ZN;q%8IMJdz%n#EC3`D zDYa@B=dNn5AvfQBP!M?q`?@S8I+mrY7YdPp98@eaJZd@ZP7gNG1_Sb8jLKN4+}H06 zvYEmrTi07SFq&iUF09`?#Y8THj*zHRn~HxgH*vdmee0yk@;qQj9E?D*>CB9(?QK~4 zq?X(QFjAOF@}+mtELt*Y^Eb+rG$pUR3Z92d-KD3X_%s4+Q4SWtc93LLSCYwwWD?PPPOsf(T~=Cu-BZg zb{P3S4T`a@k)2kbMTjYw%^r5;;9(feptvJ0vtY+kk^7=uj zlT&+=-_NtX;I#e%wj_s7jySj$PUaPMZ*ovgu*QIKKzC@Des z#G{az3sHFg$`ZvH36c-&Ie{ra%`)JNcTD)*)9~nCox07sk71jq`H?N20y386Qf#oP zhk=AIpZLF{)QdZrRW*~2l8aiVs2Ley9h;u%dv_JvxlM^9OPDRH$xLBpPSrsqdJSrT z>oI5kQ6Q;c>ig{VBk+K(c{=*mS}7j#ozmOmmc7Mb8<-u5Na3{sE5D_6@Ez1;Z`ME8 zY!2&keCIw`Ket9*j4WC%A`_6)&(i}fzzL=fU^!i3i`20DFfV!#MWScu7ibgqbcSJ- z66N5hh=haEq|NWdez_8xn%b>Pvh5sacunaLA`7mYs_FyuR#!I0^=HMQOv<2I024$|_7`=Vk+Cg*>8Z z60r)&=rM*4$NPgpOK$FW43a5qpYROs2b9-~Nk%G0jSNl0wZ%dQLvb_Zy)Zi!UgER1 zQ=6?$y<0(qPTvO!n7@{arbn-9h70$;3`%|~k$tdz15}>(@a=Tpg9+~;AEM6KqKEPVyvw@`Re)^U9_myn&0zGNf`<>_B2^+odIq5Io#WAJ;&*SE0_p(yJ9=(Chc50(`_Dqnd(=;*bd04aFsnpu05%s@;uV53@p|< zcWqS=FGDndcp}UIE&lV)4Tej=TS4$E`$wc~8px`ItT^4DT&<{nxGpqgZ{JMNVW~t-mfx=~yR~QpXdNBS&LaGX9udqQj0c|8?Qy zMOZ{SKN(JF)mJ634s^ooymAxphLNs0m9TqQxr+MIzaM{B&8U@#C7XDD-F~ilNsxR! ztl_Vhz0eKBQPWq?*w&Rz>phSj2(ggdx5x|*VVI@>uOAZ`)$b`_k3rujU4*TGG}MAC zwECeu&Bv@;whe>y*mqM1FqpuUCs1)Ywt-i?mKMfpb<9L`hXdQ{3r|(?dajP#3{T_y zjoQ4+zbmHtYkrNwtU7kGqHTCOxC&09a&^CNc2tL%@8qd(s-$U1m(?W2Fs6WS%`kp0^>uPETofv3)|OXN|?Sg1;S9B z_D)51Gd*8geOjq3S|zqcnC`;fapvLf?utW=3k*jg*U(jpoBh2!>hWxc4;DFPK~!cX0{ksfifZwLL%>m~biD{;;-6dA99UjJCg`H+&TseNF!2X$8IuOQ2{3QvTLe0-?{^U{No^&c?{cDwVISbh zk2Su_onHn0CBurpXX_JOa^V_OPbY;7L`vRM@6@t$m?n~LTd7K4<+wHm?=kn&%=-G^ z6p3n-P9yaaLb>_Xr+hX1!}YfB#TmqQ)3s_EMpPu+6okM8`f>P*z1UAF9>g5AN5fev zD*H_}E7#yazVk@f!z&gK{G`KF8DWS*=5bh57QDhH7Oui!==DWas(qD4{&;^= z20fO^Ucf9MH~wtyAi4MDk2!ur@y6wrP>cfUZ%*h@3zAq3@&SBexr~j@WCp>QD>POc zNzafwwHZoN|8uPju!<|jZy+;wlZvyprvFyKgF?dAqnj6C&*Ajz=8W=F1i$npTP-BV z|Dox;H4$2w3N(_xn?|yBN+64p?^-qsRwG)}iH4UO0xCKJcqFpQ3M{ z9~jRi@31+k;$mDddf)`Dx0O!Hq=+>KN*b6>m(qYBoR`Akd>g1toROa?$J60PtVphh zJf?{#LvVWFzsAEkKlO&j`55>j5C%Ly^$iWp&9cN z@8aFVo&M$-St8v`PLE@@+?yt=QwjL{MlJ>aty8voGIx$zjT>}#IlNZ8Kf(`!VX$W> zXYp>;k_=wYZbK=1V^-T;Pav}iu_K|U$fA|M^_d_#jh{B z^Un5n#fc1Ls!`!nJt?f*-$qHqSa?V2+s9A4TDJ;f2{!&}`mTy9W!cjxLiB&Pxb*l5 zOiJeYOI|}Q9ptI6)5j*O1Y;t~kN_5+i+fBlWKZ7GZ(H}bRzZqup{*cs)`8-s2kU7| z9=uEP7$iCK9Ipf=#GLTCqzFKNb55rTexyd4SobE0$xqUn@208_dR7kUtM3-)Jqg2; z(3Zxzc6`=y<7}8B2%g9MIWtPL4e8`kcKltq6EULaVT*#O|@qC zZ~nGu-@tc-$-fG5m-f~Fdk0;hZ*mVtc?hG&|625g@9$jdoX+@&z2iAaGi@K1`VN9b z*cnYr4hT@bUKJ%Pe#kNfip&=&Dk<`PCzt<*Pl+M3lc8(y1a=#o$2?^jWtT`@k=#Zv z3X$2*P0pu{&*7yM?R!&8-vlU26_VO|ph*lW0;~>7smgm!#PIR)@D454FkY zVSJhqUuyGTz+U^G+n(RC+Ur`%@R5fJfzo{_Bn7~hzdrb^=zNxB@h5a&Jycjq@v(fs% zv*+ChiqExKz~Azuyi0{_MKFqxmlycNwS8{`L5YN`4z*)PR9;KWCapSWtVdUcVBAR{ zxTh;IESD}miF0mIyJyzUV+SI0#8{S||T51|h@m;zcEVB9Z zp|I1oLpW)gBLq>^d9inQ$G*ovt){u|St!;NJ@;7m=Xdf_PWCjvoc_qxmbaNv?9zGg zw`n3@NF|>Yl6?y$4F)$e5P&JTWwtANOYQ0l?B(ncLs4PwV zJK7@^OulcutaVU57|wsTVIu6omBu3fYI6_EiR;o7P$qRB!!c6WI%XDtG*;#EIn%lx zFI2XUO?vtFbT9ZJ-VKm5}eV(nPJVG&3)XRt|y-Dx+{YM)BZ;L{6=K??uG#!MKL^g4UoR zSQRqKb@3G@bd>dRrK3TAK7EhbQJq%)VuGn=$@H7iHU~w@+#WOO{=*=A_i&&?!b@)_rX-)c*Gj=)K zBx)vxm9KkZ%aCT+)Nx3QzOt`WhNytiD;ax&I^>YH%nG{W5awef@sXw3jr(5k{Z^ z!taomuo*hQrByky94Ygu&PH=u2%`uW40pXgll-My`ibLXvc-J(_vfzju}_30UhA+d%yQ01S|X99mxO+6kvuBHVo?^2VBC%6J3&0$6mM7} ze(_TABDRV}j!AvpT%Rq?K%5;=}RYf1U#gF8s zI~ppE7-$y~Yq$q=>~MOMI}l`LcaoXplxL6T{7xCWTswYIM~!*7Xzh4&ZI21882l?7 zWsY{&oR)B_%Lb#?Vs3OLMw#cl@J_!`@LLh*2@PcFx&Af2z}s$n(j-w`7OmNS6do=* zQPFUCqH^G4EAISp&CLnf0D*!k+_milfsSc9ThB-6in@@9lV3jZ#xTj^s3>fvwDP7p zpFWf5bIOm@jP-nO7t`RJWp|3{F#H)tLP&m4P{3p3EfR0sUa&Vdqqzuc-D$Gk&6C?% z?lkZ<%4jXAS-+#el$fDTn!y56xHAP*c!VKJ(;`hx6@=fZYVGH)v>l%{)$&}0LRDo zP^#(M-b}CN_Xm6DBD;=y#iXBi-~V0rdaU`NEJBT!O7jC&f@!SObSG9ZD4b*}EhbH_b{X zGrV5SF#@_4fQJl>f5(dz9hxAyG5{SziL{C_rJLjEHjloL;ha{tC!Ab zhRJH#kX}dwuZIh+AZPApD(cweTQU?z;6#Y@X$yna3M#i%-}v@4n0|K6y9#@}75yi? za>AoBc{P{k?3W~;4RS;NUdz|^wmTj3(3E+m*wt`*i3`f`=J52uA2e$STx{C?&jYu; zK>oTMP3Ti=SaO2Qx?JIU4gac;(G!a0Z>;#vXS~<}&(q09^nM-OW3)GGE#!Aov}(<` z&E(&vxg&?N*(P~3?cx1p^kY?<_RD@=4I zYZZSBmj5#tN0P>a_{d&a+hp-n+h&4>6ot6+?li*5BxSoqQ9cD!-3woU+{)-~$*>;2(oF0S(agVfCArYsr{;$8PT%LcvVjlyL~ElhV? za@Vg!mY3lCh$t}WZ|-a@W^Nef3U?Aq?`_9T9oMMQLmjlxyT@waa=aq6oe2tBcd(57xM!p&F}{cgm`vwa7z1$?9va)`h?X!Qefu#n`3A;(O>G zd)ikystqxlZ{o(xR#9x)1q6ve=?JXeZn4rL$jg9Z~ zpuUfOF~8N;qYwS76bSP_nYrd2vKs6~iiAv5rl=U|(H^8txY+JCX|Ynw>-Vfk9P=vR z{?O;pQlCY1!=TGeE_H9WV(cF1eP}K)&MFzdJ&o8%#&3eemUkH;Me$Pe;-Ej^-VZ4@ zQrU~kxHZ&|XK#esm$NcO1|rvx(PmqyCZG+6!O@_#02ylR&#vBb=BVc+Dr#U4i=z#0 z@#QSr2XZN&;9Tl%>{Z7xmAss`| zc>=rfyNQqA>z0p(w%}1eJ#;2l?feJrguhlpIIzkqXdOj`YMYs~z>paic}L7%uF*a{ zPX6qvJ%Nlg96F4871JUr*r!cwoIJE5{X>#faqeW!? zxC{hhWR$+4g3+;hi1`k><;X$(U7hegZ2K56<-QrKBK30 zuhtPdAAFqBV8-^jAXEZToe~0bpnS2 zcbCX7*AP7)hVmB3IHV1E`@L~Do?qmcT>w|wVr-vsHME7_tZAY{IZs}x-Hm@qP*!Q% z@TI7@w_4dwqeP~$^0GMBE%tz`b*)*6&on@Km0E?Yu{U?IrOzK?Z3N-QqhvrFUca}i z6Qb{oMn!r{OC(sy*9(Yr9GY+oODgqATIOjffAL{jyc5uGIe}wyV)#H8%Pdi(MXjL# zLb~LSrm+_{fk~jL!)bp(7s_|+6L5#kh9o-)95lFh2g!#W26ydg+;mGXSktBR4NmQ* zX=kQe6=~8xk~8_4n|nD|w^F|vz;b;tU&fcepIWW7f)}xg&8#-xj}MsZNo8Ovi``5b z;MO<;riKs1qds#5LtV7PhJvT)28lFsopW+g{@hW{6MK-~@{0cxx30z>0B?tgF83Y! zh=m(#Z5oW{t>zUy=y~ACM3NS(D2G9?3iks&CTv;Aupkt>#m1Jr`P8DTN(B47nsa9oc*z;Yc8N};r{k+K zPY17hf$qY*7CR9GmPTjFuHpN6G0Pqj(P)iwjWK+4bvW#jU4}-CR{{@M{nvP-6ntCK1m4+`;o037%}}5>~krw z`-yGn>UBQpPQsp6u*%?M_MFDh)Xm0-Dgd?n6sIMuW|+}TmHYeOjL^elFP%rQq2d=` zRFm>ViI?zA_cuMCeaT1$V0f7Kv!@4notGkqyd?{B`cFanpwB=B{B5BernE?pTUaEM z;qGx?zjtUf4~Vp5)_;Ga-n(oZks;r0>}A!*4Ss#aMA*o}mu!3YW<-x#TCd>k>`s9* z{Ue4~+Mt@ys8#m5i^u@-4|E7>B~`&LdYp`nmP7MVS8+m+bbTt zk0l8`I~n5m*jk`opugXSF{pw(yzumeD5NPcv-X-~Y=3G%;&dl5YkK7yG7> zTrH9*uy-JftnMp$cWF$G;DCQyDob?~eX7-s>@c9)O-Z}P14nsF--OOcQm_*sjlGv8 zI=m>j=f}s6D9?yva2M1+iG^QhFL=7ZGYR?u)^|&oBlPjMqs^|fO8@J~XDt}m840vCqvOid!2ZB#<{dO) z^;h$3!sp93*}aP!X;LxIf9>yU%}71_cZj;U35(r}ThDU|3R|=Uw#&7L$p392HQ_O! zF~X$i8IuI-lfrTF$&<&Mu5(9i-S!@0PqWt);ws4Uc}vw-Sz^9-Ig4-(bk}#!|C>HD zp21U^0jfed5h7hslw2wpOIFwO;TjCW`6eGv z!%bqMMv8cly-8xF-Brt_%v$-S};8E&6Bh(JQxXof7K!?EX8VC?5r7gbtUscO@H&9GId#JO8_)*3mtCDI%ac-o0I>`#J~qxRZdzn$SHWVL(}8+~s&nt2 zcm+FH(&QZxO6)(L zdi>t*$$0B&$Y#w4CruOdpbyf(PjdC0puz9b7vd1@2<|h!1Z&sqVrC2X`Np(s>hGRC zJ$j~iVB32!q&4KWxA1((Qy5Hf5kUPI=dZJl6s0)FE6{b%+xX7jK57(DBwyrE;B|7o zk;ymG9z5?AduPZr>`dZ)0TWz!pYMscOxqUuVrE{5#{ekE(e#U<6dmUCyvZ|#dodoB zM?wA>Vd-y|$oH=);!eu)r)`Zn59&Fu{QpcOHO@i$Wi|; zAxJ~*z}@2ew}&z5%w-}^9@St|@)q<>0!6%$iLGhOF!q@Zxl_79H#Wf5UN5o&9IxIf zluNM!=iJ}#b2nbITz=-iE(2U7+^t&3N=KIYimxk3;N{Nic)Bz842Ym6OJGf+&Uh6w zfl45Ag>~X#W@cR3rbR{->qJu{Qux=InbK5|ZL0_N5iP!^cRPCS#-i764%X`lo@utC zG9zF^(1-Vf$iSI$EepH26$<_vErCCh^b~e7{L?afCCqbm&Mg!oS|~Jn4A0 zEX?WIlEU^4lOGrslb^_5Zu4prnF7bGG}Xqn-D+|L=lw3-?;PnT9R}_rKf$+JUa(Y^ z4qQRrwb%1WxGTyY!?N3AwZ8U<$K9&%!sRN(geJQ9Bw*ZTR$a<}&SYzU2yrGL0-ug= z{h7LmA)Wqk&wP$)Ym8N6jm!H^`BeLMJXG?dSI+mF0 z{WAVaTr_0e-ehgadexRsmyx!A-MlDZSk;F0@5}hUm=CKbQ`D(d5#k@5EDR3YR0!Fw zhx@l3PQx#!#|c2pAW5-=m?IL&OmRI&TdjnelMH=qDC3YDT0A1DxY{N>KRkL0930Hn zoZlo$cbSJv>z8LJeNXt)WVW7N6JEtiJbb@{4LXFoRXcF)%RB-2=yC2cg4x%FmZJdX z7`t55bL+FOSJx{%J{4ISx>!s^=-3FzR=-OTn{MF38RGm!uCjOFrZW^|R&|4^o?d#A4^HYLArUD=Me97NTV zz_%++txK~OtokTK{KBW~Lv8+CPHavc1;~fl0(D#B&uhsXXPq8%W%^m}@6?!eb2sva zlN`!+ckiV(xEILC-*p6Pu7mHVfwft9Vd`Owv;29TRE_xBMb@6C9>911W3MuG(Lj#8 zx<;QENjE!vy{cdK<98hbOjD0{1EvZQ*=|@{i$}YUq>WnC*?m)RG|TH!H_2A3L&bCr zddA<#k^Xa}Bm72i1Pc`7g+CWRRU>-4%yit)MtKrmrCr8k+#(5C$3^2 z4V^zbn6+HXCV~rwYWEAypW!_Zd%I=9F&wUaLzMw)|7G?Cr7vUx%IKNahJc-cE{T7t zncnTuW0Itay!#^OKrm3}%VW9sYGOpttL*ITopIQW%vB%manKsA@bFS;*_jrwvl78H z;1$Af`Eu|NTox0gp_L`QYW=wRe}rX|Zw$ZM=le9)#FMM>T3fi>lvK*;;8RIF?CbrL z8i7auBG;w60NW>I(uV~Wq^1Lq8@p0=PtZGp5u=yNx}LFqgUK;~#N~j~fYDrTupamf zDPlkjr#7Ao{XQE7e&3T>x~5PGpPYVQq8jb4RgLLC(nMt5R>JI%i{>!DyWw~4{{p@L z2L$99BB!pLm+xNc6NbD)e}LA&1tQlr8@_w}Zx^f)ZUM;xSWH={65Yl7&&fdpp7*0h z)~3l35}bBV&t?y7ap>IVGXPx~+K_+nz89$WIpw0Xs7R;Ux-(x+kK-(e;2VJK5CLSj z{X9cDA`@0cez^#QA)SNcDp^qSm>{hypH3<}b<2+G09g4}S z8ZvtIbc6u0K`Sy#fSxIxS(p%nH~g;iZ4rPEqOHX)%niQ|uQk}S9Ha+PH0}{+C2L}7L?11+mZ}9I_xoy7}7cI9e!EPP@-%QHPN=pWE>?9lz zaGFyf{sj%iftX-`E zJozVv_(Ry!n+M!&TR(x&sD~u>AD{+=ZSOw&@D+%s&aVPK#Up3OrRv_h{@(Dg7*Gw+K22V)aBT}nvU&j>)6V!g{!lMK8*&ds zh=KOZ|1O^^85yAe>12s{*uQ{;F^FgY^*aW{m|p>*)cU&@n!=Ct&ww;W!f<$)+CM&V zzi8nDG^|jbpP*of8F`BE{J`(FZ@wPr_VQByf5cKP^ty&@OIVt=a&X$e+J~-OB!+K%iOSwI}gw zi#Z{~AWPXYLkQkS`?3Lp+duQqGHXK1NNxk!cTd|lvuk1hlmD!v;^*fB6?J2JjDc1a^WH0Wgv?R2Yt@^&ggj|CVPk8zV~1 zQwy4)T5(peo9N8fTKEYrWwV7G>**%?Pyb4#1P$nH;kwITQ4+WCPCCWr4Ct$;^ zj&GSLSQx&z*mPDmkUu1*!AIBGRf7PO%8w|*OiOMV@~a*T12D-2oSg^5J_o-p|C!Vl z0rgo+y7B5tJ4|gSg3kA!Yc>1ZrqjHkXYklx<%LrtCCM_~ZNHij^Xz@sb&DlnDg`+t zS62n>?{>|Uj-I~}SoDf*0s83p5>ubLe=wr}%Te<`qA$9s`G003U=5T2;LeK&G?#IQ zMnJ37za9Z!A6!E>XVCq)5PuT>PlVivPM9I?3%~{egMXmJoc8dhY!>Y2uleS$5fKm$ zlh1U`fDiRp&*}-0CYf?}ns{tPktj^AWU;UIli z#1eK`k{zlt5b!bK(T9-3_qOw&)pDAMvnJ2HxaV!AYakGj3;;B}F$&M!0hZ-~FVR4I zybQl={yJVJ-hdPy8v-zpr21w6E?o~8Qe1nh#cnmH3pjS}M016Q0iU=9BnY?tR@{gk z5B373g|~W%nn=>DMfD=)+I}Rhli3ht9BTOK1#$D}uXnW*{#IA#AtoexLZh!*z~&Bng;6!f7$Wc`cdDeEW}lpnO7)bwrZ z*_#1(HX*t&oYt7b-XU>`F;ZYay&)!~$+vXZ2}=TcmpXbh);HHK2whjpH{xZcdtc`< z0|O=F6ucFt_;BB^Q^dyG>%TEvwutR%-#mbO%zh&(menVT-d0CKWohz_rLmw9Y%}0Y z2~4P%S-6VN998t)Mugn^udsp&0b>sbV*i`OC}lF!pqZk?n}y0vCXf0uJ3uttww4gQG+bJx-g$wj)O>56eW*9K!3^_ND6zYpNk1 zC_9xc2SyyiW7m#cs_zkpsBumiv6F=vzPh#V<|y9H9;b+zm?35?p<24v2W0q4%UbZXSC*r~N} zwGB+^gg(D%ANn;dUW@#@=Nne}-n{7UbP2&?fiM^&qp*#$z-`%r4O*}Lz zz1*xH(2@(FNtda2p{i>l4&(oVOif0w_>ks1y>m~cJK z>L}66iqf4pDdMns+r7gaMsFD2B`p2C#_OdBALf#{oawAI93Q-lqIhn2Il`F#tWAMa za3f8eq(z7EU~D(?BHc(y_}thed5R3TJgK`qHV;GMy}CIAHpN3``UQTx>UT*A9xwfp z61u`G>{g&M_Q)ajNkoaP`z-Uz# zcdu&BJ{Mp08Epmiv9$x50uARqo#eSWsYYy?L5tRQo#*D2i#9l>)LH#(#uaIrOwJmC zbxc_SsLrNU-dJtH5Z1agqDHLBaEB?vUL5Yy=P@Etd>5y!^!;);$tEtWw|}Xlwf35t zr!CH0rT1rPQW!@4DC=S`k)T1%c|r>Ks?V>6?MN5hW{cyP$LY9U;tbekpiZQ_8Z#~{ zQ!2gFM3>6yADvXy*r@1QSH+qaiB|6m4^IKwqi{9O2smLa%Lfk8)7{wC_8>)Kczk^^ zsT+H)2MIySZ+Vq-{n!@3%F3qyO#vqk?MvDlJC|q6uP*YmQ?L%mF40{F1ANts)1-Rc zvVC5kP5Hw9;>Akl&}(cbF|g-7c2n9r3q+UbUtnm4@BKQ-@m1lD~LT^1>6rP16olt&Rd6 z^!U;lu$k|`)w@|#n|e;cC)+{T5P@*Ety3L)Nq_3gMUlo;C_vEM3(mHU47k3!ql^|t zuGNEO_Fp^Z8&!QoOHs5kb8U>V6)vy2K%G0iL!V1$Da7&GH_lxcEvp;X!TE!m1TKTP z)U&q%uEMKUf$;a_yrCxSS&8mRe#41xerN~mdtu-DcWl|Q+=U-#@gMtq9eq&68gprp zATGDYg8w973;2W$yopD!%pKd=1#k8SnuXVsmwDPlYq`G$>`2_mauUV0$U~Vy)H|%| zpn{@w9>x_ad9sa2H5827)c?j>0+_zmhY4c2E3=lqOdnBzrrL9IV5OcO@H%T@zG6*} z{k-T7Xd;e!cNuO0Y*CXugjMu4sSSyX;&sq9qjwEdCg21R#Em5Oc&Z~dCkFMPkrXv` zLR24&;!z}%poc3vUIpOyAV#}pkTT{PCOWtoWHa>V&0pUN(&p8&+aIWLm{e^Gqoz&q zhGeo&`9FcJJZp7VgN)5!M}g6()@|ZU(XCdZUg(o=Z^^iR!l14H7eO!w$vItP$S*NC$V- zNj?Al%@c1y_rR;3pmWrK1R#)Q+*XE}T7*=J$;kv}{8tqQd7Ywr3EGZ`ly)B>W7T#p z3P6^@#CiqU%2{@()ynVol;=^cuP=|?QKhr3s5i#c>U`6U_$G6UgU9T z9T{IhAZbgHK|~+MK#AM7(2vBD^JYoRXT0byBuvP&oJswOqCkd3PJ3*85Ha_m+$QHph1YdBA&dv@4f z_?MAw7vcKp+%3+c+~kslftVqIGnA`*m!_d(eaepq;oaGP))gPpRG_LL_!3)K?kZv6 z$ww-v^t7w|j|hf&NA#fb(QUgc<7EmB(=7W?A|5$j^QuNn$J=#Ugvnh#V67;3N0`9e>Dl?#9UABM-xz(tD6!3$FghupAZ&z%+ zug0akWpyC_btP5^N1M0UBPDU)UPdZ_){-@bXy{6y-oNVr14Ni=?|tn13BV38tedF zdaQQBr?f2B%AmM2<6U(NeJ9s;U8#J_;s)Ga7oV6?9M^k7x2wD`f633AK`g#`@5@j; z*Ib|(q08c-(pWO%(-<%Jp*rVt--+rJOFwz4xT=r@@E9Cf2Sk><=EdcYi)3@G&+g@u z)O{d<1{3c3YP)Y{GSpV|7DNqFDasWUG>opZV|m3xPaBis^IZ?z4Ed{1Yl2FP^Z+qL zN}XUB**sC8w#x1NftJXwdm59{g!Y=+9q~Shw9%m?VYlgZd!t@+YVTNTv$4d2Pop zISC4o3+ajfH8s-NFT_hL9>t?w1Oi{RSrSV4457Xcx zn!q_4?wXs3s0ic1yKus)3~E3{#NH(wd=1s-U3K92=~JPz7=P5QNq)N<)}%_P>z4^v zl~6KuVnNA;3kr!0cCVNRk&YCuK>~T) z)4#IOvx329YP(3XCP7Uj5hmcb zqa#Nn`Fl}J+)9|tur`Vn?IL3q%B7MViZDnl*%0|NAJr1i3`AWc((qg_cyxc}3doz( zKrMZrT6tMAR$Qvy6T)QE@ zzfD@XO*}fWC?e_xwp(EY#$Ckn=6!LD+Ci??7nd#y35rn@CN7QO*HDcRtNSaEc3fJ3Q2s({%^W|>%1E0#wS#4}t)QQ1W zTU=;s3xda&%tS;YbNMpV!#^#*TjdO|xTy+fMpb__-(-ID_z}VINK9hGfp*o^-5ata z0|i=Ea~P{E?K5Og+bVHYar*Ca3}2bw%?&EzePoof561PjoWj4vYr#Glm&63V`Z=*w zA7&jbE{ndvU;hP>q$G6^oPHWJzM1@uQa!0=i7k}w=lDe+$&bDkINFBe-Q*ra|I( zzqbNXldQ zxYxLZ2>r}U7`gAON1c>w2O5lBy4-xMDQ<>Z@uu%+!6NN9d=yrl=|G<%H()#R+bvzb zj=>MKE@d=Xj0`?Z{6I`&>Op3ua1t@0MS7Y;>X(8-DM+nEm8anm%5^%d$CAx7J}q+D zQ4(%O!*YE^#dH<2(gd9m&kR)Onj(8XhmUr+k*#AE{zKH2kbccr5?bh$p9zUQAT-J{ zzbG_3LJY-ZRM(}*c#9IW;~1J1wSeX8qE>U8yTA*u1rH@|a)k7Q$zIcK#i%ZBdjCay zg9XKlO5f>CiDISDJAcvw_-|UkQD&GKgLx%-8G5guq-1(+ zvB?#7`->rH<3F`G<=!?CQw$Gp4cdZD$CDGzOTL22R<11e3A5bmtsHz%O@01ILx-XR z54lx_CF2;mTy|wRR0Y+SC;$W(3B@1>pzS)KJxO2%isVI4FiQVPICUKiK(n^g@6OoB_Vu zs|iW~-Fq&fhZ_2m?eS-y#ft;gfj+t;PcuDe1TviQlux5AIYWgL-0>9W({kAC6Z)gU zSN(LV!Z_z2iRZn_DdGRWj?uA(g*R6AHTh&@K z55az_DcjsU-7532-`V}rq zH?L0R!jWtfd{eO#oSP);yR>X^54utji#Y_#7@ng}TBVI8nZ2*Fb@9go5o!qwyG%J< zg{wl|v3FuePFuQGjCVf5Jlu9w2;}`3xAQf_FFeowN>Myt>knS%`N+AT8cL-QqB_Py z?OuFuJL{;v+;s1;_yO~RJYm8bsh4z`R7cI5^E7jgoBFGX1LyKl(}?F@TRV_h$OY%K z=|x=C(hxSyr3o81FD*hK6EiT8BTEF+zf5ZMk`{saFhQj0U!E8CX_0$uMwFeKeKp(} zBXJwp{4&s2U8*__j^REpZ=Gd6zuP>=Q+)kdnykSLwgX9Hm}xIbgkaZTKkKbYQ7@Dh z%_o@IPU<60pTyJNf~zN8a>YC^uTwg=EC<_O%4qrOuo&r~Og^`}52=yd;rhb@*~mcP zX-C4xFP{H!l%kQ#DGBVcA>}MBXg)tyT!v79(9)*6YeM*-u4XLtb(p=^iDSG^AF0kr zV$#Kl@=?+Vc?o4n4^_-hp0-MIknev3;9q>+?mdixp`*@<+xeLn_%1ePP4bH9QYbF@ z&g2EMh(CwU5k>(oomAHw0xXqkNLLI)G%`1h{X~Db zbGJJRr4Dr)A4$-vrV{=m^G@WE9-!4X|&ZOjpf$*bEigWGl5uQlOvnO1U zWD$?m8QokYV0(+|8eVW&t{)FJ7z05~4MN7L*>RNL82DczMj13HoAG$EXPi&HF4k}1 z11oKuHzmFav?p!qQge@RQPt@v4y+i~N+3wm`Z5k%R5V~|ylna_O48r=)pqOD@ZVd= z+91-J7C#}f%o^16-3@B+qQbSrrw0{_Cd2e@x^O}pIhDY&_RTKxj@}^#l(sPR?CMOX zRJ4}h5sfjpeA66f6^tpa5Ri%fJ0@5ycBcH`P6YxMtHbh(*FV2H=^KYG-JYh=3D1#4 z_Q#%SuH#e0c<)r7nsJ(fzwDm_lKA1ru?nXuxL>cvLqX&R{A#?sxu_YLBrt(WRhAZ> zxS?Eik#{f$=-3>t7%pX>bO22Nb@h^7`Fu$zT*mPITw@4sd0~8*16Z^R>eYR99KE0; z5~0CXuQUpG>N9`GMJaF4JM5H>U-v^)?50M%Bi!$maF_hHY*$+tM3upKx0c%d@h8N+ zL7AKD2qhuoTZ@;9?nCE$izFDvMd<53i*b+%su9FEVjSA$(Y?i}l-ug#!tA%@ppGv* z4p5p~JI(SHc!OoN#D6?9=M z+!{+W%E;IBT-``u|E}R-$N#nSl1{Bmt4fnOgb%JgY9@RM*4Vm>>=(i@`(`tE+7OVt z7SkV2R5czTr8!tBXi_s$7m$fz_EFz2H`FC;o(S43r*s5Id_U5ZzGh>uFhX0if3|sT zf%|Fnu^e;wm6Rp)!!Fcs!Zau;Y^c7|GO*i5$ILQAktPh&ZNI!^r)~R@UNPxh#^T}% zJa!&7vH7u~L(JiPSRU<^7_$rK8miwOiTi+8ZvpSnoFr>)pQsWBg`#+wR||Ec5%7)( zJy>Yx{>fAX?R3h;zP`upZWvBffoh#bWiY$9zqVs50o0vsuiiDYZ;EI0+UBMUhBCb!qW-aSHDgGho8=WDZDdfjyDGDO;12;+}RigdVWf$Ph^`Kq*rG2_@g+S6j;Uk z9(B3y>O$OqVK+&+>Fxx5f}rtwpBQS$ES^EER&N;^A8)?14s zm9$$mai;biZzO$Q?=xHcVf6-8(LgpfNmBCJl5H|xfpDEv&(&w1&UR0=Y+AoHs|~pd`^t@8`w*n0-wDWHYxF13q+Qxg2dQ+ zm~A!m62Y0TkSxdLRU*ER^67V7BJ`lxw9$n9I*QPr^-KGyifoIw3U(iRKutGAD)?dT zJpxIsi3)3yPf3oVIE-YiUus2GM{+}5Lz&eNIgm$IR=HXVS4zT%*)UHYL2r}#yF_=T zwv8^(v0Z~{*gbY6`NbK6`OGV!!IT3HvddL(5-^d{gRdM2(;~hz@XR_2LHkMHAKl>~ z%MT%^$l~(axf#&I?9YjSaylM)y=`cQiYmvb^?;p&<6EDTNojl;JyfdXP0zOr=5v&u zGJriv{_r{Z%cRiRXu*|5M}-lsQtJ=cT*NH9&DB`y=zaIl4KJbV&2+((PkIDZ89Y~# z53N%cshXWdV}d6IUR_~J7-#?`?r*{{m2!)RLY~r}k}K=k&Jib*V)eojLBrX+J}iNC zLFb~1Ur_GFDW4(O7|a=^G-uxxcH$}ovIea~g{Ny=A_Y{g#nA2s1Q$L^{Ydhn5sNYL z@$vhqGGf(#%zrX=309HDlX)L~zZk8b>?>a>(}dx#JoT}gVJ9te_qD z{3?CwJPS|>@D2zC+{A4;i;w+5>k%yO606U}-GWu6Zx{4m1NEJIN@yzxl;UnE`xjyN zGnLa64+vzf$ijAeRI7NFP1W@apeAR`6lmbHoP)zQrXW1Uh4 zJHeL7cAaoL&iOV$>TtP(Z0ug@A4Jj!p!L&4N@rp`j;56v{mJ8D~d|OL32rUEW%eSa&%)eA}Ht zu*zdY!$u^2Mq#=@g)!1&GeO>!=MA>?ICRm2;YT!lXX8mEQjl*($KBhhiIH#+`V_2S z`zmQ`!M<;8Dx7QfJ4*w6^6K*Mrh0qmk}HazU0W~ORO!`V+_BtB*p4dt)k0^C*Qcabjm~IAuq4}E5h$_MTs2+m6$h!4=(Tjcy)1i z^aYlZyD=Ix(4L*Juw|Rww)1Rj=g7(zubIWh656obG(mist` zgiD30mcfR%I^oiZtf)I6uAG9%8x?O%1LQF?2r1eqjvOrJJAy&NyoiROd+3luXI}UV zqYX5fIa;O9GSR*n#Hgs!MH#)^LYYPa`ctD%Ga?*2EozyW($TAovHw zt3z2!fl1}u>bQ^j6P`*|;Cw<=v}|pj$P%5g>(?Njs7g=UoD(j9`%d3Cu)|gqnZ5yX zZwGm6cb$dGe1e0Np&pKI$ugm8674o7bUV~`5mC_>k>vNShEsPz15FB(;my0Zvrqva9h$P`nyY>ZOK(7WXfZ~LWs zEw)j`bNoiX>(+4hwBzuFkEG(Au@uo!omvp`D{!|gEFt)HWWW$2ca$HD`z2f?BKiM#`s%nQzxVxT8{J5W zq#z|N(lsR&6~Q0`27-W!f`CYl0ZOM*(kUP+DM)UBbcdvLm(+;O!G6zte}3QB{_*uv zx92(MKKHrLbzS$liW!FzRD2mA+-g)#WVFz{`W{A3<~i!yRoG0JuV3^rsn7{yAy{2k?eY*Y(anY`PMjpxai!{@~I3*#&ysbw=~ z7jn)jP{uOs6DrD7yj5@}c8pFuVXwO5+;GgWlqbKU(eXWd&Wv*|%aUCN+Ua;3j&)7{ z?SqCOdU@jh=Hw#Q?)Ur9FPo+xc1D*<6Q)P@{lZqE=YhZOlxLCR8#Ng*>Ts4*pnJ=? z@<#x(Q1rsis{{=v$1TgVKOshyVf`yt*pD1X9vu08(O_J0w+AUnWlX_QTA%xxO@BM& zf{~U=IoDq1nNfSDH2lssWsNim5`OG2^6NyB#}Iu3@61i4-=38~-Sw}3I%ZiUu?lHHRxCy1^6!cQ5cEs`2MmPtt!G%_wD4!ENXw&fDQkYK5B=jifu zOVX2)uVM_@Et*RO6{}@dD%C`5+gBfey1cbt10{m6HXjo!I+UiDE314E0R{G3nx^v3b}>=2iH zuq!*m6pr>LLVvv3mcgq-`{(6dvcDJh9uau)C}L~1`*X1|E~9dhmws&r+?ZY3kPHsD zOn0qG?lrD6SAl7K(>ne*qh)Hi)+u9YAYwibATRgULu?c4tIBhUkCP2fbOofG56_ zE>(&krXIE9>-LEEVjSm`dX^@&rIi zh>n&o9t{bp>iE9s+~#uL2M*AY(Q0r%RMl`WV&Hbx0*zNfGqTV)S+jV)_VRLNry@gl zm=U{nwpk_oNXaV zjl0^dWX?C7*F*_MIAY@F$Bm8_gg46mGK>dYqmrOftSUqO&M5gAW+Ct?Pe>VrX4{nlpog<^KTv)6r2mTl1nmn?b`*1a($W56MH$d0 zQv$pLo>_b`-poO(rN0thXLL*M)qD=H8&zlAG=G`&jodb|7{7CLxM0#gcEdsO!$*t>%z3$_o!oGSMv=GOI&nlav3 z!DpeZf?S2{(+qGe8V>9yoz>;l){}mLqk-3-488aUZ=$LOrm*r=F|F?*SK2hI;P#Cb zMt-}T8Ud1jB+VuKsm0;2+3eL=>G00m58EVM!$PH1&3lvv+5v|S3U}#+Yh2Q_PAlil z%k^3V5?@1{1u1W`!!q?s_IfII2T=XD1wyGA~mwve>(p86st+a{Tt}SB2ip zS-z=@OE@sm%AsI6Mt5e*(oF~x`1^czUK+CS(OGAgj7xcZWQzbL>@}H4UN*z&B(c~; zowj6TM~t&x|5H|=BNFH-&Gqx-jEu`pu!tUI7X?{+MqV3dF5*)rT}y#rdDKkJAi$qYaxbW2v3o4{w(pBk9j9}{dW3s4 zP0%k7fY*C^bhPGC2R}qwjaf&G;koWyRi``Lff7HdnWBU>F}u?MY6o zg(!+@QwsJw`NB2Ng72`G-B#Vb>hEwEoPHn62OKZvK?Sn5WKBJAKwqxq z0z{vRKfnei(GwHqAlZIl(LS5Lu)9u)SO2fJ^&`9M3Yiwn-OD1Z?dO1G9qG0>N9CzQ zg^(@*36~!y1BQ{p{4i_5Hs-tOvf~vvhc&snHZ@xe6^e->4hOl~ZhD|bF%9@Ws$AWK zd|AwTa`2mZuYLBsX}3=p3FoTys4sU1pa*Q}#QM}3B;1T5 zkQ1v#?Y^*_eKicX?M0Q3tB~k&FFf6E6mmqiJ)l6_N@l6G1^Fn(@>%~gJJ=`9r|NMBV(%h+~B7_p`Ofk z7*LlmPL?}iFL?h>UCd-l*Nr`CjgkGmhZnUG)tZz-ZuvSz7VS|k?!Tzi!-Veunk1NbAzhvmWbte!dWo)3}~F5}U$wa$*{%R;caGq}M{ngU~5x%R1& zZIaLen~}6B*>BE^RBDf1kzRY|ehL3kSvT|jP)M)1!2EdS0w=rSxLOW2wmYXr#{(i{ zre9`sOp(mZE>{9ncgXTnDuC3jDj%wMTDEPb`y2sg>To@U4oLp4?7F4`jIUL}>tH{?(oT?Nt|!o7Jap0T@Fe^@ z{U+m#&7r6C**6^tbI*_FB znkdL-+24mwx!iNT*vpG^mR~GOQ*{{)mW`Z|QM|x3HpZch8BBNew(gyX9+P6;(~54d z7cEo9#mUoRL;hd32YPoIxpK$t45A+IbPe^c_36^iRy;Lmnw@ucq9&icF!Q9Z-1TL_ z%r14<*0P_1;V7E$7vMAt*_Gf*8s)8L@Sn6N_nT84r5r#3B917{hxeUWF8K)bZj>od z>OV;j$!I@o5%dU4PgiN)>8CKaeqQ#9EbA_%6ABgK%UaRiH*WuB9vD>|rgVBat}b%& zIdaTyV!w9Ed$=u#_%7zLa`ngOzD}w&J?1R_LQ|T6yYDNI&(kuj`|xu^HnVxv( z3%|!vlstn^up*jPT#B1gqxW``Ly%G9ZXXbKyhQFywv1vC=R>St1wLv2?L+Y`=V!H^ zQ=8GJzX;K{14ho=#80FXhz{{Zv`R7D%C2;@>Gvbw<_0dTz)+a$nU;PAgK#W!7wZ6Y z0Ok_-P6Y$da4&~3w{QkAiL$UtSwm>GRFB?$2rh43GaHdnEj;4c>0>`@8+a@V&>oQI z-ckji5=W5p9S)HfJ9+6T03g)n4f`S{C&8na8fMWOL!Fg!2D<}rKSmk-R;mJwJ!E03 z$4Y(@X$M&rRp{@+ta=nGk6pGq&QT1ma4-Q52~=unw`L480fmo(X{>G;+kO)cj+7~I z5%zIFj6!#SqL1-FOBIM=Z!j(4#po(1iUHJa0Da^iZH6lj6FY!!=-rM_uL|4O&fgMX za;Xn{Xb%fKJ7)2bel^YK%lsRbGtFim_;){V2S_l7YN(#Ya^msc#im)Z4(3`!{@=tV2hGNYd#q86U2VumX`d|3i1$cPG>3VcUc>rY z&@JZMZ)dFC0t0QMt*$-vV&J`QAIG8kSWg{GwX?ue6)RN!;TU@KM4B_37oXe;SSw~F zE|>I1^I({G$vW(eP-=2XN-GnhQcKKPbPmbnN^lsU6ron!ei94%WqSBZT%CW8y5=ru zOJ*9BDJeu=%k-$p)CJGAd@mKjK8y|7TcWjvw+}OVUeu&$gmsAQcd_FKI45Oel<*R_ zN?`+m_n-qG8)57@)rAHlSN9n`dyl!P?FV)L0tBbarTo|Q%9P&5#6DVq#I~H&IKG^) zc*5M?OreqOFA+HS%7d;(9JfqFm5kqmtxoXgWsWOs3DZ72v#;P+l{Uk{ON>HpGdcE+ z54`e?=_6Z$+-+T9346)>a#Rz`t-y|%IpLfrGa#0?`zp^v{01Ze<+Udn4jPh%;@emB z1_b(p#?^djPB^Bi3TYX0+G81SGBbJ~2I!&r`LK{!Ze^6_I3YM#+p?S~n71e5#g%-r zTdDa(k^1SFK9RB!FCQ~XP2!>e8XC(qXO zq4x*gnECvy+iNPlNAYs+PB*Um6MxlcTYz*&aIWQe)U=kr>9;uoAx96Ok25sz2{w#G zYCEJ@zaUIozC!Bfr#Zv2L~Bc0J{gTIv8y6; zVw(NJd^$TVGt76b#e|Bo(UobMGlUcZz86YAfc_RvZpcJ6}+on zGp(w7CZ+{Lnr#e3LCP;5b`5ULWb-|@agy10PI?TzTOp+_4ds1ArqD>O6a@J-acqP3 zN*Yx8tFfsXa6OTWXR0sFboQ&E#|3u2kY2yQ-S9g$S%`H$>uNns#mCa%H$9##h?%H# z;q@<9J<5$c`yO$6Tito)NE`iF`a9_6nok@{AVd#KV9aQviMgnxfO})ltFAY#Q`~BP zCD&dXI{)m~a}VOPy{x3Z4a94jnrXmc9ep(Be~6`})nvZ|Yyz%Wr0JKjIz=W$siSfU2Y@B&rW)f~k1)5pO?n2+A+<^5n67*w`94RE1i>Q_SGtFxpl$wN0usB8h$1UVhi3niAT~1v4Ireid~cfy^C(H5tkd80!03dgF11&>An9!HXx=%PH zzqFL4u|xlBAfbY}pF+-!){((FlCEs;mPTScs~ZE)j@*D)(0-kSU9Yi|oX2;uwO=Wn5SFy)!#6reN@8$LZk! zi;CS6uPManL63EW!X{t7qiH_Z*zhdzx5r!B`y*JT{lS0|noAO6wZ)(e0A@eTJr z;`1J&{vfW_s;7C4SX}%D!f8VpK}y`Q*h_D zGQ%EXXIYbx@2V{_pQv(h%P?uyo~P)NH;eUoCK?oFmJFi)>{?!O)U44It9>^HZMkS*P z(gGnrF9|43@xr~%yf6>oDj6cRY@i#9>|eDPyXsFv%H!gJU9em2B%c=xkdc20LZZUQ zo4v8X+VUTY72&^58Hoe$aF;jub|^f_D&NjJ(&C}#z*?cuY2$s)TjhB>UO+TUgaFge zY*0nj0)8lOZQl#i74AWQfJ9)%!;tKB$+<3cM#_*vpUf_=1jq(j2ZBQ%iBEV1k2z`* zHAe3PW2m-W1-K|6WK>;cWbFrFpE$LdXbV>;!_X^+iY!liEo4>%Fq|n(T8X2Q;^={3 zR$eS*fOUdwBt%pBb$r8LGTz`%0}+MJcNQ+j^t>?PO3h>G%(x=_l{U*iF&O(n)B$dG z)~`!tOMX}6HNP4qz`%H*DDo{VdBwTgkD)hgi6WMwe&r3^HfQ%KBhzGf1EU}06qw-j zIr7_OJL7kakeH-?x?w&$uQ~i6v6exd#t-b~F%VB=NvT@9N23Zumm75m(~4YP%Qb!& z`Qm%M`^#Ye<-N@jv@6mFbReuCpas;WFf}N}zZY7(+EYvPKGUY%jyAN9sLo(@Rc;-> zgEgSO?}@A7#2P3^+hYwG3>-cl>>CIkIzb-AH;PTc1nwyfr6K0Vh6`HF1^u2u_A;?< z?4=ttRB(+v;^#qWUiCi-v-Z+TKt?F^(nkD!4ph>^T9}dn@sy8T8~vv{)kNb$*{n`_HwM?6KbQXyvO`ks%E@Pw`$wFDYwUf;w{El0BXL%M#v9l5sNVqnnV^Xm#MD@?+%qh!ZWuYN zr%S>&%p8Fe8eaGBon8nL^SSj@drOpm9L{m=)O&4)A%~==EkpC?h8sKlhy14zS>A^! z)fq{|&lWDRuq1=JlTtBCN8|WsTBrwO%EJu@o6AWfha>12Xa5?}m`VhYwj?C{j%MBV z%X$7AeeT9jODFE&IXj&XmXee&(le|-|Dh?C%Fwat@ZnLq!wnW&+(yKY2Pn7W?HVj% z2i!tOA=_8b5AgobG;*aRmGFC)Vy)W0Tat~EhG{YkDHcE@ot}CP9lz+PORQ%AZFcY? zAvaMk{gNt6B**iJwP^U^q@=TY3G$n3lg$X^>cwNx_8*YK(C8)O?^n+9lB8z5t?*t` z29}1WA5LFXoi0CyyYe6J%xccaUgO>0YvU?mnY$G)nMCbs-0jNvD1XFv2b`wmAm8yN zh{%q&LS}u4RNmZq4PQ?+vvR_Ix89tJYNl7%#`CiXhv~)aia{gnW~1iFxBW-YTj!R= zoz4Yy10)*1zD`tOuMh zSsL6K8bWWu@fenVAf*%z(C2LFSIc^1*GjQN1|dR87OoOZm%h&&R*9TSoc?}^H#-CW zX}^`~j60W3W~NDYi4&x&54*;`m;PFKZFTX26Tl$NsKVcaiHc&-IWO$0s3>$^@`Ojjmsl z=xh2oOlZNOYQo;&Pm(?{0cNbNIoK~ccerzJ(r+&_hSn9xa^98J23!ZIgY}bQ{~_tS7;)#K%`l=jFYyNPwST~`%eX?0dDxx1@uYDeD!e%s7 z+tW^VAG%>1?VtgA4tX_v_j6UhbUJi^jUXh=6d;T@<(+-}jF4dK&(tn<*idg82b$Nv zZDoHXAyK=QV*@rBtF`O<_Qevf3=Fh!)X2SMIi{-O_uYi54?ZN#Te#mWFqqAea&MwV!u*LF~WE#|1wU8ulx_CQ^xaeHO-(Y!&D)QNUBr$rL%9? z2=HTIY4j`t_4D#6`qQTo%#9^|?g)Rl&5(vK*e!08;k80dvSK!U)yk%7e*ikRM)FH4 zPB$L(sqUR{6NzB#|DL~p28OiF`y0a4pv5oZJZRU|0%oEM`XWuUYK#M^{%4@CBMo@5 zfUPg%oMe}0NyAb2Hp$$6zFQg{xl;qCjTW%OmWkAj`VRCZlF)El`REY*%LL)~MPq+@ zAX}>ibW>y7WHRgVDM)$t*)+!2ozD+z)GkLd{N>g>C7Fn}dwcZCk409WO;(I#BXE0z zZLJ&#?xrR1OXLZ=EsU~(-r$fiPC29f+UrJ5ZKIMYV;tFFvr`wBK$o0*V?m`kh+~d{ zr~&C^BPVITt3#S4S}Fe*eKTj4uYWzT0k1I#!+*Eh>q{+|NtU&R;$x|l;o;!B)~*z* zZ_Bsr?GG8A7Wc{z_5WIPh|xe9<`Zi*`HGf*7b8Ba8GkIe^JXat&VXXqIGG0|eUV^m zzaCog!CY7pvS_t;Dg$rg#G#FLb!R^H7Czm53i${&n)~ZbD}rtM+Nh6==L6r&)F0*{ zy7mIrMDX&q)5+$@Vk|L*Mk)8aQLtRUn%lVNVpI!glE?8w_I0;pb!3+4+`9YlZH}7v z#98pK=3})Os3RoG_1!N)td!ZIz%8%S!?DFXGoOpUU4V>%x9Ce;w%lhS8N|^kPP^YL zSPgsYx!Xk9?tMlw!Xpy|b6e^U)gObpT&<0Re83#oqRasr>Uo5Qr}G$dn_}j^kD80G z=;g%FO8v0*Tuv})1{?YL(x>NJLfYS`9gf_Sbw0P4O|0m-Vh}lF@n-t&(hJB{-w7z0%hC1Z5(a=7@nP<||!jJQXwe6~+sVmxO4VqlDKpCp){o$4ezcP-i!jZk6C2#68a1V$Z(V+C6-IMVA{7b@4^?L`cX36LhXq5OyP}>nZvi_ZfCoAmrOh`37w!n6KNtBXA4gBRT#hi%{n~zgx3MS*KZYTf!!u z&gR4r_mj|XL0h%j;fi|O)$})iJdc98UQ%&6m0l~@V-C3O&QBpC&a|58k2Lk&Yqhmf zrI}M=9>s>dsfvK-DUgw10#}McFlm!rHdjXDUX^lIZIyQi-&U9)3DAQbrw+#uni-MS za$PiBR-_9jext4@dU=4&=@BBz(-AFd95D^Vl11;da5qy8E zm);xb@;p5p7qu0Xd30-a>cGUx>&cdCzgvU?7t*QLtVsT$!kYd^Qp>wwW9Ensn9ZEk z8k#?)vMHRCuG=c#1uu9J37pYs?UpXulo#L|*`~2JA$F~c50et@lgMi)dskE$?wN`&(v=}MgCnO zbOp z5Nvu8%E3>LVaxjQ5K?#s_RX?`mzrZ-elqL1qZ(2C51-uz`Vp?3Ld>NrUKplHz~Upg z11&CUQ@%g4ZJ}4B11_;n=1R>mGPCB@@cU8teQI!mkWt>MT~2)TnCvP*$Ipbh>UT*X z#sQLkf~aEhqW@vr&+9R6!-H}6@C9Z8c-{ekf>>m;LYPpC+V8*{a<ZNS;BAfVrka{fpjs{Cv9kM1OsT-{9{kY4rVQ* z99ly+LL(yjBiTKe(>^ryF)(Z(QKlV#=>6h1=o4B-Z6NvcdfM?oNtkeo=u3ukY(2u~ zRg&)U=20$?k?++1{ZqGipl$i3e!uG#OXTfq_>gEy3Hu)Hv{&Up4*j6(JXK}x=2KXM z!PJqOLf}0awV1PtQ2C64rS`7i3$<|$h26Tm*3f8WE|;|-o0i~r4<4EcX)sQYmj=yL zBJS4{-q%{Gb~;P?#OGJ9ncSH@rT~kR#rH9;goS)W0&O%QG-W;0Rp5Qu-bEKORfuPc zuJ}=YF}<4fsd0BciD12e1J6kt-YK(vK-=*~L@@xXIo%ll&ENXP*UhW0U`v zq761_bRKv}Dh?)srgoZw5j$ADN1Ljl4orYs8vP(+g z^fK2gxm5jFb5e<*ho`P}s}#-BU&UEpIb8^JZY9&ffeCn9KI6Tq!=J43@Pv<_p4k^w z!%7x{jMISezrD(72K)%64^_ByJ(Wt;Rg0xb@4km%|F?=hmH9zEzU`}xJ7}WDCnEo2 z-*B-{R3o7E>a>bLYQ^crNltjmtDDxv%;O{Bqf=I@z0NnYauTrJ(TnpTpa+0-gL~v} z9mNSvM9gLgXRXxP9aIwaHX3#3k1l<%G6v>c%* z&{t9iYd6GC1^w-or%Xdob?`zYo>S=Wl}nV~OuM7(OX6QEjmgkqwVz_U)fm9=i~djCfShfv zivxhJU_L%sjN!OUMjH6BJNw;W&+Mif<;po3;_|xlrBsLM#cNM!|bW(ERtaym{{PJ8R3#I4#+x`&h zu(vRYxpA)cdWbJ17RPJN0VgjFD1_&1Dh1z$r14+w&v}E37OnH{cdMVwd9Q5TdjMw9 zNA0&>|(zF*Eiw_Ov3;thExP5EQyQ&!Z=J zn){9NZg+H0Jc|>Y+3c2f2^&772NrxG^pTVK^&a_tU2c?ID`phjvDG%9!zPD#Jv>7W z5|SpiCXkbSqx;%3L2;m08?i{28{t&HW}?twT3P|2^z0Twr2*H*%y^MG-@@jEHzr(# zVNSWlGho>=Zgqy+`k{GtvPkuF>OjXfUn8y;8`JU95y(OmXA#eHpfphK9=TUk`DSpp z%}O{41!u1jvp$Ex)mDhK*?8}I>7f#~{YT4zg3{?<_YKSBwbgAmcZxNO$sx|Wj= zyjA0mBV49mkG1;lm<3T3T9nX7>e`89)d z^!fmv;JUj?9JC_vMH})#MB|M;du&O;a0%OKv1?OY(n$t%?J?IlD>>7GNC7|f-7@QQ z$aX7^gb|OXsf;D9qul8&J#lQN$&A{eY5U>_=oL3b>e{#NIctnVw!5A9EOi~?ul?0u z=)U;9-2inVd>GOSdBd_Fbj$7;~5{AURhW)OuOv*C0U~$pcvH)?k3JkpyjxV z)T$ip<|LKCN9X#zw&a_O*&)0%-4YeI+I50PGt~S4T#5Kwc^7xDA zVjW)iF2xCpfMpz0SXZ~MKy&iaBfq6My#*r>By^eoQKNIiVE)E@{zJQ&qk5W{BV#BV zh)Ikw_eBSF>p+}3dlMzK39k5-A)2^VUcb-`=vATpfL>|9W?#@jT3P6EO2TmGgg?V;dS8K$|qD9TsMo;B8HFv$QoD>SVMdKDO|tmnCaC14x9n(BNU z4_}{rYETi1|Pfm8+n>(YsP5mn^LRLlFfz|4rAy_xTqmFKq8E?OI;s7(M%Xfg$7rc^W+FC?t z!xB4o?=bU5B7V%QU0v3^EOTF&r5@b(KB(G zY!0kINLA+)jsDLc9UpK0lp7qxIdD>9)cP~r`jlV!0wWWZJiuLUu8^17!7O|FmjwCM z=jfTo9|DCyd6ueCFe?Tde~{{ZE$=Kr&T;#?<}lYGtC660M@vC(EF%}Cn5aI-&y@>V zj#XLD2N*5#LT0XOgppAs>mp2TS|WNud!>zgi3lXFFsTFPK%Nf29Pw>(+)3w%j%k*q(5EER3o+032llY1f3hM{R1@c`31+|7P=FGecnn~ z=zbqakN(ixXr5ovEqxRee;2rwCb9J9kB#+WlBn(|l<<>*BcbsQ`twPbfx~*z0w?pS z%J92;;Xqf1`gvHn;Q&R!rap`k)3fv2K>(kyC_ZS2?fy3V77^9UShSoAmdQ_1a_}zo z*1R!{1bAFUr_AaS zETs`%n_f85vFNFoJb;Ag0h$N-%z^9rA^z9yWc=c=^1P^9>{6K^fXA(y(~6ZnEK%4Qj_=v@i-wh zTcWLP483~JcV+79&zSENm}c*c3jofJ7WM)Efb#X#eTE{en>>C3d+lp$it;E(lrQC8 zz!mQ6KS645i=xx6l2u-r>q0&jhh#&(o`j_@<{l3Es}iC>SkHvfYvTs`elAE+-rexF z?3L!yuDbEoxt9{9ES^hoCFsfhY0cF1k)6Y;F}BkY^xkGs@d?_a()l~ZHVLF3EV*!M z0aiAw7*3f$-z)$W7<6YpYt(vDlT!gt{0B&19smi5r%15Iuu@SBLMkxa(QuK?%y*xA zOYR+K+r5-7qB1T(5thgQ_|3`gybZXGMtCJd?%*;aAf0S_f4l_TJ2f*KWCl>Y8?HBV z3!^S-1^;v?`LPi0*|kvOfBKy4=?$FCyF!OF=Wea`V{MpQPT&V&)XYPdu1?MVBDQWl z`eYJ7*{=Uwd}$Nm+;F(?t|W~BI2G7sws$%V_8tJkYK)d!<-Z7~UcA=n_xOO0G|%jq z-)ul{1#YLvVB`cumd@WhH;B$zAc4;*P5zbZVcH-@MYPkrh4wrl$LSZfxBi3VN#{I# zS0MKUKc3U4{VDI3F#3RnKZQ}mWZVLuCR0 zAeHCWku*Yf07`YO3}w$-ROw&^nB!4!vmmi|BJYw?b{e)pP(myq+#cIsf$D@x<-Gxx z{BX-j`NXp+0)4ePt=rI3CS0V^cY`N&&tvL+m%onv-%gxnJI=8ViW!e}@ZYZ^i!EHR z4=^?C*5NxZ;pZE@+0QR>Vq`SZE|g~yUKYeI&32=Y@mxilhpeZ!Y7L;OC#4^4HF4>6 z_m#*)wo0>Y`B3(Oswri0^Z9Ou*$By5v(pbm5nQ1N&o*8VQInPzUfdP!ts1EQP*{m9^i(e&&o~QI03#%0h2i z!;@!XBB|y47m8F)QZorL`i_5cA$J$;nEVeu_y{W5JQI>fcujpc6)j-YMiU2Pe3X^q z^lBA7`S?H1Do?k~{w6|qDwu6GU%0J+nDMsznBJSySJJaSn@^EKd@G7xgjb}56h{2% ze>V%vNn$8DJ<|;~YHej*j(-eEoy7PgwM!KXF9@0Y&MnmcQTgFYRV%$Z&OhW{f3grU z4_@RO7O`vIT5Nc4Y?$ETkDR(q{N5cXNIH9hxe{c$_647D5c8km$XzJh7tdfc=qXB< zXsqS9nufO42Mt5$i*df&@`r<63i8^!VwZ^r&w)nG>tx}5(JPtf%D-q#zp^S`Y= z`9aDK1!tXE*8)c3UVUBC1dr z=idxzHV64S*}fHh;yIu(J$-(Vpx}(G%VoTTUS81x4{6}VWZsAW7&Qk-U#Iiz;bq+t zxaU8zQ@mh~D;ojETQ_ACwx3oTq$%cGdfF63ud^V-%Z(_lJ%l{L zWH}Ht36w`wP%;UiU$6sY@f1``f2fgA_6shK7`eFvkmb0biF%v&rvrJCf!^lj)JgV8 zoS(7xUzfI)^=<0k3aNaubUoN`3pRTmZHH^i*?tiOJ6i|gT*`oY_r1{%r-f}u&sD+K z>b)f^)kegt%POBOPrx0|5LE$yZ_w(kiWCp2xe%f(ba(T$Q;IBFtbH)@taHvIGqV9) z;{~{cmftTKD*iuHN~UzojZxO*_`b!S6bBQ4UWHzU8Xu?{jhO8+!-q8TDxtnBW>U{m z?cU!|(a`>~uc(R3k~^WkOAmB9L1XB>%BaBnIH_7Y;rVI zvulm$QK=)aWc%L>LDK}u>!U-ftpsVnR{~$fD_|02vfm=X4wEkhX9;c=I+`RR&IDh= zjrjf^`E{TlTo&MKX<#O3Z#is2T%iw1Ocb*PMaNa^Va#oqdXWQMlqiRa|J?xeA2n4k zsA8TXC&7CHMffmxko(-4VO3R(gFgs;h8lT3$u_~93v#|E(V3_(M+XI&$Z-GRhYf)< zfMWqYav$PSomts33p9=jYvOjloG!i5K-E~pQpR@g9L;+nR|^8#2O=F#KA!IS!*;VN z(GE>#gjqt2_rUqN^Qams0*Cd3R2F>7>29>*Yjs@zGJzW$g&-(MIWN{9O+L3RSG>S+ z8>0LE{q*-Q51c=JwEz4({?O0}&dNj{F1iaMX`@27z-m9Yq)_Xs-m}xBXCyY7)Pz&) z-@0`fpO71|Hs-S)LKHbwJR*4ElwX4+KN7qdym_+(&AXEF{F}^bn%ih))7OoEwBW&i zNc8q^O}IJ67&9zU+Op-GY;gduwHc#3#`m?9uLw6o0yp+A;tDsP)j>T# zdXJ=)cu95l!?n#G87tVw+6=mH;OC`RP^!69!}blN5c)l$)43Q@e3^rrq!L;6!aSG0 zHzxkvpIm8^%r)+CfB&i3+4CyyBL98E<;*Iyzj;1U1Y-u~lvNcK9X*XY#?o$^xE6ts z{8JwG9jN$g^>q&LleCY|?;;N&C7xc2u6mD8Urw1o36B}d%~k}SZSqURvmVd?x}hAO zKjCc*?)r4(|0nE_z$hETDXP;T=T5cJx=Z>D2xTPmzdzo_*CZoAP?ZW)Tnst!w~DVm z+5Yzv(6r$auHyHz_M6$|c%9P^$fE0L5U8OB|6LllV|xsNIq^UcRAH1Tl4<|%+dvW0(d2Z3 zyZZO+<0_FL;h<42x|F2Mdt)EK@4CeII`R>dH9sb6QZRuun6xfu&X=y46p=5XJN)B_ z0>oC)tV1JZKYl#hup!DVSSoyXv*(gkQ(TMjooiv>19EF9`?{WdD(NP(@`nFX7(t_3w?0$|U- zk=Og~d=&x53QK`2Z0%>cC3Ee(y0?4#C2!o!BSUZ$*LwHkGSx|C%?qqeCILXy$`bp8;X(QHNkdwJ45>is%l4M>2R!@YR$ZQm_I(bEcsKb>A zO%vkl-F36`f8w9oWU^1}+Is-X^*vD7 zaTnaI%^R$rQ~ottj_XouuY#S{<2ArQM2~K0=8wb@Eq(@ADJK!C9wHB%kNZ-qGgeFv zrnm^hmrpezebBVmPp!AF8W`LB(&5_ zzzrYli~kq?Smdh*`%_NpmxzsNe8Tt#T3``Q`o59(FtID=ms#Kadwo{O=tmB4a2PxF zwk`R=AIN1U32ZKsmO@29#%F9MX^oJgWh|`S2$Qn=oj0cBU;0RdV?6_QRbq=apN4$> z-E0ZIC2NyeKhLkg+q4Rj=DXqjglENGFQw3b znGT$AJZTw9V7&0Pw_+#1c~3p(_%EQrs>IVefSjEQ+;=o^OQlT|TtA6#*f=e8k&07J6$2m_;sWj}Z?O3feg0c#pd74UnHf3oQRNBN zjnnI(aqN-d8DeqM?Z2U=b%|!(y=?B>;9=A;v-jy6$f3U+AalI>=9*MUe~Ac~1ZVBX zAC#o;?St~ENro&SXa%4!!{8E3tf(~!yIRTqXE*~`A1F=HOSX+xCM}OTvQH69TBwSx zN0Kk*e$|1+>FL%B>DTIqhGz!Oy9nch22c+PCYS$0>;o*y@3wWr;%!g~z-mQmCSo~V zy4#ukxnk-Q_xpE0oyub5lw4YQRM1uWDJtzolqp$W2s%Uet=>L+ykQr9o zdElpi`cu&}@pcn%EjobH9f#c82pTzD;)@^F0%1aSZb98hsvrvb@VM;n;5N*Su4Fj{ zw;^_g3A+!Zx+3_7Ncz=tE8w&I)8zaBMSd-$voM3k$(###hzyZ#JQ3&t@ibYF|KC#yi9&kHPL)i{3|Q3=Ul+a@7m38Tv8;${b;r#%7q^<(zBe$|DV!=%HNnOt(?rJd^UULc zbiX{P7vnfx;BnsY25SL7654;4|GS0v6btb|i~m2FAMMJ0s8RGFSWz0H%zIzgO`*Ey z5;m$z zUj;k-Kut_AiOF^X!$Fp(U9$T3xxX^omBYc=S|PzrL6q%Ot3rvU0umq(GY^Ud89+M+ z%kgK$UzZRf(c$P69`9ExM@(_^Z2c_QBOPw&uC*fN{>K&1tY@$^!3SlL8bu4!0rk$Xlw-> zm6RlkbC8C$oQSqidz$`N&4-|*C_IS*3C0t%eihXdsSCfKBv}ej-6rv1g7DinqZ~5* z6k;C-bv%c^>;i|884dFlcEy91JoJW|r{ES9t9K#RJc$AiICMJiB+P!3nsws%n>hK$ zhqUl!tG%__ax?!|9(m|lc(Z*4RoCULuFiKmNOB$0<`(PAdVuoVd{y|5i|;I1s#ff9 zPM`%{>n&kk4La!GU>MsB-&Uk2iODRq06Mp)v&IIJY$Rn#vT=|K$T-@jc?e`e0y<~Y zdgpJ2M;`w_&fYs5>-YZ~zb==(N4CO6glrjQkH|=<6tc^x5DhCX*(-#MtVA27BqfCG z>{+t+$d++&-Om^G`F!vDK92kN9lyh0@Ao0RUe|e^&&TsIp68&y3hGdN`(-eKQJk26 zZ$kD;U;4XDK%E0>GRe|gh|6zd7?=&qum$2?fy$HwHm{15Dd#QIvje^>-^t~fZ2VWg z%iTH48EIPYe@MW;Nkv{B{e_LiM(}6pR2&;;n&2k^E9MA zjV&+s%QW5fBWxsQx%)usH%Hydvf8V8vV~0OXKaO|UCTBFPI6#`W597|6Dj(Eyr68$ z`Qy+%Dk7sQO@Vlg5|Ik8?u!o5PBMt$?$#U1mNr`JMGKGb%pI50yk}n6jD!o`XJ{yW zMeasgF>s^y;ECO&Nu>-vhj+Q7Lj>6VxAn()D@v{^Uq40=QonQU{`w4!E3~Y5lg(%I z3z_HlQQIC;%)#|AKwpbsOLpJG|2eZ-Lte_B1?CCf0Y}}e!IszO`43b(+kax5M4p1b z&y#}tGFX11Dwd#&VK;H6=2ljmIQ5&-O<-^tTu?vbj7J0Ml*!@6n|uWGl->4z<2m)ptni)?{) zsQ*}IL%H~*HUcoawdd)kyyv}Upc)wO<75x_tTqvO%Ya2BIS=+~RoVb9q9-^o^#cqH z_luq~fqrA(53;|sES09ytz3P6y{H}cvv{vf84b6)h!T|^P&Lm^Gz3rDBACsB!DuUf z?{iK8k9x^@-Du?gLkl3qYgl1OZJ*b8C@BTma~LoSZVK!Ho2>~F%)P6n~+X!z|)TB zj!a2r$&?#>4;Y?5O>|^b_^*92`PF8u$z70k)$i5Y^8fZZ|6PcqMMsEUOW!CBF(fr0 z&11mTBXS&^9+?}4oF{)sq?U;>?LnCSW3fGt<~Z+?jExs6V`K6Z!MNJmD`)IJ^4- z-*JBX*?%4!F@+D^N+?}J>bx_lRX8Bp#63v({_&3#)~!_E{l~V%d03J1lgNwQ`xr*p z{@d~Ai8pS(g$hmfuPr)FE8|TQc_8CJ2I|!O;s@{0o>v9?DmbEqUoAqIu{}(gL1z5( zoU6~}`GS31*+C|J?r{0{ZjH-pky~)02O}psr^8rXoZUy&$CgXNIIvE4JU4nWLP9q) zNHD6<0})g)i+6WcA9yQ;GHAk7V7Y?T*OVJPMue=iL(Z&QZ})7|Y%-2#*bw8isX0JQ zE6wut>_K8Cefkbp8ILNR-2i-Gr+3v)%g?=X3!WxDOgjzJKL5|=_{Cg|`LZWOwoUx$>hJ z;8t$u5ej(`GFydU>|VQ&`hT@$+nm_(#8cB=N@ zV|OmkYxCWfFz-1*>x$Ue5a#0a@48$Ta#x=hvtA^!Yd&AVnleuP4mQzR$U(;fWzYln zdwJ6JpIXX$#K3P(+zM1nR$1s1+O3lF~c*7-= z8C3E5Ql~Cl%`2(|mnf4!s$;erSnfmM!Z|?%WDSG&n=VFa{Q}YfR3Ucc{7*A47XiM6 zz)&i!j$@+WzZSU;D(RXUq+d#Rul zdI$(3{p~BDYkbQl1z#^{0*aLizW%K;s>V3`ZOnrgU}935PICUcZ0=&*ETQ45gw2k9 z{u$6#2>hKYdyZ+qU=$_>$Ic-D8>{LIw$LCjL4=$$y>x62MDe%H@D1#;zixI&*w@9n z)$xV~!7EU| zyWkGe;$a`+;Ms=nr9nXbTnj`w4hBqtf%4lQ1P%v#CBk!kxM9Zaba$+161W4E=QS!i zO2D_tly^1#cfXLxYg2l{gJ)2mJVsh|f*U$zzhmX!TOt3!3~RdDe_T#+e^s2ot-u+6 z2z?o4mmTpe**0RYRYAPP_u7y|OO)fu!G*WAhlb%lC=gUkj57s4@ge|9E$h_foj-%2 zkkL`p2h_%J;Hg-7)n~-?OPrQu=KR8~{gmLhby`T=^wPF9Cjcyzdkx*qFtkV!s%!8& zxDNraV;j~dpfYMaGO$_K&CL|{!C8~1_eT{1TW8rqrBpe{8a-sbfIpM~V+dyaM$HrWwE z05dN}X_i41<2$1t5e?M9+rUdYPi`Qs1tcaUeBonT^2yE(zaWsxQQ`F@S~kMz_#1Ad z{CNV3v(=?*0A#`V@Y3zRjq(`fts}1s8-#}e2Hod57T4)3kZer^mxU5%)a^FF*Tdp+ zOMf7%0V%UmBJrM;A6E;arw7aZFTL1=)@zhd^6|0dsY@|;?m}!pZHM#GjOFo2zi<8f zY&1d@&I{=(dq4C=xb9(ig>)X@kD2P5o_Xat`5iiYCk5rH`c-r2VJeaS z>O;SKU-KNL9a;;ZLuWE?%_Z1#V<=Zods1?OO;vwl=nzcsKs8G^HoP zwAX8I|Dc;rnt^S(n~6=Mn&^VjBu^S_x>9i2R&NUz|8dxR^}lra;itbfasGmy@=Kr; z_$-$B9uWF7cBPGK5$GnrHTA?UNZP30d*&e6PP8@`VDDOEZoJ5H8>BFX> zv-_GpRGuVHB6WeQN*NB2CXjYEpOlYh@@`T3GZ*GYdPcx3^+B}T(@SFbN09Xz-HNW8 zKx0UQ5WI+rn2a=b?(A-RFu0yz#hD2F*(2{?qt}6B+1}hs7gD*scsG#00TQ799OWo!;aX?ADuU9?jv?BBueKwtfkj@ zVillkR=$~V7lAdB3ULC4zZui4smT-!`2kikY$5deZcp+;C7!3DNvVBG#KbPR{b>#gwiefogbHZ^zQrXC@QB-FdqxZoL#SU+xpx z@cnT_mKrm8ljl7DmPUG+z=eOM5h+BwR`&<@#nyPg-(`NJZW9m6z_W2g5p1PP%jNG{ zluw+LGPynAHR;xM&h*<&PGBYUh_ld7p|6`xX5pigR{=OHsYIrb=?{0L1ta}B5J>_j z^${mLk_@_mX|}jfw;ldq7anjlH*n0?64pEw8Pwn(g+j==fMi?^w@S9R`Z5yW5Fn9v z@Khz@4OryJf*0Ou-%3nQLGOHwCt;6U1yBgE(0>PHM?OP?%|G+ggBbbvP%mKC=03{2 z`6YWmr%_GJag*$aM8on2{400Ac!a5=FPOjc^dZ7pY00xTU7ev=L)sQppnHTl(2v9) z->VEgFk3)kVsU$4PRFd_x^)mXdtOlf`07XQmQO3Oh)6ZdR{NZHyEEZ1a&YyCZT*!Z zC&ykK*h;PNu6+a<{3E5EH@Uh$Q5;(9LEs)R-U`)v ztR)8uEl(k3prP6I-R9Yy4v;?R@>I;e>^}d8bUY5G3x>XwzA?6#=#j9Opyb)X?u>Mi zL2dYIewwa`3UOsa>&OYP`;9Atq*yP~7o@weZV&JAKbd$|^!hk@`#%L1;SOUp&NSm9 z{j#n2-1$RGxXPi|v*JVSOif6p6j>n6E2_?rq&Dq_cYv^+3Ok?P3U6Nja7g%-Rj0~y zk}$H_2j#(7$R%xEf%_y;Vv5em*M9cV=)w-Ck9X-!mYoLix^Mp<$vH|48SU#57J6W( zS=$P}OvwS271Z`b`wq44%1_G`bCMxJ0_;uaFT)VG#j_&l`zq^SQR+>cWz98KB%6H; zoH}WN0{?^ULTcu%KQl!(VuY8qvy;Gjd)=lV!d85nbM1j#|WXtWZlJy64{GS?g82vdGXVP|EO7(jz07H z7q4>^NgbioE;t26p2;0bU1w zlINP*jJFwm;vQ9uZZ0~gI_)q$Y36PE7tkBlc<-d$1|DFKW3MOd3D*A@dM+WJ!1K*0 z&<*ys5m3wj`!-ruqR$dhl|y(}k*g}mrULcO@7IMy{KRDQwxuhqi2qyKQy2Ru?NPal z)_nm@QAwd`d^Hy75lB@__f|u0r=x7_NKxSaYEVCiQs0Z@z4nzcZ|+jrH2w0mTvs#+ z-u1FkEN3-4-? zhVYKy&YrBbGV@;&{qH_q1UaJRrwY=?yznpNUYn~$y5y5FhkmK?Lyx_$ZsLzkPrBog zo)xhKfjo$A@uJBjK2P*BEMRpn;kDNohsRX=lP@w2O?Ev)V%s^HK7mL&KW>s7`6xQQ zdwyD8+<~5N_gq}?s|dpK!aDr&g4e#!{gI~Zs!6ja{uP{UdtLHuosUB3ML5_?5eL;7 zOW4nP-%SVB0XU;i0gmfIqTxl6`14*lQZ0FiP`r1>o%c&L*4Xv9sA{lfxV85VL|zeI z_MVCLm9bL(+qs>mowV)e^Aic~@eNM;5$pZ415ddaNQ9W^k;h;nbV-&KmIqfa&F~f= zrmOuvndWTH!x*!93b*bp=n?Ug-iM+VF~ekt7;0(wFZhoIeK_kJr8v?LdLbTIn3$k~ ze(*h3PdqaM4zQ8(b+T$`ATsRGq(s!j@_NdtHe@E7rmPljYVu%6sDfYtX9ghLMI@vS zUCVwOhfqyORvAH4qb-P<05?}N+)xL?2)|X67eF%zkYAk3AHQS+CMGJ@OE-;PL>q5D z8^@pC+aI3zq`x%-#2zI05`9Dwaj_>g48ME(5}bfvlA2Qo-B)6|kn zv(?4)zvEd(j^pWaci$hsS4B97@z839BA2OCA3QD-#V##?hbvlX0O2?h(;^bs8;Fbt z#i;Dc>jTksE8y$zMI|Y^#SKK^_;Yu)O*8)}2Cv>J3H?%x~OsE3gXv9K7aPcb<)lquT3v7F~e4&Y^K{{aKG#Z@@ zV23M_xNpFf288mJg@6s%4&Rp}nN7IMx0v1yp}(tyo~1Efd361QL_L3eb$nIS-8L;& z8U&q=5r9eLLx}FTF)z?J41IpZJsD{BSGuGOehjXOag&0by!}$%ceyq7?cyqBkZA>y zje0-iNIK}Vd3Jy8+?gJal^x_=n|al_hRizg-#bC?9K1M{$XBm@Y`JZY+ySSiTC9D@ zi=TE1WJOve4$LzT+KM@co8NS9QY9pVSkc-IFoJ=dl!oH~K=_Yu*1FAW7f|^1L4U%V zYDT@FW8D|3NH%`K+Z#p_%*hZsKOvCaePJCaQgZoj5zDea-6A5~a{X=^h-P)-)7;&H z;sF4IoaZfWeEV492D(m=-_91a?$F}W%;tY~@tk?_%wpC+gC~cT# zX3j_gxU>}cto`zonKC|e%d$qwMn@MiH23Lr9v#vzM=bt}KxdQK zzq=N+g6MT9UmBe<_)A9?kf0!3&Fa_o6^H6*-GDI%w~}yUAiS_l0CHzbL2~nV-p7%p zZIUezAb5Q6$?e(144HB|xKk ze2DtvU7!+fDwO|vTfT;xL~`C!lhD893k)gRR)?|u+JWnY^M!V1DXsssrrh_MP3#i) z@*G0ll#R$=GK?npx^9OMbl>52*0k@ z{nTxtYoV*<-PBKTRJQByb-t9y-Q`tq^j85-#0GNm!-%D9M=)m5 ze|IIbw+EROG6x|q8zh-;$NzF0>55@^$tgtU9Fc}RlIuq#!VH=P+m)2d4>$acC1ZCn zLq3S~ip24I5jD;%yvNc<6c3^8sFsyE#C93RM!Hc3eeelmAJi&imlDXl-x9E6+#!~@ zC#%!29-~@fAKE@kzfGB=`q&@2AO6Cswb*mROfS}z+Xz(#`trBux3CfKR@BQq$=;uU z%~1uH+n=Qijak`-kr>s=u@5TSv*;O+1b?l=7u*V$M-zh|95*^o>IE|un@3^fK+lrj z2I-mPRpBUtxc{c!h0BTOMhNVJFC_CJ;Qo<9J>Su|OmVRS^EMAw?JpX+eu+}hZsKZk z&eb=#8U;x&{DkAi)(0OXcFj2rj`k&txRjsccLB16AhEhC$YW2|nVxsdv}i|2t_>jd zMhW3;;=+2OMQ77Qw)E=*35(8gzBh|3MPFGh-<@~bGTgZuqi3?G5VAV*6j=Rn)wpI6me=3=5a(W1C=Gsg}^PwYvycnP}ADpo6B zhVzg{CqIgbRo63?lh_5pPWC=4z57r_z%UH|Fuwq01v^9??Zv7p)b<^fshMJa5tJvp z%qhE!Lm>Uz>Wf!CMYUjr;b0j*Hc5hF=3zmVve2il>D7Iw{@BcxJP5p>((*km5g+s< zkgzA7E@W9i*MFk}W=wjIPm*Tv$oKxDvJF@^@t)WoQSOy6!LEhIk6!@e~>3>BRnDCCU6_q=ojTYP?>Mk&;<`z zITjz4Uu7;BSnM8E+B*S|L|-GK@9()LsC7#%PZPT9Iz-X4dCjsw9#J z#(_sJ|3F>i!*-zTaW>g<=$QEtCDn7PautTzftx!p-X_}m`iDj8Ex3N8U;)J(-b5@x z;>Pcs{M;I|jIu|+30JfP0Sp_aO?-qzf_ae^m!y0hTro>`aAqP!HSS+YSgW6R`PM)E zlYAxBsS*k?Ypq(jkf!M*-y8HZblbGbkYtuzT$d&jCe|eE-WNE7?3Ay)bHyY29$#{k z&vT6gvG-XL%>V{eT1|Ired$RI0uDvZL)b99!a3tm4j=l0;5YKMsXK(-ozYU3?^HXe zn}=ktU5L7wZyK>NL+8Jt;mbXw5wTJ3FEK=&K(=(8ccZGhqjWU9k>Apf;Nhc!yyn|I4bdm^GxPjdn$$W6s8*W zfZDiYKd;&7qXw>>Pu{J%9Tx#sQ@4~U--(FID`cfw35t(cigfRc7Z3D9B;x7)q-I5- ztz(&fdr}!$pXWNNEgj`rN7h!DW0-aN11gJXgwaNKOsE1|A>!qIN?h$_m>JIF z!$E9}=!4{Slw=7#y=ux$#vHLXCB$By%`3-Y9 zl9!2E73t*7RbF6p6qc%)Y7@vb1u$YnSuE&g)H<#&r&FRLB?0S8mrO0u@;PkdSj8qV z^5Hz&kOSzWa1(HTsAFHzEc7%vn@MRIC(Xw|FjrOx%QY*41^*}R%RqbJZUhQM>*ud|zdH%$X3t)v9e zoO`^katbe3*0S`JDpI0ZJV|3SX<8G{n+Xw(ocx2W=*#XLHpp&0wWb!N9K89hMsmUY z+FLC_<97kLv@YSM>mNMV>_5+zD}F(o>P$O7S`bsoJ%#IYU{(f#Bqh2?K-;AbH1o6+yL*4Ja{$pZ?j^A|DH!-Rf-%Q{jY}BgQ-W0EReaSf~H$1l!m2-GG**30bjJ>&q z%1QV<5S;RNkikQb`83OQ{3`{Bjp}f3P`w9g>V2A32>6X+xm?oKd%N8#6)uhyq$sB& zVWd~y1(a!s;4Ire(7&beq!w5Id`)r--7j^1B3Q>(R{S)?Snj{Pc${_I?L!1=FsE!N zyAfc>@sswLGv>UxqzuhDFxOE{mp*%!o$L`QOukDu*zOJId&Jqi+;G}u5LpI8V z4=oxrbBX56s`JMcn;Wm+K|5^e@=H#O9XV8y;&{bars7VkBFT`xrE`0Ns$9G1nZl%s zakK*_AzWtAKHlIsdlyWDJd?Rza!pc29QSR^cOt!s_0|fBb+Y3v%DA$oMhF)$5l)lH zdkpi%t!gqerBmzZzgIj=CBGuav#{JK&b?ycpvU4SkW2bPtoH&+Lg0@K%58w|6`6d; zAx@zj58hgtwk1skb0MO%`e6fF_W=WY<+e88<%N08u_ji_FB5q^zqiB<^^lqY4oVjT zB$Wk*P!f;S{Zx!#K;B7fla@!tNJ7Ju_M8%HdvVolVllXP5EnM|+ecc35vqnysyuc0d?&+gg}};Ba&1t z0V8jcl=k7FSl}lMa-C|?q;E!HV~06XI}ebi5_x0J$8#rsF@`1F^skd4+h(!*I7Uc(9R;a2`Q2DWM}_#|2X#d6D`~+*9?8vh~nr60%MI8}`LsP9aTb9U$8_Gprn-1&+?;jfDB z14L#WWKX0EPYsx(EL9iS9nvIEn#IedoIQxku#nYlws5ClI&3cUaIO@+`oJ}*v<(+n z8Z&j-kMRa|tKO>gW;FV0_zzME{+G#$&A95QXoB=@)jKsO2QH(GE3R%;Z+Md7_;^Iu z?kjIiv61*R2tB)Lu~-Z!pmwV@=*|*XTP>;%X^>?OkCTmJe4pOMEgV%sRZYtoeOV3g zGvBqYFq$%#5pNRz#Jb4L@sq?SCxj=DSmDdG2q!7tzA8Wg2BICa)n$}rK?OV(s*qlx zr8%X6eohzm9ghsNSgKT7KKYJ8_4g7YBHsI&^9t4bG}bT30o@gz71BKxEijUDV@2sE z&t)ow)bOwj5a0Q@m7xocYt9k1Q4V|KFj^_L^n}ogyU`}FR+13+_9zS=IcU^%(I4YS z66J8ZkR+ApQ-^kqzAkO#dEhmiug)75!AXLT0Vh3n3Q08}NTk zX0J&u8!+()n`)+(J&oP>=2DyOdvjl+Un4_;5u><>YU%xz5q+<-2!*=RtNmx(YHfff zuMp?)#ZhmR>Uy1{hCtEa*!6<0UFIP<>p#GYbWymGPF)E^f*6a-9y8u~4`a^$AHk)Z z*haKGV3D3MuA<1Q2S_Lr9QtDhuoSaY|0{oV{doaRFDPuxu}^58tj?YsOXC{XEQ>$H zXBFWt-}CdfL7rUcJha8%yW>IHDu4MOyRmWIKLh^zz=DImdu9n*Cj}*bIL)K4sTLsu zQcs8oVz<9qpZnJU@*(3b$W~J8sv8w{1vaGDeweD6uW;H$t;SAf&PZ1_K`s=KUJpC8QLw<-h zr4Dan*0D-5V5yfPf$}|ub!~4M-_TmUx&t59J7Z@VPMxI&uZ!FNEc>%HFUN}eyq~}J zGa}?zy=)avBx;6%fX;xkY<=FzBz-7|9%%kai4y*i~W` z`k}O8R8(S==orA<|-nWDFPaD=lQWWteUG^y$c2d&$wA z<%0;p1aoBUGEO5CO`!bUP8Hm$A`pOr4Mrv-(3W3X8$^;aB&-(n2>L=s-UQRg_VRdH zn}2R!0B?K5`_EA7D#3izbt5zd`N)4H`V}2`giOrM%i{iImw4_x1!B!)13r27M9XQ! zF8Yjo@}AIaJ#|kVQ4urIM8d`!Cv6mK}s(Usn)-L@;Hs%Ouwj;b$#=TaZHvf9yei!pLy6dGDOPM421F_<*8 zVgbxqa{~%VOCi}YJG>H#Gl##*+?%ROFtPO5ZHOD*M9dPo53C%$!3P}zy^Vs4D$JF< zX5^|5Igs>?3E-osVVdVHgru(mxAi&yCEkin|J4`X>P0-x7Q^*6yWAx2d1jg@9t^ru z9sJ*gt3ucD)ia-4L0Xx=^$1B;odf7Ohp=y3YqLFStBQnMtZty!=T<$?6Jx)BeCD|> znz!aXq*ALT(Lsnw%LW+&3c<+J$qpG_skHJyzIOz)(4Lq(!(kmthj4p~wJN)k*T8cl z>0I1>T!Bs=KHpj!B;^9c+x!JpACWbxxc6RujWN$G2`(RpL*Br=#hz*pZ{Z;eeuwBG z<4c=1nun_f$++B9iKL~`d5GTetmiQ zxwGVz4`o}XE{~5uMIpjFZxbV@6S#M54lE*x63_Blo70HpV!j=TPer3RM#^QhuY7^? z3G`oWX57j@DtWPn(=`YrgF+4corlC(X(SuUgI4zdxWI^mB0H{@NwFmFWP?H~f;@?Dr^UoM8 z-P!~{8Z}Hbbnsm7bGS;>42$qmG9;J1&jxvjDqOwk7wN{4J%~*FL&cT@APo`y(3|}e zl00yw8;Kxm?}g)#vcOt(idg#PSNa_-?svBVO@thL)u*+SIOn;AA`sWx5i0Q9+_?b! zQ!R(zmvtBL7kzX>jAys6RiL?;Bx%Des091ru`5jsm6&&7>|Tv9s%NSMT=H^UH(S*h zs@L21ToN8%qhuT0h|+2WvZn+qba9C&5k$)>G#)CVf}~Ms!{6TQ#F4$&87r5i>XQ`j zDv?P9thOLVxWx3q-xeG?V|+&W$b?Z(Sh#yMXtE_5H4GE<5J7|%FsL@Mu>aQdv*Zr} zqqCYc$bn}A12-hMzPrnN%w;^csR)G$lJfNDD0@}MW~>NXJ&3bUb7&3mG42gw9Ww{) zho#r~iCFs)c@1Oak>j0p$=H=%s2EIEHHw9ZF+0lA7raw;oMZm7su=5}{&pbt^sNsz zZBMBzyqC;_d4)Fc$_nDuZh)}iyck)+l)_{U5E(}?+Q4)}A1kG$TXQWVd1 z(%sM0(4GywF@wG!ltzvZ?&NA@8H&ZsDw%VONY&|X z071tkSRQmuDa6JV(B6;Tku;2`@FYm(WPpe>< z+H<1W2)H)8nE>!$X?jw<@A2VsK3lG0dBZYK02 z-q!*Zcz385PD~`*jj*AKrd39t#u^i4B}#%oC$^-t8maz?xSjir5piC!xpBi{88Wo8 z96v5@SkauJL{^pY*^G~->4)RDpV#CaT!zbOnuK9#kqNM0P!-Z z+KWrwM0}#K7sc1tV3mX!eQKg$!1rx>fx*!B6Sb~#Wy-=7=c>wBtj36E_{{+zp`YN@ z45!qNXTs3q;|+A9E$ztexPLrSMy3f}TD6fyrx5vg6S3J&`oZX%2wxbos8R#TdtdZU z9G|1(r^~BTZ3jy$g{a)0BO{=1c*K-t0B?1}&asBwff|uR5;l?46e63hC(@{F#42bw zsMXF{EW7;3a*IHx<97Q&^}t?w%dj^0a9VF7rgQ;ruM3}A3T82T-iqotG&=dK>6oD4 zYaSomVAKlC3Xap55=$b_G} zNC}GsJ=4bmNys8^vP2da#2bccAHvQb zzBed($_?!ZQoIq+|5HQ~L>C^xkfxEVs1q9~N+Z;Hlo8ByG)JX}-8Tn*ozKyhB=!o} z`678DwK(E6*eh7F0*O4Sc??#Cd8oC&O^d%^LJgt15eMhuqe1-B(RMXeX6%XZev;no z#e;D^vI1Bibe*U85Yw!esz`-6<#mMlV>m#TwTVnMAH+OYFLSPVdrc*a!CLPFcPBzk zVv_iX8kWjSeNp5XQX)qpTcatu>q_8*V;CzeLn*hh;%?7Ij$aDdMOC6a4@^+dNBT({ zE}|KlSJS<0@Av88^80kIW;2GLe`ZB$X$gbD8E`q7m#1ykg#aojI3|}cFPHpEHs-C>| zJMJ9i&B7Ax2}A|&g~zV&o2p7f_Zs)oXBkY8S+mfm8Srkfm{pi)E)YF5L0P<@dfuYA z#Dt6QA8BGLZJwad67sqyB&%{witUZlU5tc-n2>-usuuSQ*MoY~YTuMB$-sXF6FFr* zuoG#8$2JhH28Ct`UVqD8O-f9n9PH3n`uI&fhMZ=jiIh@`64k_&Mb@UWc+{J@FWcUW zr1jBe&~7W)K>aM%ll9Yz;v@Sh%H0}mqSD}kLXY~7E1Qc>tmzFWd9${W7$y3TnV@2p zryncRLLA zs+st}PK4Iay>0~y5Y-DN>>$Z(1tgw&Bo>)8us`Z%FCd9n13lNyA$^_I6P$FGKwB8@ zGV0SsR5m#|rj$6j(DBOFBeJwF>rbdbp%(pdpZs*ib!-UwSe=~i0gdWPh*sVmqi^erLy*-25` zq1=99%A6jwAIZ;Hh(vUv9B^&}2E1lm3fYEz3H+L3(tmK#UKQ*Eb{2@2Pz2RsPgB88 z&%M|?MNLm(?QEwihRQ_;3Betjw=0Z(Nogc|G%Nd#eyMrAnjOqLv{Ca&_p&(eFl*DN2)^=_c89X$A2L`pHST$e}++-&*j z1?BKsl^3KjS*ObvHNhe1vbU z0EXn#WB)ms;3a9eJY(fy5#7a48m8IbQ#@3+eVct>nM3E3z(Ha>aV|>nC0AfN#gQ%M zS1$ikdAo?QynJ8nN$FhZTqxq;69lgnB%_d-_#am@SHb5valPR6?p2vw(h32iP)%bZ z36wk9mhCwiJ<*V()8Vk={0$x0pHr@PfI~E+%MyaT*z;mS?SPl+&vR!9{zQ+78FeXx z1{igxv_4%hs<_wtR)pmZ70VeTwYR)S!j1?>e36@rxM1XMWUZ$2d^k>vk&&7=!QYdx z#_qT61*YGO&kK)ACcoSnywiPrwoK|ybJLymfp?Q9w&ySunz}TU)eMI6^a_+}6yZAW zrO4AzA=faePvUCu9nn5+atLWDR?5H){8>=J#S6A3na6@4;`d8si#&uDyB0n zF}&9FS8rfE+=M1yCfcl3BXW|K-{1)*G5T;){^5>K{3*`jwKxrQhK$4}A7iqlyGosk z1WyrBp~-u3`hNO|STFi^&2$#3WwH`|m6VQ=ekRGrc+|P|mubd<&rNHzh&{WMgOA+E zpvUPHt+Qcv4u2ssR9RG-;UPQE#k=6B{AO*(*os=L-1$(+9fl>@1y>b|ty zv8OQC@S)!bdG?FyQa1NPQ?4CS|2akyspR8RPDW)%?`3SOm$i$nm_201=JCcVTu?tR z|MAEV><`WSH>SNLb5g+=Iye5i6o!uvOcplys|n~xSKDw&eZ63iAMug#2Hi02`QA&$ z_hV<%b#C8`Dfz1W#_~YN)n4B>r&Zr4c_?Y%ncl{T^53N?3>3X~E_IclDR0`OZbGZK z**&oBJ=<2swe^_kDE_5E3@h+F^;R6sm4+LM2PYm{YOrE2ANGHsQg4-BvlimtH5T$D z>M4?twe**|1AezWlggBBwfky1st@SDP=%u@&I(kvQiV}wU*SFRItsGGG8~FhbZQq8 zT{|`8Ih`&Er17$aPXygurf&|Rld@H~9o~AFL`~9wo*^gpUTjrA#<&AZ9Ig|j`b=D_ zvTsYh0Zr*)9rW&d2egxNv6&_M@*3G{_ZVm>>K%iR+|l7{S4+?nvxt>z=cU)8pa?Ei z+wvlJ*wS1*RxLI7n9y{rH-W-|ToHTbR25O%Np`VeYCo}HrR-^=9XZ}cW*r+RBDkmi(GIueC0%x50H_9rW|NxChci$0E}dQOjdxT&9s z*&wGR)a}C!wFDmu2U^qE6YV!%!+^xZAZdGv9KoPlui7KrYVNB!KD-gqdsvtA;JfEb zBg5xKDpQU)tupmFM6DilKauDrdr>H+iZ(K0bL_3%)R8}Vl_l`QlT-A5Ush<3c62Fn zi4(x6548KAp>t1JmF^|E#!7a)H`n&>^H$(Uj)b_f#U9P|le=MEc?kYtldh3H(Y8$C_#FpJR?$x$Yz_qr76b^4Sm zTBr&-yF;8M*M!ukBFCTRD$P=j$>i$D|5Q}VXXw%qVrm}JE3?4XM8*`wn{eI>-(GJ2 zXj>l;S_FhZcI59Cn_2zjOw(cOR}r$%Y@-_7m-qTqdK?bH*&AO@1&m zcgwl|ejpIQRaK|=irir$(`RI6dIMEGLpSqM5dbTp|&N30$vSNU3=>$m>xZ|XjE)IvI~RM=dFGuZ6(saq0bxPXx* zE7+~TcRaSi%Dikky=xI~84Q#7f5XR`$^{>(sfA8rq3v@;Pp*6Yx8drQRnskzoxF$W z?W^KbuJN4rrBc7(GJ7&6@&&gQ3)f3vw`zaoWfip&*5r?Sq8Z01s`Dw@)|*mCce#7n zUDjAP_QXSS#q6u1!by(u?QF58?6wTcsoKAI^?8$CpUv&WwFX-729vhA(`#xT^)_*C zm8~XU59~zIzdMk9lDF}?{%Z8DX>R8mSstf3E%%%@o;yNDdGzP1c+;-;(;trw`9^-N z&LYgJ@!6$Ly&4Uxqt3B19NnyXBF6k_C3GvAmgdVjQm1PSiAA7;W2(m5pG~PFW&Lrl zdR)utq0f)ScV*P9tnqSsYiPB|(i>J=>f3qkk0r#i`NMw%lk+pi-{gojA7c)E{>*CZ z;zY0`JwC^Q_ZUUoLXI%%PIi5qO_*YsOSVd!WW0fn+j5h7=>ixYZv055Yxk4Tj-7ZP z=xa7yf$L<~^n1X>sm|s8NaymKQB>fEWQ}Znrx*g$F|%BUCnuO~EED|RaW?A&)t zjLqHJ*p;||^FbarexLHF>VLv5`{#0HUyYWftBy`&M$wHV3J0Att4j$cr^7e%g~k&& z#wxDRp0fG6b1KmTtZ?4~DfB+utOQ;&|G_nD4yzA)y*@KqUxPyq&--`bI0$!o%6qAM}cwZGkYi2cqnG(UXR z?;et5ntOh>Ecd$FvyVw#c1@Z+@yaB9+WwwqRg`N{oyCG8VL>M+8cVvBcF)D{W|B`* zW`3JxYZ&Id7~1lxw`xxiNX6Jw-9CnmA#`1Z)D*w}gL0R;#4&M${GiN5X<|j%FQ%66 zJuCU#1}^dq4?3te#xvwxeFiomtn=Bj;7@@`CA%#B@3-&RNF{c7Z-mx>jM zjny1dKNK6eVd%eQ!mqirr$L7olaSMl4v#1P=-6$ zHmcdcGCr_X$u>N_j7qOZ(0TxVNr@ug(lYwdoCq6yRLtAQ ztn}G+N3VOqMpcW6^SQ}#EsVOjg3~x%RaW|sb(MUw&&={!jL}rxp%Ssp*uoi4dU`$DWByO-v!Je%soQC7)6x$ESDx6KcLd3Wl-CpF2+l&T}NCR0rO zg5tNTPpgi!JGcqYFx*u;AFFgTTGCd6@(j0~vOm;^_PLpX zq1ZZ;rZ16G>Hxd<;$5e$Po zzUhwND)s*LSXg898;s@K$i3bloVh4++%baZkT4xqCd3xIopwcTZ%uoDEt4r3W@`?q zd*xa(3tC~vie)V<7 z()zfKpCxCjEz8ON(MGNbYPw@Elk#aKWl&&8g`pXn_D=hXH!F)MoBNbja(6y!K9|1b z-Ns9XaBL^;>iVBJY+QOdoJxzuht ztY~-!|MB><0_PQj&bU~V&PNO2&HD6)|Hw<7Q%Gx^)mO+3nUK4l&Mt%xa}#XHAos)8 zX~{P}=HSOzVlqDu=m-Vf;hGujmGMj+eDB5NS82$3zuV=(s>T-Yj^k++00cE=K{aQt z*=KUsY6iU{pA>Fv(^b*ws%Y%9TzhFs6Wd%UUMOzqV`d8Pw=B?N!8eN7U(4@qdwFbO z*nv~?5w&G9(*%R5+1Xj~@^fz$$rp&n^0ig!>~eN#G8n|0E5)ymis6H}$vxLZ?V)whhL^5<@uJ4_>a9Vfq`!$`7H5R|I3j%+hGJkh<{q8E~ zNx$5&Y^>x?+*ocH&k7IjSDNskzxDO-n596AghEXuL-Uxtz@%hJTz!1gVSG&nekuE; zrE0^Ej^C$*7VxsugmAa~ScQVoq4YDZJ|z%{v1`e?sV;#ex6|t)=k!ubp}_Wpm(?pj ztB+i~N?I-;$2HC{U12xH<y-_=Ud{r3gOA%H_){T<>V$Q3`8qHi&0^Cbu{P9$k)Qte{?7xRH?; znVo>t>m=t)$hK@Zo~^vNZn!R9d$@!r<$qXMDvsHfWT!8;)TmHJI%R{OBLwW#CY~mw_KjZ<0 zv*y2VHU1oMn_F2PZ>)l-OeJvYiV`=#UV46L{CvEC0poDmo6L0>`#0b=QG9=3bpVME z#QRII;4uY?lRzdoo>bXu&IFIf8i!pufGy@-`GCms4ZP1iz9zaqr99Y|B=eaU-Z5>z z+0S47TW!c8@eXU3IZ2nhv`6%rb`_mJ=IT53)*4Kj2qMN1WR476va} zv72JjHsPI?T>R(0&n{lG^~#OPfj9&M#N$ux^84rgdHf}t`|kp|s`l!w(D8t7ikmLl z%|9&*jyeu30T9KHNi^F0z8k7g{A=+;58N|VPZLV+z|d@$?)H?RsgMcb_hImJk!#A` zu}hz3-X1A>Rx(Qgk-%TAdIf2^@LTNEcHuP%tgfxC#mB$Q_hdeLM`=sb<>;F0;+*FV z7Qbmy!{wxt@~|)_i%j7R->u(`eqSZHEx4WZGas4Gq9V)XG1*MxG0{Zv&Y{Jngn1D@ zioIMMatxTMnB{S6Dj>$FVNjjv_WqDKc)m}(rj{p!Hjzhle$kdOB=~5rldg-Wc29lG zj4Tj(SaIL3(!uMfO1lyYd%Pn=gOgT6bzR~6A#xQJznz`a30JiKc>#8ozWIKeMFhbZ z#bq5&xPWuSVaXdpQ-PO!$zH#xbyzYhaz2MoHxvsSMGWe@`a2_9+;eyQJ{aTsc4ciU zgDM6$Ev^@Jh4?PZeSY1{arO0{Y2MQyP5#DEeCE7aiTmYVm|=*y70FuiWPW5D?+Q|F zxIf=_?WGaFUvEa2>F~?_&dhDQI@?eBXW^of>4H^OVvfUUtYQ|uuzW)mZ#0#fvQ=biMmj+_Bwix&T=#@lduttDW^6KfBx$g+I2bbJk@RdT1-Xb z)AB`rx5zOumBBlMcWow`QZV)s{vu2@v2C#*-k&pV%3r!$=fL-RzC-7Fjnj3GrR)5j zaa^IQzxb?IB1USHO_sgMK| z-sKf`XWn3^$Ot*|E&ZVmZTf$iBnX9id`t6Y)~0NWQin0taIk?%GDvU!kRI7RGnG|Z z90zk-hymw^e!p;-bV9&ZmJ|```D;Wi!=x?(Co&L_jiK~fU*U%Im_+7nHLjl|^^_Ou z?@&12>wN!8-(7udxwt-FP@A!nynXq#=lB3PN4N*p-Fq6lRQMKC1o_rCkH!;jA5m30 zK|I7%m|pTe(yNea#0P>FVL9@w{-Mu9ZWce_WQCfwINL4XsWLBKRNlGko-?n9pm+xc zQY^J5nUc*F^^*s^0|2RBn9w^g^;(d3VtTE?g+nA}^*t8A@2R$b-iy%uI+CZ)pn`#r-B zWXyG3PI^bx%RDQ-ws7h51X|Q_6v|!y@gAk{y~RSaoKD+Pru^5V!QrM*Q*p!&m1;Hb z-|?sXzcsGyV)`O2i8ig;4|Q=U;NlG6oNm4ZXh}v13c%l!u^GDkBjOhe`J2*D$f-B9 z+*VtR`^?(joUP0I`$|(X1BGJUmjl?AtF@P*wESRDzb!$uS?akof_6_0WEe}1RQy#Z zsVT#{+&FoX^ZEgm#-N)Fx8?`m`>*_X)xTy>uqXWJD|b26?>YYW(EeXkop&JBegFRp znK_P~?Hntk%&ct3x>Qn$!jV-}MwD#HI`%n6va=dWA}TVnk8C9)naAFn<2ZirkFNXv z-oLBAuA+|5=RIEI`Fy;?R6DrolTE2%SW;w4*fsZM+{q1cEXwdK7 zgD&4e6J_0I=FIl!$=y)wM-#GA0CCf>*#-3LaNd)TqhTUNVeG|Kg!ePawZl6QEkBlV z|H3>Ba|d><`}k&XHlkpUubyjgMyp8!k++vw^^*WEmPGrs0_u^HBB6NLxR}=oY@zi- z>b57)Q9c-{&qYws1p!Ys;stGNPO0DlHqcP>rdv*dKyUn|P2Et>d)D*Y#IJYz3O~4y z*YB=de_ed+d*Z=uLTZ3cJ}yuzR6i|?p?2h3{6jw8MmDB9f&~@$tDV6oD=F9R_qV6Y z+b_?RXwmxgZ$RNqb&kNJh%$$yZx#DuPkYb7;hZ=x%E(LByF($bR2)rR*_xl)5 zQl%|m*>01GevsjeOeT9jx2FN5WS7!bQPa`xbAb>AXYy6`YH3Vde=*>(k+y0`ts2pq zt3x5fTUFZyfA8F**Z0+%j(+#ZGkem`URQQw-AKxdv?4(Up@8@rAhWo(&z|`oguB{%$`Pu#x;IEJgB9mn9{d)HSIF!pXlp!wJ z*3%$*qw-sIPYthe**4`7vDfuwHt0$gLDE&sxoUs%@BQDqc)57uj)?%aPWM zD4Vl?2k(?Le}57hargy*?dwY&+mM4O*M7(%f!3F~n5JMv_Kc0>GNz#~3Lh`0saV*G z3TTau{#@-k84>$|^7JAlXMj9KnwY+FXuW=!L~nc!KIYlp#d4ZI&osAI9#B=SCxEI; zyW8Jup!14_O1rh}aJ3!QQyOq8Kwy4VDdJi0treROvKSZ61@fY2$hfOq;C81h+-4Ei^Mvzgj1h7?-vKmpU(Ac1$f zPiLZeOViN2G3;Uq5(@TRzk>OZAael+Bork5jo88O$DZ`&Tpivb!0`xtHDW#=O8tsi zeQ*cHi7B}H;#S5MCq#x%i2!pXR+k`SRbtnq1-lo0Aqo6B^v25qLk%_T?WN4;phClW>&fv(+g; zh44IMO%U0}u#LC3xd2D`w9Mn>;Zr>gD`4r^4*-?OWRvl?!YEKwU4R0~$jinb6Z6<> zq5a7J&^G!#(CdBxL!`s_yao_M2n{ilEH$UGimC6vN1U39Y2y|@E`Edx1Pkm3VBfL+ z-zS06CzA{dKP#yS5f{qS(0XSUub$|hdFJQlhbDoKq+q0+$RASjfVty>FhW>9zI?+& z(?jmFBvJw8bd)g{lc#Hd4th)4MO}%+52QEVr@VlSQ_oT_&ig()=cn)I1u6i9Y@=OT zI^msU1vHi$NXQu|{u`VB&&Sa%(+Z8IM#_5Djuj6J*cGv2kL?R?e^C>mh~8xglLX4^z)-*b8FoC zH6o<&s86f+ZrkEW1>;AzDGb~H-sj(g=4SsFF_NSrFM(Br?Px -kkd245}o%!yZI#=b{`nuFjYf^rf2NXkj{kiO+Eou;l9LmG#||0bVMdU{9q zmhjRB$ft#RJizejP@j$IFt92mK`z0MbK++_$OGjqloKI|66)?O{IU_s0vO$S=-f6` zvu8CDxn^GFWwkE02gV&8O1}y!NaBtR{pex!lKC!Z0s8R07~axCgFkLN185w=L36Nb zXfyPE9^UfR3rhFTFO@1<+DyUJw4MDb1(|1U0T1gW5I36)00E*TW5l#lM;1K@=IH8v zN`flJu;1^t$WZ|EV?SF5$LNUJwN9RoQT>#z1)E?)4P zQhBWYZ>ff3JmbKWw|K&vz5X{E>VFK?i-Gs4#LHZqW&HOBi2DG_SH~$pLar9NG0_y= zP>_P=j+};q?F23gM-cbFWxu^rIR<#r*g&U)ZlW!%_Y#<{sLI*uAm*V?*UL!yS(`&#hOSbD+KzYs6C$p?8PXfW15Ur{Gu|sI z*F0WV#pUqU+~(Ez&;E!Lt=|LE(j!^eg-i&0*L)99J`w<}RRJlupK`l6bac0V1)v{j zfk9kzQg1#d1g3g=x?<-$K$j}`fLpkjXW_c*0sFWW_9}B=gP5Vis>5pY)4C1r?!N_| zxd>Ry!&_w!ir|D#UqOzaonF!fRVIY$YsJJiMi0q5tAN8Dt_xEU<^`n|Tc9cwcs>6& zF6hA9Ec^VeG{n&yy7d*B_0)J2l{9nXyfwz2P!8#H8*AnZOloU>|EM@8TE&?dt6xH@x zO)hn+%$M_w_(C5fxu|9Reb8(uP|!`RAfe}ubix(YvD#`|Tz4lh2ip zkKvXfp`|Sd7-3!h)A9|^EM2*Tmh>&D_^8^m0lIp%?3MZ%|C#fnZ23-r<4j{vVMYR9c&s?f~O z?BLDEAO;R8TEC@ob8qm#1t3$vr|m%HNhl4^yC^!k0pJT9S-Xv5FqYCDr|f9HYlmf; z0&&AL7Ox^c)=YgVeq1#|J~wsPBXCr#KA%?ccfX;-IsyO;dfvv5rYg&oF3+PuGUBe? zv)Hdt#rWukogvchySgdAuO$W-q}`T_rvSVFrrl z0MHT)c&fn5rfJ?UC(8O+Nej*n~wNC zyxWtj2Oy7X>h;pGhX2O1T>p({jfKHku$d6}Vf^{|$F9%i?_7GW?&oD(xAVQa&k`zM ztkyG7Iy8G`?+-($o0HCd3h!%CHiR4Eh@F-|cZxhy#Q` zl)=Nkb1o*($xrI@xTOjdg`AZo<6nS07x~};AdZCQ#isW6xh}BZkErIby6pS!%6^@aNj{m+=m~R&WD-!Pq2>5XYXCO5N|4)u`mydaEIn- zG(bl_8b~1{IICy%#-U!QXUTXtPdFfaD^>wYi9je%{%VWHc8OuQG`dRp*KbhznVj4P z^wI?_>t8L>0dvOVJfpk%g60e0TR1*}i_^Vbwj~tuqF#6|gMdWqG*vvd>zD#%zNaQrGO5YPC=$P^CDw#eW_Yq)n;5*#@kxf>Bgd(Q=8sxS3 zme;?mc7rv&yW{1^X7(b*s_S>`ixR}AiGX$Uz#A+hR>wj!=c!`rfUUlxYV`{@s8Bcn z=Gh`U@pE9Df?~JTjg+3N4l`hD2)v#YNh#tt0f2>-z@4uRpw7+u3(Pq1cDs)%@t#g0 zn%M`-1a{m*%ga0~)W~)FsclfZ*WpI(z$xW5{GjCnAmD~)lL%r$6PkyMcQhG*#J)C{ zQMgavdW|>bd>fRe6h)8);q*V5@ZJCs9nRS+UvvMedFLy z86Q+0ut|y@oIT z0Dw|SaS}wWsakSzi`KM(m^DTwzE{2g8%zUOovd`?#4jM2m2vI`iwxLe67Wf%YqpBU z8EkfSNar2IP^a_xQ9_~O`NXdyTkj3sFiCu@~0u z*XfzX8Yxe^eo=8ym^5!*8zuZ3a3>m0_4z}XC5Z77Le$k5L>?b-wU$vsoNYC>j+nWl zxj%T5O9kRwJlhPG)fQychRPopcJGwm79joNecPH^t{qSlt#+_KdKkw%^)s(1RsK%% zY*6kb&RnRr3y3;bo0V#0m*cOj2^kcRGvQ+VIoBdG+?0NmUNGx;BsrgVcLZx-Xk)za zblDo*0H)V*Ctv}mM||di9MQzm{Yi+_2uSHn(TjojM}5_n9d15S>25VXKl|4$z6Ufe zdticLo&%dXOyGMs2X#gS(^Vcii|vH_E&C2|-X{R@G7Fm6n{r)$>)wHj$X5JeHP1qN zzsBdcugGRyndb4nCu%duHs}2WLC!wioc_(HB^{)gr|(1ENjl9lrN07jFdrK9Oj5wihCzv1p?1%5|DoO|XxH+s>E7 z!<<(2Ji4VNvP$GT4x@|lh+^*KaVazPud}uOGY;?6*Lon0aVK^R-NRJ^Q`n6!AZtJ} zw~k`CBifPcSQGq7TGTvz{A8*TVx5a3mUhOxdfu$$BJIUoqe$4nP=5`_0#_vMan%go zPPMx>RZK1{d;7R}e}^%RP3B&X^?8g?90nye8T@*hjHegY43ek#O}zSy>=kIlY?Ave zQg!A$M}bu%E0UkfcH~>0ln`FFX(&Gk!nwtufdispNJeGygCnQ`FhD4zq>mmv{womD zOBoc#h3X6&Wy4lvKEqd0>Oyx!O;lEOYw83jvR=Vv{g@9ScgTbduOzCL`NdD^wxo`8 zTeG#T*OHqG)0A=_Bc4an)%i3OWVZ&XTbyE+}-42 ze@FkD#}3jog92YC;Jh|0`59=`^BYgc10~kstbt~fgZ##oBzB_S&vU4+>P_93mcLe$ zbB%9NUZ1`F%6D*y9Q!GylVgo}T z`}qm)#UeyHV4(0!Ls7BS3ns!9bM3WhE3<~t%{2;b!~K(R$c&{L_>+9$CwRXGo)I`^ zGt>fEcuI%CBc5XI4z3J-6DxuJqOYt8h)a~Xp`!Vh@J>QU_`~w`Ig=pOs+EA1=-OTr z_u)SnIN6pHgNwahg@CLKy7+t+wIM09;(27 zVgb{+yg2soHd=6991k;1ssP%XW}GvRG{C9^$fy}f+nw;>^ly`+cw-`)0Iz4a`6Ti9 zV=LDixMAC|IoR}+;&WZ(^O7K5cMB9SE@kr@>C@-)`!sM-IyguKKC2A*6nQ-Wcxd<@ zh);BBpP=DrDj5OLDp{S8e{Q_I|2|@J)boq2=aYB8hv9P!6-E*`C~woZvlO3S2P>WU z>=cj|A`4XnLxyi_V2DajGgagQCdrRBEYEQaS%Sun8Y*|d;-rlRP6(Nb$czjWOO9fP z33vN#rcPK(d-hL`DAo>K$kG?IhEe&I5J0h@+U}70(d$i6Pa59(Sv!AYZ9I*Dj~%a{dkD_&AD9|QCpSfL+%q?K{?d-X1@AHYgH%w)kMUFCkEDgYwA%|`z>j@XxSzCie6QzXRtXQG{N=U5*h{k8EMT7L zFlY6I^OH-C`FEv?7PDIu_}ty47nyy^qg6ia07BpMY3lQR^qfqcMbrLYwo@Nb_=RY& z`nUo%$+GXT&3;?X$Hk@_yJq+B-VM@yhmv^~5s(-wmB)=@hR)cEI1daXj+W7TZ`*q*ri`4B2s!MSP$FQHy{QD@Q!}kT@h;F!`e7TB zT zrGy;oE7T0|spYmF)JD5+OadhAo@ONfmPugMjF>fp9X~zG9)tqlH)(jj&kC=-HLHpyGSE&4s7Du=FCQHC)zYWdYTB8uVh|3iRc5Z%VuaIglZCwoYJYhF^aja^x z!@AgE(2 zh|u&QWa)%{R42QrU}N3SvII6MaG_9%0;!ylT;uI|-zqMKhpePI67ld62|=TuJSPyamn5$Eoz)E`q(n6#u_VuEeS56f}?Krsk=aPg<6 z^vFjaiFvsQswJ71)vEOs&&4vxIsN%>bF>OsDy(WA_mQs0hWS>#V6}TzZ(;b>_8|E6 z)M~UY9HxucZokR~QepJF_zqz9F4wmP;_frfs3VQuOUyMXv|sjD{sUZ)>b1!#Wro^+ zJg1(`pzGYekZsQSZiznu!u1!TVHdBWK(2lAxC)gA?UAMg{eaeEa>4~Dy9OWpw=du` z<)+L7Lp$j$-!f!1jIdjaYk<(VKhFbiD2?Goys9AzeQjrWMnj3^Gr^?>*q><(tuT&w z2ikX#g`Q;=E}k@DOc|+M^cK7vPieG_0XL)jAK;or>a~Ral8*uc_HFadk3GV|qop z6bL*APm`)3sQ<_)l$T&bQieZN4KP_5cHQ#v1bS(wo4NX}bN8rRz7GE`Gx&$LN@1kp zKUx6Vg^7S!u^rjo^!;udBbdSfom+hWbZ%7mz@-&hMRSYpU-p&R1YqucLEQ{FALmGs7NaA3v@l`B^rD_6}+0 z8VT#RDi)O?yZW>SNrnba36YC{x+}FX?Vj{_`(vLQf?lt7>pPJz@i5_!9akl-UBezc zST5|nzj_Fz6|#NwrSbf@uY(WYC+?kzKe0EZRZny!UKjn=!#+I?Qv4{>ZR{5|+Pkm| z`d48y>;H7x3OVcnQ|759(I$H(k6#PT#2hl?)}ZQ*ps>+_1~+B$XtybOMf%Y?Xrxo9 z6hCbpGmFk#HEz&5Dtd+!&ivbKty(Z$_D!h0O@Q94p$_`pBVhbIIhSt^tCX$9 zD!|98W*JHoowRc;MoH63OmNkF(I*Z80-pE+zwU|-;Vex3U9eS)S5hQejnU|AbQ0%?DcOGLQQu(*q)eDgsXRJFx3dzCbiS@&b78s{AtAsdB(?wod zX572U%RqB>_TB@jBBCe6!P_&K*@bx+k$n~!BSfm+m1Yk6ZRP#&U0Rg)>5cSKwEJVP z} z$C7*ikwt%8(h6@+J10p#+1egGD9C}}LO9*&%;36ej-{qZNEz-TjDi;>}&jQ7#s)4y%*5LMbWerI*gH$u?!{hb>JJfl~r_^goJt*BZB%01q%wMT9@p9tlh7*h@l<_n!>RBnF8SC80#Y@p3Pf_22Ei{GV6fC)eFdtqpW_{73mhrmM1` zW2D7YfpaGOg4v^%k3pC9v|7oGLDsK@E?AbUW=$2di>{me_!V}Ig!k)Y5b;qBNpbc4 z-Dcf2Dd7I1!B|@5p&=qS3gg!@J_qCwKmBanK)38-d?+BwJeof&n60>SSD2;Km!KYd z84~w>eKPZ&W*u@mxmE!7{+{vU*ZH-*HpMkl@B1Z*zgvZpQIsqPv~}2!5Az}iOA8A; zY5vQH$nG5ni3Z|tPt^rf=CEA4Hb%Utm$QNfO%KJ!iuv~Ye8$Ivy+ohdlqfA@% zNZ)tPOgQ6mmlT^T)_A(M3)IY;_Qbn*JoOp_Dq`-CzFVyRJW(-VBwwE>mgKdrc4c>s zKr?4=c=d|8eqm6>3K+bxtw#HRY;w)+z8jNhI6u4F~}zTPiE^WYDC?w0*&)sa0c=~0)aBMh&^*m}YD86Xk_{AB5@Jy>$HubDHi zSx-i07}Mfw@HO5`&da{&{ozO;PW1`(+{C}YzaS{G0i-?kQMfyAv%}kK*Y=-&CB$DKdS)FtYhpOBD zk>ikc|1RAfw6`yqvPiYA(x2=_yf?b_EAcSpwh7R8QwComfgTziESq;)A>zg^T`h6x zJ#w~i6{nm0sT-&R@6gVEd;D4odODf224Xz0E4-^;9$OEcIO<97ZG1Og_C;Ft60U)z z3LkUKcJpboq^=&5cO0GU^8{bR^l!%9^rwD;P#bmo+JajekT5McH01DhHjHYukqeF# z6+k_?KPglo00}R4LFQM?+sS@a2z&^j=m=^(vjZaQ0&p<^2{CA9wb8bYwT}5PIxYJ` zGKIe(oXZDD9PJP?-)DTZGTfxV1LlbX$-bX0Xh*}kK=t`1F|#*bp@I{Vr*xAq?kho! z(XNHsBRYLFr!)*Ip5Lp$;O1?X_kDkFOw71QKZd&HKv7BpKjmLV0x5_VFZhbID`5ltJi@%8(n<#uY++}vLK|Wr4!5Yw(F;xff}uYAMY?#b~Q$_ zHh}3|e9+vj)p4dEWuVScq&$Sid!zeE8sM}Z2hn-pE)<~6j)L%==jFw84glHstW}Ad z(b%bI{bF%=Y6fuG)w}tn??NMS8-NUndj#aN5n>u^k=mgSfjS8)j`y_JATX$IA<;Ec~D;IHWFmSOt}kfbyYV_H2{1_g1f^bf3l{ zNQ+}=?m>-*T=e1c<5~vb4d4B?0s&7Nu4WT|u71oukWyRm=~@_oF}@1=^@>5dN=9u) zrHd<6NpbI<&Hl|J!3!W`Kf51Mb+8Xgsr*?7i+2ooEX=C_dTeGX`=}_Px%bMxPIIa8 z&|Yo#Rzu$g-Ntms!kwJAk`VTvq{MJJaqCMzsIJ2~Cq!2k_=<_<>XP-+jF9g8{n>jClNk>i>$A*rXCAX-kCSl#z=72AKTFvvsr>P)=6Co6)Eu_7%qoONL7B z>9~PLu~z=MonY@l;5xO}tlHW772==@@K8uPC!Ut04hCp7Fm-{Szu`CyFC40}LmiM+ zpeR3W1x_cZ!9nNd`y*YoIm{2@q8tI*CoO=Sfwv#%eR-jnd;lF3boi|r;u0FNtY@N! z1=!bQWFQiod3jLCSP-bxvqHKD)M7T&AXm|!4g~)j!7h0n^>kN6+NTw(^8V`z1c!M9 zVelhM8&dqTEozsr8A)xzUSCf7qA=LF4jqlt3Yz>^krEHcWMf z0Hh zeA9$Umnf)WIuC}!bJBgQyrLw4y#`Qix4lU3G#g5!9o62Q zDYjx?U~e%GGc5@M6&RbOk7jSL1FI-9E!v~psV8?e>6MzuF!=s)F#nbTl&pJRh%OLm zR9fqrpeXbc=q0%W-AowNm{{9U@th8Yjxbzf=K06VFJ6DyU$FRH*kjq~aXgq{t5khDfvcO)M-EO2uU$dmnm zA?GcyyY~z0n1-e##+w`_!AJ_7wemh+kf8phMpaD zSL8+MC9DU(?r z8hQ2ACBE6|qYim$m%pgKFssppd_i2>&k+AG3#8p3y|9df3_(eFR}-|-p7-Weoz=Sb zEKhBYVFzjVY?BI7af~rPjD{6g_CuI4Lj~QtKg@+nqm^qx*_RQT!4mU+7&H?nYQYnC zYr@>1MSX{cl4t;ZLbD0|t~yWv#GAf>0CmD@i1*M~hL*!0{2TOhH%Ni9VN|AH+ES$B z#m&5fg&P;3=7_+Q0YBy;392Ff0)Nr!r_a@As>T`JFe6J5utYguKXPF|P*c{M`Ty1{RBf8=yJdLbirb1L{67@9m&!7oSl1Agmddl7CDKMvpNJ zo}9a|2Pq7)KvOW{W2dU*OB(TKD?q6*&R*=TC#=~ci>!p!STh@&TIv(tT!XTR#Z#SQ zo&cX^kXi`_L+9})YCL6Ag{XTj$0+S{R`7x*{O+ehq^hRQxN*&LN5JunJ-@3~T_Huj ziQmwV+$;}i$^4D+NTw^!mlx^aKTNN2{U#UK>gwKGH5_oDs&f2H$M=E}Fz&dUfZ1rzQ&Gv~atON2R z0eSeS7j>X=T*iMbuw~r$C@#&>g-+TVgkNHvD5}*@g`uiM{;*6<{AyCduV! zmx+Y3nAV^USe$8Esbo*kF|5)1(9lsCoqXdpm2BQ@^$|OW^~buAm=a=Xe?rQMaUc|l z6o$>Q3N8tz%`$|-=9s)yIG788U83;ME$w`rRqeY8X_UDekdFx(_KG~t*mJFllj>9` z_nx`UPG!Fy1=qf5T>jKcI01W?BlDji!Q+TsK^KIzCTQ|Ct;p9BQmHi!rJ5-bCv#|- zX$S9(irq~t;jA?g*wZ+XAaEt?wN{LHO_-PkO-WsiDIsJ!m*AQTUlE1L=9zl>6$*U5M3d7TI;&1f&tx`46Xztb>OO1>9 zq0~FC^{&?|SzXJ9Nriy*9@qro&iU_kpFyr;=$>ahOC7c;z$?|Qv7uGTmi24w7BxlY zQ-7R24`1-NPopJb1M15sydVK>Rkyie;7}vA_O5Hi)U)KBko+l`$bIcydzwOGTwtOq zCPZ*mBtI*#mV;?tSJ@t{unycikd04w?A4k`#^ur%rLEHdkETk#Mi<#oyRG zbAYlK?fb8eQMG^c^ElsfIA!ng1UeS_($0Ti2Vof^60AB=_-?tJI{PWCF&qn+E_Te0 zbPt+@rmjo595Zcz_i_}RG`${kJM_s-0NkTSH#SvAT7^1%q(vd6ZMI8On(&5(9JyyJ zFvPG^(ku_L9`FD-=)k09By6xvygS8wcL$05Ij^m+Z8bQ#-*iKCi?Wf=K|-22F9?2F zZOBC74LkBFB0CZlx+#)>~*Ly zi(EjMQQ^AU89lFs(QIMW@%O+qw>)u2wZ*UPBF+@=g0`(25@=0%1VXc!Z53y3wIagU zr+IthD5)^l@r{KkqkXNO%#YvF)X^=6^W>{B4-$mLC;~u@rzCs(Je9;@#Qku!GADmx z+whex{BWVqr)$z)ntrL7wr-PpUUe)_lWROoXq}rcQOTebs9sgayuYAaJ8xD@k3a44 zr0O@ZZJaSe<<_0BPtPS2)nSjZ7a!8xe<#Rui$b>kOQcT3DKQgO-z&fDSGCdKx1WsG zh2tG*U4?9VJg?+wRq0@LhH~TK9nT7>rDka=zaJu6&v@|gjbU2@Y3{)s&Q=f-o=>Jf zJdfhB3{|F*JDfp}nLW}Z(vRTf3G$5(%|>wyhMtSO-X12otrY@0%^Eoyyz?&-MS}(| z5t{Mqxl1zpSHf&6vO3(>=uVLJvrD4K@Vq`t0n5HcM5W=i!XPu27fG1D*J@;Qrkr%h zU*fZ`WOZ1~eimA2Tq`IriX6i8AK@sD0|#6&E6qh>x~MdivgW##b~&#jA&+nVePv8{ z=&g_sB^ZrjzT!m9+zoHF3_~rvgOH)%IL1)aHwB~9E1-q+&~EQeky~RJr?RDaK+|d{ zEa7kp7X?IP0!XSVY`g&pQ7ahxJ(Qs*6nh7it#i5vTuV5A1WQ6TsKvwG;a|5w?=84Y zwEkHO3E>j81FC88grC96VwHXLxJoPu#lgA-j)y~4mU~m=8v%HABwt-L?M;okvW$?S z`IMQC8aL)5{dL`>-~u2KFetk#n^J_!Lfv68vkarzIl+WC#%m&@->!aG(Lsh3{OPZ` zXOY$}Q_TC+LOtCxJ#Qj|$5ty;g1IjL^oNXJMefd@dp^-#ZHV1Gg__slW>GnLqZZPT zkvcOH&R0sohNenR=P7eE>(g_)muP4U+C1#hD+xeH84{me zCw>K^9$IK!jR)N(2Uw-eN<6-#Q+6q8V3hLvYNv4E~fZ}8uGbTOdLjei2 zHlPsdDCEJ9wA&kR_s*R6I8*)WY!&ansx)USH`FNt`g7G%E2u0z(V7QGRVrQsfDvVh zyz6(UO!`i0)8Z(TTQ+@oM{^kwI)Frg(+C&aGP{&v_`%BTGQ}7W7rk`i=Qdp z=<#2}^J2bT52X%eGA)2>!|$QOb-c9UYhTovn~LbAe?N5GPN0fW!5v_ zj~~x>swhji(@?~|9GSrr1Ne|WD8 z@-(Rt95w5SsC+3JW`W&rG++ixw{#sOG9`{Nk?mijE(#` z^A*ozk1~%s!DV}aR~X^b51-limzG;t>B~&}9^dOH#OpG{ zehOCM_-dm0r26xM@{NNXB985R_hg^eBUy-Hb&)@p!qSQe1%+oIF?Nf(FQWeE`H|tR zP@D4PwTWTL-2!4z<^Y%?@*Z39ugJJxl0N-!ZHytDx8}NKV$0##h=->&N`d`J7pf$` z;9NA0W28~>MwAE&Nv#)I=UZe3d`g+nb{#bCbc(07Gn}?dl5jp|$f?yRD}5+2(!bux zaaH6+E!+t8sP_&n8lM5ppuZ&PpkUgFHRe#c@%eiqrb4<3zu2!_Skin4?qTl<_g|w}$ z&y6B)i61*95I?sOKpZf_g&ghcIx2FLhv%|mb`N_l4#x-+P^KOK zKwX-e?Zf>I`8E=!e^xMAoD_$4r)>&wrl}}9{C%K2ASj?R>cBv-sW`!^<18V)yxD1jQbax&9d|($3VB##0#UK9|X{A zti7~(aa-jg`bZmV1w<8M$bCT+0??rt9<9?L0KF}4>DLM4gUfo#8#?FlxACI*8j1b- zyfxIy%}`O(3~T^4{Jyu*-Z7tlvaVsuJ#vEb+4g!ierzOm3lPMf`XvOV`2TsdwVT~h z!H2TA8zIr#MKqw2p#p0;G=m#a*}-Z+sjJg%sa^~bsH;=K6P^UUAaJfaf|gERg# z*;TxXye-oIa4+y@xzE#>@YOH};c~`Nk-LNR5hu%-O1enBV&vu@$MYW@J1;YK-pu2! z=|H~Iha`@&4idZ?_JF}BK5p%)X71S1twxUvoY`_?U)7F(Lold**A6eB*4~No8EEsg z?$ca^nc`9RLK4}hBLm^K7Y|Pfn5es(NTM#X-f(f2!T8=*!}IJFEj@-&3u06RgP)5P zQQ1p!Amdd08%6{<&35cZxCC}WoWdL5&!3M_8K&_+v!e9o>NnhLccMA38#Cv__cK$J@OpOMtnaZLWRtdtmNU-RsAywe=v*PH8*bnB5Dlsi`4P zM6(s13^xaZ#Pg(qDTsSBmu9Q@BkMooO4sbFJS6tsdI^NuaGwQhhl`@LtF#O4hy9xM z>$Af7pOU*J<4+8Q3bC`}*PDcb;I~k`y3gRaf2bU--;@RoM>v^cl##nbe^Q2PP%IEE zSeDp}am5H=ggu0GnYF%l{YMM%P1}L_qJ`*Lp9>inlObCYN?wOshpI;tFNTapWB^DV z^9R%&?%)=CBB8_4943cC*OL2Bheq5KZ*Ef?lVUH^AykRmjh;zu*K@`2y ziea9*`}_%lId!Ux8ji_l8sA4nUj0_Q8{n}xlmXXkXCS}hXD&vCSPsF+u8+7R13vKn zc2vWNzEnrn)y3~$AGftYntR#opKX-27*gjFn6r47)lo6kqUe9W^Gxlzo;vJ)yJo1W za5noD9qf8Y3CnnEoer~7L#Bq-<)@rqHjES`9mFcN*ghTr8}AafGnkT|))8hQ*e@Ov z|M2YWaVp2YgV%R9s7hMZEaKdI@fM-7TY|wV*R;DTA_~69yz^cYnhf9llw!oe6f**< z9tKa7G;(v)01k#9lx7T8bmSZ=E$mB(S^%2rX6i;6#_2Z*@ZQpPbhoZ2e9_U=5gi-IaEWU zSh|x(bm`e4qs6l>y6;nr&->lon&XU+ETfclrSWQT>4GY2M8u&Fe)E&Hbo3}3U=>~+ z-tp@HH7Zw$%!v4&&i08S(d+3oQWQtb-DA~Xyurx$iw#f*%XQ$~oZFF?-&|DT<`-eg zMjc6-$I5}N-ifuY_mu^m@huFpl-j+8*Ic|?W!vq4!%Xk-_3T@3=?_TmM2tK}IM?JiyP*gE=6YAJ z0TD%{#z+u#U9IsZ%f{EHKn;p#gT@k6Di{5;57z>E>Dg+T;!yYH#IP@)G4SZH@FF%a z3FUSh20<9%Fd5e{nk5!T#;h{)u!sOol94q9Gt)DFP8CYyuVVI38WsI+d3GAZ5KJmk zr#n@c2RPiDzSJ@3a6CQbkw{6E`x}vu*mlmQ3`4qxpO=0~rG&if+|paB#mloi%Ux?( z^8kMhY(P_tORu904ZIFzqbsxC`>k-FeI}JOmoF4tC3=%@f?bZ21?L}^b=a=lfmr)DyPOC6T(MTGPH|R<#<%2lg%lm2YzKl8VwHxGz%OMw-AGw%#ojMC?TOufUOK)kmHQAd)K8u(Q-xR$V z^=tGHF;>@gH@cfoLZ!a=W_lG^Zx1-*Nsk&1C5r_V?FxRre!hQ6^qR`;-jh+FYY6eG zmyB!{Z?73l8z*Ps?q!s7=Kiu)Yv@frxhM_ocJ1X)lA9qvd9X1t zkq;7$xjfhsVo$rd2Y(A^lbSC5^vP%Md4RxrbawP;#ID$SipuytJp5BmzO~`5op3>i z0ImYw3ZFIe*5XfWt^=xK);4b~kKd0GWW4;zXWQ09;}XAT`z?n6wrr1USkKUfa|(z@ zkX|xj55SPOApDtpTiIl)C5{J_k7>1Ka^1#YuJ9yY4~sNf%*DWnb|E6a2X`+glKIBu zW6T|l-3LVB2+_PIyF(r~i5l|IH0%OxHRTN7`8R8VRLCSJbBxjX(=sgly(tQk*Sk59 zZgq4jQu@leFXT8kb1W_doiSHs9>H6dJYfu#*wq@=apif_WlQUPvpsHu?U*V>&p)F$ zfz3qUeB(DCcYe~aqG|C}Ixpm14Q(Vh6JOh3*^=Rh$#;8V+`!=96EMWLPLE7at%j9n zJiIpq3s0TDrRnM2q}H9qCz`63vO@C-zFXFkbDRcY7C=Ljn+9ahr)As*RH9$&U+UJ} zq^VFkZzRDv&B8l-wc_XCRapE7U0$7F%Nv@7i?|jI0PSyZe!x{BCy z=CD|zAPopzZ*-~WKnddpy%p4}y?R*4SoB~07tDBvj{|B0z1kAG*mcy^2PQMY=a{p? zl}Q+LLzcmrU+MRYGn0-eGz3NivQ#hMMAdVT@CE@Gtzi8bkIH0%A0UD#p|?WV5U4y) z_shxDdGwmFw;y_Vuiwh^nXs*z=wJRr3Fr%7B}w|6fq%9Sqw?5+TYHdQyO*&=pW$ZO z_>lwCS3E-Sh_3rG-Z z@+=&(9kV?b3O-kt*d2PzOWV5=zvYP^#+Ur{PFFND^d4cqj=&a5v0nhD$%IC(+|Qpm zaQ(27f7~kcIjM|bH-+r=0o~Kr7s^dZmzE#({m%NuR3vayv(WJt@Oh4dAuKU_pS!@I zsOSHLs`X5HfZ}~i83*e3uWD==k8-@yfrx5qnfidmy4Ppq`wu|`VX-M{mDxR*^R2?D z?b@M3-~P7Yt>$!c99tkVgF*Fdh*(I9C3E#s>zH#x52Jy_dWxv|QLJ<{vu;TnH2Nn5 z0ANqXIqbvJ)iGID4{p{It;Y3*|3DVhe#wmmNb(HmaGCrayK8n@9>R%VpOdBtYGSW^ zD&UUB%eLrEi{z14xs9eZ%-SjRSH)kW7jT`1+W)evy`z7PwyXh(0hf+gVp=a+ z84b7|V3=|j4L`5_uYESe5c?OG1jK5ITX;$H5no6-1lyZ*XWIXMVlJp#o2Ds6`IFy6 z8^}D32y$d|JnSV%C-nVGxo^CoCSSl6Xf}i-Pse7mKQhq z0x1j(6}nR8TZ#-O`D(AtE6FXO%=N33;`W64M!)>T4RtW-X9O1lx zZ!^!-;C&*Ub(xdt!Sx+G zX(;jICqWieLp9yp)u(k(Z(%yFNCLuK`w)Q*eeQ^#mogmzDLf>J(E(Lpni8c-no7oR zXdDjc<0UwVL6jQEPa2bFzL^IhTRg)q;u=zJJAw0u6){LWX#yyT&|^Az9MYW>oAXXX zO1A}e2;GoWKH_()%wy{N^SdFOcDN0$UM`9>gQl+q2Gw%4N9>^8f;|O}WWCcBIY=h^0-z#r!;Xpyypb|qaNm49*IR&d8iyKkLOu26z&_j3 z61#<8yOreGRt)qg9~m1AZa2S0}3&3sAmZy z{toiMJBb&0yWje8tZA43rF6xjUJvi77v|IbN7bKflHcBrGx;Dv#28uO-@vHU_||Du zFg02ah@jlS&-=)(*w-=O@5E?r|3j)2-d-HE1>7-# zq~%xiOT!D#KC1(I2H(H`Ty!V^FXNE?*H4D z>?smiD=HyND2%d3QQRdZls#EO$vQ|>2t~4vo1{e)N@Oc#Um}q$WHd4%>=!w{icOqYV5NB#796c-8^jB58Z1tPo=pR{6pTq?{ulL z~}}Ewj}abL=6xX2j1I5in9>TLcgp&R2$1FFss*9IXFRXje#@YqFQ<3I? zYx(%Q0S$a0Xw{W%B6bQ(3DQr9H7g5mE0V zA)H>7m8t5MPqht&rS%8WA;#j2hy`cR0fEft{C#B4Tx<79dt)AiLqgaD&4VZ_{$+Pl z8*P`bxfOa!EZb`m0chL*vxBa1ib{&X*1%a+{g^ffBg=Gc2*Wt1LHmKlU!G0j)^la+ z)Eq5qU+Aiv@H0*QzEIVfQf3lS^Q`r(@?5q5pu6Q@-izCcfCO|GUuxL&%J|49vd2$B z9Xjjw6i1*;UYjokwLRf9PI6H?8;hzN2SDGfA(D1 zyW7Jy+OL7uBER+0TCFHh=)(JE8S~#Et`UsEByV?@nQ{j#q5 zKh+gQcy@db=%@|A|LHERpHGNu>7K|c0cc72f!Co5bsGz?HHBgMREmKGJm`Z1a_AxWuOPox z$6I)hO+I4%1!oNf_pj44JCzWe7Z9vvNNKcN`>1#Rc-*@G*x=>nh9--5{2hJ3h*h{z z`v+xgxhP3wt0#bVLIc|YVEcR0TQBj5+{q**4~nZ~G!J<1J;B&{Q0oeW^Eyd!Kmt~) zs7qrcFBQlVRbc*HY?bfJ&-7jON-zi>X_kR zOA*^wG+4IoI#i1ES&LY=xh0?0{{UGud+(TN!bRnFwjh|VbS%7-^cm2D^~%t58hL9_ zc&8@>(a=CX+|$t>&Xmma{UuLd$9&FmU5l`|+h6IGYv#tN_= zI{112sfG$^Uu60-!1QgyM*lYm4!`W$ zTJA0pIv=IHVGH(Ud)J$}AMC;^K6TL^h*&6Ai}Mme{r!L7-@`%)MS=Z@cTr_?S5sR< z`>OBW`vux6$Fx_~-u#oh23muADGzYy?3qPrBM`jIggsFTY}uLl|;QnY~P`` zS_)qGMy@Dp>s3|*#X#m^4STHSwVnBPPetcL83wSL5f49d|8A4GYe%G0tV_SN;I5J{ zcyzhn%X_PX19#&Ga7)}H{MIntQ&RS|Tk!n2yDyrsS>tv1v0Wr^Nh0tlci=~#Ce{+Q zr3>-iF4LF5QAkvpm`2b?)->Fl_VFG3UTGoKchs9plITREa%(CllCWm7m$WP}zjXA= zAsW~1%T5Y&$AQi*+|ZGaSlB*Vb{|6>J4g5T^giaIZ^Ezl6({Pb`#mq|hCMlNu>MD=p#=`bu&$*byBq z-;KwF^vkUd{hsV>)t(S$bi`aN5%dXD$kVU9DboJJhURGh8;85cRMR)K!2^g zkD(IS9g_$sc!C%=%?3dq6h#iDANf0j%Z+?grXkv|_Lj@{Awb4pV?(ZMSmM5qMSxL* z;Yb#Zdkkb%F~nr6+xmQ88APP%!j&_HS&?-bO>GR9wC(Q?WM#hh_4R@ixOW9M@?#Mz z>GQ97T?WWg5&(9$nf6(`uo0qCebxFe`MI=yU*9#X1UyC`fLt{K3JdihEL71IGf2q* zI4xxSnA`&lJGG7DUl@%zuxvDw6=m@HIqeNRnH(*gVBf;5(ZdXsR=c>=bqxEeFo2`s z7vGc8&QU80fPv~3)?@(k0hkc)kYzL$;$DcT(yyI_zi8YIVb2o%vH_G@CyNUPOV%BO zv7jtY<)$F9|H@dk^67=$G=|BHlM9$!K#8#n?K|#rO>5W__0AM!Zzg&PiMDh5Z)S;Q ztbdqdt;>@EfL}i@Gij!pJd!%EhNovBd-05@omMvsQChJ;SuT~1!@>~?BTVIp50{inH(|?ZH zRSxeVn3o*_KBE+j`AK;#j6DvRjP`O;hApEf%wl(Ib6!PO>xG^GO3$;g^YmNzSILXb1CIf9Im6b}59ciywAA{| z_9f>$r@erMVYAL9*w_FdTDUiZ&&f`-5N;58dPPtY>J#L5LK(3_7gEp2y|SK{3AQtl z)!APbsC>~}bJ7?NUS`UFA6OM8si0XPQMgS?Tw|`A>6;ms= zzN-VE9>0d5603#}MpZe#uu*R^-Y0j} z?Hq@~{nDy&F^`T%YI}lpSkD%y4IrW*jN5g;O-2bI3Lp|Ss>oFJ7mOD?#DCiSQ=0O` zk{Z_u(ttnYsOO+m6t`myf>oS29LcxxPYo~gaSY@k>&8RGas=FroIo2>J0H9cP|P8j zg-N;v+D$xZy`F4!Pa2Oj+*xKyjh}4Mv}m8a(&#C&7c9aJ@(lE~@ilR$y0DH3axxISC0Xn#Ov=F*a0!ct-Dx-U`fEke>DhOEW3JVUZ6!|Lnm4I$3t#Z=XE`ZSxh zm3-^E?Uw~Vm*UM4YDa^$=DhQH7bxl}lh;)?2b3BS3m5_iRA455Aatw;iAV)Q_OMVF&U!2y9(qS zYlmeJxATupeSB4OQ2W=yaC3@ANWhBxEl-&J69!(X4GgnB-6aMX`XQt~0kSkXecsmsyjs$!L8OGkhf0Hf%aPMl zD{ANyY|Al#%RIv$36(Bg$B(`18-Y!%v>Af@%R;bvY4tZ0r1!~fWn@V+(RO_TmR?iD zeyfAp1HxO$WP zl!;n=Dw#or4%2@S&#rYsk$;^3FWNG4e>9FyMbUeNgN|HFh13H!PwR2+Ux4axk)HWh zsN+HAmSsm)pRwUl;;6r5yp)@4cMO8%@h0{N5EfxW^RZu&JX_5;>R&tLQn3bI>3gSU zx(pIQACp$rhoAA5$`3dF{V-744DeVdLglb-%Ff`1q!=;F2B4N zAalV!cibdzxvp>D)U&Kq`4gzy&qn!%N)S-(_zAGhocQ{=?v-oG{}QJ)@t!hh)@A1HDB zwsS#`nsu8ehDvxnN!=82%2#d_BWOSJg0+%MVi&T5qenQtA78O7 zJycntRa2Wc<$q^-z@^-;yL&a^eG<$iw%)_+CP7apkO}{>^j~)X`=kjbT*^rZnz{>r_RUZFe zjN6(bKcx1%Oi%W5u+w?hd}rzQmRscoK0%hhpj!WVUwXn;IPv)mm3;0AXxw^JwKra! zs-2hz+|8-{1|JD=(158;P?Ujllg6*E<52rT#AY1@d`8aaaO_&2GiVtF-(lhovitt* zhPsL8_$=aNPR+!Wc=zx_J*1+6~naSTVtHoazUJjNpoX%St`WnKm`4pVwLW`n;?T3c0;fG=(?IE}~2olmZsPkdaJkSNoeP3_*bEUwM)zmQd*+ zKid8gFrp$H?r@}k2V4`quNh2umwn|n<3s1n6vp->MB_UT)w16tG|Bh|4$j{1Nf3Zp zMJdo(nF5&WAw(KvK@i9#V9o3TxAfkB(z62p4vX6MKf4#tCOWT<=h07;e4^uJ?i4UU zGb)~IIHceEmAK2LQV3O#LKgQU(I33Wbd5*10AX0GH-Jfk0Zru#zr{k)@(v6n?=2jZ z%X$%&=(sG2Kza&*0V;zbQoi>f((y}ZQ`TwT=}@iW--+U?&1WB^{_Qf6$krP(F@9BJurDUl*jMLXZ?Q?Zu6b+J5VsS^y}U-2Q_)7IJgf>me$GG68qo?T;^s4K z?!YL!vdOL9J7vNN%J>crEk2IP=^8Vlw8{0a+b?(fEbjV1iqzugSy^4}BYaFNLDbLl zP*Jw{vV^Fu93ly^lFnjWY`%)ug|c_8=^q`>eLvH5il9uG`(B1<8|d6j3PiImbJp{t zj)SPO{Lh0Pv>)QBj2!L@kf5Yjc&8fR|4@DCIaEoOw^`V8N)IwDJlqX(FkLSbu=ge` zM%pe*3Xi#VT!LCbR?}F~6t9Z!(?*5GPFD+R2wm3{V6LBTXATKr)Y0@mqtmT2-ruCV z=LfK6OP{$vuPx86qiF{axHcp2muIAr(9rC~?v<9QQj*hTr+FNNaZdx85ngG@-~ zQVc>dnsnc%f{zO2M?H(woyboUl9kwp6+AA@K>84c7vpi^X;puE2>ap2!^_n>be=5F z8<(_g&y-e&@H+O<8d!6w@b~Umb{7ZLzU0y zq*Q=E@dMCI3Q9!KIKY3nel^^g1E5qjxnTUx=u8p@b2tgw;znVGydDle>Ex$? zXW{9oy|+3l-qHAiDYhLX9@O@qIIg($;e`eaX>D``pCvG8gj$348y?5nE}-9Q+~ z@>iMyLT~u$=W`6|?e*&V`T@qyPb{thl3};4&(tUS^wqxN44MYQ)I%gG>xX12^ z+nL_=hS~NJs|sj_IbR2RhQP3!9YV8KthUw4fW%(>fq1T?k98}C4liPr~?My-k>tz0^djUqL%L_}r|mf(qRF)kbw9f$()!#W8l=PxH& z3e(&ur{X?P+WHJ7y1#_sw2YJo65Rp|@S8)Qr^fd&B|jFoEl(4tOXi$n*-g9c;afC^ zCVI;Q+?D8?0)Spb-C&tQ4Fkay$q!`S#Yq+nr~IWGJMI6e*#U3{iLYK|7VMASB9yZ_ z`YJ1vAJF@&2kkgSq-jClFD~EDGV8jPI$uDPt+F>Zl`ow^JJQY`05Ko2E#j z@XYmr(DD)_nPkGsy#1#A+{_O~a52=F%>3YSAL#>;2OU2wbWrHx6d~{!j6|3`dV+I; zN5W|OUBGi{6;Lf{*YJjVifO`qYtXx->#CYp!1Sb6rdE>wNd>$;t~#0GwLCD_{z{>V zz(ULt6=bfYPciJ{jc)^7z5$mad%_T#DzPsdWAM?))MHN@$B{ZB1E;JtmbR9LhHCx@wnCILz4V-~R?ES*_Y}XqDUZFFWRCiZtvbDz ziARdLGtJCb|M6A%^Ls9G-(q;P+x`htrH-`}&gSHx<_oq_Y1e}n-;2+e!0~^xJO6E! zC)}rFOO}=1rTaf%!>l=1XV*{DQk>#p&9=@rlfF#()q{DINL4VqBkHbT$O>xfE~f91 zZ7ZB(YPPJe?FAyq3FAV`YeVgyN{##^l2y!dX@iC&4aVLZnk(~khsI0#NfRC&)T2|x zVSq&Q5`v?&T(3;w4hrUO=v*7IQ8Jr&^mf^&i`LmDGto-_TVf2Qt#*s7)K2Y&!r6pd znadh9rHO~YtrW~{y=_Bpt3q7r`_64>An!d{w5Zp2x?9m~SY0tjsotNS|A`7|c?id5 zV@bV$5M|A5+>53*!G(c6R@4o#EB%%qA1xEjT5E<^8V8B`Jth{P8pG7){58op`$u`Y zl(`XR9&f4*{X``F`EKil6yMR-Z~-1%d|SxM3xO@U9zDb7I=Q7&9i$3aM@v?mFnFux z%#`t5%(#kIya?DqccpauPT8&$`2sML{nVNv^p8Ie?XGrZlaOs(jJ+` z`lFK>ax^ORCbZUuNt6^dJAlcm&C>>KYkuk{IzetW2d#9(r#z(J?qhpcLHmZPobqkx zu|dh60!QX-FUoR1XTcj?D_&rBX&D!E&P7x)*G{elXy|AsQ@4GiSfOTBNM7)rU8@uq z<&BfaphoUaZ=>~)v- zuKe^xmG5o%uK9;rXLl_>l9Zlm^mh?3>C{i$HRJt_`Fy=DmGtbswnm@m2MrDrB$|4; zOv9D+8_(j;8*ndIXgGuZ>|*~Mv=L;$P1jd)CfHg_j;8w(mO9$+H#qPsjItK`+AV9J zO}T0AeW>_k;L_H2LrvpzYy^>2dODnUrWfaxw_48CMUF3AeUInCPE%oK;e9SuL`V5K7?MYnhgLor@|tzXGZP%5@`?pU|w8dblm z&#;mNPeIf!%5Bugs>U>5IbK?l9}uUgx77)fV&8RFx64*K01Rx3#w0%&nj~Mh+opyY z$Vc@G#((78PyI5Mo2MW0OuSekWl>bRs!jF%{tyoY!eC^76TERgP9ri_hX{EnZe zUW5YLFQXlT%Xgd!FP~TM9U4D{@Ujxuy^0o;GtdPL;!IPXK#dU*JmNs{LrGNjZ?`Rb z)JMl6W_A~jCGXyHxO%pxy?JEKsc`pBQhO(fF~RwBwA!c#WVp6w7 zi$o#j(hb}9FXR%y}}&sbe=ZlpbDFqFr;Mq%soZLbf{hrg&= z{lcmLw4iEPIAr_zQ`X`}nLl(7%j}V?%&qG){Oo@Ssa`cdUY=D<7BhRJr73ISX0vEO z3^-I-o%UE}9_Nq0!EKL;h+2J?Kr_YDb7HS+SMh@dzsuRTOJJp+F^`k%rd~5}=lPH% z>8BuvqwJ=z%}(&(vYZ`ca&DOPZ(Lp(h%wtLCR)b3APl zWK7u?GQDRfn@*l9_M12>%Y9!y3B|F9ktnvz8`ZYhx-ktxv*hhqF)3$k4>8%T^X`JY z7@l{LNjoh_#%h~bmg)#q@wG`vo3+{9K(FA$fI#I_+J0Y1LKwHr%FB7;{RGS-R=z}4 zQm|9=`TIeJc{AXrrP!k%Is}XC7gbjv>V+G=zN}+h>vscDpmzj@liTtl#&Be@fu#gNLijQ|5Pxc&gLH)f)W{-e`^ZxAkp#~hT=Lim;*b{k7`(UVQwuE8spq z*WNMMUZ_nW*bNyeW74d=B_kTfgMZD7eR9bd22r_D^`rkmkBU9{ILl4MJWWp?kM~4$}oU%Kcpu zqwHxFVGBcRal6=?^$9DA<{AX)cD>c_#M}~3-b&1N+YcoB+0w?>?-$Yu8m+TU)*AyR|6b3k{w06#papuTW$ zimQ~_b}ui_j#&FWlF+sneNC{;9U4>_DZb0M)v5l5iiboP=e0$iSDbC(~AEOC8$K zTIeymwey)Wf$Gk({30dufC^9Q$GvVXccgi2;ZaKM5Y(qTv`4~kxYWL99F8_#ii9h< z1X4b9@PZ*CxL-xBryl`Xct zyWld`@4BFJL?XUh=S+rE13YzHsd3LWRsPjNp@?P-N&@z=cimO*$QQaKfB2~fSK|;c|pHrIed+7vb`x4`tj#GaM zZ1fzL_O1kF%`$4j_t7K4j8!;)d`u59t&TbvextZE@#u*ur=GI;I(WTaUg7G}f9#k} z8ZF`cv>r`p>*2Dl2BoxlZB#GF`D(t9!$#^9Kb`-}v|Y)WA%V0@R&%_0$!1N4$hbR) zs__GdgAJrnJIv@X#kuQSm5|$-+_~0F(JL>^&XqE+Hoi~B4hyVQ{N%k%P$LajV93{@ zt?xR_9%hu*!Pp%4^#nwH|Nb88s;2Am?YQZ*#HA*bSBpDyDJ-6sq4A|E8KyEWNMv$6 z%H%jQ#5LY9grA)+9rQnVfIW=2@n$3ZxxX6u&o3IJN{njtUC6y8OVs$Y_ZcEO`LsiF zviTEP&gQU0z}Lh0_WP&C7CpzU%=bB!4|#fVD1;YL%J!B9-fp@s(vHK&qnE0S19g>c z;r_1rrB=N)d)P|Zfv-PBTqu8Q@-bIPW7R?hdJzSPH%|;LVriSRVntr!R;*}4CF@wD zz~MLEx=@g~dfWQC389&dR>4{PQbBIqAq&lh?}S@RllQkDcnPHO_eUJ(RGvi5+P^P; zv9ZnxdCM!$;Q6G1@4*o}l1PJuZrHsX88j7N>GXWZsnA-pVw^JLDK99fMqu>VIA zwJ<;b=d1Dvq7S2&8Yv+Muq**A*dMdA1mnY1MRvm4Y{L(LljXw-8){`{KyO1fAeIHo zHf?-=J8q$sIpLMr>dGmcicf9P`FpdGatZeRVOx%h(LWd_z&iDu|hu!V1XdSV(2DV&oEiSv+P|Cc}*s%L*y5mw@Th7W09Pc3V z=yEE&xK#WZvQ6he+LW+es#rFJ9)02$!&dSJ?shtI72?hlZ$-E7U|`m9l1(M#_pts7 zz%!-3j03e1{VmBG39Q+z&|O!+m#BdimNR|fT~B&Lrmf}_*B3QY!Xv82FoWF0(%iOk zT|%>y(wYIuft()jiz%P>Odccr{zlNvQ2;v}B4*h*26RvFDcN6tu(Mk}?>OsC1-iKWA^D*8RFbQpvlQ<5IXzo|*@E>H{dj%q_8A zl>$@Wc6a6C$j$cdp0;)G%dz<#X2m4g(Y+PeokH;F*-=OxtlBAg3TofBN@8GrSG+l% z&nqk~eSUr}!In>&{8dC8%$q>+u;m)miIr39M}NHfJ}Ljx{PwwyL%^wiwSP$7U}1Rn zbcv_y*%IpluyioYhE#`RWC!^+?>_m54GS2rf5hyW9EGR8qrp^Uql4&fhgFUoJ5r%v z=bY(bTKwtig0&8$X9vuK`fV?Qi)ABNjDZhMs4g+!UZ%1I^Xf1$ZsC+|B1 z#p(0hDK}rD0wAtbqes|zWR9ve+!`N1_R6zqLp5+u8};DfNwDrr4I1&!Zu^`LtIfZY zI-lN!t)IWL#)l^!lt%F{!Aevu#(eIr|ICV8;~im=sUVicU$=}LURMRba4bv*6l>{H%H;YyBE)7X13I#aeUc-Em@ZP?n0i#g#O0LQ{2bS zV%c~Tf29=f+w92D$*w-0?{<`+W-iBnUH)o1_=j`?<4M|QREGX9O8+unaertI++t;J z+C`(fg;S$>#8FYS2I<4-qUPn38Qi+wxOJ$?@<%y}p0?)-2G2XifVW~KzM3_I)62V4~_t~p_E$ae{D`n~eoP2)5>>wZJHz^Q5r?q=W0 zz_Nv1`ttM6kUfj^6KtIVp}GDi)YR1M5xz0$V^9-)qAm91)#aIi=vm(7Tg_Rm6HcIR?4zMJ5q=_=-FAb@OCY*i6Ab zTa~^)htc)hL+oAd{Q>v@UbLk{8);MmB!`*b{wS7BH_!inUE_RKH;H2KNMdl$eclub P_;*4>Pd)RvdBFbxXl)NL literal 0 HcmV?d00001 diff --git a/rclcpp/doc/proposed_node_parameter_callbacks.md b/rclcpp/doc/proposed_node_parameter_callbacks.md new file mode 100644 index 0000000000..cd39f352db --- /dev/null +++ b/rclcpp/doc/proposed_node_parameter_callbacks.md @@ -0,0 +1,29 @@ +# Proposed node parameters callback Design + +## Introduction: + +The original requirement came in **gazebo_ros_pkgs** for setting individual wheel slip parameters based on global wheel slip value [link to original issue](https://github.com/ros-simulation/gazebo_ros_pkgs/pull/1365). + +The main requirement is to set one or more parameters after another parameter is set successfully. + +Additionally, it would be nice if users could be notified locally (via a callback) when parameters have been set successfully (i.e. post validation). + +Related discussion can be found in [#609](https://github.com/ros2/rclcpp/issues/609) [#1789](https://github.com/ros2/rclcpp/pull/1789) + +With the current parameters API, the `add_on_set_parameters_callback` is intended for validation of parameter values before they are set, it should **not** cause any side-effects. + +There is also the `ParameterEventHandler` that publishes changes to node parameters on `/parameter_events` topic for external nodes to see. Though the node could subscribe to the `/parameter_events` topic to be notified of changes to its own parameters, it is less than ideal since there is a delay caused by waiting for an executor to process the callback. + +We propose adding a `PostSetParametersCallbackHandle` for successful parameter set similar to `OnSetParametersCallbackHandle` for parameter validation. Also, we propose adding a `PreSetParametersCallbackHandle` useful for modifying list of parameters being set. + +The validation callback is often abused to trigger side effects in the code, for instance updating class attributes even before a parameter has been set successfully. Instead of relying on the `/parameter_events` topic to be notified of parameter changes, users can register a callback with a new API, `add_post_set_parameters_callback`. + +It is possible to use the proposed `add_post_set_parameters_callback` for setting additional parameters, but this might result in infinite recursion and does not allow those additional parameters to be set atomically with the original parameter(s) changed. +To workaround these issues, we propose adding a "pre set" callback type that can be registered with `add_pre_set_parameters_callback`, which will be triggered before the validation callbacks and can be used to modify the parameter list. + +![Desgin API](https://github.com/ros2/rclcpp/blob/deepanshu/local-param-changed-callback-support/rclcpp/doc/param_callback_design.png?raw=true) + +## Alternatives + +* Users could call `set_parameter` while processing a message from the `/parameter_events` topic, however, there is extra overhead in having to create subscription (as noted earlier). +* Users could call `set_parameter` inside the "on set" parameters callback, however it is not well-defined how side-effects should handle cases where parameter validation fails. \ No newline at end of file diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 54a4b922ab..bd5d39f6bb 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -370,11 +370,21 @@ class Node : public std::enable_shared_from_this * * If `ignore_override` is `true`, the parameter override will be ignored. * - * This method, if successful, will result in any callback registered with - * add_on_set_parameters_callback to be called. + * This method will result in any callback registered with + * `add_on_set_parameters_callback` and `add_post_set_parameters_callback` + * to be called for the parameter being set. + * + * If a callback was registered previously with `add_on_set_parameters_callback`, + * it will be called prior to setting the parameter for the node. * If that callback prevents the initial value for the parameter from being * set then rclcpp::exceptions::InvalidParameterValueException is thrown. * + * If a callback was registered previously with `add_post_set_parameters_callback`, + * it will be called after setting the parameter successfully for the node. + * + * This method will _not_ result in any callbacks registered with + * `add_pre_set_parameters_callback` to be called. + * * The returned reference will remain valid until the parameter is * undeclared. * @@ -491,11 +501,22 @@ class Node : public std::enable_shared_from_this * If `ignore_overrides` is `true`, all the overrides of the parameters declared * by the function call will be ignored. * + * This method will result in any callback registered with + * `add_on_set_parameters_callback` and `add_post_set_parameters_callback` + * to be called once for each parameter. + * * This method, if successful, will result in any callback registered with - * add_on_set_parameters_callback to be called, once for each parameter. + * `add_on_set_parameters_callback` to be called, once for each parameter. * If that callback prevents the initial value for any parameter from being * set then rclcpp::exceptions::InvalidParameterValueException is thrown. * + * If a callback was registered previously with `add_post_set_parameters_callback`, + * it will be called after setting the parameters successfully for the node, + * once for each parameter. + * + * This method will _not_ result in any callbacks registered with + * `add_pre_set_parameters_callback` to be called. + * * \param[in] namespace_ The namespace in which to declare the parameters. * \param[in] parameters The parameters to set in the given namespace. * \param[in] ignore_overrides When `true`, the parameters overrides are ignored. @@ -533,8 +554,9 @@ class Node : public std::enable_shared_from_this /// Undeclare a previously declared parameter. /** - * This method will not cause a callback registered with - * add_on_set_parameters_callback to be called. + * This method will _not_ cause a callback registered with any of the + * `add_pre_set_parameters_callback`, `add_on_set_parameters_callback` and + * `add_post_set_parameters_callback` to be called. * * \param[in] name The name of the parameter to be undeclared. * \throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter @@ -568,11 +590,24 @@ class Node : public std::enable_shared_from_this * Parameter overrides are ignored by set_parameter. * * This method will result in any callback registered with - * add_on_set_parameters_callback to be called. + * `add_pre_set_parameters_callback`, add_on_set_parameters_callback` and + * `add_post_set_parameters_callback` to be called once for the parameter + * being set. + * + * This method will result in any callback registered with + * `add_on_set_parameters_callback` to be called. * If the callback prevents the parameter from being set, then it will be * reflected in the SetParametersResult that is returned, but no exception * will be thrown. * + * If a callback was registered previously with `add_pre_set_parameters_callback`, + * it will be called once prior to the validation of the parameter for the node. + * If this callback makes modified parameter list empty, then it will be reflected + * in the returned result; no exceptions will be raised in this case. + * + * If a callback was registered previously with `add_post_set_parameters_callback`, + * it will be called once after setting the parameter successfully for the node. + * * If the value type of the parameter is rclcpp::PARAMETER_NOT_SET, and the * existing parameter type is something else, then the parameter will be * implicitly undeclared. @@ -609,11 +644,25 @@ class Node : public std::enable_shared_from_this * corresponding SetParametersResult in the vector returned by this function. * * This method will result in any callback registered with - * add_on_set_parameters_callback to be called, once for each parameter. + * `add_pre_set_parameters_callback`, `add_on_set_parameters_callback` and + * `add_post_set_parameters_callback` to be called once for each parameter. + + * If a callback was registered previously with `add_pre_set_parameters_callback`, + * it will be called prior to the validation of parameters for the node, + * once for each parameter. + * If this callback makes modified parameter list empty, then it will be reflected + * in the returned result; no exceptions will be raised in this case. + * + * This method will result in any callback registered with + * `add_on_set_parameters_callback` to be called, once for each parameter. * If the callback prevents the parameter from being set, then, as mentioned * before, it will be reflected in the corresponding SetParametersResult * that is returned, but no exception will be thrown. * + * If a callback was registered previously with `add_post_set_parameters_callback`, + * it will be called after setting the parameters successfully for the node, + * once for each parameter. + * * Like set_parameter() this method will implicitly undeclare parameters * with the type rclcpp::PARAMETER_NOT_SET. * @@ -640,11 +689,25 @@ class Node : public std::enable_shared_from_this * If the exception is thrown then none of the parameters will have been set. * * This method will result in any callback registered with - * add_on_set_parameters_callback to be called, just one time. + * `add_pre_set_parameters_callback`, `add_on_set_parameters_callback` and + * `add_post_set_parameters_callback` to be called only 'once' for all parameters. + * + * If a callback was registered previously with `add_pre_set_parameters_callback`, + * it will be called prior to the validation of node parameters, just one time + * for all parameters. + * If this callback makes modified parameter list empty, then it will be reflected + * in the returned result; no exceptions will be raised in this case. + * + * This method will result in any callback registered with + * 'add_on_set_parameters_callback' to be called, just one time. * If the callback prevents the parameters from being set, then it will be * reflected in the SetParametersResult which is returned, but no exception * will be thrown. * + * If a callback was registered previously with `add_post_set_parameters_callback`, + * it will be called after setting the node parameters successfully, just one time + * for all parameters. + * * If you pass multiple rclcpp::Parameter instances with the same name, then * only the last one in the vector (forward iteration) will be set. * @@ -893,12 +956,82 @@ class Node : public std::enable_shared_from_this rcl_interfaces::msg::ListParametersResult list_parameters(const std::vector & prefixes, uint64_t depth) const; + using PreSetParametersCallbackHandle = + rclcpp::node_interfaces::PreSetParametersCallbackHandle; + using PreSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::PreSetParametersCallbackType; + using OnSetParametersCallbackHandle = rclcpp::node_interfaces::OnSetParametersCallbackHandle; - using OnParametersSetCallbackType = - rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; + using OnSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType; + using OnParametersSetCallbackType [[deprecated("use OnSetParametersCallbackType instead")]] = + OnSetParametersCallbackType; + + using PostSetParametersCallbackHandle = + rclcpp::node_interfaces::PostSetParametersCallbackHandle; + using PostSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::PostSetParametersCallbackType; - /// Add a callback for when parameters are being set. + /// Add a callback that gets triggered before parameters are validated. + /** + * This callback can be used to modify the original list of parameters being + * set by the user. + * + * The modified list of parameters is then forwarded to the "on set parameter" + * callback for validation. + * + * The callback is called whenever any of the `set_parameter*` methods are called + * or when a set parameter service request is received. + * + * The callback takes a reference to the vector of parameters to be set. + * + * The vector of parameters may be modified by the callback. + * + * One of the use case of "pre set callback" can be updating additional parameters + * conditioned on changes to a parameter. + * + * For an example callback: + * + *```cpp + * void + * preSetParameterCallback(std::vector & parameters) + * { + * for (auto & param : parameters) { + * if (param.get_name() == "param1") { + * parameters.push_back(rclcpp::Parameter("param2", 4.0)); + * } + * } + * } + * ``` + * The above callback appends 'param2' to the list of parameters to be set if + * 'param1' is being set by the user. + * + * All parameters in the vector will be set atomically. + * + * Note that the callback is only called while setting parameters with `set_parameter`, + * `set_parameters`, `set_parameters_atomically`, or externally with a parameters service. + * + * The callback is not called when parameters are declared with `declare_parameter` + * or `declare_parameters`. + * + * The callback is not called when parameters are undeclared with `undeclare_parameter`. + * + * An empty modified parameter list from the callback will result in "set_parameter*" + * returning an unsuccessful result. + * + * The `remove_pre_set_parameters_callback` can be used to deregister the callback. + * + * \param callback The callback to register. + * \returns A shared pointer. The callback is valid as long as the smart pointer is alive. + * \throws std::bad_alloc if the allocation of the PreSetParametersCallbackHandle fails. + */ + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + PreSetParametersCallbackHandle::SharedPtr + add_pre_set_parameters_callback(PreSetParametersCallbackType callback); + + /// Add a callback to validate parameters before they are set. /** * The callback signature is designed to allow handling of any of the above * `set_parameter*` or `declare_parameter*` methods, and so it takes a const @@ -937,6 +1070,8 @@ class Node : public std::enable_shared_from_this * this callback, so when checking a new value against the existing one, you * must account for the case where the parameter is not yet set. * + * The callback is not called when parameters are undeclared with `undeclare_parameter`. + * * Some constraints like read_only are enforced before the callback is called. * * The callback may introspect other already set parameters (by calling any @@ -965,7 +1100,77 @@ class Node : public std::enable_shared_from_this RCLCPP_PUBLIC RCUTILS_WARN_UNUSED OnSetParametersCallbackHandle::SharedPtr - add_on_set_parameters_callback(OnParametersSetCallbackType callback); + add_on_set_parameters_callback(OnSetParametersCallbackType callback); + + /// Add a callback that gets triggered after parameters are set successfully. + /** + * The callback is called when any of the `set_parameter*` or `declare_parameter*` + * methods are successful. + * + * The callback takes a reference to a const vector of parameters that have been + * set successfully. + * + * The post callback can be valuable as a place to cause side-effects based on + * parameter changes. + * For instance updating internally tracked class attributes once parameters + * have been changed successfully. + * + * For an example callback: + * + * ```cpp + * void + * postSetParameterCallback(const std::vector & parameters) + * { + * for(const auto & param:parameters) { + * // the internal class member can be changed after + * // successful change to param1 or param2 + * if(param.get_name() == "param1") { + * internal_tracked_class_parameter_1_ = param.get_value(); + * } + * else if(param.get_name() == "param2") { + * internal_tracked_class_parameter_2_ = param.get_value(); + * } + * } + * } + * ``` + * + * The above callback takes a const reference to list of parameters that have been + * set successfully and as a result of this updates the internally tracked class attributes + * `internal_tracked_class_parameter_1_` and `internal_tracked_class_parameter_2_` + * respectively. + * + * This callback should not modify parameters. + * + * The callback is called when parameters are declared with `declare_parameter` + * or `declare_parameters`. See `declare_parameter` or `declare_parameters` above. + * + * The callback is not called when parameters are undeclared with `undeclare_parameter`. + * + * If you want to make changes to parameters based on changes to another, use + * `add_pre_set_parameters_callback`. + * + * The `remove_post_set_parameters_callback` can be used to deregister the callback. + * + * \param callback The callback to register. + * \returns A shared pointer. The callback is valid as long as the smart pointer is alive. + * \throws std::bad_alloc if the allocation of the OnSetParametersCallbackHandle fails. + */ + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + PostSetParametersCallbackHandle::SharedPtr + add_post_set_parameters_callback(PostSetParametersCallbackType callback); + + /// Remove a callback registered with `add_pre_set_parameters_callback`. + /** + * Delete a handler returned by `add_pre_set_parameters_callback`. + * + * \param handler The callback handler to remove. + * \throws std::runtime_error if the handler was not created with `add_pre_set_parameters_callback`, + * or if it has been removed before. + */ + RCLCPP_PUBLIC + void + remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler); /// Remove a callback registered with `add_on_set_parameters_callback`. /** @@ -994,6 +1199,18 @@ class Node : public std::enable_shared_from_this void remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler); + /// Remove a callback registered with `add_post_set_parameters_callback`. + /** + * Delete a handler returned by `add_post_set_parameters_callback`. + * + * \param handler The callback handler to remove. + * \throws std::runtime_error if the handler was not created with `add_post_set_parameters_callback`, + * or if it has been removed before. + */ + RCLCPP_PUBLIC + void + remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler); + /// Get the fully-qualified names of all available nodes. /** * The fully-qualified name includes the local namespace and name of the node. diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index 7cc130f256..eda68451a9 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -181,20 +181,43 @@ class NodeParameters : public NodeParametersInterface rcl_interfaces::msg::ListParametersResult list_parameters(const std::vector & prefixes, uint64_t depth) const override; + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + PreSetParametersCallbackHandle::SharedPtr + add_pre_set_parameters_callback(PreSetParametersCallbackType callback) override; + RCLCPP_PUBLIC RCUTILS_WARN_UNUSED OnSetParametersCallbackHandle::SharedPtr - add_on_set_parameters_callback(OnParametersSetCallbackType callback) override; + add_on_set_parameters_callback(OnSetParametersCallbackType callback) override; + + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + PostSetParametersCallbackHandle::SharedPtr + add_post_set_parameters_callback(PostSetParametersCallbackType callback) override; RCLCPP_PUBLIC void remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) override; + RCLCPP_PUBLIC + void + remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler) + override; + + RCLCPP_PUBLIC + void + remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler) override; + RCLCPP_PUBLIC const std::map & get_parameter_overrides() const override; - using CallbacksContainerType = std::list; + using PreSetCallbacksHandleContainer = std::list; + using OnSetCallbacksHandleContainer = std::list; + using PostSetCallbacksHandleContainer = std::list; + using CallbacksContainerType [[deprecated("use OnSetCallbacksHandleContainer instead")]] = + OnSetCallbacksHandleContainer; protected: RCLCPP_PUBLIC @@ -211,7 +234,11 @@ class NodeParameters : public NodeParametersInterface // declare_parameter, etc). In those cases, this will be set to false. bool parameter_modification_enabled_{true}; - CallbacksContainerType on_parameters_set_callback_container_; + PreSetCallbacksHandleContainer pre_set_parameters_callback_container_; + + OnSetCallbacksHandleContainer on_set_parameters_callback_container_; + + PostSetCallbacksHandleContainer post_set_parameters_callback_container_; std::map parameters_; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index 743c1b8d4f..104e00327a 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -33,16 +33,38 @@ namespace rclcpp namespace node_interfaces { +struct PreSetParametersCallbackHandle +{ + RCLCPP_SMART_PTR_DEFINITIONS(PreSetParametersCallbackHandle) + + using PreSetParametersCallbackType = + std::function &)>; + + PreSetParametersCallbackType callback; +}; + struct OnSetParametersCallbackHandle { RCLCPP_SMART_PTR_DEFINITIONS(OnSetParametersCallbackHandle) - using OnParametersSetCallbackType = + using OnSetParametersCallbackType = std::function< rcl_interfaces::msg::SetParametersResult( const std::vector &)>; + using OnParametersSetCallbackType [[deprecated("use OnSetParametersCallbackType instead")]] = + OnSetParametersCallbackType; + + OnSetParametersCallbackType callback; +}; + +struct PostSetParametersCallbackHandle +{ + RCLCPP_SMART_PTR_DEFINITIONS(PostSetParametersCallbackHandle) + + using PostSetParametersCallbackType = + std::function &)>; - OnParametersSetCallbackType callback; + PostSetParametersCallbackType callback; }; /// Pure virtual interface class for the NodeParameters part of the Node API. @@ -185,16 +207,46 @@ class NodeParametersInterface rcl_interfaces::msg::ListParametersResult list_parameters(const std::vector & prefixes, uint64_t depth) const = 0; - using OnParametersSetCallbackType = OnSetParametersCallbackHandle::OnParametersSetCallbackType; + using OnSetParametersCallbackType = OnSetParametersCallbackHandle::OnSetParametersCallbackType; + using PostSetParametersCallbackType = + PostSetParametersCallbackHandle::PostSetParametersCallbackType; + using PreSetParametersCallbackType = PreSetParametersCallbackHandle::PreSetParametersCallbackType; + + /// Add a callback that gets triggered before parameters are validated. + /** + * \sa rclcpp::Node::add_pre_set_parameters_callback + */ + RCLCPP_PUBLIC + virtual + PreSetParametersCallbackHandle::SharedPtr + add_pre_set_parameters_callback(PreSetParametersCallbackType callback) = 0; - /// Add a callback for when parameters are being set. + /// Add a callback to validate parameters before they are set. /** * \sa rclcpp::Node::add_on_set_parameters_callback */ RCLCPP_PUBLIC virtual OnSetParametersCallbackHandle::SharedPtr - add_on_set_parameters_callback(OnParametersSetCallbackType callback) = 0; + add_on_set_parameters_callback(OnSetParametersCallbackType callback) = 0; + + /// Add a callback that gets triggered after parameters are set successfully. + /** + * \sa rclcpp::Node::add_post_set_parameters_callback + */ + RCLCPP_PUBLIC + virtual + PostSetParametersCallbackHandle::SharedPtr + add_post_set_parameters_callback(PostSetParametersCallbackType callback) = 0; + + /// Remove a callback registered with `add_pre_set_parameters_callback`. + /** + * \sa rclcpp::Node::remove_pre_set_parameters_callback + */ + RCLCPP_PUBLIC + virtual + void + remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler) = 0; /// Remove a callback registered with `add_on_set_parameters_callback`. /** @@ -205,6 +257,15 @@ class NodeParametersInterface void remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) = 0; + /// Remove a callback registered with `add_post_set_parameters_callback`. + /** + * \sa rclcpp::Node::remove_post_set_parameters_callback + */ + RCLCPP_PUBLIC + virtual + void + remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler) = 0; + /// Return the initial parameter values used by the NodeParameters to override default values. RCLCPP_PUBLIC virtual diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 8107458f43..7dba3e51ad 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -353,7 +353,7 @@ Node::has_parameter(const std::string & name) const rcl_interfaces::msg::SetParametersResult Node::set_parameter(const rclcpp::Parameter & parameter) { - return this->set_parameters_atomically({parameter}); + return node_parameters_->set_parameters_atomically({parameter}); } std::vector @@ -418,16 +418,40 @@ Node::list_parameters(const std::vector & prefixes, uint64_t depth) return node_parameters_->list_parameters(prefixes, depth); } +rclcpp::Node::PreSetParametersCallbackHandle::SharedPtr +Node::add_pre_set_parameters_callback(PreSetParametersCallbackType callback) +{ + return node_parameters_->add_pre_set_parameters_callback(callback); +} + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr -Node::add_on_set_parameters_callback(OnParametersSetCallbackType callback) +Node::add_on_set_parameters_callback(OnSetParametersCallbackType callback) { return node_parameters_->add_on_set_parameters_callback(callback); } +rclcpp::Node::PostSetParametersCallbackHandle::SharedPtr +Node::add_post_set_parameters_callback(PostSetParametersCallbackType callback) +{ + return node_parameters_->add_post_set_parameters_callback(callback); +} + +void +Node::remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler) +{ + node_parameters_->remove_pre_set_parameters_callback(handler); +} + +void +Node::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) +{ + node_parameters_->remove_on_set_parameters_callback(handler); +} + void -Node::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const callback) +Node::remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler) { - return node_parameters_->remove_on_set_parameters_callback(callback); + node_parameters_->remove_post_set_parameters_callback(handler); } std::vector diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 0f366792b7..c17eb887b7 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -305,18 +305,54 @@ __check_parameters( return result; } -using OnParametersSetCallbackType = - rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; -using CallbacksContainerType = - rclcpp::node_interfaces::NodeParameters::CallbacksContainerType; +using PreSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::PreSetParametersCallbackType; +using PreSetParametersCallbackHandle = + rclcpp::node_interfaces::PreSetParametersCallbackHandle; +using PreSetCallbacksHandleContainer = + rclcpp::node_interfaces::NodeParameters::PreSetCallbacksHandleContainer; + +using OnSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType; using OnSetParametersCallbackHandle = rclcpp::node_interfaces::OnSetParametersCallbackHandle; +using OnSetCallbacksHandleContainer = + rclcpp::node_interfaces::NodeParameters::OnSetCallbacksHandleContainer; + +using PostSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::PostSetParametersCallbackType; +using PostSetParametersCallbackHandle = + rclcpp::node_interfaces::PostSetParametersCallbackHandle; +using PostSetCallbacksHandleContainer = + rclcpp::node_interfaces::NodeParameters::PostSetCallbacksHandleContainer; + +RCLCPP_LOCAL +void +__call_pre_set_parameters_callbacks( + std::vector & parameters, + PreSetCallbacksHandleContainer & callback_container) +{ + if (callback_container.empty()) { + return; + } + + auto it = callback_container.begin(); + while (it != callback_container.end()) { + auto shared_handle = it->lock(); + if (nullptr != shared_handle) { + shared_handle->callback(parameters); + it++; + } else { + it = callback_container.erase(it); + } + } +} RCLCPP_LOCAL rcl_interfaces::msg::SetParametersResult -__call_on_parameters_set_callbacks( +__call_on_set_parameters_callbacks( const std::vector & parameters, - CallbacksContainerType & callback_container) + OnSetCallbacksHandleContainer & callback_container) { rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -336,12 +372,34 @@ __call_on_parameters_set_callbacks( return result; } +RCLCPP_LOCAL +void __call_post_set_parameters_callbacks( + const std::vector & parameters, + PostSetCallbacksHandleContainer & callback_container) +{ + if (callback_container.empty()) { + return; + } + + auto it = callback_container.begin(); + while (it != callback_container.end()) { + auto shared_handle = it->lock(); + if (nullptr != shared_handle) { + shared_handle->callback(parameters); + it++; + } else { + it = callback_container.erase(it); + } + } +} + RCLCPP_LOCAL rcl_interfaces::msg::SetParametersResult __set_parameters_atomically_common( const std::vector & parameters, std::map & parameter_infos, - CallbacksContainerType & callback_container, + OnSetCallbacksHandleContainer & on_set_callback_container, + PostSetCallbacksHandleContainer & post_set_callback_container, bool allow_undeclared = false) { // Check if the value being set complies with the descriptor. @@ -352,7 +410,7 @@ __set_parameters_atomically_common( } // Call the user callbacks to see if the new value(s) are allowed. result = - __call_on_parameters_set_callbacks(parameters, callback_container); + __call_on_set_parameters_callbacks(parameters, on_set_callback_container); if (!result.successful) { return result; } @@ -364,6 +422,8 @@ __set_parameters_atomically_common( parameter_infos[name].descriptor.type = parameters[i].get_type(); parameter_infos[name].value = parameters[i].get_parameter_value(); } + // Call the user post set parameter callback + __call_post_set_parameters_callbacks(parameters, post_set_callback_container); } // Either way, return the result. @@ -378,7 +438,8 @@ __declare_parameter_common( const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, std::map & parameters_out, const std::map & overrides, - CallbacksContainerType & callback_container, + OnSetCallbacksHandleContainer & on_set_callback_container, + PostSetCallbacksHandleContainer & post_set_callback_container, rcl_interfaces::msg::ParameterEvent * parameter_event_out, bool ignore_override = false) { @@ -414,7 +475,9 @@ __declare_parameter_common( auto result = __set_parameters_atomically_common( parameter_wrappers, parameter_infos, - callback_container); + on_set_callback_container, + post_set_callback_container + ); if (!result.successful) { return result; @@ -441,7 +504,8 @@ declare_parameter_helper( bool ignore_override, std::map & parameters, const std::map & overrides, - CallbacksContainerType & callback_container, + OnSetCallbacksHandleContainer & on_set_callback_container, + PostSetCallbacksHandleContainer & post_set_callback_container, rclcpp::Publisher * events_publisher, const std::string & combined_name, rclcpp::node_interfaces::NodeClockInterface & node_clock) @@ -477,7 +541,8 @@ declare_parameter_helper( parameter_descriptor, parameters, overrides, - callback_container, + on_set_callback_container, + post_set_callback_container, ¶meter_event, ignore_override); @@ -524,7 +589,8 @@ NodeParameters::declare_parameter( ignore_override, parameters_, parameter_overrides_, - on_parameters_set_callback_container_, + on_set_parameters_callback_container_, + post_set_parameters_callback_container_, events_publisher_.get(), combined_name_, *node_clock_); @@ -559,7 +625,8 @@ NodeParameters::declare_parameter( ignore_override, parameters_, parameter_overrides_, - on_parameters_set_callback_container_, + on_set_parameters_callback_container_, + post_set_parameters_callback_container_, events_publisher_.get(), combined_name_, *node_clock_); @@ -633,12 +700,27 @@ NodeParameters::set_parameters_atomically(const std::vector & rcl_interfaces::msg::SetParametersResult result; + // call any user registered pre set parameter callbacks + // this callback can make changes to the original parameters list + // also check if the changed parameter list is empty or not, if empty return + std::vector parameters_after_pre_set_callback(parameters); + __call_pre_set_parameters_callbacks( + parameters_after_pre_set_callback, + pre_set_parameters_callback_container_); + + if (parameters_after_pre_set_callback.empty()) { + result.successful = false; + result.reason = "parameter list cannot be empty, this might be due to " + "pre_set_parameters_callback modifying the original parameters list."; + return result; + } + // Check if any of the parameters are read-only, or if any parameters are not // declared. // If not declared, keep track of them in order to declare them later, when // undeclared parameters are allowed, and if they're not allowed, fail. std::vector parameters_to_be_declared; - for (const auto & parameter : parameters) { + for (const auto & parameter : parameters_after_pre_set_callback) { const std::string & name = parameter.get_name(); // Check to make sure the parameter name is valid. @@ -678,7 +760,8 @@ NodeParameters::set_parameters_atomically(const std::vector & std::map staged_parameter_changes; rcl_interfaces::msg::ParameterEvent parameter_event_msg; parameter_event_msg.node = combined_name_; - CallbacksContainerType empty_callback_container; + OnSetCallbacksHandleContainer empty_on_set_callback_container; + PostSetCallbacksHandleContainer empty_post_set_callback_container; // Implicit declare uses dynamic type descriptor. rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -693,7 +776,8 @@ NodeParameters::set_parameters_atomically(const std::vector & staged_parameter_changes, parameter_overrides_, // Only call callbacks once below - empty_callback_container, // callback_container is explicitly empty + empty_on_set_callback_container, // callback_container is explicitly empty + empty_post_set_callback_container, // callback_container is explicitly empty ¶meter_event_msg, true); if (!result.successful) { @@ -706,12 +790,14 @@ NodeParameters::set_parameters_atomically(const std::vector & // If there were implicitly declared parameters, then we may need to copy the input parameters // and then assign the value that was selected after the declare (could be affected by the // initial parameter values). - const std::vector * parameters_to_be_set = ¶meters; + const std::vector * parameters_to_be_set = ¶meters_after_pre_set_callback; std::vector parameters_copy; if (0 != staged_parameter_changes.size()) { // If there were any implicitly declared parameters. bool any_initial_values_used = false; for (const auto & staged_parameter_change : staged_parameter_changes) { - auto it = __find_parameter_by_name(parameters, staged_parameter_change.first); + auto it = __find_parameter_by_name( + parameters_after_pre_set_callback, + staged_parameter_change.first); if (it->get_parameter_value() != staged_parameter_change.second.value) { // In this case, the value of the staged parameter differs from the // input from the user, and therefore we need to update things before setting. @@ -721,7 +807,7 @@ NodeParameters::set_parameters_atomically(const std::vector & } } if (any_initial_values_used) { - parameters_copy = parameters; + parameters_copy = parameters_after_pre_set_callback; for (const auto & staged_parameter_change : staged_parameter_changes) { auto it = __find_parameter_by_name(parameters_copy, staged_parameter_change.first); *it = Parameter(staged_parameter_change.first, staged_parameter_change.second.value); @@ -754,8 +840,9 @@ NodeParameters::set_parameters_atomically(const std::vector & // they are actually set on the official parameter storage parameters_, // These callbacks are called once. When a callback returns an unsuccessful result, - // the remaining aren't called. - on_parameters_set_callback_container_, + // the remaining aren't called + on_set_parameters_callback_container_, + post_set_parameters_callback_container_, allow_undeclared_); // allow undeclared // If not successful, then stop here. @@ -811,7 +898,6 @@ NodeParameters::set_parameters_atomically(const std::vector & parameter_event_msg.stamp = node_clock_->get_clock()->now(); events_publisher_->publish(parameter_event_msg); } - return result; } @@ -997,6 +1083,26 @@ NodeParameters::list_parameters(const std::vector & prefixes, uint6 return result; } +void +NodeParameters::remove_pre_set_parameters_callback( + const PreSetParametersCallbackHandle * const handle) +{ + std::lock_guard lock(mutex_); + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto it = std::find_if( + pre_set_parameters_callback_container_.begin(), + pre_set_parameters_callback_container_.end(), + [handle](const auto & weak_handle) { + return handle == weak_handle.lock().get(); + }); + if (it != pre_set_parameters_callback_container_.end()) { + pre_set_parameters_callback_container_.erase(it); + } else { + throw std::runtime_error("Pre set parameter callback doesn't exist"); + } +} + void NodeParameters::remove_on_set_parameters_callback( const OnSetParametersCallbackHandle * const handle) @@ -1005,20 +1111,53 @@ NodeParameters::remove_on_set_parameters_callback( ParameterMutationRecursionGuard guard(parameter_modification_enabled_); auto it = std::find_if( - on_parameters_set_callback_container_.begin(), - on_parameters_set_callback_container_.end(), + on_set_parameters_callback_container_.begin(), + on_set_parameters_callback_container_.end(), + [handle](const auto & weak_handle) { + return handle == weak_handle.lock().get(); + }); + if (it != on_set_parameters_callback_container_.end()) { + on_set_parameters_callback_container_.erase(it); + } else { + throw std::runtime_error("On set parameter callback doesn't exist"); + } +} + +void +NodeParameters::remove_post_set_parameters_callback( + const PostSetParametersCallbackHandle * const handle) +{ + std::lock_guard lock(mutex_); + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto it = std::find_if( + post_set_parameters_callback_container_.begin(), + post_set_parameters_callback_container_.end(), [handle](const auto & weak_handle) { return handle == weak_handle.lock().get(); }); - if (it != on_parameters_set_callback_container_.end()) { - on_parameters_set_callback_container_.erase(it); + if (it != post_set_parameters_callback_container_.end()) { + post_set_parameters_callback_container_.erase(it); } else { - throw std::runtime_error("Callback doesn't exist"); + throw std::runtime_error("Post set parameter callback doesn't exist"); } } +PreSetParametersCallbackHandle::SharedPtr +NodeParameters::add_pre_set_parameters_callback(PreSetParametersCallbackType callback) +{ + std::lock_guard lock(mutex_); + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto handle = std::make_shared(); + handle->callback = callback; + // the last callback registered is executed first. + pre_set_parameters_callback_container_.emplace_front(handle); + return handle; +} + OnSetParametersCallbackHandle::SharedPtr -NodeParameters::add_on_set_parameters_callback(OnParametersSetCallbackType callback) +NodeParameters::add_on_set_parameters_callback(OnSetParametersCallbackType callback) { std::lock_guard lock(mutex_); ParameterMutationRecursionGuard guard(parameter_modification_enabled_); @@ -1026,7 +1165,21 @@ NodeParameters::add_on_set_parameters_callback(OnParametersSetCallbackType callb auto handle = std::make_shared(); handle->callback = callback; // the last callback registered is executed first. - on_parameters_set_callback_container_.emplace_front(handle); + on_set_parameters_callback_container_.emplace_front(handle); + return handle; +} + +PostSetParametersCallbackHandle::SharedPtr +NodeParameters::add_post_set_parameters_callback( + PostSetParametersCallbackType callback) +{ + std::lock_guard lock(mutex_); + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto handle = std::make_shared(); + handle->callback = callback; + // the last callback registered is executed first. + post_set_parameters_callback_container_.emplace_front(handle); return handle; } diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp index 97e3a3188d..58bf010767 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp @@ -175,7 +175,7 @@ TEST_F(TestNodeParameters, set_parameters) { EXPECT_TRUE(result[0].successful); } -TEST_F(TestNodeParameters, add_remove_parameters_callback) { +TEST_F(TestNodeParameters, add_remove_on_set_parameters_callback) { rcl_interfaces::msg::ParameterDescriptor bool_descriptor; bool_descriptor.name = "bool_parameter"; bool_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; @@ -202,7 +202,123 @@ TEST_F(TestNodeParameters, add_remove_parameters_callback) { RCLCPP_EXPECT_THROW_EQ( node_parameters->remove_on_set_parameters_callback(handle.get()), - std::runtime_error("Callback doesn't exist")); + std::runtime_error("On set parameter callback doesn't exist")); +} + +TEST_F(TestNodeParameters, add_remove_pre_set_parameters_callback) { + // `add_pre_set_parameters_callback` used to modify parameters list. + auto modify_parameter_list_callback = [](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == "param1") { + parameters.emplace_back("param2", 2.0); + } + } + }; + + // `add_pre_set_parameters_callback` used to make the parameters list empty. + auto empty_parameter_list_callback = [](std::vector & parameters) { + parameters = {}; + }; + + auto handle1 = + node_parameters->add_pre_set_parameters_callback(modify_parameter_list_callback); + + double default_value = 0.0; + node_parameters->declare_parameter( + "param1", rclcpp::ParameterValue(default_value)); + node_parameters->declare_parameter( + "param2", rclcpp::ParameterValue(default_value)); + + // verify that `declare_parameter` does not call any of the callbacks registered with + // `add_pre_set_parameters_callback` + EXPECT_TRUE(node_parameters->has_parameter("param1")); + EXPECT_EQ(node_parameters->get_parameter("param1").get_value(), default_value); + EXPECT_TRUE(node_parameters->has_parameter("param2")); + EXPECT_EQ(node_parameters->get_parameter("param2").get_value(), default_value); + + // verify that the `param2` was set successfully conditioned on setting of + // `param1` + const std::vector parameters_to_be_set = { + rclcpp::Parameter("param1", 1.0)}; + auto result = node_parameters->set_parameters(parameters_to_be_set); + // we expect the result size to be same as the original "parameters_to_be_set" + // since the pre set parameter callback will set the modified param list atomically. + ASSERT_EQ(1u, result.size()); + EXPECT_TRUE(result[0].successful); + EXPECT_TRUE(node_parameters->has_parameter("param1")); + EXPECT_EQ(node_parameters->get_parameter("param1").get_value(), 1.0); + EXPECT_TRUE(node_parameters->has_parameter("param2")); + EXPECT_EQ(node_parameters->get_parameter("param2").get_value(), 2.0); + EXPECT_NO_THROW(node_parameters->remove_pre_set_parameters_callback(handle1.get())); + RCLCPP_EXPECT_THROW_EQ( + node_parameters->remove_pre_set_parameters_callback(handle1.get()), + std::runtime_error("Pre set parameter callback doesn't exist")); + + // verify that the result should be unsuccessful if the pre set callback makes + // parameter list empty + auto handle2 = + node_parameters->add_pre_set_parameters_callback(empty_parameter_list_callback); + auto results = node_parameters->set_parameters(parameters_to_be_set); + + std::string reason = "parameter list cannot be empty, this might be due to " + "pre_set_parameters_callback modifying the original parameters list."; + EXPECT_FALSE(results[0].successful); + EXPECT_EQ(results[0].reason, reason); + EXPECT_NO_THROW(node_parameters->remove_pre_set_parameters_callback(handle2.get())); + RCLCPP_EXPECT_THROW_EQ( + node_parameters->remove_pre_set_parameters_callback(handle2.get()), + std::runtime_error("Pre set parameter callback doesn't exist")); +} + +TEST_F(TestNodeParameters, add_remove_post_set_parameters_callback) { + rcl_interfaces::msg::ParameterDescriptor param1_descriptor; + param1_descriptor.name = "double_parameter1"; + param1_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + param1_descriptor.read_only = false; + + rcl_interfaces::msg::ParameterDescriptor param2_descriptor; + param2_descriptor.name = "double_parameter2"; + param2_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + param2_descriptor.read_only = false; + + double variable_tracking_param1_internally = node_parameters->declare_parameter( + "param1", rclcpp::ParameterValue(0.0), param1_descriptor, false).get(); + double variable_tracking_param2_internally = node_parameters->declare_parameter( + "param2", rclcpp::ParameterValue(0.0), param2_descriptor, false).get(); + + EXPECT_EQ(variable_tracking_param1_internally, 0.0); + EXPECT_EQ(variable_tracking_param2_internally, 0.0); + + const std::vector parameters_to_be_set = { + rclcpp::Parameter("param1", 1.0), + rclcpp::Parameter("param2", 2.0)}; + + // register a callback for successful set parameter and change the internally tracked variables. + auto callback = [&](const std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == "param1") { + variable_tracking_param1_internally = param.get_value(); + } else if (param.get_name() == "param2") { + variable_tracking_param2_internally = param.get_value(); + } + } + }; + + auto handle = node_parameters->add_post_set_parameters_callback(callback); + auto result = node_parameters->set_parameters(parameters_to_be_set); + ASSERT_EQ(2u, result.size()); + EXPECT_TRUE(result[0].successful); + EXPECT_TRUE(result[1].successful); + EXPECT_TRUE(node_parameters->has_parameter("param1")); + EXPECT_TRUE(node_parameters->has_parameter("param2")); + EXPECT_EQ(variable_tracking_param1_internally, 1.0); + EXPECT_EQ(variable_tracking_param2_internally, 2.0); + + EXPECT_NO_THROW(node_parameters->remove_post_set_parameters_callback(handle.get())); + + RCLCPP_EXPECT_THROW_EQ( + node_parameters->remove_post_set_parameters_callback(handle.get()), + std::runtime_error("Post set parameter callback doesn't exist")); } TEST_F(TestNodeParameters, wildcard_with_namespace) diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index 58af342561..c40811a4e4 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -1444,6 +1444,36 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) { EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_STRING); EXPECT_EQ(value.get_value(), "asd"); } + { + // adding a parameter in "pre set parameter" callback, when that + // parameter has not been declared before will throw + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto default_value = 0; // default value of name1 param + + // declare name1 parameter only + node->declare_parameter(name1, default_value); + + // add undeclared parameter with name2 to modified list of parameters + auto pre_set_parameters = + [&](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == name1) { + parameters.emplace_back(rclcpp::Parameter(name2, 2)); + } + } + }; + + auto handler = node->add_pre_set_parameters_callback(pre_set_parameters); + EXPECT_THROW( + node->set_parameter(rclcpp::Parameter(name1, 4)), + rclcpp::exceptions::ParameterNotDeclaredException); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_EQ(node->get_parameter(name1).get_value(), default_value); + EXPECT_FALSE(node->has_parameter(name2)); + RCPPUTILS_SCOPE_EXIT( + {node->remove_pre_set_parameters_callback(handler.get());}); // always reset + } } TEST_F(TestNode, set_parameter_undeclared_parameters_allowed) { @@ -1481,6 +1511,36 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_allowed) { EXPECT_TRUE(node->has_parameter(name)); EXPECT_EQ(node->get_parameter(name).get_value(), 43); } + { + // adding a parameter in "pre set parameter" callback, when that + // parameter has not been declared will not throw if undeclared + // parameters are allowed + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + // declare name1 parameter only + node->declare_parameter(name1, 0); + + // add undeclared parameter with name2 to modified list of parameters + auto pre_set_parameters = + [&](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == name1) { + parameters.emplace_back(rclcpp::Parameter(name2, 2)); + } + } + }; + + auto handler = node->add_pre_set_parameters_callback(pre_set_parameters); + auto result = node->set_parameter(rclcpp::Parameter(name1, 1)); + EXPECT_TRUE(result.successful); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_EQ(node->get_parameter(name1).get_value(), 1); + EXPECT_EQ(node->get_parameter(name2).get_value(), 2); + RCPPUTILS_SCOPE_EXIT( + {node->remove_pre_set_parameters_callback(handler.get());}); // always reset + } } TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) { @@ -1625,6 +1685,51 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) { EXPECT_FALSE(node->has_parameter(name)); } + { + // adding a parameter in "pre set parameter" callback when that + // parameter has not been declared before will throw. However, when + // multiple params are being set using "set_parameters", the params + // which are not conditioned on each other in "pre set callback" will + // still be set successfully. This is the desired behaviour since + // "set_parameters" sets params non atomically. + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto default_value = 0; + + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rclcpp::PARAMETER_INTEGER; + + // declare name1 and name2 parameter only + node->declare_parameter(name1, default_value, descriptor); + node->declare_parameter(name2, default_value, descriptor); + + // add undeclared parameter with name3 to modified list of parameters + // conditioned of name2 param + auto pre_set_parameters = + [&](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == name2) { + parameters.emplace_back(rclcpp::Parameter(name3, 3)); + } + } + }; + + auto handler = node->add_pre_set_parameters_callback(pre_set_parameters); + EXPECT_THROW( + node->set_parameters({rclcpp::Parameter(name1, 1), rclcpp::Parameter(name2, 2)}), + rclcpp::exceptions::ParameterNotDeclaredException); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_FALSE(node->has_parameter(name3)); + + // we still expect the value of name1 param to be set successfully, since + // the setting of name2 param is only conditioned on setting of name3 param + EXPECT_EQ(node->get_parameter(name1).get_value(), 1); + EXPECT_EQ(node->get_parameter(name2).get_value(), default_value); + RCPPUTILS_SCOPE_EXIT( + {node->remove_pre_set_parameters_callback(handler.get());}); // always reset + } } // test set_parameters with undeclared allowed @@ -1673,6 +1778,48 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_allowed) { EXPECT_EQ(node->get_parameter(name1).get_value(), 42); EXPECT_EQ(node->get_parameter(name2).get_value(), "test"); } + { + // adding a parameter in "pre set parameter" callback when that + // parameter has not been declared before will not throw when + // undeclared parameters are allowed. + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto default_value = 0; + + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rclcpp::PARAMETER_INTEGER; + + // declare name1 and name2 parameter only + node->declare_parameter(name1, default_value, descriptor); + node->declare_parameter(name2, default_value, descriptor); + + // add undeclared parameter with name3 to modified list of parameters + // conditioned of name2 param + auto pre_set_parameters = + [&](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == name2) { + parameters.emplace_back(rclcpp::Parameter(name3, 3)); + } + } + }; + + auto handler = node->add_pre_set_parameters_callback(pre_set_parameters); + auto results = node->set_parameters({rclcpp::Parameter(name1, 1), rclcpp::Parameter(name2, 2)}); + EXPECT_EQ(2u, results.size()); + EXPECT_TRUE(results[0].successful); + EXPECT_TRUE(results[1].successful); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + EXPECT_EQ(node->get_parameter(name1).get_value(), 1); + EXPECT_EQ(node->get_parameter(name2).get_value(), 2); + EXPECT_EQ(node->get_parameter(name3).get_value(), 3); + + RCPPUTILS_SCOPE_EXIT( + {node->remove_pre_set_parameters_callback(handler.get());}); // always reset + } } TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) { @@ -1815,6 +1962,48 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) { EXPECT_FALSE(node->has_parameter(name)); } + { + // adding a parameter in "pre set parameter" callback when that + // parameter has not been declared before will throw and since + // multiple params are being set using "set_parameters_atomically", + // a failure in set of one param will result in all params being + // set unsuccessfully. + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto default_value = 0; + + // declare name1 and name2 parameter only + node->declare_parameter(name1, default_value); + node->declare_parameter(name2, default_value); + + // add undeclared parameter with name3 to modified list of parameters + // conditioned of name2 param + auto pre_set_parameters = + [&](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == name2) { + parameters.emplace_back(rclcpp::Parameter(name3, 3)); + } + } + }; + + auto handler = node->add_pre_set_parameters_callback(pre_set_parameters); + EXPECT_THROW( + node->set_parameters_atomically( + {rclcpp::Parameter(name1, 1), + rclcpp::Parameter(name2, 2)}), + rclcpp::exceptions::ParameterNotDeclaredException); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_FALSE(node->has_parameter(name3)); + + // the values of all the params is still default. + EXPECT_EQ(node->get_parameter(name1).get_value(), default_value); + EXPECT_EQ(node->get_parameter(name2).get_value(), default_value); + RCPPUTILS_SCOPE_EXIT( + {node->remove_pre_set_parameters_callback(handler.get());}); // always reset + } } // test set_parameters with undeclared allowed @@ -1903,6 +2092,45 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_allowed) { EXPECT_FALSE(node->has_parameter(name2)); // important! name2 remains undeclared EXPECT_EQ(node->get_parameter(name3).get_value(), "test"); } + { + // adding a parameter in "pre set parameter" callback when that + // parameter has not been declared before will not throw when + // undeclared parameters are allowed. + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto default_value = 0; + + // declare name1 and name2 parameter only + node->declare_parameter(name1, default_value); + node->declare_parameter(name2, default_value); + + // add undeclared parameter with name3 to modified list of parameters + // conditioned of name2 param + auto pre_set_parameters = + [&](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == name2) { + parameters.emplace_back(rclcpp::Parameter(name3, 3)); + } + } + }; + + auto handler = node->add_pre_set_parameters_callback(pre_set_parameters); + auto result = node->set_parameters_atomically( + {rclcpp::Parameter(name1, 1), + rclcpp::Parameter(name2, 2)}); + EXPECT_TRUE(result.successful); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + EXPECT_EQ(node->get_parameter(name1).get_value(), 1); + EXPECT_EQ(node->get_parameter(name2).get_value(), 2); + EXPECT_EQ(node->get_parameter(name3).get_value(), 3); + + RCPPUTILS_SCOPE_EXIT( + {node->remove_pre_set_parameters_callback(handler.get());}); // always reset + } } // test get_parameter with undeclared not allowed diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 2978a5ecaa..84c62b9e6a 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -538,10 +538,32 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rcl_interfaces::msg::ListParametersResult list_parameters(const std::vector & prefixes, uint64_t depth) const; + using PreSetParametersCallbackHandle = + rclcpp::node_interfaces::PreSetParametersCallbackHandle; + using PreSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::PreSetParametersCallbackType; + using OnSetParametersCallbackHandle = rclcpp::node_interfaces::OnSetParametersCallbackHandle; - using OnParametersSetCallbackType = - rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; + using OnSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType; + using OnParametersSetCallbackType [[deprecated("use OnSetParametersCallbackType instead")]] = + OnSetParametersCallbackType; + + using PostSetParametersCallbackHandle = + rclcpp::node_interfaces::PostSetParametersCallbackHandle; + using PostSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::PostSetParametersCallbackType; + + /// Add a callback that gets triggered before parameters are validated. + /** + * \sa rclcpp::Node::add_pre_set_parameters_callback + */ + RCLCPP_LIFECYCLE_PUBLIC + RCUTILS_WARN_UNUSED + rclcpp_lifecycle::LifecycleNode::PreSetParametersCallbackHandle::SharedPtr + add_pre_set_parameters_callback( + rclcpp_lifecycle::LifecycleNode::PreSetParametersCallbackType callback); /// Add a callback for when parameters are being set. /** @@ -551,7 +573,26 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, RCUTILS_WARN_UNUSED rclcpp_lifecycle::LifecycleNode::OnSetParametersCallbackHandle::SharedPtr add_on_set_parameters_callback( - rclcpp_lifecycle::LifecycleNode::OnParametersSetCallbackType callback); + rclcpp_lifecycle::LifecycleNode::OnSetParametersCallbackType callback); + + /// Add a callback that gets triggered after parameters are set successfully. + /** + * \sa rclcpp::Node::add_post_set_parameters_callback + */ + RCLCPP_LIFECYCLE_PUBLIC + RCUTILS_WARN_UNUSED + rclcpp_lifecycle::LifecycleNode::PostSetParametersCallbackHandle::SharedPtr + add_post_set_parameters_callback( + rclcpp_lifecycle::LifecycleNode::PostSetParametersCallbackType callback); + + /// Remove a callback registered with `add_pre_set_parameters_callback`. + /** + * \sa rclcpp::Node::remove_pre_set_parameters_callback + */ + RCLCPP_LIFECYCLE_PUBLIC + void + remove_pre_set_parameters_callback( + const rclcpp_lifecycle::LifecycleNode::PreSetParametersCallbackHandle * const handler); /// Remove a callback registered with `add_on_set_parameters_callback`. /** @@ -562,6 +603,15 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, remove_on_set_parameters_callback( const rclcpp_lifecycle::LifecycleNode::OnSetParametersCallbackHandle * const handler); + /// Remove a callback registered with `add_post_set_parameters_callback`. + /** + * \sa rclcpp::Node::remove_post_set_parameters_callback + */ + RCLCPP_LIFECYCLE_PUBLIC + void + remove_post_set_parameters_callback( + const rclcpp_lifecycle::LifecycleNode::PostSetParametersCallbackHandle * const handler); + /// Return a vector of existing node names (string). /** * \sa rclcpp::Node::get_node_names diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 06378b594b..0851c596ec 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -283,12 +283,31 @@ LifecycleNode::list_parameters( return node_parameters_->list_parameters(prefixes, depth); } +rclcpp::Node::PreSetParametersCallbackHandle::SharedPtr +LifecycleNode::add_pre_set_parameters_callback(PreSetParametersCallbackType callback) +{ + return node_parameters_->add_pre_set_parameters_callback(callback); +} + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr -LifecycleNode::add_on_set_parameters_callback(OnParametersSetCallbackType callback) +LifecycleNode::add_on_set_parameters_callback(OnSetParametersCallbackType callback) { return node_parameters_->add_on_set_parameters_callback(callback); } +rclcpp::Node::PostSetParametersCallbackHandle::SharedPtr +LifecycleNode::add_post_set_parameters_callback(PostSetParametersCallbackType callback) +{ + return node_parameters_->add_post_set_parameters_callback(callback); +} + +void +LifecycleNode::remove_pre_set_parameters_callback( + const PreSetParametersCallbackHandle * const callback) +{ + node_parameters_->remove_pre_set_parameters_callback(callback); +} + void LifecycleNode::remove_on_set_parameters_callback( const OnSetParametersCallbackHandle * const callback) @@ -296,6 +315,13 @@ LifecycleNode::remove_on_set_parameters_callback( node_parameters_->remove_on_set_parameters_callback(callback); } +void +LifecycleNode::remove_post_set_parameters_callback( + const PostSetParametersCallbackHandle * const callback) +{ + node_parameters_->remove_post_set_parameters_callback(callback); +} + std::vector LifecycleNode::get_node_names() const { From 3d69031d2a614cf02dc10ea5db153471ea32b1f2 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 18 Aug 2022 12:30:51 +0800 Subject: [PATCH 127/455] fix memory leak (#1994) Signed-off-by: Chen Lihui --- rclcpp/src/rclcpp/parameter_map.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index 8f7bc0655c..cb458e7db3 100644 --- a/rclcpp/src/rclcpp/parameter_map.cpp +++ b/rclcpp/src/rclcpp/parameter_map.cpp @@ -17,6 +17,7 @@ #include #include "rcpputils/find_and_replace.hpp" +#include "rcpputils/scope_exit.hpp" #include "rclcpp/parameter_map.hpp" using rclcpp::exceptions::InvalidParametersException; @@ -146,9 +147,11 @@ rclcpp::parameter_map_from_yaml_file(const std::string & yaml_filename) { rcutils_allocator_t allocator = rcutils_get_default_allocator(); rcl_params_t * rcl_parameters = rcl_yaml_node_struct_init(allocator); + RCPPUTILS_SCOPE_EXIT(rcl_yaml_node_struct_fini(rcl_parameters); ); const char * path = yaml_filename.c_str(); if (!rcl_parse_yaml_file(path, rcl_parameters)) { rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR); } + return rclcpp::parameter_map_from(rcl_parameters); } From dab9f5e0a331b9694c4b5aceb8263d49aa052ecb Mon Sep 17 00:00:00 2001 From: Brian Date: Mon, 29 Aug 2022 12:14:09 -0700 Subject: [PATCH 128/455] [docs] add note about callback lifetime for {on, post}_set_parameter_callback (#1981) [docs] add note about {on, post, pre}_set_parameter_callback lifetime Signed-off-by: Brian Chen --- rclcpp/include/rclcpp/node.hpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index bd5d39f6bb..35162de03b 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -991,6 +991,9 @@ class Node : public std::enable_shared_from_this * One of the use case of "pre set callback" can be updating additional parameters * conditioned on changes to a parameter. * + * Users should retain a copy of the returned shared pointer, as the callback + * is valid only as long as the smart pointer is alive. + * * For an example callback: * *```cpp @@ -1039,6 +1042,9 @@ class Node : public std::enable_shared_from_this * rcl_interfaces::msg::SetParametersResult to indicate whether or not the * parameter should be set or not, and if not why. * + * Users should retain a copy of the returned shared pointer, as the callback + * is valid only as long as the smart pointer is alive. + * * For an example callback: * * ```cpp @@ -1107,6 +1113,9 @@ class Node : public std::enable_shared_from_this * The callback is called when any of the `set_parameter*` or `declare_parameter*` * methods are successful. * + * Users should retain a copy of the returned shared pointer, as the callback + * is valid only as long as the smart pointer is alive. + * * The callback takes a reference to a const vector of parameters that have been * set successfully. * From 6167a575b3eb85c90240e20736f4e5823ac4c681 Mon Sep 17 00:00:00 2001 From: Andrew Symington Date: Tue, 30 Aug 2022 09:10:11 -0400 Subject: [PATCH 129/455] Add a `create_timer` method to `Node` and `LifecycleNode` classes (#1975) Signed-off-by: Andrew Symington --- rclcpp/include/rclcpp/create_timer.hpp | 155 +++++++++++++----- rclcpp/include/rclcpp/node.hpp | 15 +- rclcpp/include/rclcpp/node_impl.hpp | 16 ++ rclcpp/test/rclcpp/test_create_timer.cpp | 62 ++++++- rclcpp/test/rclcpp/test_time_source.cpp | 69 ++++---- rclcpp/test/rclcpp/test_timer.cpp | 105 +++++++++--- .../rclcpp_lifecycle/lifecycle_node.hpp | 15 +- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 27 ++- .../test/test_lifecycle_publisher.cpp | 49 +++++- 9 files changed, 394 insertions(+), 119 deletions(-) diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index 345f43fcbb..5225f0fde0 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -23,12 +23,63 @@ #include "rclcpp/duration.hpp" #include "rclcpp/node_interfaces/get_node_base_interface.hpp" +#include "rclcpp/node_interfaces/get_node_clock_interface.hpp" #include "rclcpp/node_interfaces/get_node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" namespace rclcpp { +namespace detail +{ +/// Perform a safe cast to a timer period in nanoseconds +/** + * + * \tparam DurationRepT + * \tparam DurationT + * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() + * \return period, expressed as chrono::duration::nanoseconds + * \throws std::invalid_argument if period is negative or too large + */ +template +std::chrono::nanoseconds +safe_cast_to_period_in_ns(std::chrono::duration period) +{ + if (period < std::chrono::duration::zero()) { + throw std::invalid_argument{"timer period cannot be negative"}; + } + + // Casting to a double representation might lose precision and allow the check below to succeed + // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max. + constexpr auto maximum_safe_cast_ns = + std::chrono::nanoseconds::max() - std::chrono::duration(1); + + // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow + // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is + // greater than nanoseconds::max() is a difficult general problem. This is a more conservative + // version of Howard Hinnant's (the guy>) response here: + // https://stackoverflow.com/a/44637334/2089061 + // However, this doesn't solve the issue for all possible duration types of period. + // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177 + constexpr auto ns_max_as_double = + std::chrono::duration_cast>( + maximum_safe_cast_ns); + if (period > ns_max_as_double) { + throw std::invalid_argument{ + "timer period must be less than std::chrono::nanoseconds::max()"}; + } + + const auto period_ns = std::chrono::duration_cast(period); + if (period_ns < std::chrono::nanoseconds::zero()) { + throw std::runtime_error{ + "Casting timer period to nanoseconds resulted in integer overflow."}; + } + + return period_ns; +} +} // namespace detail + /// Create a timer with a given clock /// \internal template @@ -41,14 +92,13 @@ create_timer( CallbackT && callback, rclcpp::CallbackGroup::SharedPtr group = nullptr) { - auto timer = rclcpp::GenericTimer::make_shared( + return create_timer( + node_base.get(), + node_timers.get(), clock, period.to_chrono(), std::forward(callback), - node_base->get_context()); - - node_timers->add_timer(timer, group); - return timer; + group); } /// Create a timer with a given clock @@ -62,82 +112,99 @@ create_timer( rclcpp::CallbackGroup::SharedPtr group = nullptr) { return create_timer( - rclcpp::node_interfaces::get_node_base_interface(node), - rclcpp::node_interfaces::get_node_timers_interface(node), clock, - period, + period.to_chrono(), std::forward(callback), - group); + group, + rclcpp::node_interfaces::get_node_base_interface(node).get(), + rclcpp::node_interfaces::get_node_timers_interface(node).get()); } -/// Convenience method to create a timer with node resources. +/// Convenience method to create a general timer with node resources. /** * * \tparam DurationRepT * \tparam DurationT * \tparam CallbackT + * \param clock clock to be used * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() * \param callback callback to execute via the timer period - * \param group - * \param node_base - * \param node_timers - * \return - * \throws std::invalid argument if either node_base or node_timers - * are null, or period is negative or too large + * \param group callback group + * \param node_base node base interface + * \param node_timers node timer interface + * \return shared pointer to a generic timer + * \throws std::invalid_argument if either clock, node_base or node_timers + * are nullptr, or period is negative or too large */ template -typename rclcpp::WallTimer::SharedPtr -create_wall_timer( +typename rclcpp::GenericTimer::SharedPtr +create_timer( + rclcpp::Clock::SharedPtr clock, std::chrono::duration period, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group, node_interfaces::NodeBaseInterface * node_base, node_interfaces::NodeTimersInterface * node_timers) { + if (clock == nullptr) { + throw std::invalid_argument{"clock cannot be null"}; + } if (node_base == nullptr) { throw std::invalid_argument{"input node_base cannot be null"}; } - if (node_timers == nullptr) { throw std::invalid_argument{"input node_timers cannot be null"}; } - if (period < std::chrono::duration::zero()) { - throw std::invalid_argument{"timer period cannot be negative"}; - } + const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period); - // Casting to a double representation might lose precision and allow the check below to succeed - // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max. - constexpr auto maximum_safe_cast_ns = - std::chrono::nanoseconds::max() - std::chrono::duration(1); + // Add a new generic timer. + auto timer = rclcpp::GenericTimer::make_shared( + std::move(clock), period_ns, std::move(callback), node_base->get_context()); + node_timers->add_timer(timer, group); + return timer; +} - // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow - // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is - // greater than nanoseconds::max() is a difficult general problem. This is a more conservative - // version of Howard Hinnant's (the guy>) response here: - // https://stackoverflow.com/a/44637334/2089061 - // However, this doesn't solve the issue for all possible duration types of period. - // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177 - constexpr auto ns_max_as_double = - std::chrono::duration_cast>( - maximum_safe_cast_ns); - if (period > ns_max_as_double) { - throw std::invalid_argument{ - "timer period must be less than std::chrono::nanoseconds::max()"}; +/// Convenience method to create a wall timer with node resources. +/** + * + * \tparam DurationRepT + * \tparam DurationT + * \tparam CallbackT + * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() + * \param callback callback to execute via the timer period + * \param group callback group + * \param node_base node base interface + * \param node_timers node timer interface + * \return shared pointer to a wall timer + * \throws std::invalid_argument if either node_base or node_timers + * are null, or period is negative or too large + */ +template +typename rclcpp::WallTimer::SharedPtr +create_wall_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group, + node_interfaces::NodeBaseInterface * node_base, + node_interfaces::NodeTimersInterface * node_timers) +{ + if (node_base == nullptr) { + throw std::invalid_argument{"input node_base cannot be null"}; } - const auto period_ns = std::chrono::duration_cast(period); - if (period_ns < std::chrono::nanoseconds::zero()) { - throw std::runtime_error{ - "Casting timer period to nanoseconds resulted in integer overflow."}; + if (node_timers == nullptr) { + throw std::invalid_argument{"input node_timers cannot be null"}; } + const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period); + + // Add a new wall timer. auto timer = rclcpp::WallTimer::make_shared( period_ns, std::move(callback), node_base->get_context()); node_timers->add_timer(timer, group); return timer; } - } // namespace rclcpp #endif // RCLCPP__CREATE_TIMER_HPP_ diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 35162de03b..e514137b51 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -227,7 +227,7 @@ class Node : public std::enable_shared_from_this ) ); - /// Create a timer. + /// Create a wall timer that uses the wall clock to drive the callback. /** * \param[in] period Time interval between triggers of the callback. * \param[in] callback User-defined callback function. @@ -240,6 +240,19 @@ class Node : public std::enable_shared_from_this CallbackT callback, rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create a timer that uses the node clock to drive the callback. + /** + * \param[in] period Time interval between triggers of the callback. + * \param[in] callback User-defined callback function. + * \param[in] group Callback group to execute this timer's callback in. + */ + template + typename rclcpp::GenericTimer::SharedPtr + create_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a Client. /** * \param[in] service_name The topic to service on. diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 5b427b5b25..d6b090d4d1 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -120,6 +120,22 @@ Node::create_wall_timer( this->node_timers_.get()); } +template +typename rclcpp::GenericTimer::SharedPtr +Node::create_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_timer( + this->get_clock(), + period, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get()); +} + template typename Client::SharedPtr Node::create_client( diff --git a/rclcpp/test/rclcpp/test_create_timer.cpp b/rclcpp/test/rclcpp/test_create_timer.cpp index 13c3564544..f253af4838 100644 --- a/rclcpp/test/rclcpp/test_create_timer.cpp +++ b/rclcpp/test/rclcpp/test_create_timer.cpp @@ -62,7 +62,7 @@ TEST(TestCreateTimer, call_with_node_wrapper_compiles) rclcpp::shutdown(); } -TEST(TestCreateTimer, call_wall_timer_with_bad_arguments) +TEST(TestCreateWallTimer, call_wall_timer_with_bad_arguments) { rclcpp::init(0, nullptr); NodeWrapper node("test_create_wall_timers_with_bad_arguments"); @@ -117,6 +117,66 @@ TEST(TestCreateTimer, call_wall_timer_with_bad_arguments) rclcpp::shutdown(); } +TEST(TestCreateTimer, call_timer_with_bad_arguments) +{ + rclcpp::init(0, nullptr); + NodeWrapper node("test_create_timers_with_bad_arguments"); + auto callback = []() {}; + rclcpp::CallbackGroup::SharedPtr group = nullptr; + auto node_interface = + rclcpp::node_interfaces::get_node_base_interface(node).get(); + auto timers_interface = + rclcpp::node_interfaces::get_node_timers_interface(node).get(); + + auto clock = node.get_node_clock_interface()->get_clock(); + + // Negative period + EXPECT_THROW( + rclcpp::create_timer( + clock, -1ms, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // Very negative period + constexpr auto nanoseconds_min = std::chrono::nanoseconds::min(); + EXPECT_THROW( + rclcpp::create_timer( + clock, nanoseconds_min, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // Period must be less than nanoseconds::max() + constexpr auto nanoseconds_max = std::chrono::nanoseconds::min(); + EXPECT_THROW( + rclcpp::create_timer( + clock, nanoseconds_max, callback, group, node_interface, timers_interface), + std::invalid_argument); + + EXPECT_NO_THROW( + rclcpp::create_timer( + clock, nanoseconds_max - 1us, callback, group, node_interface, timers_interface)); + + EXPECT_NO_THROW( + rclcpp::create_timer(clock, 0ms, callback, group, node_interface, timers_interface)); + + // Period must be less than nanoseconds::max() + constexpr auto hours_max = std::chrono::hours::max(); + EXPECT_THROW( + rclcpp::create_timer( + clock, hours_max, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // node_interface is null + EXPECT_THROW( + rclcpp::create_timer(clock, 1ms, callback, group, nullptr, timers_interface), + std::invalid_argument); + + // timers_interface is null + EXPECT_THROW( + rclcpp::create_timer(clock, 1ms, callback, group, node_interface, nullptr), + std::invalid_argument); + + rclcpp::shutdown(); +} + static void test_timer_callback(void) {} TEST(TestCreateTimer, timer_function_pointer) diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index 2bf89a5b09..a5aad9a498 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -81,7 +81,7 @@ void spin_until_time( executor.spin_once(10ms); - if (clock->now().nanoseconds() == end_time.count()) { + if (clock->now().nanoseconds() >= end_time.count()) { return; } } @@ -108,7 +108,7 @@ void spin_until_ros_time_updated( executor.add_node(node); auto start = std::chrono::system_clock::now(); - while (std::chrono::system_clock::now() < (start + 1s)) { + while (std::chrono::system_clock::now() < (start + 2s)) { if (!rclcpp::ok()) { break; // Break for ctrl-c } @@ -630,7 +630,8 @@ class SimClockPublisherNode : public rclcpp::Node { public: SimClockPublisherNode() - : rclcpp::Node("sim_clock_publisher_node") + : rclcpp::Node("sim_clock_publisher_node"), + pub_time_(0, 0) { // Create a clock publisher clock_pub_ = this->create_publisher( @@ -645,10 +646,6 @@ class SimClockPublisherNode : public rclcpp::Node &SimClockPublisherNode::timer_callback, this) ); - - // Init clock msg to zero - clock_msg_.clock.sec = 0; - clock_msg_.clock.nanosec = 0; } ~SimClockPublisherNode() @@ -671,13 +668,15 @@ class SimClockPublisherNode : public rclcpp::Node private: void timer_callback() { - // Increment clock msg and publish it - clock_msg_.clock.nanosec += 1000000; + // Increment the time, update the clock msg and publish it + pub_time_ += rclcpp::Duration(0, 1000000); + clock_msg_.clock = pub_time_; clock_pub_->publish(clock_msg_); } rclcpp::Publisher::SharedPtr clock_pub_; rclcpp::TimerBase::SharedPtr pub_timer_; + rclcpp::Time pub_time_; rosgraph_msgs::msg::Clock clock_msg_; std::thread node_thread_; rclcpp::executors::SingleThreadedExecutor node_executor; @@ -695,7 +694,7 @@ class ClockThreadTestingNode : public rclcpp::Node this->set_parameter(rclcpp::Parameter("use_sim_time", true)); // Create a 100ms timer - timer_ = this->create_wall_timer( + timer_ = this->create_timer( std::chrono::milliseconds(100), std::bind( &ClockThreadTestingNode::timer_callback, @@ -735,29 +734,33 @@ class ClockThreadTestingNode : public rclcpp::Node bool is_callback_frozen_ = true; }; -TEST_F(TestTimeSource, check_sim_time_updated_in_callback_if_use_clock_thread) { - // Test if clock time of a node with - // parameter use_sim_time = true and option use_clock_thread = true - // is updated while node is not spinning - // (in a timer callback) - - // Create a "sim time" publisher and spin it - SimClockPublisherNode pub_node; - pub_node.SpinNode(); - - // Spin node for 2 seconds - ClockThreadTestingNode clock_thread_testing_node; - auto steady_clock = rclcpp::Clock(RCL_STEADY_TIME); - auto start_time = steady_clock.now(); - while (rclcpp::ok() && - (steady_clock.now() - start_time).seconds() < 2.0) - { - rclcpp::spin_some(clock_thread_testing_node.get_node_base_interface()); - } - - // Node should have get out of timer callback - ASSERT_FALSE(clock_thread_testing_node.GetIsCallbackFrozen()); -} +// TODO(ivanpauno): This test was using a wall timer, when it was supposed to use sim time. +// It was also using `use_clock_tread = false`, when it was supposed to be `true`. +// Fixing the test to work as originally intended makes it super flaky. +// Disabling it until the test is fixed. +// TEST_F(TestTimeSource, check_sim_time_updated_in_callback_if_use_clock_thread) { +// // Test if clock time of a node with +// // parameter use_sim_time = true and option use_clock_thread = true +// // is updated while node is not spinning +// // (in a timer callback) + +// // Create a "sim time" publisher and spin it +// SimClockPublisherNode pub_node; +// pub_node.SpinNode(); + +// // Spin node for 2 seconds +// ClockThreadTestingNode clock_thread_testing_node; +// auto steady_clock = rclcpp::Clock(RCL_STEADY_TIME); +// auto start_time = steady_clock.now(); +// while (rclcpp::ok() && +// (steady_clock.now() - start_time).seconds() < 2.0) +// { +// rclcpp::spin_some(clock_thread_testing_node.get_node_base_interface()); +// } + +// // Node should have get out of timer callback +// ASSERT_FALSE(clock_thread_testing_node.GetIsCallbackFrozen()); +// } TEST_F(TestTimeSource, clock_sleep_until_with_ros_time_basic) { SimClockPublisherNode pub_node; diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index 7a6599dfe4..59a1218519 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -30,8 +30,15 @@ using namespace std::chrono_literals; +/// We want to test everything for both the wall and generic timer. +enum class TimerType +{ + WALL_TIMER, + GENERIC_TIMER, +}; + /// Timer testing bring up and teardown -class TestTimer : public ::testing::Test +class TestTimer : public ::testing::TestWithParam { protected: void SetUp() override @@ -44,10 +51,7 @@ class TestTimer : public ::testing::Test test_node = std::make_shared("test_timer_node"); - timer = test_node->create_wall_timer( - 100ms, - [this]() -> void - { + auto timer_callback = [this]() -> void { this->has_timer_run.store(true); if (this->cancel_timer.load()) { @@ -55,10 +59,20 @@ class TestTimer : public ::testing::Test } // prevent any tests running timer from blocking this->executor->cancel(); - } - ); - EXPECT_TRUE(timer->is_steady()); - + }; + + // Store the timer type for use in TEST_P declarations. + timer_type = GetParam(); + switch (timer_type) { + case TimerType::WALL_TIMER: + timer = test_node->create_wall_timer(100ms, timer_callback); + EXPECT_TRUE(timer->is_steady()); + break; + case TimerType::GENERIC_TIMER: + timer = test_node->create_timer(100ms, timer_callback); + EXPECT_FALSE(timer->is_steady()); + break; + } executor->add_node(test_node); // don't start spinning, let the test dictate when } @@ -72,6 +86,7 @@ class TestTimer : public ::testing::Test } // set to true if the timer callback executed, false otherwise + TimerType timer_type; std::atomic has_timer_run; // flag used to cancel the timer in the timer callback. If true cancel the timer, otherwise // cancel the executor (preventing any tests from blocking) @@ -91,7 +106,7 @@ void test_initial_conditions( } /// Simple test -TEST_F(TestTimer, test_simple_cancel) +TEST_P(TestTimer, test_simple_cancel) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -104,7 +119,7 @@ TEST_F(TestTimer, test_simple_cancel) } /// Test state when using reset -TEST_F(TestTimer, test_is_canceled_reset) +TEST_P(TestTimer, test_is_canceled_reset) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -129,7 +144,7 @@ TEST_F(TestTimer, test_is_canceled_reset) } /// Run and check state, cancel the executor -TEST_F(TestTimer, test_run_cancel_executor) +TEST_P(TestTimer, test_run_cancel_executor) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -146,7 +161,7 @@ TEST_F(TestTimer, test_run_cancel_executor) } /// Run and check state, cancel the timer -TEST_F(TestTimer, test_run_cancel_timer) +TEST_P(TestTimer, test_run_cancel_timer) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -159,7 +174,7 @@ TEST_F(TestTimer, test_run_cancel_timer) EXPECT_TRUE(timer->is_canceled()); } -TEST_F(TestTimer, test_bad_arguments) { +TEST_P(TestTimer, test_bad_arguments) { auto node_base = rclcpp::node_interfaces::get_node_base_interface(test_node); auto context = node_base->get_context(); @@ -198,13 +213,19 @@ TEST_F(TestTimer, test_bad_arguments) { rclcpp::exceptions::RCLError); } -TEST_F(TestTimer, callback_with_timer) { +TEST_P(TestTimer, callback_with_timer) { rclcpp::TimerBase * timer_ptr = nullptr; - timer = test_node->create_wall_timer( - std::chrono::milliseconds(1), - [&timer_ptr](rclcpp::TimerBase & timer) { + auto timer_callback = [&timer_ptr](rclcpp::TimerBase & timer) { timer_ptr = &timer; - }); + }; + switch (timer_type) { + case TimerType::WALL_TIMER: + timer = test_node->create_wall_timer(1ms, timer_callback); + break; + case TimerType::GENERIC_TIMER: + timer = test_node->create_timer(1ms, timer_callback); + break; + } auto start = std::chrono::steady_clock::now(); while (nullptr == timer_ptr && (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100)) @@ -216,13 +237,19 @@ TEST_F(TestTimer, callback_with_timer) { EXPECT_FALSE(timer_ptr->is_ready()); } -TEST_F(TestTimer, callback_with_period_zero) { +TEST_P(TestTimer, callback_with_period_zero) { rclcpp::TimerBase * timer_ptr = nullptr; - timer = test_node->create_wall_timer( - std::chrono::milliseconds(0), - [&timer_ptr](rclcpp::TimerBase & timer) { + auto timer_callback = [&timer_ptr](rclcpp::TimerBase & timer) { timer_ptr = &timer; - }); + }; + switch (timer_type) { + case TimerType::WALL_TIMER: + timer = test_node->create_wall_timer(0ms, timer_callback); + break; + case TimerType::GENERIC_TIMER: + timer = test_node->create_timer(0ms, timer_callback); + break; + } auto start = std::chrono::steady_clock::now(); while (nullptr == timer_ptr && (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100)) @@ -235,7 +262,7 @@ TEST_F(TestTimer, callback_with_period_zero) { } /// Test internal failures using mocks -TEST_F(TestTimer, test_failures_with_exceptions) +TEST_P(TestTimer, test_failures_with_exceptions) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -245,8 +272,16 @@ TEST_F(TestTimer, test_failures_with_exceptions) auto mock = mocking_utils::inject_on_return("lib:rclcpp", rcl_timer_fini, RCL_RET_ERROR); EXPECT_NO_THROW( { - timer_to_test_destructor = - test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); + switch (timer_type) { + case TimerType::WALL_TIMER: + timer_to_test_destructor = + test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); + break; + case TimerType::GENERIC_TIMER: + timer_to_test_destructor = + test_node->create_timer(std::chrono::milliseconds(0), [](void) {}); + break; + } timer_to_test_destructor.reset(); }); } @@ -283,3 +318,19 @@ TEST_F(TestTimer, test_failures_with_exceptions) std::runtime_error("Timer could not get time until next call: error not set")); } } + +INSTANTIATE_TEST_SUITE_P( + PerTimerType, TestTimer, + ::testing::Values(TimerType::WALL_TIMER, TimerType::GENERIC_TIMER), + [](const ::testing::TestParamInfo & info) -> std::string { + switch (info.param) { + case TimerType::WALL_TIMER: + return std::string("wall_timer"); + case TimerType::GENERIC_TIMER: + return std::string("generic_timer"); + default: + break; + } + return std::string("unknown"); + } +); diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 84c62b9e6a..5e06876018 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -242,7 +242,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, ) ); - /// Create a timer. + /// Create a timer that uses the wall clock to drive the callback. /** * \param[in] period Time interval between triggers of the callback. * \param[in] callback User-defined callback function. @@ -255,6 +255,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create a timer that uses the node clock to drive the callback. + /** + * \param[in] period Time interval between triggers of the callback. + * \param[in] callback User-defined callback function. + * \param[in] group Callback group to execute this timer's callback in. + */ + template + typename rclcpp::GenericTimer::SharedPtr + create_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a Client. /** * \sa rclcpp::Node::create_client diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 88d981e051..e2a5696b92 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -90,11 +90,28 @@ LifecycleNode::create_wall_timer( CallbackT callback, rclcpp::CallbackGroup::SharedPtr group) { - auto timer = rclcpp::WallTimer::make_shared( - std::chrono::duration_cast(period), - std::move(callback), this->node_base_->get_context()); - node_timers_->add_timer(timer, group); - return timer; + return rclcpp::create_wall_timer( + period, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get()); +} + +template +typename rclcpp::GenericTimer::SharedPtr +LifecycleNode::create_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_timer( + this->get_clock(), + period, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get()); } template diff --git a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp index 2a5d08f728..d10544ef1f 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp @@ -42,10 +42,17 @@ class TestDefaultStateMachine : public ::testing::Test } }; +/// We want to test everything for both the wall and generic timer. +enum class TimerType +{ + WALL_TIMER, + GENERIC_TIMER, +}; + class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode { public: - explicit EmptyLifecycleNode(const std::string & node_name) + explicit EmptyLifecycleNode(const std::string & node_name, const TimerType & timer_type) : rclcpp_lifecycle::LifecycleNode(node_name) { rclcpp::PublisherOptionsWithAllocator> options; @@ -55,8 +62,20 @@ class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode add_managed_entity(publisher_); // For coverage this is being added here - auto timer = create_wall_timer(std::chrono::seconds(1), []() {}); - add_timer_handle(timer); + switch (timer_type) { + case TimerType::WALL_TIMER: + { + auto timer = create_wall_timer(std::chrono::seconds(1), []() {}); + add_timer_handle(timer); + break; + } + case TimerType::GENERIC_TIMER: + { + auto timer = create_timer(std::chrono::seconds(1), []() {}); + add_timer_handle(timer); + break; + } + } } std::shared_ptr> publisher() @@ -68,13 +87,13 @@ class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode std::shared_ptr> publisher_; }; -class TestLifecyclePublisher : public ::testing::Test +class TestLifecyclePublisher : public ::testing::TestWithParam { public: void SetUp() { rclcpp::init(0, nullptr); - node_ = std::make_shared("node"); + node_ = std::make_shared("node", GetParam()); } void TearDown() @@ -86,7 +105,7 @@ class TestLifecyclePublisher : public ::testing::Test std::shared_ptr node_; }; -TEST_F(TestLifecyclePublisher, publish_managed_by_node) { +TEST_P(TestLifecyclePublisher, publish_managed_by_node) { // transition via LifecycleNode auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; @@ -125,7 +144,7 @@ TEST_F(TestLifecyclePublisher, publish_managed_by_node) { } } -TEST_F(TestLifecyclePublisher, publish) { +TEST_P(TestLifecyclePublisher, publish) { // transition via LifecyclePublisher node_->publisher()->on_deactivate(); EXPECT_FALSE(node_->publisher()->is_activated()); @@ -148,3 +167,19 @@ TEST_F(TestLifecyclePublisher, publish) { EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr))); } } + +INSTANTIATE_TEST_SUITE_P( + PerTimerType, TestLifecyclePublisher, + ::testing::Values(TimerType::WALL_TIMER, TimerType::GENERIC_TIMER), + [](const ::testing::TestParamInfo & info) -> std::string { + switch (info.param) { + case TimerType::WALL_TIMER: + return std::string("wall_timer"); + case TimerType::GENERIC_TIMER: + return std::string("generic_timer"); + default: + break; + } + return std::string("unknown"); + } +); From 11b5f8db2191f7921364f88cd20c04a80526121a Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 30 Aug 2022 13:42:14 -0700 Subject: [PATCH 130/455] Revert "Add a `create_timer` method to `Node` and `LifecycleNode` classes (#1975)" (#2009) This reverts commit 6167a575b3eb85c90240e20736f4e5823ac4c681. Signed-off-by: Shane Loretz Signed-off-by: Shane Loretz --- rclcpp/include/rclcpp/create_timer.hpp | 155 +++++------------- rclcpp/include/rclcpp/node.hpp | 15 +- rclcpp/include/rclcpp/node_impl.hpp | 16 -- rclcpp/test/rclcpp/test_create_timer.cpp | 62 +------ rclcpp/test/rclcpp/test_time_source.cpp | 69 ++++---- rclcpp/test/rclcpp/test_timer.cpp | 105 +++--------- .../rclcpp_lifecycle/lifecycle_node.hpp | 15 +- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 27 +-- .../test/test_lifecycle_publisher.cpp | 49 +----- 9 files changed, 119 insertions(+), 394 deletions(-) diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index 5225f0fde0..345f43fcbb 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -23,63 +23,12 @@ #include "rclcpp/duration.hpp" #include "rclcpp/node_interfaces/get_node_base_interface.hpp" -#include "rclcpp/node_interfaces/get_node_clock_interface.hpp" #include "rclcpp/node_interfaces/get_node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" -#include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" namespace rclcpp { -namespace detail -{ -/// Perform a safe cast to a timer period in nanoseconds -/** - * - * \tparam DurationRepT - * \tparam DurationT - * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() - * \return period, expressed as chrono::duration::nanoseconds - * \throws std::invalid_argument if period is negative or too large - */ -template -std::chrono::nanoseconds -safe_cast_to_period_in_ns(std::chrono::duration period) -{ - if (period < std::chrono::duration::zero()) { - throw std::invalid_argument{"timer period cannot be negative"}; - } - - // Casting to a double representation might lose precision and allow the check below to succeed - // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max. - constexpr auto maximum_safe_cast_ns = - std::chrono::nanoseconds::max() - std::chrono::duration(1); - - // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow - // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is - // greater than nanoseconds::max() is a difficult general problem. This is a more conservative - // version of Howard Hinnant's (the guy>) response here: - // https://stackoverflow.com/a/44637334/2089061 - // However, this doesn't solve the issue for all possible duration types of period. - // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177 - constexpr auto ns_max_as_double = - std::chrono::duration_cast>( - maximum_safe_cast_ns); - if (period > ns_max_as_double) { - throw std::invalid_argument{ - "timer period must be less than std::chrono::nanoseconds::max()"}; - } - - const auto period_ns = std::chrono::duration_cast(period); - if (period_ns < std::chrono::nanoseconds::zero()) { - throw std::runtime_error{ - "Casting timer period to nanoseconds resulted in integer overflow."}; - } - - return period_ns; -} -} // namespace detail - /// Create a timer with a given clock /// \internal template @@ -92,13 +41,14 @@ create_timer( CallbackT && callback, rclcpp::CallbackGroup::SharedPtr group = nullptr) { - return create_timer( - node_base.get(), - node_timers.get(), + auto timer = rclcpp::GenericTimer::make_shared( clock, period.to_chrono(), std::forward(callback), - group); + node_base->get_context()); + + node_timers->add_timer(timer, group); + return timer; } /// Create a timer with a given clock @@ -112,99 +62,82 @@ create_timer( rclcpp::CallbackGroup::SharedPtr group = nullptr) { return create_timer( + rclcpp::node_interfaces::get_node_base_interface(node), + rclcpp::node_interfaces::get_node_timers_interface(node), clock, - period.to_chrono(), + period, std::forward(callback), - group, - rclcpp::node_interfaces::get_node_base_interface(node).get(), - rclcpp::node_interfaces::get_node_timers_interface(node).get()); + group); } -/// Convenience method to create a general timer with node resources. +/// Convenience method to create a timer with node resources. /** * * \tparam DurationRepT * \tparam DurationT * \tparam CallbackT - * \param clock clock to be used * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() * \param callback callback to execute via the timer period - * \param group callback group - * \param node_base node base interface - * \param node_timers node timer interface - * \return shared pointer to a generic timer - * \throws std::invalid_argument if either clock, node_base or node_timers - * are nullptr, or period is negative or too large + * \param group + * \param node_base + * \param node_timers + * \return + * \throws std::invalid argument if either node_base or node_timers + * are null, or period is negative or too large */ template -typename rclcpp::GenericTimer::SharedPtr -create_timer( - rclcpp::Clock::SharedPtr clock, +typename rclcpp::WallTimer::SharedPtr +create_wall_timer( std::chrono::duration period, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group, node_interfaces::NodeBaseInterface * node_base, node_interfaces::NodeTimersInterface * node_timers) { - if (clock == nullptr) { - throw std::invalid_argument{"clock cannot be null"}; - } if (node_base == nullptr) { throw std::invalid_argument{"input node_base cannot be null"}; } + if (node_timers == nullptr) { throw std::invalid_argument{"input node_timers cannot be null"}; } - const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period); + if (period < std::chrono::duration::zero()) { + throw std::invalid_argument{"timer period cannot be negative"}; + } - // Add a new generic timer. - auto timer = rclcpp::GenericTimer::make_shared( - std::move(clock), period_ns, std::move(callback), node_base->get_context()); - node_timers->add_timer(timer, group); - return timer; -} + // Casting to a double representation might lose precision and allow the check below to succeed + // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max. + constexpr auto maximum_safe_cast_ns = + std::chrono::nanoseconds::max() - std::chrono::duration(1); -/// Convenience method to create a wall timer with node resources. -/** - * - * \tparam DurationRepT - * \tparam DurationT - * \tparam CallbackT - * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() - * \param callback callback to execute via the timer period - * \param group callback group - * \param node_base node base interface - * \param node_timers node timer interface - * \return shared pointer to a wall timer - * \throws std::invalid_argument if either node_base or node_timers - * are null, or period is negative or too large - */ -template -typename rclcpp::WallTimer::SharedPtr -create_wall_timer( - std::chrono::duration period, - CallbackT callback, - rclcpp::CallbackGroup::SharedPtr group, - node_interfaces::NodeBaseInterface * node_base, - node_interfaces::NodeTimersInterface * node_timers) -{ - if (node_base == nullptr) { - throw std::invalid_argument{"input node_base cannot be null"}; + // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow + // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is + // greater than nanoseconds::max() is a difficult general problem. This is a more conservative + // version of Howard Hinnant's (the guy>) response here: + // https://stackoverflow.com/a/44637334/2089061 + // However, this doesn't solve the issue for all possible duration types of period. + // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177 + constexpr auto ns_max_as_double = + std::chrono::duration_cast>( + maximum_safe_cast_ns); + if (period > ns_max_as_double) { + throw std::invalid_argument{ + "timer period must be less than std::chrono::nanoseconds::max()"}; } - if (node_timers == nullptr) { - throw std::invalid_argument{"input node_timers cannot be null"}; + const auto period_ns = std::chrono::duration_cast(period); + if (period_ns < std::chrono::nanoseconds::zero()) { + throw std::runtime_error{ + "Casting timer period to nanoseconds resulted in integer overflow."}; } - const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period); - - // Add a new wall timer. auto timer = rclcpp::WallTimer::make_shared( period_ns, std::move(callback), node_base->get_context()); node_timers->add_timer(timer, group); return timer; } + } // namespace rclcpp #endif // RCLCPP__CREATE_TIMER_HPP_ diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index e514137b51..35162de03b 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -227,7 +227,7 @@ class Node : public std::enable_shared_from_this ) ); - /// Create a wall timer that uses the wall clock to drive the callback. + /// Create a timer. /** * \param[in] period Time interval between triggers of the callback. * \param[in] callback User-defined callback function. @@ -240,19 +240,6 @@ class Node : public std::enable_shared_from_this CallbackT callback, rclcpp::CallbackGroup::SharedPtr group = nullptr); - /// Create a timer that uses the node clock to drive the callback. - /** - * \param[in] period Time interval between triggers of the callback. - * \param[in] callback User-defined callback function. - * \param[in] group Callback group to execute this timer's callback in. - */ - template - typename rclcpp::GenericTimer::SharedPtr - create_timer( - std::chrono::duration period, - CallbackT callback, - rclcpp::CallbackGroup::SharedPtr group = nullptr); - /// Create and return a Client. /** * \param[in] service_name The topic to service on. diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index d6b090d4d1..5b427b5b25 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -120,22 +120,6 @@ Node::create_wall_timer( this->node_timers_.get()); } -template -typename rclcpp::GenericTimer::SharedPtr -Node::create_timer( - std::chrono::duration period, - CallbackT callback, - rclcpp::CallbackGroup::SharedPtr group) -{ - return rclcpp::create_timer( - this->get_clock(), - period, - std::move(callback), - group, - this->node_base_.get(), - this->node_timers_.get()); -} - template typename Client::SharedPtr Node::create_client( diff --git a/rclcpp/test/rclcpp/test_create_timer.cpp b/rclcpp/test/rclcpp/test_create_timer.cpp index f253af4838..13c3564544 100644 --- a/rclcpp/test/rclcpp/test_create_timer.cpp +++ b/rclcpp/test/rclcpp/test_create_timer.cpp @@ -62,7 +62,7 @@ TEST(TestCreateTimer, call_with_node_wrapper_compiles) rclcpp::shutdown(); } -TEST(TestCreateWallTimer, call_wall_timer_with_bad_arguments) +TEST(TestCreateTimer, call_wall_timer_with_bad_arguments) { rclcpp::init(0, nullptr); NodeWrapper node("test_create_wall_timers_with_bad_arguments"); @@ -117,66 +117,6 @@ TEST(TestCreateWallTimer, call_wall_timer_with_bad_arguments) rclcpp::shutdown(); } -TEST(TestCreateTimer, call_timer_with_bad_arguments) -{ - rclcpp::init(0, nullptr); - NodeWrapper node("test_create_timers_with_bad_arguments"); - auto callback = []() {}; - rclcpp::CallbackGroup::SharedPtr group = nullptr; - auto node_interface = - rclcpp::node_interfaces::get_node_base_interface(node).get(); - auto timers_interface = - rclcpp::node_interfaces::get_node_timers_interface(node).get(); - - auto clock = node.get_node_clock_interface()->get_clock(); - - // Negative period - EXPECT_THROW( - rclcpp::create_timer( - clock, -1ms, callback, group, node_interface, timers_interface), - std::invalid_argument); - - // Very negative period - constexpr auto nanoseconds_min = std::chrono::nanoseconds::min(); - EXPECT_THROW( - rclcpp::create_timer( - clock, nanoseconds_min, callback, group, node_interface, timers_interface), - std::invalid_argument); - - // Period must be less than nanoseconds::max() - constexpr auto nanoseconds_max = std::chrono::nanoseconds::min(); - EXPECT_THROW( - rclcpp::create_timer( - clock, nanoseconds_max, callback, group, node_interface, timers_interface), - std::invalid_argument); - - EXPECT_NO_THROW( - rclcpp::create_timer( - clock, nanoseconds_max - 1us, callback, group, node_interface, timers_interface)); - - EXPECT_NO_THROW( - rclcpp::create_timer(clock, 0ms, callback, group, node_interface, timers_interface)); - - // Period must be less than nanoseconds::max() - constexpr auto hours_max = std::chrono::hours::max(); - EXPECT_THROW( - rclcpp::create_timer( - clock, hours_max, callback, group, node_interface, timers_interface), - std::invalid_argument); - - // node_interface is null - EXPECT_THROW( - rclcpp::create_timer(clock, 1ms, callback, group, nullptr, timers_interface), - std::invalid_argument); - - // timers_interface is null - EXPECT_THROW( - rclcpp::create_timer(clock, 1ms, callback, group, node_interface, nullptr), - std::invalid_argument); - - rclcpp::shutdown(); -} - static void test_timer_callback(void) {} TEST(TestCreateTimer, timer_function_pointer) diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index a5aad9a498..2bf89a5b09 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -81,7 +81,7 @@ void spin_until_time( executor.spin_once(10ms); - if (clock->now().nanoseconds() >= end_time.count()) { + if (clock->now().nanoseconds() == end_time.count()) { return; } } @@ -108,7 +108,7 @@ void spin_until_ros_time_updated( executor.add_node(node); auto start = std::chrono::system_clock::now(); - while (std::chrono::system_clock::now() < (start + 2s)) { + while (std::chrono::system_clock::now() < (start + 1s)) { if (!rclcpp::ok()) { break; // Break for ctrl-c } @@ -630,8 +630,7 @@ class SimClockPublisherNode : public rclcpp::Node { public: SimClockPublisherNode() - : rclcpp::Node("sim_clock_publisher_node"), - pub_time_(0, 0) + : rclcpp::Node("sim_clock_publisher_node") { // Create a clock publisher clock_pub_ = this->create_publisher( @@ -646,6 +645,10 @@ class SimClockPublisherNode : public rclcpp::Node &SimClockPublisherNode::timer_callback, this) ); + + // Init clock msg to zero + clock_msg_.clock.sec = 0; + clock_msg_.clock.nanosec = 0; } ~SimClockPublisherNode() @@ -668,15 +671,13 @@ class SimClockPublisherNode : public rclcpp::Node private: void timer_callback() { - // Increment the time, update the clock msg and publish it - pub_time_ += rclcpp::Duration(0, 1000000); - clock_msg_.clock = pub_time_; + // Increment clock msg and publish it + clock_msg_.clock.nanosec += 1000000; clock_pub_->publish(clock_msg_); } rclcpp::Publisher::SharedPtr clock_pub_; rclcpp::TimerBase::SharedPtr pub_timer_; - rclcpp::Time pub_time_; rosgraph_msgs::msg::Clock clock_msg_; std::thread node_thread_; rclcpp::executors::SingleThreadedExecutor node_executor; @@ -694,7 +695,7 @@ class ClockThreadTestingNode : public rclcpp::Node this->set_parameter(rclcpp::Parameter("use_sim_time", true)); // Create a 100ms timer - timer_ = this->create_timer( + timer_ = this->create_wall_timer( std::chrono::milliseconds(100), std::bind( &ClockThreadTestingNode::timer_callback, @@ -734,33 +735,29 @@ class ClockThreadTestingNode : public rclcpp::Node bool is_callback_frozen_ = true; }; -// TODO(ivanpauno): This test was using a wall timer, when it was supposed to use sim time. -// It was also using `use_clock_tread = false`, when it was supposed to be `true`. -// Fixing the test to work as originally intended makes it super flaky. -// Disabling it until the test is fixed. -// TEST_F(TestTimeSource, check_sim_time_updated_in_callback_if_use_clock_thread) { -// // Test if clock time of a node with -// // parameter use_sim_time = true and option use_clock_thread = true -// // is updated while node is not spinning -// // (in a timer callback) - -// // Create a "sim time" publisher and spin it -// SimClockPublisherNode pub_node; -// pub_node.SpinNode(); - -// // Spin node for 2 seconds -// ClockThreadTestingNode clock_thread_testing_node; -// auto steady_clock = rclcpp::Clock(RCL_STEADY_TIME); -// auto start_time = steady_clock.now(); -// while (rclcpp::ok() && -// (steady_clock.now() - start_time).seconds() < 2.0) -// { -// rclcpp::spin_some(clock_thread_testing_node.get_node_base_interface()); -// } - -// // Node should have get out of timer callback -// ASSERT_FALSE(clock_thread_testing_node.GetIsCallbackFrozen()); -// } +TEST_F(TestTimeSource, check_sim_time_updated_in_callback_if_use_clock_thread) { + // Test if clock time of a node with + // parameter use_sim_time = true and option use_clock_thread = true + // is updated while node is not spinning + // (in a timer callback) + + // Create a "sim time" publisher and spin it + SimClockPublisherNode pub_node; + pub_node.SpinNode(); + + // Spin node for 2 seconds + ClockThreadTestingNode clock_thread_testing_node; + auto steady_clock = rclcpp::Clock(RCL_STEADY_TIME); + auto start_time = steady_clock.now(); + while (rclcpp::ok() && + (steady_clock.now() - start_time).seconds() < 2.0) + { + rclcpp::spin_some(clock_thread_testing_node.get_node_base_interface()); + } + + // Node should have get out of timer callback + ASSERT_FALSE(clock_thread_testing_node.GetIsCallbackFrozen()); +} TEST_F(TestTimeSource, clock_sleep_until_with_ros_time_basic) { SimClockPublisherNode pub_node; diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index 59a1218519..7a6599dfe4 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -30,15 +30,8 @@ using namespace std::chrono_literals; -/// We want to test everything for both the wall and generic timer. -enum class TimerType -{ - WALL_TIMER, - GENERIC_TIMER, -}; - /// Timer testing bring up and teardown -class TestTimer : public ::testing::TestWithParam +class TestTimer : public ::testing::Test { protected: void SetUp() override @@ -51,7 +44,10 @@ class TestTimer : public ::testing::TestWithParam test_node = std::make_shared("test_timer_node"); - auto timer_callback = [this]() -> void { + timer = test_node->create_wall_timer( + 100ms, + [this]() -> void + { this->has_timer_run.store(true); if (this->cancel_timer.load()) { @@ -59,20 +55,10 @@ class TestTimer : public ::testing::TestWithParam } // prevent any tests running timer from blocking this->executor->cancel(); - }; - - // Store the timer type for use in TEST_P declarations. - timer_type = GetParam(); - switch (timer_type) { - case TimerType::WALL_TIMER: - timer = test_node->create_wall_timer(100ms, timer_callback); - EXPECT_TRUE(timer->is_steady()); - break; - case TimerType::GENERIC_TIMER: - timer = test_node->create_timer(100ms, timer_callback); - EXPECT_FALSE(timer->is_steady()); - break; - } + } + ); + EXPECT_TRUE(timer->is_steady()); + executor->add_node(test_node); // don't start spinning, let the test dictate when } @@ -86,7 +72,6 @@ class TestTimer : public ::testing::TestWithParam } // set to true if the timer callback executed, false otherwise - TimerType timer_type; std::atomic has_timer_run; // flag used to cancel the timer in the timer callback. If true cancel the timer, otherwise // cancel the executor (preventing any tests from blocking) @@ -106,7 +91,7 @@ void test_initial_conditions( } /// Simple test -TEST_P(TestTimer, test_simple_cancel) +TEST_F(TestTimer, test_simple_cancel) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -119,7 +104,7 @@ TEST_P(TestTimer, test_simple_cancel) } /// Test state when using reset -TEST_P(TestTimer, test_is_canceled_reset) +TEST_F(TestTimer, test_is_canceled_reset) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -144,7 +129,7 @@ TEST_P(TestTimer, test_is_canceled_reset) } /// Run and check state, cancel the executor -TEST_P(TestTimer, test_run_cancel_executor) +TEST_F(TestTimer, test_run_cancel_executor) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -161,7 +146,7 @@ TEST_P(TestTimer, test_run_cancel_executor) } /// Run and check state, cancel the timer -TEST_P(TestTimer, test_run_cancel_timer) +TEST_F(TestTimer, test_run_cancel_timer) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -174,7 +159,7 @@ TEST_P(TestTimer, test_run_cancel_timer) EXPECT_TRUE(timer->is_canceled()); } -TEST_P(TestTimer, test_bad_arguments) { +TEST_F(TestTimer, test_bad_arguments) { auto node_base = rclcpp::node_interfaces::get_node_base_interface(test_node); auto context = node_base->get_context(); @@ -213,19 +198,13 @@ TEST_P(TestTimer, test_bad_arguments) { rclcpp::exceptions::RCLError); } -TEST_P(TestTimer, callback_with_timer) { +TEST_F(TestTimer, callback_with_timer) { rclcpp::TimerBase * timer_ptr = nullptr; - auto timer_callback = [&timer_ptr](rclcpp::TimerBase & timer) { + timer = test_node->create_wall_timer( + std::chrono::milliseconds(1), + [&timer_ptr](rclcpp::TimerBase & timer) { timer_ptr = &timer; - }; - switch (timer_type) { - case TimerType::WALL_TIMER: - timer = test_node->create_wall_timer(1ms, timer_callback); - break; - case TimerType::GENERIC_TIMER: - timer = test_node->create_timer(1ms, timer_callback); - break; - } + }); auto start = std::chrono::steady_clock::now(); while (nullptr == timer_ptr && (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100)) @@ -237,19 +216,13 @@ TEST_P(TestTimer, callback_with_timer) { EXPECT_FALSE(timer_ptr->is_ready()); } -TEST_P(TestTimer, callback_with_period_zero) { +TEST_F(TestTimer, callback_with_period_zero) { rclcpp::TimerBase * timer_ptr = nullptr; - auto timer_callback = [&timer_ptr](rclcpp::TimerBase & timer) { + timer = test_node->create_wall_timer( + std::chrono::milliseconds(0), + [&timer_ptr](rclcpp::TimerBase & timer) { timer_ptr = &timer; - }; - switch (timer_type) { - case TimerType::WALL_TIMER: - timer = test_node->create_wall_timer(0ms, timer_callback); - break; - case TimerType::GENERIC_TIMER: - timer = test_node->create_timer(0ms, timer_callback); - break; - } + }); auto start = std::chrono::steady_clock::now(); while (nullptr == timer_ptr && (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100)) @@ -262,7 +235,7 @@ TEST_P(TestTimer, callback_with_period_zero) { } /// Test internal failures using mocks -TEST_P(TestTimer, test_failures_with_exceptions) +TEST_F(TestTimer, test_failures_with_exceptions) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -272,16 +245,8 @@ TEST_P(TestTimer, test_failures_with_exceptions) auto mock = mocking_utils::inject_on_return("lib:rclcpp", rcl_timer_fini, RCL_RET_ERROR); EXPECT_NO_THROW( { - switch (timer_type) { - case TimerType::WALL_TIMER: - timer_to_test_destructor = - test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); - break; - case TimerType::GENERIC_TIMER: - timer_to_test_destructor = - test_node->create_timer(std::chrono::milliseconds(0), [](void) {}); - break; - } + timer_to_test_destructor = + test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); timer_to_test_destructor.reset(); }); } @@ -318,19 +283,3 @@ TEST_P(TestTimer, test_failures_with_exceptions) std::runtime_error("Timer could not get time until next call: error not set")); } } - -INSTANTIATE_TEST_SUITE_P( - PerTimerType, TestTimer, - ::testing::Values(TimerType::WALL_TIMER, TimerType::GENERIC_TIMER), - [](const ::testing::TestParamInfo & info) -> std::string { - switch (info.param) { - case TimerType::WALL_TIMER: - return std::string("wall_timer"); - case TimerType::GENERIC_TIMER: - return std::string("generic_timer"); - default: - break; - } - return std::string("unknown"); - } -); diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 5e06876018..84c62b9e6a 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -242,7 +242,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, ) ); - /// Create a timer that uses the wall clock to drive the callback. + /// Create a timer. /** * \param[in] period Time interval between triggers of the callback. * \param[in] callback User-defined callback function. @@ -255,19 +255,6 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group = nullptr); - /// Create a timer that uses the node clock to drive the callback. - /** - * \param[in] period Time interval between triggers of the callback. - * \param[in] callback User-defined callback function. - * \param[in] group Callback group to execute this timer's callback in. - */ - template - typename rclcpp::GenericTimer::SharedPtr - create_timer( - std::chrono::duration period, - CallbackT callback, - rclcpp::CallbackGroup::SharedPtr group = nullptr); - /// Create and return a Client. /** * \sa rclcpp::Node::create_client diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index e2a5696b92..88d981e051 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -90,28 +90,11 @@ LifecycleNode::create_wall_timer( CallbackT callback, rclcpp::CallbackGroup::SharedPtr group) { - return rclcpp::create_wall_timer( - period, - std::move(callback), - group, - this->node_base_.get(), - this->node_timers_.get()); -} - -template -typename rclcpp::GenericTimer::SharedPtr -LifecycleNode::create_timer( - std::chrono::duration period, - CallbackT callback, - rclcpp::CallbackGroup::SharedPtr group) -{ - return rclcpp::create_timer( - this->get_clock(), - period, - std::move(callback), - group, - this->node_base_.get(), - this->node_timers_.get()); + auto timer = rclcpp::WallTimer::make_shared( + std::chrono::duration_cast(period), + std::move(callback), this->node_base_->get_context()); + node_timers_->add_timer(timer, group); + return timer; } template diff --git a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp index d10544ef1f..2a5d08f728 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp @@ -42,17 +42,10 @@ class TestDefaultStateMachine : public ::testing::Test } }; -/// We want to test everything for both the wall and generic timer. -enum class TimerType -{ - WALL_TIMER, - GENERIC_TIMER, -}; - class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode { public: - explicit EmptyLifecycleNode(const std::string & node_name, const TimerType & timer_type) + explicit EmptyLifecycleNode(const std::string & node_name) : rclcpp_lifecycle::LifecycleNode(node_name) { rclcpp::PublisherOptionsWithAllocator> options; @@ -62,20 +55,8 @@ class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode add_managed_entity(publisher_); // For coverage this is being added here - switch (timer_type) { - case TimerType::WALL_TIMER: - { - auto timer = create_wall_timer(std::chrono::seconds(1), []() {}); - add_timer_handle(timer); - break; - } - case TimerType::GENERIC_TIMER: - { - auto timer = create_timer(std::chrono::seconds(1), []() {}); - add_timer_handle(timer); - break; - } - } + auto timer = create_wall_timer(std::chrono::seconds(1), []() {}); + add_timer_handle(timer); } std::shared_ptr> publisher() @@ -87,13 +68,13 @@ class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode std::shared_ptr> publisher_; }; -class TestLifecyclePublisher : public ::testing::TestWithParam +class TestLifecyclePublisher : public ::testing::Test { public: void SetUp() { rclcpp::init(0, nullptr); - node_ = std::make_shared("node", GetParam()); + node_ = std::make_shared("node"); } void TearDown() @@ -105,7 +86,7 @@ class TestLifecyclePublisher : public ::testing::TestWithParam std::shared_ptr node_; }; -TEST_P(TestLifecyclePublisher, publish_managed_by_node) { +TEST_F(TestLifecyclePublisher, publish_managed_by_node) { // transition via LifecycleNode auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; @@ -144,7 +125,7 @@ TEST_P(TestLifecyclePublisher, publish_managed_by_node) { } } -TEST_P(TestLifecyclePublisher, publish) { +TEST_F(TestLifecyclePublisher, publish) { // transition via LifecyclePublisher node_->publisher()->on_deactivate(); EXPECT_FALSE(node_->publisher()->is_activated()); @@ -167,19 +148,3 @@ TEST_P(TestLifecyclePublisher, publish) { EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr))); } } - -INSTANTIATE_TEST_SUITE_P( - PerTimerType, TestLifecyclePublisher, - ::testing::Values(TimerType::WALL_TIMER, TimerType::GENERIC_TIMER), - [](const ::testing::TestParamInfo & info) -> std::string { - switch (info.param) { - case TimerType::WALL_TIMER: - return std::string("wall_timer"); - case TimerType::GENERIC_TIMER: - return std::string("generic_timer"); - default: - break; - } - return std::string("unknown"); - } -); From 93222cc2cde3e05f89673a4a8450575300c9c33a Mon Sep 17 00:00:00 2001 From: schrodinbug Date: Wed, 31 Aug 2022 08:40:11 -0400 Subject: [PATCH 131/455] force compiler warning if callback handles not captured (#2000) clarify documentation to show that not capturing returned handles will result in callbacks immediately being unregistered Signed-off-by: Jason Beach --- .../rclcpp/parameter_event_handler.hpp | 23 +++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter_event_handler.hpp b/rclcpp/include/rclcpp/parameter_event_handler.hpp index 52a0233966..93b8177fb2 100644 --- a/rclcpp/include/rclcpp/parameter_event_handler.hpp +++ b/rclcpp/include/rclcpp/parameter_event_handler.hpp @@ -86,6 +86,9 @@ struct ParameterEventCallbackHandle * the ROS node supplied in the ParameterEventHandler constructor. * The callback, a lambda function in this case, simply prints out the value of the parameter. * + * Note: the object returned from add_parameter_callback must be captured or the callback will + * be immediately unregistered. + * * You may also monitor for changes to parameters in other nodes by supplying the node * name to add_parameter_callback: * @@ -103,8 +106,8 @@ struct ParameterEventCallbackHandle * In this case, the callback will be invoked whenever "some_remote_param_name" changes * on remote node "some_remote_node_name". * - * To remove a parameter callback, call remove_parameter_callback, passing the handle returned - * from add_parameter_callback: + * To remove a parameter callback, reset the callback handle smart pointer or call + * remove_parameter_callback, passing the handle returned from add_parameter_callback: * * param_handler->remove_parameter_callback(handle2); * @@ -152,9 +155,12 @@ struct ParameterEventCallbackHandle * For both parameter callbacks and parameter event callbacks, when multiple callbacks are added, * the callbacks are invoked last-in, first-called order (LIFO). * - * To remove a parameter event callback, use: + * Note: the callback handle returned from add_parameter_event_callback must be captured or + * the callback will immediately be unregistered. + * + * To remove a parameter event callback, reset the callback smart pointer or use: * - * param_handler->remove_event_parameter_callback(handle); + * param_handler->remove_event_parameter_callback(handle3); */ class ParameterEventHandler { @@ -189,10 +195,14 @@ class ParameterEventHandler /** * This function may be called multiple times to set multiple parameter event callbacks. * + * Note: if the returned callback handle smart pointer is not captured, the callback is + * immediatedly unregistered. A compiler warning should be generated to warn of this. + * * \param[in] callback Function callback to be invoked on parameter updates. * \returns A handle used to refer to the callback. */ RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED ParameterEventCallbackHandle::SharedPtr add_parameter_event_callback( ParameterEventCallbackType callback); @@ -212,12 +222,17 @@ class ParameterEventHandler /** * If a node_name is not provided, defaults to the current node. * + * Note: if the returned callback handle smart pointer is not captured, the callback + * is immediately unregistered. A compiler warning should be generated to warn + * of this. + * * \param[in] parameter_name Name of parameter to monitor. * \param[in] callback Function callback to be invoked upon parameter update. * \param[in] node_name Name of node which hosts the parameter. * \returns A handle used to refer to the callback. */ RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED ParameterCallbackHandle::SharedPtr add_parameter_callback( const std::string & parameter_name, From df994e435df6cbdb19ab7ada839f403367fb769a Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 31 Aug 2022 10:21:13 -0300 Subject: [PATCH 132/455] Revert "Revert "Add a create_timer method to Node and `LifecycleNode` classes (#1975)" (#2009) (#2010) Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/create_timer.hpp | 155 +++++++++++++----- rclcpp/include/rclcpp/node.hpp | 15 +- rclcpp/include/rclcpp/node_impl.hpp | 16 ++ rclcpp/test/rclcpp/test_create_timer.cpp | 62 ++++++- rclcpp/test/rclcpp/test_time_source.cpp | 69 ++++---- rclcpp/test/rclcpp/test_timer.cpp | 105 +++++++++--- .../rclcpp_lifecycle/lifecycle_node.hpp | 15 +- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 27 ++- .../test/test_lifecycle_publisher.cpp | 49 +++++- 9 files changed, 394 insertions(+), 119 deletions(-) diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index 345f43fcbb..d371466f0d 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -23,12 +23,63 @@ #include "rclcpp/duration.hpp" #include "rclcpp/node_interfaces/get_node_base_interface.hpp" +#include "rclcpp/node_interfaces/get_node_clock_interface.hpp" #include "rclcpp/node_interfaces/get_node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" namespace rclcpp { +namespace detail +{ +/// Perform a safe cast to a timer period in nanoseconds +/** + * + * \tparam DurationRepT + * \tparam DurationT + * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() + * \return period, expressed as chrono::duration::nanoseconds + * \throws std::invalid_argument if period is negative or too large + */ +template +std::chrono::nanoseconds +safe_cast_to_period_in_ns(std::chrono::duration period) +{ + if (period < std::chrono::duration::zero()) { + throw std::invalid_argument{"timer period cannot be negative"}; + } + + // Casting to a double representation might lose precision and allow the check below to succeed + // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max. + constexpr auto maximum_safe_cast_ns = + std::chrono::nanoseconds::max() - std::chrono::duration(1); + + // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow + // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is + // greater than nanoseconds::max() is a difficult general problem. This is a more conservative + // version of Howard Hinnant's (the guy>) response here: + // https://stackoverflow.com/a/44637334/2089061 + // However, this doesn't solve the issue for all possible duration types of period. + // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177 + constexpr auto ns_max_as_double = + std::chrono::duration_cast>( + maximum_safe_cast_ns); + if (period > ns_max_as_double) { + throw std::invalid_argument{ + "timer period must be less than std::chrono::nanoseconds::max()"}; + } + + const auto period_ns = std::chrono::duration_cast(period); + if (period_ns < std::chrono::nanoseconds::zero()) { + throw std::runtime_error{ + "Casting timer period to nanoseconds resulted in integer overflow."}; + } + + return period_ns; +} +} // namespace detail + /// Create a timer with a given clock /// \internal template @@ -41,14 +92,13 @@ create_timer( CallbackT && callback, rclcpp::CallbackGroup::SharedPtr group = nullptr) { - auto timer = rclcpp::GenericTimer::make_shared( + return create_timer( clock, period.to_chrono(), std::forward(callback), - node_base->get_context()); - - node_timers->add_timer(timer, group); - return timer; + group, + node_base.get(), + node_timers.get()); } /// Create a timer with a given clock @@ -62,82 +112,99 @@ create_timer( rclcpp::CallbackGroup::SharedPtr group = nullptr) { return create_timer( - rclcpp::node_interfaces::get_node_base_interface(node), - rclcpp::node_interfaces::get_node_timers_interface(node), clock, - period, + period.to_chrono(), std::forward(callback), - group); + group, + rclcpp::node_interfaces::get_node_base_interface(node).get(), + rclcpp::node_interfaces::get_node_timers_interface(node).get()); } -/// Convenience method to create a timer with node resources. +/// Convenience method to create a general timer with node resources. /** * * \tparam DurationRepT * \tparam DurationT * \tparam CallbackT + * \param clock clock to be used * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() * \param callback callback to execute via the timer period - * \param group - * \param node_base - * \param node_timers - * \return - * \throws std::invalid argument if either node_base or node_timers - * are null, or period is negative or too large + * \param group callback group + * \param node_base node base interface + * \param node_timers node timer interface + * \return shared pointer to a generic timer + * \throws std::invalid_argument if either clock, node_base or node_timers + * are nullptr, or period is negative or too large */ template -typename rclcpp::WallTimer::SharedPtr -create_wall_timer( +typename rclcpp::GenericTimer::SharedPtr +create_timer( + rclcpp::Clock::SharedPtr clock, std::chrono::duration period, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group, node_interfaces::NodeBaseInterface * node_base, node_interfaces::NodeTimersInterface * node_timers) { + if (clock == nullptr) { + throw std::invalid_argument{"clock cannot be null"}; + } if (node_base == nullptr) { throw std::invalid_argument{"input node_base cannot be null"}; } - if (node_timers == nullptr) { throw std::invalid_argument{"input node_timers cannot be null"}; } - if (period < std::chrono::duration::zero()) { - throw std::invalid_argument{"timer period cannot be negative"}; - } + const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period); - // Casting to a double representation might lose precision and allow the check below to succeed - // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max. - constexpr auto maximum_safe_cast_ns = - std::chrono::nanoseconds::max() - std::chrono::duration(1); + // Add a new generic timer. + auto timer = rclcpp::GenericTimer::make_shared( + std::move(clock), period_ns, std::move(callback), node_base->get_context()); + node_timers->add_timer(timer, group); + return timer; +} - // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow - // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is - // greater than nanoseconds::max() is a difficult general problem. This is a more conservative - // version of Howard Hinnant's (the guy>) response here: - // https://stackoverflow.com/a/44637334/2089061 - // However, this doesn't solve the issue for all possible duration types of period. - // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177 - constexpr auto ns_max_as_double = - std::chrono::duration_cast>( - maximum_safe_cast_ns); - if (period > ns_max_as_double) { - throw std::invalid_argument{ - "timer period must be less than std::chrono::nanoseconds::max()"}; +/// Convenience method to create a wall timer with node resources. +/** + * + * \tparam DurationRepT + * \tparam DurationT + * \tparam CallbackT + * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() + * \param callback callback to execute via the timer period + * \param group callback group + * \param node_base node base interface + * \param node_timers node timer interface + * \return shared pointer to a wall timer + * \throws std::invalid_argument if either node_base or node_timers + * are null, or period is negative or too large + */ +template +typename rclcpp::WallTimer::SharedPtr +create_wall_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group, + node_interfaces::NodeBaseInterface * node_base, + node_interfaces::NodeTimersInterface * node_timers) +{ + if (node_base == nullptr) { + throw std::invalid_argument{"input node_base cannot be null"}; } - const auto period_ns = std::chrono::duration_cast(period); - if (period_ns < std::chrono::nanoseconds::zero()) { - throw std::runtime_error{ - "Casting timer period to nanoseconds resulted in integer overflow."}; + if (node_timers == nullptr) { + throw std::invalid_argument{"input node_timers cannot be null"}; } + const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period); + + // Add a new wall timer. auto timer = rclcpp::WallTimer::make_shared( period_ns, std::move(callback), node_base->get_context()); node_timers->add_timer(timer, group); return timer; } - } // namespace rclcpp #endif // RCLCPP__CREATE_TIMER_HPP_ diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 35162de03b..e514137b51 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -227,7 +227,7 @@ class Node : public std::enable_shared_from_this ) ); - /// Create a timer. + /// Create a wall timer that uses the wall clock to drive the callback. /** * \param[in] period Time interval between triggers of the callback. * \param[in] callback User-defined callback function. @@ -240,6 +240,19 @@ class Node : public std::enable_shared_from_this CallbackT callback, rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create a timer that uses the node clock to drive the callback. + /** + * \param[in] period Time interval between triggers of the callback. + * \param[in] callback User-defined callback function. + * \param[in] group Callback group to execute this timer's callback in. + */ + template + typename rclcpp::GenericTimer::SharedPtr + create_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a Client. /** * \param[in] service_name The topic to service on. diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 5b427b5b25..d6b090d4d1 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -120,6 +120,22 @@ Node::create_wall_timer( this->node_timers_.get()); } +template +typename rclcpp::GenericTimer::SharedPtr +Node::create_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_timer( + this->get_clock(), + period, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get()); +} + template typename Client::SharedPtr Node::create_client( diff --git a/rclcpp/test/rclcpp/test_create_timer.cpp b/rclcpp/test/rclcpp/test_create_timer.cpp index 13c3564544..f253af4838 100644 --- a/rclcpp/test/rclcpp/test_create_timer.cpp +++ b/rclcpp/test/rclcpp/test_create_timer.cpp @@ -62,7 +62,7 @@ TEST(TestCreateTimer, call_with_node_wrapper_compiles) rclcpp::shutdown(); } -TEST(TestCreateTimer, call_wall_timer_with_bad_arguments) +TEST(TestCreateWallTimer, call_wall_timer_with_bad_arguments) { rclcpp::init(0, nullptr); NodeWrapper node("test_create_wall_timers_with_bad_arguments"); @@ -117,6 +117,66 @@ TEST(TestCreateTimer, call_wall_timer_with_bad_arguments) rclcpp::shutdown(); } +TEST(TestCreateTimer, call_timer_with_bad_arguments) +{ + rclcpp::init(0, nullptr); + NodeWrapper node("test_create_timers_with_bad_arguments"); + auto callback = []() {}; + rclcpp::CallbackGroup::SharedPtr group = nullptr; + auto node_interface = + rclcpp::node_interfaces::get_node_base_interface(node).get(); + auto timers_interface = + rclcpp::node_interfaces::get_node_timers_interface(node).get(); + + auto clock = node.get_node_clock_interface()->get_clock(); + + // Negative period + EXPECT_THROW( + rclcpp::create_timer( + clock, -1ms, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // Very negative period + constexpr auto nanoseconds_min = std::chrono::nanoseconds::min(); + EXPECT_THROW( + rclcpp::create_timer( + clock, nanoseconds_min, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // Period must be less than nanoseconds::max() + constexpr auto nanoseconds_max = std::chrono::nanoseconds::min(); + EXPECT_THROW( + rclcpp::create_timer( + clock, nanoseconds_max, callback, group, node_interface, timers_interface), + std::invalid_argument); + + EXPECT_NO_THROW( + rclcpp::create_timer( + clock, nanoseconds_max - 1us, callback, group, node_interface, timers_interface)); + + EXPECT_NO_THROW( + rclcpp::create_timer(clock, 0ms, callback, group, node_interface, timers_interface)); + + // Period must be less than nanoseconds::max() + constexpr auto hours_max = std::chrono::hours::max(); + EXPECT_THROW( + rclcpp::create_timer( + clock, hours_max, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // node_interface is null + EXPECT_THROW( + rclcpp::create_timer(clock, 1ms, callback, group, nullptr, timers_interface), + std::invalid_argument); + + // timers_interface is null + EXPECT_THROW( + rclcpp::create_timer(clock, 1ms, callback, group, node_interface, nullptr), + std::invalid_argument); + + rclcpp::shutdown(); +} + static void test_timer_callback(void) {} TEST(TestCreateTimer, timer_function_pointer) diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index 2bf89a5b09..a5aad9a498 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -81,7 +81,7 @@ void spin_until_time( executor.spin_once(10ms); - if (clock->now().nanoseconds() == end_time.count()) { + if (clock->now().nanoseconds() >= end_time.count()) { return; } } @@ -108,7 +108,7 @@ void spin_until_ros_time_updated( executor.add_node(node); auto start = std::chrono::system_clock::now(); - while (std::chrono::system_clock::now() < (start + 1s)) { + while (std::chrono::system_clock::now() < (start + 2s)) { if (!rclcpp::ok()) { break; // Break for ctrl-c } @@ -630,7 +630,8 @@ class SimClockPublisherNode : public rclcpp::Node { public: SimClockPublisherNode() - : rclcpp::Node("sim_clock_publisher_node") + : rclcpp::Node("sim_clock_publisher_node"), + pub_time_(0, 0) { // Create a clock publisher clock_pub_ = this->create_publisher( @@ -645,10 +646,6 @@ class SimClockPublisherNode : public rclcpp::Node &SimClockPublisherNode::timer_callback, this) ); - - // Init clock msg to zero - clock_msg_.clock.sec = 0; - clock_msg_.clock.nanosec = 0; } ~SimClockPublisherNode() @@ -671,13 +668,15 @@ class SimClockPublisherNode : public rclcpp::Node private: void timer_callback() { - // Increment clock msg and publish it - clock_msg_.clock.nanosec += 1000000; + // Increment the time, update the clock msg and publish it + pub_time_ += rclcpp::Duration(0, 1000000); + clock_msg_.clock = pub_time_; clock_pub_->publish(clock_msg_); } rclcpp::Publisher::SharedPtr clock_pub_; rclcpp::TimerBase::SharedPtr pub_timer_; + rclcpp::Time pub_time_; rosgraph_msgs::msg::Clock clock_msg_; std::thread node_thread_; rclcpp::executors::SingleThreadedExecutor node_executor; @@ -695,7 +694,7 @@ class ClockThreadTestingNode : public rclcpp::Node this->set_parameter(rclcpp::Parameter("use_sim_time", true)); // Create a 100ms timer - timer_ = this->create_wall_timer( + timer_ = this->create_timer( std::chrono::milliseconds(100), std::bind( &ClockThreadTestingNode::timer_callback, @@ -735,29 +734,33 @@ class ClockThreadTestingNode : public rclcpp::Node bool is_callback_frozen_ = true; }; -TEST_F(TestTimeSource, check_sim_time_updated_in_callback_if_use_clock_thread) { - // Test if clock time of a node with - // parameter use_sim_time = true and option use_clock_thread = true - // is updated while node is not spinning - // (in a timer callback) - - // Create a "sim time" publisher and spin it - SimClockPublisherNode pub_node; - pub_node.SpinNode(); - - // Spin node for 2 seconds - ClockThreadTestingNode clock_thread_testing_node; - auto steady_clock = rclcpp::Clock(RCL_STEADY_TIME); - auto start_time = steady_clock.now(); - while (rclcpp::ok() && - (steady_clock.now() - start_time).seconds() < 2.0) - { - rclcpp::spin_some(clock_thread_testing_node.get_node_base_interface()); - } - - // Node should have get out of timer callback - ASSERT_FALSE(clock_thread_testing_node.GetIsCallbackFrozen()); -} +// TODO(ivanpauno): This test was using a wall timer, when it was supposed to use sim time. +// It was also using `use_clock_tread = false`, when it was supposed to be `true`. +// Fixing the test to work as originally intended makes it super flaky. +// Disabling it until the test is fixed. +// TEST_F(TestTimeSource, check_sim_time_updated_in_callback_if_use_clock_thread) { +// // Test if clock time of a node with +// // parameter use_sim_time = true and option use_clock_thread = true +// // is updated while node is not spinning +// // (in a timer callback) + +// // Create a "sim time" publisher and spin it +// SimClockPublisherNode pub_node; +// pub_node.SpinNode(); + +// // Spin node for 2 seconds +// ClockThreadTestingNode clock_thread_testing_node; +// auto steady_clock = rclcpp::Clock(RCL_STEADY_TIME); +// auto start_time = steady_clock.now(); +// while (rclcpp::ok() && +// (steady_clock.now() - start_time).seconds() < 2.0) +// { +// rclcpp::spin_some(clock_thread_testing_node.get_node_base_interface()); +// } + +// // Node should have get out of timer callback +// ASSERT_FALSE(clock_thread_testing_node.GetIsCallbackFrozen()); +// } TEST_F(TestTimeSource, clock_sleep_until_with_ros_time_basic) { SimClockPublisherNode pub_node; diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index 7a6599dfe4..59a1218519 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -30,8 +30,15 @@ using namespace std::chrono_literals; +/// We want to test everything for both the wall and generic timer. +enum class TimerType +{ + WALL_TIMER, + GENERIC_TIMER, +}; + /// Timer testing bring up and teardown -class TestTimer : public ::testing::Test +class TestTimer : public ::testing::TestWithParam { protected: void SetUp() override @@ -44,10 +51,7 @@ class TestTimer : public ::testing::Test test_node = std::make_shared("test_timer_node"); - timer = test_node->create_wall_timer( - 100ms, - [this]() -> void - { + auto timer_callback = [this]() -> void { this->has_timer_run.store(true); if (this->cancel_timer.load()) { @@ -55,10 +59,20 @@ class TestTimer : public ::testing::Test } // prevent any tests running timer from blocking this->executor->cancel(); - } - ); - EXPECT_TRUE(timer->is_steady()); - + }; + + // Store the timer type for use in TEST_P declarations. + timer_type = GetParam(); + switch (timer_type) { + case TimerType::WALL_TIMER: + timer = test_node->create_wall_timer(100ms, timer_callback); + EXPECT_TRUE(timer->is_steady()); + break; + case TimerType::GENERIC_TIMER: + timer = test_node->create_timer(100ms, timer_callback); + EXPECT_FALSE(timer->is_steady()); + break; + } executor->add_node(test_node); // don't start spinning, let the test dictate when } @@ -72,6 +86,7 @@ class TestTimer : public ::testing::Test } // set to true if the timer callback executed, false otherwise + TimerType timer_type; std::atomic has_timer_run; // flag used to cancel the timer in the timer callback. If true cancel the timer, otherwise // cancel the executor (preventing any tests from blocking) @@ -91,7 +106,7 @@ void test_initial_conditions( } /// Simple test -TEST_F(TestTimer, test_simple_cancel) +TEST_P(TestTimer, test_simple_cancel) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -104,7 +119,7 @@ TEST_F(TestTimer, test_simple_cancel) } /// Test state when using reset -TEST_F(TestTimer, test_is_canceled_reset) +TEST_P(TestTimer, test_is_canceled_reset) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -129,7 +144,7 @@ TEST_F(TestTimer, test_is_canceled_reset) } /// Run and check state, cancel the executor -TEST_F(TestTimer, test_run_cancel_executor) +TEST_P(TestTimer, test_run_cancel_executor) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -146,7 +161,7 @@ TEST_F(TestTimer, test_run_cancel_executor) } /// Run and check state, cancel the timer -TEST_F(TestTimer, test_run_cancel_timer) +TEST_P(TestTimer, test_run_cancel_timer) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -159,7 +174,7 @@ TEST_F(TestTimer, test_run_cancel_timer) EXPECT_TRUE(timer->is_canceled()); } -TEST_F(TestTimer, test_bad_arguments) { +TEST_P(TestTimer, test_bad_arguments) { auto node_base = rclcpp::node_interfaces::get_node_base_interface(test_node); auto context = node_base->get_context(); @@ -198,13 +213,19 @@ TEST_F(TestTimer, test_bad_arguments) { rclcpp::exceptions::RCLError); } -TEST_F(TestTimer, callback_with_timer) { +TEST_P(TestTimer, callback_with_timer) { rclcpp::TimerBase * timer_ptr = nullptr; - timer = test_node->create_wall_timer( - std::chrono::milliseconds(1), - [&timer_ptr](rclcpp::TimerBase & timer) { + auto timer_callback = [&timer_ptr](rclcpp::TimerBase & timer) { timer_ptr = &timer; - }); + }; + switch (timer_type) { + case TimerType::WALL_TIMER: + timer = test_node->create_wall_timer(1ms, timer_callback); + break; + case TimerType::GENERIC_TIMER: + timer = test_node->create_timer(1ms, timer_callback); + break; + } auto start = std::chrono::steady_clock::now(); while (nullptr == timer_ptr && (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100)) @@ -216,13 +237,19 @@ TEST_F(TestTimer, callback_with_timer) { EXPECT_FALSE(timer_ptr->is_ready()); } -TEST_F(TestTimer, callback_with_period_zero) { +TEST_P(TestTimer, callback_with_period_zero) { rclcpp::TimerBase * timer_ptr = nullptr; - timer = test_node->create_wall_timer( - std::chrono::milliseconds(0), - [&timer_ptr](rclcpp::TimerBase & timer) { + auto timer_callback = [&timer_ptr](rclcpp::TimerBase & timer) { timer_ptr = &timer; - }); + }; + switch (timer_type) { + case TimerType::WALL_TIMER: + timer = test_node->create_wall_timer(0ms, timer_callback); + break; + case TimerType::GENERIC_TIMER: + timer = test_node->create_timer(0ms, timer_callback); + break; + } auto start = std::chrono::steady_clock::now(); while (nullptr == timer_ptr && (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100)) @@ -235,7 +262,7 @@ TEST_F(TestTimer, callback_with_period_zero) { } /// Test internal failures using mocks -TEST_F(TestTimer, test_failures_with_exceptions) +TEST_P(TestTimer, test_failures_with_exceptions) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -245,8 +272,16 @@ TEST_F(TestTimer, test_failures_with_exceptions) auto mock = mocking_utils::inject_on_return("lib:rclcpp", rcl_timer_fini, RCL_RET_ERROR); EXPECT_NO_THROW( { - timer_to_test_destructor = - test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); + switch (timer_type) { + case TimerType::WALL_TIMER: + timer_to_test_destructor = + test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); + break; + case TimerType::GENERIC_TIMER: + timer_to_test_destructor = + test_node->create_timer(std::chrono::milliseconds(0), [](void) {}); + break; + } timer_to_test_destructor.reset(); }); } @@ -283,3 +318,19 @@ TEST_F(TestTimer, test_failures_with_exceptions) std::runtime_error("Timer could not get time until next call: error not set")); } } + +INSTANTIATE_TEST_SUITE_P( + PerTimerType, TestTimer, + ::testing::Values(TimerType::WALL_TIMER, TimerType::GENERIC_TIMER), + [](const ::testing::TestParamInfo & info) -> std::string { + switch (info.param) { + case TimerType::WALL_TIMER: + return std::string("wall_timer"); + case TimerType::GENERIC_TIMER: + return std::string("generic_timer"); + default: + break; + } + return std::string("unknown"); + } +); diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 84c62b9e6a..5e06876018 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -242,7 +242,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, ) ); - /// Create a timer. + /// Create a timer that uses the wall clock to drive the callback. /** * \param[in] period Time interval between triggers of the callback. * \param[in] callback User-defined callback function. @@ -255,6 +255,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create a timer that uses the node clock to drive the callback. + /** + * \param[in] period Time interval between triggers of the callback. + * \param[in] callback User-defined callback function. + * \param[in] group Callback group to execute this timer's callback in. + */ + template + typename rclcpp::GenericTimer::SharedPtr + create_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a Client. /** * \sa rclcpp::Node::create_client diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 88d981e051..e2a5696b92 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -90,11 +90,28 @@ LifecycleNode::create_wall_timer( CallbackT callback, rclcpp::CallbackGroup::SharedPtr group) { - auto timer = rclcpp::WallTimer::make_shared( - std::chrono::duration_cast(period), - std::move(callback), this->node_base_->get_context()); - node_timers_->add_timer(timer, group); - return timer; + return rclcpp::create_wall_timer( + period, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get()); +} + +template +typename rclcpp::GenericTimer::SharedPtr +LifecycleNode::create_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_timer( + this->get_clock(), + period, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get()); } template diff --git a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp index 2a5d08f728..d10544ef1f 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp @@ -42,10 +42,17 @@ class TestDefaultStateMachine : public ::testing::Test } }; +/// We want to test everything for both the wall and generic timer. +enum class TimerType +{ + WALL_TIMER, + GENERIC_TIMER, +}; + class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode { public: - explicit EmptyLifecycleNode(const std::string & node_name) + explicit EmptyLifecycleNode(const std::string & node_name, const TimerType & timer_type) : rclcpp_lifecycle::LifecycleNode(node_name) { rclcpp::PublisherOptionsWithAllocator> options; @@ -55,8 +62,20 @@ class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode add_managed_entity(publisher_); // For coverage this is being added here - auto timer = create_wall_timer(std::chrono::seconds(1), []() {}); - add_timer_handle(timer); + switch (timer_type) { + case TimerType::WALL_TIMER: + { + auto timer = create_wall_timer(std::chrono::seconds(1), []() {}); + add_timer_handle(timer); + break; + } + case TimerType::GENERIC_TIMER: + { + auto timer = create_timer(std::chrono::seconds(1), []() {}); + add_timer_handle(timer); + break; + } + } } std::shared_ptr> publisher() @@ -68,13 +87,13 @@ class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode std::shared_ptr> publisher_; }; -class TestLifecyclePublisher : public ::testing::Test +class TestLifecyclePublisher : public ::testing::TestWithParam { public: void SetUp() { rclcpp::init(0, nullptr); - node_ = std::make_shared("node"); + node_ = std::make_shared("node", GetParam()); } void TearDown() @@ -86,7 +105,7 @@ class TestLifecyclePublisher : public ::testing::Test std::shared_ptr node_; }; -TEST_F(TestLifecyclePublisher, publish_managed_by_node) { +TEST_P(TestLifecyclePublisher, publish_managed_by_node) { // transition via LifecycleNode auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; @@ -125,7 +144,7 @@ TEST_F(TestLifecyclePublisher, publish_managed_by_node) { } } -TEST_F(TestLifecyclePublisher, publish) { +TEST_P(TestLifecyclePublisher, publish) { // transition via LifecyclePublisher node_->publisher()->on_deactivate(); EXPECT_FALSE(node_->publisher()->is_activated()); @@ -148,3 +167,19 @@ TEST_F(TestLifecyclePublisher, publish) { EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr))); } } + +INSTANTIATE_TEST_SUITE_P( + PerTimerType, TestLifecyclePublisher, + ::testing::Values(TimerType::WALL_TIMER, TimerType::GENERIC_TIMER), + [](const ::testing::TestParamInfo & info) -> std::string { + switch (info.param) { + case TimerType::WALL_TIMER: + return std::string("wall_timer"); + case TimerType::GENERIC_TIMER: + return std::string("generic_timer"); + default: + break; + } + return std::string("unknown"); + } +); From ea8daa37845e6137cba07a18eb653d97d87e6174 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Fri, 2 Sep 2022 11:58:56 -0600 Subject: [PATCH 133/455] operator+= and operator-= for Duration (#1988) * operator+= and operator-= for Duration Signed-off-by: Tyler Weaver --- rclcpp/include/rclcpp/duration.hpp | 7 +++++++ rclcpp/src/rclcpp/duration.cpp | 21 +++++++++++++++++++++ rclcpp/test/rclcpp/test_duration.cpp | 14 ++++++++++++++ 3 files changed, 42 insertions(+) diff --git a/rclcpp/include/rclcpp/duration.hpp b/rclcpp/include/rclcpp/duration.hpp index 28b3718660..537e2a9bf6 100644 --- a/rclcpp/include/rclcpp/duration.hpp +++ b/rclcpp/include/rclcpp/duration.hpp @@ -92,9 +92,13 @@ class RCLCPP_PUBLIC Duration Duration operator+(const rclcpp::Duration & rhs) const; + Duration & operator+=(const rclcpp::Duration & rhs); + Duration operator-(const rclcpp::Duration & rhs) const; + Duration & operator-=(const rclcpp::Duration & rhs); + /// Get the maximum representable value. /** * \return the maximum representable value @@ -106,6 +110,9 @@ class RCLCPP_PUBLIC Duration Duration operator*(double scale) const; + Duration & + operator*=(double scale); + /// Get duration in nanosecods /** * \return the duration in nanoseconds as a rcl_duration_value_t. diff --git a/rclcpp/src/rclcpp/duration.cpp b/rclcpp/src/rclcpp/duration.cpp index 2966cc4899..2eac7f6b58 100644 --- a/rclcpp/src/rclcpp/duration.cpp +++ b/rclcpp/src/rclcpp/duration.cpp @@ -161,6 +161,13 @@ Duration::operator+(const rclcpp::Duration & rhs) const rcl_duration_.nanoseconds + rhs.rcl_duration_.nanoseconds); } +Duration & +Duration::operator+=(const rclcpp::Duration & rhs) +{ + *this = *this + rhs; + return *this; +} + void bounds_check_duration_difference(int64_t lhsns, int64_t rhsns, uint64_t max) { @@ -190,6 +197,13 @@ Duration::operator-(const rclcpp::Duration & rhs) const rcl_duration_.nanoseconds - rhs.rcl_duration_.nanoseconds); } +Duration & +Duration::operator-=(const rclcpp::Duration & rhs) +{ + *this = *this - rhs; + return *this; +} + void bounds_check_duration_scale(int64_t dns, double scale, uint64_t max) { @@ -222,6 +236,13 @@ Duration::operator*(double scale) const static_cast(rcl_duration_.nanoseconds) * scale_ld)); } +Duration & +Duration::operator*=(double scale) +{ + *this = *this * scale; + return *this; +} + rcl_duration_value_t Duration::nanoseconds() const { diff --git a/rclcpp/test/rclcpp/test_duration.cpp b/rclcpp/test/rclcpp/test_duration.cpp index 319ce06601..d5908c7d4b 100644 --- a/rclcpp/test/rclcpp/test_duration.cpp +++ b/rclcpp/test/rclcpp/test_duration.cpp @@ -52,9 +52,23 @@ TEST_F(TestDuration, operators) { EXPECT_EQ(sub.nanoseconds(), young.nanoseconds() - old.nanoseconds()); EXPECT_EQ(sub, young - old); + rclcpp::Duration addequal = old; + addequal += young; + EXPECT_EQ(addequal.nanoseconds(), old.nanoseconds() + young.nanoseconds()); + EXPECT_EQ(addequal, old + young); + + rclcpp::Duration subequal = young; + subequal -= old; + EXPECT_EQ(subequal.nanoseconds(), young.nanoseconds() - old.nanoseconds()); + EXPECT_EQ(subequal, young - old); + rclcpp::Duration scale = old * 3; EXPECT_EQ(scale.nanoseconds(), old.nanoseconds() * 3); + rclcpp::Duration scaleequal = old; + scaleequal *= 3; + EXPECT_EQ(scaleequal.nanoseconds(), old.nanoseconds() * 3); + rclcpp::Duration time = rclcpp::Duration(0, 0); rclcpp::Duration copy_constructor_duration(time); rclcpp::Duration assignment_op_duration = rclcpp::Duration(1, 0); From 92d4f3e347c8d12f25ee23b2dba5ae9179c14853 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 9 Sep 2022 03:55:41 +0800 Subject: [PATCH 134/455] support regex match for parameter client (#1992) * support regex match for parameter client Signed-off-by: Chen Lihui * address review that change behavior of a public method Signed-off-by: Chen Lihui * remove an unnecessary comment Signed-off-by: Chen Lihui * update gmock compilation setting Signed-off-by: Chen Lihui Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/parameter_client.hpp | 3 + rclcpp/include/rclcpp/parameter_map.hpp | 11 +- rclcpp/src/rclcpp/parameter_client.cpp | 28 ++-- rclcpp/src/rclcpp/parameter_map.cpp | 31 +++- rclcpp/test/rclcpp/CMakeLists.txt | 2 +- rclcpp/test/rclcpp/test_parameter_client.cpp | 137 +++++++++++++++++- .../load_complicated_parameters.yaml | 25 ++++ .../test_node/no_valid_parameters.yaml | 4 + 8 files changed, 217 insertions(+), 24 deletions(-) create mode 100644 rclcpp/test/resources/test_node/load_complicated_parameters.yaml create mode 100644 rclcpp/test/resources/test_node/no_valid_parameters.yaml diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 5e509bbf10..1bbe4c2ebc 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -185,6 +185,9 @@ class AsyncParametersClient /** * This function filters the parameters to be set based on the node name. * + * If two duplicate keys exist in node names belongs to one FQN, there is no guarantee + * which one could be set. + * * \param parameter_map named parameters to be loaded * \return the future of the set_parameter service used to load the parameters * \throw InvalidParametersException if there is no parameter to set diff --git a/rclcpp/include/rclcpp/parameter_map.hpp b/rclcpp/include/rclcpp/parameter_map.hpp index 303eac4a95..17e2128a7b 100644 --- a/rclcpp/include/rclcpp/parameter_map.hpp +++ b/rclcpp/include/rclcpp/parameter_map.hpp @@ -53,11 +53,20 @@ parameter_value_from(const rcl_variant_t * const c_value); /// Get the ParameterMap from a yaml file. /// \param[in] yaml_filename full name of the yaml file. +/// \param[in] node_fqn a Fully Qualified Name of node, default value is nullptr. /// \returns an instance of a parameter map /// \throws from rcl error of rcl_parse_yaml_file() RCLCPP_PUBLIC ParameterMap -parameter_map_from_yaml_file(const std::string & yaml_filename); +parameter_map_from_yaml_file(const std::string & yaml_filename, const char * node_fqn = nullptr); + +/// Get the Parameters from ParameterMap. +/// \param[in] parameter_map a parameter map. +/// \param[in] node_fqn a Fully Qualified Name of node, default value is nullptr. +/// \returns a list of a parameter +RCLCPP_PUBLIC +std::vector +parameters_from_map(const ParameterMap & parameter_map, const char * node_fqn = nullptr); } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index 38ced0e1a5..64e415e82f 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -290,28 +290,24 @@ std::shared_future> AsyncParametersClient::load_parameters( const std::string & yaml_filename) { - rclcpp::ParameterMap parameter_map = rclcpp::parameter_map_from_yaml_file(yaml_filename); - return this->load_parameters(parameter_map); + rclcpp::ParameterMap parameter_map = + rclcpp::parameter_map_from_yaml_file(yaml_filename, remote_node_name_.c_str()); + + auto iter = parameter_map.find(remote_node_name_); + if (iter == parameter_map.end() || iter->second.size() == 0) { + throw rclcpp::exceptions::InvalidParametersException("No valid parameter"); + } + auto future_result = set_parameters(iter->second); + + return future_result; } std::shared_future> AsyncParametersClient::load_parameters( const rclcpp::ParameterMap & parameter_map) { - std::vector parameters; - std::string remote_name = remote_node_name_.substr(remote_node_name_.substr(1).find("/") + 2); - for (const auto & params : parameter_map) { - std::string node_full_name = params.first; - std::string node_name = node_full_name.substr(node_full_name.find("/*/") + 3); - if (node_full_name == remote_node_name_ || - node_full_name == "/**" || - (node_name == remote_name)) - { - for (const auto & param : params.second) { - parameters.push_back(param); - } - } - } + std::vector parameters = + rclcpp::parameters_from_map(parameter_map, remote_node_name_.c_str()); if (parameters.size() == 0) { throw rclcpp::exceptions::InvalidParametersException("No valid parameter"); diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index cb458e7db3..5ed67daae6 100644 --- a/rclcpp/src/rclcpp/parameter_map.cpp +++ b/rclcpp/src/rclcpp/parameter_map.cpp @@ -25,6 +25,13 @@ using rclcpp::exceptions::InvalidParameterValueException; using rclcpp::ParameterMap; using rclcpp::ParameterValue; +static bool is_node_name_matched(const std::string & node_name, const char * node_fqn) +{ + // Update the regular expression ["/*" -> "(/\\w+)" and "/**" -> "(/\\w+)*"] + std::string regex = rcpputils::find_and_replace(node_name, "/*", "(/\\w+)"); + return std::regex_match(node_fqn, std::regex(regex)); +} + ParameterMap rclcpp::parameter_map_from(const rcl_params_t * const c_params, const char * node_fqn) { @@ -53,9 +60,7 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params, const char * nod } if (node_fqn) { - // Update the regular expression ["/*" -> "(/\\w+)" and "/**" -> "(/\\w+)*"] - std::string regex = rcpputils::find_and_replace(node_name, "/*", "(/\\w+)"); - if (!std::regex_match(node_fqn, std::regex(regex))) { + if (!is_node_name_matched(node_name, node_fqn)) { // No need to parse the items because the user just care about node_fqn continue; } @@ -143,7 +148,7 @@ rclcpp::parameter_value_from(const rcl_variant_t * const c_param_value) } ParameterMap -rclcpp::parameter_map_from_yaml_file(const std::string & yaml_filename) +rclcpp::parameter_map_from_yaml_file(const std::string & yaml_filename, const char * node_fqn) { rcutils_allocator_t allocator = rcutils_get_default_allocator(); rcl_params_t * rcl_parameters = rcl_yaml_node_struct_init(allocator); @@ -153,5 +158,21 @@ rclcpp::parameter_map_from_yaml_file(const std::string & yaml_filename) rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR); } - return rclcpp::parameter_map_from(rcl_parameters); + return rclcpp::parameter_map_from(rcl_parameters, node_fqn); +} + +std::vector +rclcpp::parameters_from_map(const ParameterMap & parameter_map, const char * node_fqn) +{ + std::vector parameters; + std::string node_name_old; + for (auto & [node_name, node_parameters] : parameter_map) { + if (node_fqn && !is_node_name_matched(node_name, node_fqn)) { + // No need to parse the items because the user just care about node_fqn + continue; + } + parameters.insert(parameters.end(), node_parameters.begin(), node_parameters.end()); + } + + return parameters; } diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 6f915feef5..d4e497f4bf 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -299,7 +299,7 @@ if(TARGET test_init_options) ament_target_dependencies(test_init_options "rcl") target_link_libraries(test_init_options ${PROJECT_NAME} mimick) endif() -ament_add_gtest(test_parameter_client test_parameter_client.cpp) +ament_add_gmock(test_parameter_client test_parameter_client.cpp) if(TARGET test_parameter_client) ament_target_dependencies(test_parameter_client "rcl_interfaces" diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index 4cd9e671be..64ef2d903b 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -948,3 +948,138 @@ TEST_F(TestParameterClient, sync_parameter_load_parameters) { auto list_parameters = synchronous_client->list_parameters({}, 3); ASSERT_EQ(list_parameters.names.size(), static_cast(5)); } + +/* + Coverage for async load_parameters with complicated regex expression + */ +TEST_F(TestParameterClient, async_parameter_load_parameters_complicated_regex) { + auto load_node = std::make_shared( + "load_node", + "namespace", + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + auto asynchronous_client = + std::make_shared(load_node, "/namespace/load_node"); + // load parameters + rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY}; + const std::string parameters_filepath = ( + test_resources_path / "test_node" / "load_complicated_parameters.yaml").string(); + auto load_future = asynchronous_client->load_parameters(parameters_filepath); + auto result_code = rclcpp::spin_until_future_complete( + load_node, load_future, std::chrono::milliseconds(100)); + ASSERT_EQ(result_code, rclcpp::FutureReturnCode::SUCCESS); + ASSERT_EQ(load_future.get()[0].successful, true); + // list parameters + auto list_parameters = asynchronous_client->list_parameters({}, 3); + rclcpp::spin_until_future_complete( + load_node, list_parameters, std::chrono::milliseconds(100)); + ASSERT_EQ(list_parameters.get().names.size(), static_cast(6)); + // to check the parameter "a_value" + std::string param_name = "a_value"; + auto param = load_node->get_parameter(param_name); + ASSERT_EQ(param.get_value(), "last_one_win"); +} + +/* + Coverage for async load_parameters to load file without valid parameters + */ +TEST_F(TestParameterClient, async_parameter_load_no_valid_parameter) { + auto load_node = std::make_shared( + "load_node", + "namespace", + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + auto asynchronous_client = + std::make_shared(load_node, "/namespace/load_node"); + // load parameters + rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY}; + const std::string parameters_filepath = ( + test_resources_path / "test_node" / "no_valid_parameters.yaml").string(); + EXPECT_THROW( + asynchronous_client->load_parameters(parameters_filepath), + rclcpp::exceptions::InvalidParametersException); +} + +/* + Coverage for async load_parameters from maps with complicated regex expression + */ +TEST_F(TestParameterClient, async_parameter_load_parameters_from_map) { + auto load_node = std::make_shared( + "load_node", + "namespace", + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + auto asynchronous_client = + std::make_shared(load_node, "/namespace/load_node"); + // load parameters + rclcpp::ParameterMap parameter_map = { + {"/**", + { + {"bar", 5}, + {"foo", 3.5}, + {"a_value", "first"} + } + }, + {"/*/load_node", + { + {"bar_foo", "ok"}, + {"a_value", "second"} + } + }, + {"/namespace/load_node", + { + {"foo_bar", true}, + {"a_value", "third"} + } + }, + {"/bar", + { + {"fatal", 10} + } + }, + {"/**/namespace/*", + { + {"a_value", "not_win"} + } + } + }; + + auto load_future = asynchronous_client->load_parameters(parameter_map); + auto result_code = rclcpp::spin_until_future_complete( + load_node, load_future, std::chrono::milliseconds(100)); + ASSERT_EQ(result_code, rclcpp::FutureReturnCode::SUCCESS); + ASSERT_EQ(load_future.get()[0].successful, true); + // list parameters + auto list_parameters = asynchronous_client->list_parameters({}, 3); + rclcpp::spin_until_future_complete( + load_node, list_parameters, std::chrono::milliseconds(100)); + ASSERT_EQ(list_parameters.get().names.size(), static_cast(6)); + // to check the parameter "a_value" + std::string param_name = "a_value"; + auto param = load_node->get_parameter(param_name); + // rclcpp::ParameterMap is an unordered map, no guarantee which value will be set for `a_value`. + EXPECT_THAT( + (std::array{"first", "second", "third", "not_win"}), + testing::Contains(param.get_value())); +} + +/* + Coverage for async load_parameters from maps without valid parameters + */ +TEST_F(TestParameterClient, async_parameter_load_from_map_no_valid_parameter) { + auto load_node = std::make_shared( + "load_node", + "namespace", + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + auto asynchronous_client = + std::make_shared(load_node, "/namespace/load_node"); + // load parameters + rclcpp::ParameterMap parameter_map = { + {"/no/valid/parameters/node", + { + {"bar", 5}, + {"bar", 3.5} + } + } + }; + EXPECT_THROW( + asynchronous_client->load_parameters(parameter_map), + rclcpp::exceptions::InvalidParametersException); +} diff --git a/rclcpp/test/resources/test_node/load_complicated_parameters.yaml b/rclcpp/test/resources/test_node/load_complicated_parameters.yaml new file mode 100644 index 0000000000..7722f636c2 --- /dev/null +++ b/rclcpp/test/resources/test_node/load_complicated_parameters.yaml @@ -0,0 +1,25 @@ +/**: + ros__parameters: + bar: 5 + foo: 3.5 + a_value: "first" + +/*: + load_node: + ros__parameters: + bar_foo: "ok" + a_value: "second" + +namespace: + load_node: + ros__parameters: + foo_bar: true + a_value: "third" + +bar: + ros__parameters: + fatal: 10 + +/**/namespace/*: + ros__parameters: + a_value: "last_one_win" diff --git a/rclcpp/test/resources/test_node/no_valid_parameters.yaml b/rclcpp/test/resources/test_node/no_valid_parameters.yaml new file mode 100644 index 0000000000..a75356cd77 --- /dev/null +++ b/rclcpp/test/resources/test_node/no_valid_parameters.yaml @@ -0,0 +1,4 @@ +/no/valid/parameters/node: + ros__parameters: + bar: 5 + foo: 3.5 From d0e1e837b512569a6dc58b5579bcb87c5a5bf2bc Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Sat, 10 Sep 2022 06:06:33 +0800 Subject: [PATCH 135/455] Make ParameterService and Sync/AsyncParameterClient accept rclcpp::QoS (#1978) * Make ParameterService and Sync/AsyncParameterClient accept rclcpp::QoS Signed-off-by: Barry Xu * Avoid deprecated function using another deprecated function Signed-off-by: Barry Xu Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/parameter_client.hpp | 196 ++++++++++++++++++-- rclcpp/include/rclcpp/parameter_service.hpp | 19 +- rclcpp/src/rclcpp/parameter_client.cpp | 4 +- rclcpp/src/rclcpp/parameter_service.cpp | 2 +- 4 files changed, 197 insertions(+), 24 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 1bbe4c2ebc..7dff6e9ad6 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -39,6 +39,7 @@ #include "rclcpp/node.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/parameter_map.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" #include "rmw/rmw.h" @@ -51,6 +52,37 @@ class AsyncParametersClient public: RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient) + /// Create an async parameters client. + /** + * \param[in] node_base_interface The node base interface of the corresponding node. + * \param[in] node_topics_interface Node topic base interface. + * \param[in] node_graph_interface The node graph interface of the corresponding node. + * \param[in] node_services_interface Node service interface. + * \param[in] remote_node_name Name of the remote node + * \param[in] qos_profile The rmw qos profile to use to subscribe + * \param[in] group (optional) The async parameter client will be added to this callback group. + * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t + */ + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + RCLCPP_PUBLIC + AsyncParametersClient( + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile, + rclcpp::CallbackGroup::SharedPtr group = nullptr) + : AsyncParametersClient( + node_base_interface, + node_topics_interface, + node_graph_interface, + node_services_interface, + remote_node_name, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)), + group) + {} + /// Create an async parameters client. /** * \param[in] node_base_interface The node base interface of the corresponding node. @@ -58,7 +90,7 @@ class AsyncParametersClient * \param[in] node_graph_interface The node graph interface of the corresponding node. * \param[in] node_services_interface Node service interface. * \param[in] remote_node_name (optional) name of the remote node - * \param[in] qos_profile (optional) The rmw qos profile to use to subscribe + * \param[in] qos_profile (optional) The qos profile to use to subscribe * \param[in] group (optional) The async parameter client will be added to this callback group. */ RCLCPP_PUBLIC @@ -68,21 +100,45 @@ class AsyncParametersClient const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters, + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); /// Constructor /** - * \param[in] node The async paramters client will be added to this node. + * \param[in] node The async parameters client will be added to this node. + * \param[in] remote_node_name name of the remote node + * \param[in] qos_profile The rmw qos profile to use to subscribe + * \param[in] group (optional) The async parameter client will be added to this callback group. + * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t + */ + template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + AsyncParametersClient( + const std::shared_ptr node, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile, + rclcpp::CallbackGroup::SharedPtr group = nullptr) + : AsyncParametersClient( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + remote_node_name, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)), + group) + {} + + /** + * \param[in] node The async parameters client will be added to this node. * \param[in] remote_node_name (optional) name of the remote node - * \param[in] qos_profile (optional) The rmw qos profile to use to subscribe + * \param[in] qos_profile (optional) The qos profile to use to subscribe * \param[in] group (optional) The async parameter client will be added to this callback group. */ template explicit AsyncParametersClient( const std::shared_ptr node, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters, + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr) : AsyncParametersClient( node->get_node_base_interface(), @@ -96,16 +152,40 @@ class AsyncParametersClient /// Constructor /** - * \param[in] node The async paramters client will be added to this node. + * \param[in] node The async parameters client will be added to this node. + * \param[in] remote_node_name Name of the remote node + * \param[in] qos_profile The rmw qos profile to use to subscribe + * \param[in] group (optional) The async parameter client will be added to this callback group. + * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t + */ + template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + AsyncParametersClient( + NodeT * node, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile, + rclcpp::CallbackGroup::SharedPtr group = nullptr) + : AsyncParametersClient( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + remote_node_name, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)), + group) + {} + + /** + * \param[in] node The async parameters client will be added to this node. * \param[in] remote_node_name (optional) name of the remote node - * \param[in] qos_profile (optional) The rmw qos profile to use to subscribe + * \param[in] qos_profile (optional) The qos profile to use to subscribe * \param[in] group (optional) The async parameter client will be added to this callback group. */ template explicit AsyncParametersClient( NodeT * node, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters, + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr) : AsyncParametersClient( node->get_node_base_interface(), @@ -211,9 +291,7 @@ class AsyncParametersClient typename rclcpp::Subscription::SharedPtr on_parameter_event( CallbackT && callback, - const rclcpp::QoS & qos = ( - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)) - ), + const rclcpp::QoS & qos = rclcpp::ParameterEventsQoS(), const rclcpp::SubscriptionOptionsWithAllocator & options = ( rclcpp::SubscriptionOptionsWithAllocator() )) @@ -238,9 +316,7 @@ class AsyncParametersClient on_parameter_event( NodeT && node, CallbackT && callback, - const rclcpp::QoS & qos = ( - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)) - ), + const rclcpp::QoS & qos = rclcpp::ParameterEventsQoS(), const rclcpp::SubscriptionOptionsWithAllocator & options = ( rclcpp::SubscriptionOptionsWithAllocator() )) @@ -307,11 +383,24 @@ class SyncParametersClient public: RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient) + template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + SyncParametersClient( + std::shared_ptr node, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile) + : SyncParametersClient( + std::make_shared(), + node, + remote_node_name, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile))) + {} + template explicit SyncParametersClient( std::shared_ptr node, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters) + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()) : SyncParametersClient( std::make_shared(), node, @@ -319,12 +408,29 @@ class SyncParametersClient qos_profile) {} + template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + SyncParametersClient( + rclcpp::Executor::SharedPtr executor, + std::shared_ptr node, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile) + : SyncParametersClient( + executor, + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + remote_node_name, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile))) + {} + template SyncParametersClient( rclcpp::Executor::SharedPtr executor, std::shared_ptr node, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters) + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()) : SyncParametersClient( executor, node->get_node_base_interface(), @@ -335,11 +441,24 @@ class SyncParametersClient qos_profile) {} + template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + SyncParametersClient( + NodeT * node, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile) + : SyncParametersClient( + std::make_shared(), + node, + remote_node_name, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile))) + {} + template explicit SyncParametersClient( NodeT * node, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters) + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()) : SyncParametersClient( std::make_shared(), node, @@ -347,12 +466,29 @@ class SyncParametersClient qos_profile) {} + template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + SyncParametersClient( + rclcpp::Executor::SharedPtr executor, + NodeT * node, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile) + : SyncParametersClient( + executor, + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + remote_node_name, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile))) + {} + template SyncParametersClient( rclcpp::Executor::SharedPtr executor, NodeT * node, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters) + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()) : SyncParametersClient( executor, node->get_node_base_interface(), @@ -363,6 +499,28 @@ class SyncParametersClient qos_profile) {} + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + RCLCPP_PUBLIC + SyncParametersClient( + rclcpp::Executor::SharedPtr executor, + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile) + : executor_(executor), node_base_interface_(node_base_interface) + { + async_parameters_client_ = + std::make_shared( + node_base_interface, + node_topics_interface, + node_graph_interface, + node_services_interface, + remote_node_name, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile))); + } + RCLCPP_PUBLIC SyncParametersClient( rclcpp::Executor::SharedPtr executor, @@ -371,7 +529,7 @@ class SyncParametersClient const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters) + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()) : executor_(executor), node_base_interface_(node_base_interface) { async_parameters_client_ = diff --git a/rclcpp/include/rclcpp/parameter_service.hpp b/rclcpp/include/rclcpp/parameter_service.hpp index 143a5223c8..a952939aca 100644 --- a/rclcpp/include/rclcpp/parameter_service.hpp +++ b/rclcpp/include/rclcpp/parameter_service.hpp @@ -28,6 +28,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" #include "rclcpp/parameter.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/visibility_control.hpp" #include "rmw/rmw.h" @@ -39,12 +40,26 @@ class ParameterService public: RCLCPP_SMART_PTR_DEFINITIONS(ParameterService) + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] RCLCPP_PUBLIC - explicit ParameterService( + ParameterService( const std::shared_ptr node_base, const std::shared_ptr node_services, rclcpp::node_interfaces::NodeParametersInterface * node_params, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters); + const rmw_qos_profile_t & qos_profile) + : ParameterService( + node_base, + node_services, + node_params, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile))) + {} + + RCLCPP_PUBLIC + ParameterService( + const std::shared_ptr node_base, + const std::shared_ptr node_services, + rclcpp::node_interfaces::NodeParametersInterface * node_params, + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()); private: rclcpp::Service::SharedPtr get_parameters_service_; diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index 64e415e82f..b60585ef00 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -35,7 +35,7 @@ AsyncParametersClient::AsyncParametersClient( const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, const std::string & remote_node_name, - const rmw_qos_profile_t & qos_profile, + const rclcpp::QoS & qos_profile, rclcpp::CallbackGroup::SharedPtr group) : node_topics_interface_(node_topics_interface) { @@ -46,7 +46,7 @@ AsyncParametersClient::AsyncParametersClient( } rcl_client_options_t options = rcl_client_get_default_options(); - options.qos = qos_profile; + options.qos = qos_profile.get_rmw_qos_profile(); using rclcpp::Client; using rclcpp::ClientBase; diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp index 5c30917499..501ac399f8 100644 --- a/rclcpp/src/rclcpp/parameter_service.cpp +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -29,7 +29,7 @@ ParameterService::ParameterService( const std::shared_ptr node_base, const std::shared_ptr node_services, rclcpp::node_interfaces::NodeParametersInterface * node_params, - const rmw_qos_profile_t & qos_profile) + const rclcpp::QoS & qos_profile) { const std::string node_name = node_base->get_name(); From 4744fb6f500cbb3d7a0619f4ef6a7bdf34370e34 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 13 Sep 2022 20:28:54 +0000 Subject: [PATCH 136/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 29 +++++++++++++++++++++++++++++ rclcpp_action/CHANGELOG.rst | 6 ++++++ rclcpp_components/CHANGELOG.rst | 6 ++++++ rclcpp_lifecycle/CHANGELOG.rst | 10 ++++++++++ 4 files changed, 51 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 76ba0e5c4c..ac76f47319 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,35 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Make ParameterService and Sync/AsyncParameterClient accept rclcpp::QoS (`#1978 `_) +* support regex match for parameter client (`#1992 `_) +* operator+= and operator-= for Duration (`#1988 `_) +* Revert "Revert "Add a create_timer method to Node and `LifecycleNode` classes (`#1975 `_)" (`#2009 `_) (`#2010 `_) +* force compiler warning if callback handles not captured (`#2000 `_) +* Revert "Add a `create_timer` method to `Node` and `LifecycleNode` classes (`#1975 `_)" (`#2009 `_) +* Add a `create_timer` method to `Node` and `LifecycleNode` classes (`#1975 `_) +* [docs] add note about callback lifetime for {on, post}_set_parameter_callback (`#1981 `_) +* fix memory leak (`#1994 `_) +* Support pre-set and post-set parameter callbacks in addition to on-set-parameter-callback. (`#1947 `_) +* Make create_service accept rclcpp::QoS (`#1969 `_) +* Make create_client accept rclcpp::QoS (`#1964 `_) +* Fix the documentation for rclcpp::ok to be accurate. (`#1965 `_) +* use regex for wildcard matching (`#1839 `_) +* Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_)" (`#1956 `_) +* Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_) +* test adjustment for LoanedMessage. (`#1951 `_) +* fix virtual dispatch issues identified by clang-tidy (`#1816 `_) +* Remove unused on_parameters_set_callback\_ (`#1945 `_) +* Fix subscription.is_serialized() for callbacks with message info (`#1950 `_) +* wait for subscriptions on another thread. (`#1940 `_) +* Fix documentation of `RCLCPP\_[INFO,WARN,...]` (`#1943 `_) +* Always trigger guard condition waitset (`#1923 `_) +* Add statistics for handle_loaned_message (`#1927 `_) +* Drop wrong template specialization (`#1926 `_) +* Contributors: Alberto Soragna, Andrew Symington, Barry Xu, Brian, Chen Lihui, Chris Lalancette, Daniel Reuter, Deepanshu Bansal, Hubert Liberacki, Ivan Santiago Paunovic, Jochen Sprickerhof, Nikolai Morin, Shane Loretz, Tomoya Fujita, Tyler Weaver, William Woodall, schrodinbug + 16.2.0 (2022-05-03) ------------------- * Update get_parameter_from_event to follow the function description (`#1922 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 9838922b49..f519631256 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,12 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_)" (`#1956 `_) +* Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_) +* Contributors: Hubert Liberacki, William Woodall + 16.2.0 (2022-05-03) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 805592a07f..48724d03fc 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_)" (`#1956 `_) +* Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_) +* Contributors: Hubert Liberacki, William Woodall + 16.2.0 (2022-05-03) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 420252b66a..94901f6eb7 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,16 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Revert "Revert "Add a create_timer method to Node and `LifecycleNode` classes (`#1975 `_)" (`#2009 `_) (`#2010 `_) +* Revert "Add a `create_timer` method to `Node` and `LifecycleNode` classes (`#1975 `_)" (`#2009 `_) +* Add a `create_timer` method to `Node` and `LifecycleNode` classes (`#1975 `_) +* Support pre-set and post-set parameter callbacks in addition to on-set-parameter-callback. (`#1947 `_) +* Make create_service accept rclcpp::QoS (`#1969 `_) +* Make create_client accept rclcpp::QoS (`#1964 `_) +* Contributors: Andrew Symington, Deepanshu Bansal, Ivan Santiago Paunovic, Shane Loretz + 16.2.0 (2022-05-03) ------------------- From 7bc05da5d1e5cf3209af264e2c270ceecd4c9d18 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 13 Sep 2022 20:29:01 +0000 Subject: [PATCH 137/455] 17.0.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index ac76f47319..08914deb08 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +17.0.0 (2022-09-13) +------------------- * Make ParameterService and Sync/AsyncParameterClient accept rclcpp::QoS (`#1978 `_) * support regex match for parameter client (`#1992 `_) * operator+= and operator-= for Duration (`#1988 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 6de7d25d07..d05253db94 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 16.2.0 + 17.0.0 The ROS client library in C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index f519631256..ef4104bcac 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +17.0.0 (2022-09-13) +------------------- * Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_)" (`#1956 `_) * Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_) * Contributors: Hubert Liberacki, William Woodall diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index dd6e61338c..f9cbaeb721 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 16.2.0 + 17.0.0 Adds action APIs for C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 48724d03fc..c496eb406d 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +17.0.0 (2022-09-13) +------------------- * Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_)" (`#1956 `_) * Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_) * Contributors: Hubert Liberacki, William Woodall diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 18275306ff..2032fc84ee 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 16.2.0 + 17.0.0 Package containing tools for dynamically loadable components Ivan Paunovic Jacob Perron diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 94901f6eb7..8b88b682b1 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +17.0.0 (2022-09-13) +------------------- * Revert "Revert "Add a create_timer method to Node and `LifecycleNode` classes (`#1975 `_)" (`#2009 `_) (`#2010 `_) * Revert "Add a `create_timer` method to `Node` and `LifecycleNode` classes (`#1975 `_)" (`#2009 `_) * Add a `create_timer` method to `Node` and `LifecycleNode` classes (`#1975 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 3ff98f3069..85280c5e36 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 16.2.0 + 17.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Jacob Perron From 978439191fdb3924af900a506dd35b68f8da725c Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 15 Sep 2022 20:17:28 +0800 Subject: [PATCH 138/455] fix mismatched issue if using zero_allocate (#1995) * fix mismatched issue if uzing zero_allocated Signed-off-by: Chen Lihui --- .../rclcpp/allocator/allocator_common.hpp | 18 +++++++ .../allocator/test_allocator_common.cpp | 47 +++++++++++++++++++ 2 files changed, 65 insertions(+) diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp index d117376517..12b2f383b6 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_ #define RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_ +#include #include #include "rcl/allocator.h" @@ -39,6 +40,22 @@ void * retyped_allocate(size_t size, void * untyped_allocator) return std::allocator_traits::allocate(*typed_allocator, size); } +template +void * retyped_zero_allocate(size_t number_of_elem, size_t size_of_elem, void * untyped_allocator) +{ + auto typed_allocator = static_cast(untyped_allocator); + if (!typed_allocator) { + throw std::runtime_error("Received incorrect allocator type"); + } + size_t size = number_of_elem * size_of_elem; + void * allocated_memory = + std::allocator_traits::allocate(*typed_allocator, size); + if (allocated_memory) { + std::memset(allocated_memory, 0, size); + } + return allocated_memory; +} + template void retyped_deallocate(void * untyped_pointer, void * untyped_allocator) { @@ -73,6 +90,7 @@ rcl_allocator_t get_rcl_allocator(Alloc & allocator) rcl_allocator_t rcl_allocator = rcl_get_default_allocator(); #ifndef _WIN32 rcl_allocator.allocate = &retyped_allocate; + rcl_allocator.zero_allocate = &retyped_zero_allocate; rcl_allocator.deallocate = &retyped_deallocate; rcl_allocator.reallocate = &retyped_reallocate; rcl_allocator.state = &allocator; diff --git a/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp b/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp index 341846a3eb..4619b7665d 100644 --- a/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp +++ b/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp @@ -51,6 +51,53 @@ TEST(TestAllocatorCommon, retyped_allocate) { EXPECT_NO_THROW(code2()); } +TEST(TestAllocatorCommon, retyped_zero_allocate_basic) { + std::allocator allocator; + void * untyped_allocator = &allocator; + void * allocated_mem = + rclcpp::allocator::retyped_zero_allocate>(20u, 1u, untyped_allocator); + ASSERT_TRUE(nullptr != allocated_mem); + + auto code = [&untyped_allocator, allocated_mem]() { + rclcpp::allocator::retyped_deallocate>( + allocated_mem, untyped_allocator); + }; + EXPECT_NO_THROW(code()); +} + +TEST(TestAllocatorCommon, retyped_zero_allocate) { + std::allocator allocator; + void * untyped_allocator = &allocator; + void * allocated_mem = + rclcpp::allocator::retyped_zero_allocate>(20u, 1u, untyped_allocator); + // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static + // analysis throws a false-positive memory leak warning. Use ASSERT_TRUE instead. + ASSERT_TRUE(nullptr != allocated_mem); + + auto code = [&untyped_allocator, allocated_mem]() { + rclcpp::allocator::retyped_deallocate>( + allocated_mem, untyped_allocator); + }; + EXPECT_NO_THROW(code()); + + allocated_mem = allocator.allocate(1); + // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static + // analysis throws a false-positive memory leak warning. Use ASSERT_TRUE instead. + ASSERT_TRUE(nullptr != allocated_mem); + void * reallocated_mem = + rclcpp::allocator::retyped_reallocate>( + allocated_mem, 2u, untyped_allocator); + // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static + // analysis throws a false-positive memory leak warning. Use ASSERT_TRUE instead. + ASSERT_TRUE(nullptr != reallocated_mem); + + auto code2 = [&untyped_allocator, reallocated_mem]() { + rclcpp::allocator::retyped_deallocate>( + reallocated_mem, untyped_allocator); + }; + EXPECT_NO_THROW(code2()); +} + TEST(TestAllocatorCommon, get_rcl_allocator) { std::allocator allocator; auto rcl_allocator = rclcpp::allocator::get_rcl_allocator(allocator); From 6a8c61c0269187c9505632647f6a9f5698b523e8 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 20 Sep 2022 01:08:25 +0800 Subject: [PATCH 139/455] use unique ptr and remove unuseful container (#2013) Signed-off-by: Chen Lihui Signed-off-by: Chen Lihui --- rclcpp_components/src/node_main.cpp.in | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/rclcpp_components/src/node_main.cpp.in b/rclcpp_components/src/node_main.cpp.in index 0ca5eb8c61..71754d1f82 100644 --- a/rclcpp_components/src/node_main.cpp.in +++ b/rclcpp_components/src/node_main.cpp.in @@ -30,14 +30,13 @@ int main(int argc, char * argv[]) rclcpp::executors::@executor@ exec; rclcpp::NodeOptions options; options.arguments(args); - std::vector loaders; std::vector node_wrappers; std::string library_name = "@library_name@"; std::string class_name = "rclcpp_components::NodeFactoryTemplate<@component@>"; RCLCPP_DEBUG(logger, "Load library %s", library_name.c_str()); - auto loader = new class_loader::ClassLoader(library_name); + auto loader = std::make_unique(library_name); auto classes = loader->getAvailableClasses(); for (const auto & clazz : classes) { std::string name = clazz.c_str(); @@ -59,8 +58,6 @@ int main(int argc, char * argv[]) exec.add_node(node); } } - loaders.push_back(loader); - exec.spin(); From 145933b03793af008a54c9203185a23ec2fde9b0 Mon Sep 17 00:00:00 2001 From: mauropasse Date: Fri, 23 Sep 2022 00:30:59 +0100 Subject: [PATCH 140/455] Do not clear entities callbacks on destruction (#2002) * Do not clear entities callbacks on destruction Removing these clearings since they were not necessary, since the objects are being destroyed anyway. Signed-off-by: Mauro Passerino * Fix CI Signed-off-by: Mauro Passerino * Restore clear_on_ready_callback on ~QOSEventHandlerBase Needed since QOSEventHandlerBase does not own the pub/sub listeners. So the QOSEventHandler can be destroyed while the corresponding listeners are still alive, so we need to clear these callbacks. Signed-off-by: Mauro Passerino * Add coment on clearing callback for QoS event Signed-off-by: Mauro Passerino Signed-off-by: Mauro Passerino Co-authored-by: Mauro Passerino --- rclcpp/include/rclcpp/client.hpp | 2 +- .../experimental/subscription_intra_process_base.hpp | 2 +- rclcpp/include/rclcpp/service.hpp | 2 +- rclcpp/src/rclcpp/client.cpp | 7 ------- rclcpp/src/rclcpp/publisher_base.cpp | 5 ----- rclcpp/src/rclcpp/qos_event.cpp | 5 +++++ rclcpp/src/rclcpp/service.cpp | 4 ---- rclcpp/src/rclcpp/subscription_base.cpp | 7 ------- rclcpp/src/rclcpp/subscription_intra_process_base.cpp | 5 ----- rclcpp_action/src/client.cpp | 1 - rclcpp_action/src/server.cpp | 1 - 11 files changed, 8 insertions(+), 33 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index e88fa8a949..04e0ce9a33 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -130,7 +130,7 @@ class ClientBase rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph); RCLCPP_PUBLIC - virtual ~ClientBase(); + virtual ~ClientBase() = default; /// Take the next response for this client as a type erased pointer. /** diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 037d1aaa2a..6583e74ae7 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -52,7 +52,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable {} RCLCPP_PUBLIC - virtual ~SubscriptionIntraProcessBase(); + virtual ~SubscriptionIntraProcessBase() = default; RCLCPP_PUBLIC size_t diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 3f500eaa09..65b457ffda 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -55,7 +55,7 @@ class ServiceBase explicit ServiceBase(std::shared_ptr node_handle); RCLCPP_PUBLIC - virtual ~ServiceBase(); + virtual ~ServiceBase() = default; /// Return the name of the service. /** \return The name of the service. */ diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 17cc68e153..4adc6d4a96 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -65,13 +65,6 @@ 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(); -} - bool ClientBase::take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out) { diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index 723d56e0d5..9517af6d3c 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -102,11 +102,6 @@ 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 8bfd5b3304..91df5bb346 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -35,6 +35,11 @@ UnsupportedEventTypeException::UnsupportedEventTypeException( QOSEventHandlerBase::~QOSEventHandlerBase() { + // Since the rmw event listener holds a reference to + // this callback, we need to clear it on destruction of this class. + // This clearing is not needed for other rclcpp entities like pub/subs, since + // they do own the underlying rmw entities, which are destroyed + // on their rclcpp destructors, thus no risk of dangling pointers. if (on_new_event_callback_) { clear_on_ready_callback(); } diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 7bc580ce68..ea1a83a2a6 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -32,10 +32,6 @@ ServiceBase::ServiceBase(std::shared_ptr node_handle) node_logger_(rclcpp::get_node_logger(node_handle_.get())) {} -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 300f465a41..871321fbe8 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -87,13 +87,6 @@ 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 21ccb433ff..b13123b65b 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -17,11 +17,6 @@ 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_action/src/client.cpp b/rclcpp_action/src/client.cpp index f9144ecf49..2266b7cee6 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -136,7 +136,6 @@ ClientBase::ClientBase( ClientBase::~ClientBase() { - clear_on_ready_callback(); } bool diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index b0afb9aa50..42c4074495 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -132,7 +132,6 @@ ServerBase::ServerBase( ServerBase::~ServerBase() { - clear_on_ready_callback(); } size_t From 95837d34f1a717780bb629eaa5d889ee4d222fef Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 29 Sep 2022 07:54:41 -0400 Subject: [PATCH 141/455] Make sure to include-what-you-use in the node_interfaces. (#2018) Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/node_interfaces/node_base.hpp | 2 +- rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp | 4 ++-- rclcpp/include/rclcpp/node_interfaces/node_graph.hpp | 2 ++ rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp | 3 ++- .../rclcpp/node_interfaces/node_parameters_interface.hpp | 2 +- .../include/rclcpp/node_interfaces/node_topics_interface.hpp | 2 -- 6 files changed, 8 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index 90011a26e0..a6c84e4aa0 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -15,7 +15,7 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_ -#include +#include #include #include #include diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index b0f8fbd65e..2e94b80d9c 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -15,10 +15,10 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_ +#include +#include #include -#include #include -#include #include "rcl/node.h" diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp index 794038d386..d167515784 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -15,7 +15,9 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_ +#include #include +#include #include #include #include diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index eda68451a9..ffbb400c11 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -15,9 +15,10 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_ +#include #include #include -#include +#include #include #include diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index 104e00327a..3156668a73 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -15,8 +15,8 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_ +#include #include -#include #include #include diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp index ca69e86e73..7aa40a3d5e 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp @@ -15,8 +15,6 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_ -#include -#include #include #include "rcl/publisher.h" From dec766228dae4e050d37c8d5b8185383fdec76ab Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 4 Oct 2022 14:04:12 -0400 Subject: [PATCH 142/455] Cleanup the rclcpp_lifecycle dependencies. (#2021) That is, make sure they are all listed in package.xml, found in the CMakeLists.txt, and properly included where they are used. Signed-off-by: Chris Lalancette --- rclcpp_lifecycle/CMakeLists.txt | 97 +++++++------------ .../rclcpp_lifecycle/lifecycle_node.hpp | 8 +- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 16 ++- .../rclcpp_lifecycle/lifecycle_publisher.hpp | 3 + .../lifecycle_node_interface.hpp | 2 - .../include/rclcpp_lifecycle/state.hpp | 1 + .../include/rclcpp_lifecycle/transition.hpp | 1 + rclcpp_lifecycle/package.xml | 11 ++- rclcpp_lifecycle/src/lifecycle_node.cpp | 11 ++- .../src/lifecycle_node_interface_impl.hpp | 5 +- .../lifecycle_node_interface.cpp | 2 +- rclcpp_lifecycle/src/state.cpp | 1 + rclcpp_lifecycle/src/transition.cpp | 1 + rclcpp_lifecycle/test/test_client.cpp | 1 + .../test/test_lifecycle_service_client.cpp | 2 + rclcpp_lifecycle/test/test_service.cpp | 2 + .../test/test_transition_wrapper.cpp | 4 +- 17 files changed, 87 insertions(+), 81 deletions(-) diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 7a5b4cf2f0..70eea6f808 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -11,10 +11,13 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake_ros REQUIRED) +find_package(lifecycle_msgs REQUIRED) +find_package(rcl REQUIRED) find_package(rclcpp REQUIRED) +find_package(rcl_interfaces REQUIRED) find_package(rcl_lifecycle REQUIRED) +find_package(rcutils REQUIRED) find_package(rosidl_typesupport_cpp REQUIRED) -find_package(lifecycle_msgs REQUIRED) ### CPP High level library add_library(rclcpp_lifecycle @@ -28,12 +31,14 @@ target_include_directories(${PROJECT_NAME} PUBLIC "$" "$") -# specific order: dependents before dependencies -ament_target_dependencies(rclcpp_lifecycle - "rclcpp" - "rcl_lifecycle" - "lifecycle_msgs" - "rosidl_typesupport_cpp" +target_link_libraries(${PROJECT_NAME} + ${lifecycle_msgs_TARGETS} + rcl::rcl + rclcpp::rclcpp + ${rcl_interfaces_TARGETS} + rcl_lifecycle::rcl_lifecycle + rcutils::rcutils + rosidl_typesupport_cpp::rosidl_typesupport_cpp ) # Causes the visibility macros to use dllexport rather than dllimport, @@ -46,6 +51,9 @@ install(TARGETS LIBRARY DESTINATION lib RUNTIME DESTINATION bin) +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME}) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # Give cppcheck hints about macro definitions coming from outside this package @@ -58,15 +66,13 @@ if(BUILD_TESTING) benchmark_lifecycle_client test/benchmark/benchmark_lifecycle_client.cpp) if(TARGET benchmark_lifecycle_client) - target_link_libraries(benchmark_lifecycle_client ${PROJECT_NAME}) - ament_target_dependencies(benchmark_lifecycle_client rclcpp) + target_link_libraries(benchmark_lifecycle_client ${PROJECT_NAME} rclcpp::rclcpp) endif() add_performance_test( benchmark_lifecycle_node test/benchmark/benchmark_lifecycle_node.cpp) if(TARGET benchmark_lifecycle_node) - target_link_libraries(benchmark_lifecycle_node ${PROJECT_NAME}) - ament_target_dependencies(benchmark_lifecycle_node rclcpp) + target_link_libraries(benchmark_lifecycle_node ${PROJECT_NAME} rclcpp::rclcpp) endif() add_performance_test( benchmark_state @@ -83,31 +89,21 @@ if(BUILD_TESTING) ament_add_gtest(test_lifecycle_node test/test_lifecycle_node.cpp TIMEOUT 120) if(TARGET test_lifecycle_node) - ament_target_dependencies(test_lifecycle_node - "rcl_lifecycle" - "rclcpp" - "rcutils" - ) - target_link_libraries(test_lifecycle_node ${PROJECT_NAME} mimick) + target_link_libraries(test_lifecycle_node ${PROJECT_NAME} mimick rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp rcutils::rcutils) endif() ament_add_gtest(test_lifecycle_publisher test/test_lifecycle_publisher.cpp) if(TARGET test_lifecycle_publisher) - ament_target_dependencies(test_lifecycle_publisher - "rcl_lifecycle" - "rclcpp" - "test_msgs" - ) - target_link_libraries(test_lifecycle_publisher ${PROJECT_NAME}) + target_link_libraries(test_lifecycle_publisher ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp ${test_msgs_TARGETS}) endif() ament_add_gtest(test_lifecycle_service_client test/test_lifecycle_service_client.cpp TIMEOUT 120) if(TARGET test_lifecycle_service_client) - ament_target_dependencies(test_lifecycle_service_client - "rcl_lifecycle" - "rclcpp" - "rcpputils" - "rcutils" - ) - target_link_libraries(test_lifecycle_service_client ${PROJECT_NAME} mimick) + target_link_libraries(test_lifecycle_service_client + ${PROJECT_NAME} + mimick + rcl_lifecycle::rcl_lifecycle + rclcpp::rclcpp + rcpputils::rcpputils + rcutils::rcutils) endif() ament_add_gtest(test_client test/test_client.cpp TIMEOUT 120) if(TARGET test_client) @@ -127,44 +123,23 @@ if(BUILD_TESTING) endif() ament_add_gtest(test_state_machine_info test/test_state_machine_info.cpp) if(TARGET test_state_machine_info) - ament_target_dependencies(test_state_machine_info - "rcl_lifecycle" - "rclcpp" - ) - target_link_libraries(test_state_machine_info ${PROJECT_NAME}) + target_link_libraries(test_state_machine_info ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp) endif() ament_add_gtest(test_register_custom_callbacks test/test_register_custom_callbacks.cpp) if(TARGET test_register_custom_callbacks) - ament_target_dependencies(test_register_custom_callbacks - "rcl_lifecycle" - "rclcpp" - ) - target_link_libraries(test_register_custom_callbacks ${PROJECT_NAME}) + target_link_libraries(test_register_custom_callbacks ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp) endif() ament_add_gtest(test_callback_exceptions test/test_callback_exceptions.cpp) if(TARGET test_callback_exceptions) - ament_target_dependencies(test_callback_exceptions - "rcl_lifecycle" - "rclcpp" - ) - target_link_libraries(test_callback_exceptions ${PROJECT_NAME}) + target_link_libraries(test_callback_exceptions ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp) endif() ament_add_gtest(test_state_wrapper test/test_state_wrapper.cpp) if(TARGET test_state_wrapper) - ament_target_dependencies(test_state_wrapper - "rcl_lifecycle" - "rclcpp" - ) - target_link_libraries(test_state_wrapper ${PROJECT_NAME}) + target_link_libraries(test_state_wrapper ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp) endif() ament_add_gtest(test_transition_wrapper test/test_transition_wrapper.cpp) if(TARGET test_transition_wrapper) - ament_target_dependencies(test_transition_wrapper - "rcl_lifecycle" - "rclcpp" - "rcutils" - ) - target_link_libraries(test_transition_wrapper ${PROJECT_NAME} mimick) + target_link_libraries(test_transition_wrapper ${PROJECT_NAME} mimick rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp rcutils::rcutils) target_compile_definitions(test_transition_wrapper PUBLIC RCUTILS_ENABLE_FAULT_INJECTION ) @@ -178,11 +153,7 @@ ament_export_libraries(${PROJECT_NAME}) # Export modern CMake targets ament_export_targets(${PROJECT_NAME}) -# specific order: dependents before dependencies -ament_export_dependencies(rclcpp) -ament_export_dependencies(rcl_lifecycle) -ament_export_dependencies(lifecycle_msgs) -ament_package() +# Export dependencies +ament_export_dependencies(lifecycle_msgs rcl rclcpp rcl_interfaces rcl_lifecycle rcutils rosidl_typesupport_cpp) -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME}) +ament_package() diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 5e06876018..914cc4a2d3 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -36,6 +36,8 @@ #ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ #define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ +#include +#include #include #include #include @@ -49,13 +51,11 @@ #include "rcl_interfaces/msg/list_parameters_result.hpp" #include "rcl_interfaces/msg/parameter_descriptor.hpp" -#include "rcl_interfaces/msg/parameter_event.hpp" #include "rcl_interfaces/msg/set_parameters_result.hpp" #include "rclcpp/callback_group.hpp" #include "rclcpp/client.hpp" #include "rclcpp/clock.hpp" -#include "rclcpp/context.hpp" #include "rclcpp/event.hpp" #include "rclcpp/generic_publisher.hpp" #include "rclcpp/generic_subscription.hpp" @@ -83,6 +83,8 @@ #include "rclcpp/time.hpp" #include "rclcpp/timer.hpp" +#include "rmw/types.h" + #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -137,7 +139,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, const rclcpp::NodeOptions & options = rclcpp::NodeOptions(), bool enable_communication_interface = true); - /// Create a node based on the node name and a rclcpp::Context. + /// Create a node based on the node name /** * \param[in] node_name Name of the node. * \param[in] namespace_ Namespace of the node. diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index e2a5696b92..5ffc8a5f1b 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -15,29 +15,35 @@ #ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ #define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ +#include +#include +#include #include #include #include #include #include -#include "rclcpp/contexts/default_context.hpp" +#include "rcl_interfaces/msg/parameter_descriptor.hpp" + +#include "rclcpp/callback_group.hpp" #include "rclcpp/create_client.hpp" #include "rclcpp/create_generic_publisher.hpp" #include "rclcpp/create_generic_subscription.hpp" #include "rclcpp/create_publisher.hpp" #include "rclcpp/create_service.hpp" #include "rclcpp/create_subscription.hpp" -#include "rclcpp/event.hpp" -#include "rclcpp/experimental/intra_process_manager.hpp" #include "rclcpp/parameter.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/subscription_options.hpp" #include "rclcpp/type_support_decl.hpp" -#include "lifecycle_publisher.hpp" -#include "rclcpp_lifecycle/visibility_control.h" +#include "rmw/types.h" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" +#include "rclcpp_lifecycle/visibility_control.h" namespace rclcpp_lifecycle { diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp index 8df08dcb41..fa7c5b0b5e 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -20,7 +20,10 @@ #include #include "rclcpp/logging.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/publisher.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp_lifecycle/managed_entity.hpp" diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp index 9f2459e296..90f98ec8bc 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp @@ -17,8 +17,6 @@ #include "lifecycle_msgs/msg/transition.hpp" -#include "rcl_lifecycle/rcl_lifecycle.h" - #include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/visibility_control.h" diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp index a0ac997ff3..86750f1fed 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp @@ -18,6 +18,7 @@ #include #include "rcl_lifecycle/data_types.h" + #include "rclcpp_lifecycle/visibility_control.h" #include "rcutils/allocator.h" diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp index 874be69aa6..4093579e04 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp @@ -18,6 +18,7 @@ #include #include "rcl_lifecycle/data_types.h" + #include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/visibility_control.h" diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 85280c5e36..cfd97eafd6 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -13,24 +13,29 @@ ament_cmake_ros lifecycle_msgs + rcl rclcpp + rcl_interfaces rcl_lifecycle + rcutils + rmw rosidl_typesupport_cpp lifecycle_msgs + rcl rclcpp + rcl_interfaces rcl_lifecycle + rcutils + rmw rosidl_typesupport_cpp - rmw - ament_cmake_gtest ament_lint_auto ament_lint_common mimick_vendor performance_test_fixture rcpputils - rcutils test_msgs diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 0851c596ec..6765b8037e 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -14,15 +14,22 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include +#include +#include #include #include -#include +#include +#include #include +#include #include "lifecycle_msgs/msg/state.hpp" #include "lifecycle_msgs/msg/transition.hpp" +#include "rcl_interfaces/msg/list_parameters_result.hpp" +#include "rcl_interfaces/msg/parameter_descriptor.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" + #include "rclcpp/exceptions.hpp" #include "rclcpp/graph_listener.hpp" #include "rclcpp/logger.hpp" diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 51de8eab07..4b9a3ff5fd 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -20,9 +20,10 @@ #include #include #include +#include #include -#include #include +#include #include "lifecycle_msgs/msg/transition_description.hpp" #include "lifecycle_msgs/msg/transition_event.h" // for getting the c-typesupport @@ -42,6 +43,8 @@ #include "rcutils/logging_macros.h" +#include "rmw/types.h" + namespace rclcpp_lifecycle { diff --git a/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp b/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp index 1c8a5a646a..06b889be05 100644 --- a/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp +++ b/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp @@ -14,7 +14,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rcl_lifecycle/rcl_lifecycle.h" +#include "rclcpp_lifecycle/state.hpp" namespace rclcpp_lifecycle { diff --git a/rclcpp_lifecycle/src/state.cpp b/rclcpp_lifecycle/src/state.cpp index f7aca2688e..959b40ed2a 100644 --- a/rclcpp_lifecycle/src/state.cpp +++ b/rclcpp_lifecycle/src/state.cpp @@ -14,6 +14,7 @@ #include "rclcpp_lifecycle/state.hpp" +#include #include #include "lifecycle_msgs/msg/state.hpp" diff --git a/rclcpp_lifecycle/src/transition.cpp b/rclcpp_lifecycle/src/transition.cpp index 50f0b33735..360c1cfb13 100644 --- a/rclcpp_lifecycle/src/transition.cpp +++ b/rclcpp_lifecycle/src/transition.cpp @@ -14,6 +14,7 @@ #include "rclcpp_lifecycle/transition.hpp" +#include #include #include "lifecycle_msgs/msg/transition.hpp" diff --git a/rclcpp_lifecycle/test/test_client.cpp b/rclcpp_lifecycle/test/test_client.cpp index 89e1f42fa6..7ded8e346e 100644 --- a/rclcpp_lifecycle/test/test_client.cpp +++ b/rclcpp_lifecycle/test/test_client.cpp @@ -24,6 +24,7 @@ #include "rcl_interfaces/srv/list_parameters.hpp" +#include "rmw/qos_profiles.h" class TestClient : public ::testing::Test { diff --git a/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp index 6dd2c0ec17..3698c807f6 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp @@ -33,6 +33,8 @@ #include "lifecycle_msgs/srv/get_available_transitions.hpp" #include "lifecycle_msgs/srv/get_state.hpp" +#include "rcl_lifecycle/rcl_lifecycle.h" + #include "rclcpp/node_interfaces/node_graph.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" diff --git a/rclcpp_lifecycle/test/test_service.cpp b/rclcpp_lifecycle/test/test_service.cpp index 4084b0da0c..b3108a9042 100644 --- a/rclcpp_lifecycle/test/test_service.cpp +++ b/rclcpp_lifecycle/test/test_service.cpp @@ -22,6 +22,8 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rmw/qos_profiles.h" + #include "test_msgs/srv/empty.hpp" using namespace std::chrono_literals; diff --git a/rclcpp_lifecycle/test/test_transition_wrapper.cpp b/rclcpp_lifecycle/test/test_transition_wrapper.cpp index 4f72d90bb6..39791d5da2 100644 --- a/rclcpp_lifecycle/test/test_transition_wrapper.cpp +++ b/rclcpp_lifecycle/test/test_transition_wrapper.cpp @@ -18,10 +18,12 @@ #include #include -#include "rcutils/testing/fault_injection.h" +#include "rcl_lifecycle/rcl_lifecycle.h" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rcutils/testing/fault_injection.h" + #include "./mocking_utils/patch.hpp" class TestTransitionWrapper : public ::testing::Test From 3aca271ef5d25c1b8c12e67ecf85166856a88000 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crist=C3=B3bal=20Arroyo?= <69475004+Crola1702@users.noreply.github.com> Date: Mon, 10 Oct 2022 06:17:24 -0700 Subject: [PATCH 143/455] Set cpplint test timeout to 3 minutes (#2022) Signed-off-by: Crola1702 --- rclcpp/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 9572f422a8..3abd999f3d 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -259,4 +259,8 @@ if(TEST cppcheck) set_tests_properties(cppcheck PROPERTIES TIMEOUT 500) endif() +if(TEST cpplint) + set_tests_properties(cpplint PROPERTIES TIMEOUT 180) +endif() + ament_generate_version_header(${PROJECT_NAME}) From b9b1468d15c7ddc697c079e6934d54f183294280 Mon Sep 17 00:00:00 2001 From: uupks Date: Thu, 13 Oct 2022 20:30:02 +0800 Subject: [PATCH 144/455] check thread whether joinable before join (#2019) Signed-off-by: uupks --- rclcpp/src/rclcpp/signal_handler.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/signal_handler.cpp b/rclcpp/src/rclcpp/signal_handler.cpp index 35ceb56781..7085c63bdf 100644 --- a/rclcpp/src/rclcpp/signal_handler.cpp +++ b/rclcpp/src/rclcpp/signal_handler.cpp @@ -191,7 +191,9 @@ SignalHandler::uninstall() signal_handlers_options_ = SignalHandlerOptions::None; RCLCPP_DEBUG(get_logger(), "SignalHandler::uninstall(): notifying deferred signal handler"); notify_signal_handler(); - signal_handler_thread_.join(); + if (signal_handler_thread_.joinable()) { + signal_handler_thread_.join(); + } teardown_wait_for_signal(); } catch (...) { installed_.exchange(true); From 28890bf126ff16adc1fb4763a7f9750d7397790d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 24 Oct 2022 08:43:17 -0400 Subject: [PATCH 145/455] Cleanup the lifecycle implementation (#2027) * Split lifecycle_node_interface_impl into header and implementation. There is no reason it should all be in the header file. No functional change. * Mark LifecycleNodeInterfaceImpl as final. * Update documentation about return codes. * Mark a bunch of LifecycleNodeInterfaceImpl methods as const. * Make most of LifecycleNodeInterfaceImpl private. * Mark some LifecycleNode methods as const. * Disable copies on LifecycleNodeInterfaceImpl. Signed-off-by: Chris Lalancette --- rclcpp_lifecycle/CMakeLists.txt | 1 + .../rclcpp_lifecycle/lifecycle_node.hpp | 6 +- .../lifecycle_node_interface.hpp | 10 +- rclcpp_lifecycle/src/lifecycle_node.cpp | 6 +- .../src/lifecycle_node_interface_impl.cpp | 540 ++++++++++++++++++ .../src/lifecycle_node_interface_impl.hpp | 514 ++--------------- 6 files changed, 612 insertions(+), 465 deletions(-) create mode 100644 rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 70eea6f808..b3461a7333 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(rosidl_typesupport_cpp REQUIRED) ### CPP High level library add_library(rclcpp_lifecycle src/lifecycle_node.cpp + src/lifecycle_node_interface_impl.cpp src/managed_entity.cpp src/node_interfaces/lifecycle_node_interface.cpp src/state.cpp diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 914cc4a2d3..80ec76a769 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -847,7 +847,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC std::vector - get_available_states(); + get_available_states() const; /// Return a list with the current available transitions. /** @@ -855,7 +855,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC std::vector - get_available_transitions(); + get_available_transitions() const; /// Return a list with all the transitions. /** @@ -863,7 +863,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC std::vector - get_transition_graph(); + get_transition_graph() const; /// Trigger the specified transition. /* diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp index 90f98ec8bc..e653ad412d 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp @@ -61,7 +61,7 @@ class LifecycleNodeInterface /// Callback function for cleanup transition /* - * \return true by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC virtual CallbackReturn @@ -69,7 +69,7 @@ class LifecycleNodeInterface /// Callback function for shutdown transition /* - * \return true by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC virtual CallbackReturn @@ -77,7 +77,7 @@ class LifecycleNodeInterface /// Callback function for activate transition /* - * \return true by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC virtual CallbackReturn @@ -85,7 +85,7 @@ class LifecycleNodeInterface /// Callback function for deactivate transition /* - * \return true by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC virtual CallbackReturn @@ -93,7 +93,7 @@ class LifecycleNodeInterface /// Callback function for errorneous transition /* - * \return false by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC virtual CallbackReturn diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 6765b8037e..f2db5f3950 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -541,19 +541,19 @@ LifecycleNode::get_current_state() } std::vector -LifecycleNode::get_available_states() +LifecycleNode::get_available_states() const { return impl_->get_available_states(); } std::vector -LifecycleNode::get_available_transitions() +LifecycleNode::get_available_transitions() const { return impl_->get_available_transitions(); } std::vector -LifecycleNode::get_transition_graph() +LifecycleNode::get_transition_graph() const { return impl_->get_transition_graph(); } diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp new file mode 100644 index 0000000000..f176cbf535 --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp @@ -0,0 +1,540 @@ +// Copyright 2016 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. + +#include +#include +#include +#include +#include +#include + +#include "lifecycle_msgs/msg/transition_description.hpp" +#include "lifecycle_msgs/msg/transition_event.h" // for getting the c-typesupport +#include "lifecycle_msgs/msg/transition_event.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" +#include "lifecycle_msgs/srv/get_state.hpp" +#include "lifecycle_msgs/srv/get_available_states.hpp" +#include "lifecycle_msgs/srv/get_available_transitions.hpp" + +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" + +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +#include "rcl/error_handling.h" +#include "rcl/node.h" + +#include "rcl_lifecycle/rcl_lifecycle.h" +#include "rcl_lifecycle/transition_map.h" + +#include "rcutils/logging_macros.h" + +#include "rmw/types.h" + +#include "lifecycle_node_interface_impl.hpp" + +namespace rclcpp_lifecycle +{ + +LifecycleNode::LifecycleNodeInterfaceImpl::LifecycleNodeInterfaceImpl( + std::shared_ptr node_base_interface, + std::shared_ptr node_services_interface) +: node_base_interface_(node_base_interface), + node_services_interface_(node_services_interface) +{ +} + +LifecycleNode::LifecycleNodeInterfaceImpl::~LifecycleNodeInterfaceImpl() +{ + rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); + auto ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_FATAL_NAMED( + "rclcpp_lifecycle", + "failed to destroy rcl_state_machine"); + } +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interface) +{ + rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); + const rcl_node_options_t * node_options = + rcl_node_get_options(node_base_interface_->get_rcl_node_handle()); + state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); + auto state_machine_options = rcl_lifecycle_get_default_state_machine_options(); + state_machine_options.enable_com_interface = enable_communication_interface; + state_machine_options.allocator = node_options->allocator; + + // The call to initialize the state machine takes + // currently five different typesupports for all publishers/services + // created within the RCL_LIFECYCLE structure. + // The publisher takes a C-Typesupport since the publishing (i.e. creating + // the message) is done fully in RCL. + // Services are handled in C++, so that it needs a C++ typesupport structure. + rcl_ret_t ret = rcl_lifecycle_state_machine_init( + &state_machine_, + node_handle, + ROSIDL_GET_MSG_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + &state_machine_options); + if (ret != RCL_RET_OK) { + throw std::runtime_error( + std::string("Couldn't initialize state machine for node ") + + node_base_interface_->get_name()); + } + + if (enable_communication_interface) { + { // change_state + auto cb = std::bind( + &LifecycleNode::LifecycleNodeInterfaceImpl::on_change_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_change_state_ = std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_change_state, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_change_state_), + nullptr); + } + + { // get_state + auto cb = std::bind( + &LifecycleNode::LifecycleNodeInterfaceImpl::on_get_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_state_ = std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_state, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_state_), + nullptr); + } + + { // get_available_states + auto cb = std::bind( + &LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_states, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_available_states_ = std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_available_states, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_available_states_), + nullptr); + } + + { // get_available_transitions + auto cb = std::bind( + &LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_transitions, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_available_transitions_ = + std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_available_transitions, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_available_transitions_), + nullptr); + } + + { // get_transition_graph + auto cb = std::bind( + &LifecycleNode::LifecycleNodeInterfaceImpl::on_get_transition_graph, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_transition_graph_ = + std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_transition_graph, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_transition_graph_), + nullptr); + } + } +} + +bool +LifecycleNode::LifecycleNodeInterfaceImpl::register_callback( + std::uint8_t lifecycle_transition, + std::function & cb) +{ + cb_map_[lifecycle_transition] = cb; + return true; +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_change_state( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) +{ + (void)header; + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get state. State machine is not initialized."); + } + + auto transition_id = req->transition.id; + // if there's a label attached to the request, + // we check the transition attached to this label. + // we further can't compare the id of the looked up transition + // because ros2 service call defaults all intergers to zero. + // that means if we call ros2 service call ... {transition: {label: shutdown}} + // the id of the request is 0 (zero) whereas the id from the lookup up transition + // can be different. + // the result of this is that the label takes presedence of the id. + if (req->transition.label.size() != 0) { + auto rcl_transition = rcl_lifecycle_get_transition_by_label( + state_machine_.current_state, req->transition.label.c_str()); + if (rcl_transition == nullptr) { + resp->success = false; + return; + } + transition_id = static_cast(rcl_transition->id); + } + + node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code; + auto ret = change_state(transition_id, cb_return_code); + (void) ret; + // TODO(karsten1987): Lifecycle msgs have to be extended to keep both returns + // 1. return is the actual transition + // 2. return is whether an error occurred or not + resp->success = + (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_get_state( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) const +{ + (void)header; + (void)req; + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get state. State machine is not initialized."); + } + resp->current_state.id = static_cast(state_machine_.current_state->id); + resp->current_state.label = state_machine_.current_state->label; +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_states( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) const +{ + (void)header; + (void)req; + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get available states. State machine is not initialized."); + } + + resp->available_states.resize(state_machine_.transition_map.states_size); + for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) { + resp->available_states[i].id = + static_cast(state_machine_.transition_map.states[i].id); + resp->available_states[i].label = + static_cast(state_machine_.transition_map.states[i].label); + } +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_transitions( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) const +{ + (void)header; + (void)req; + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get available transitions. State machine is not initialized."); + } + + resp->available_transitions.resize(state_machine_.current_state->valid_transition_size); + for (unsigned int i = 0; i < state_machine_.current_state->valid_transition_size; ++i) { + lifecycle_msgs::msg::TransitionDescription & trans_desc = resp->available_transitions[i]; + + auto rcl_transition = state_machine_.current_state->valid_transitions[i]; + trans_desc.transition.id = static_cast(rcl_transition.id); + trans_desc.transition.label = rcl_transition.label; + trans_desc.start_state.id = static_cast(rcl_transition.start->id); + trans_desc.start_state.label = rcl_transition.start->label; + trans_desc.goal_state.id = static_cast(rcl_transition.goal->id); + trans_desc.goal_state.label = rcl_transition.goal->label; + } +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_get_transition_graph( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) const +{ + (void)header; + (void)req; + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get available transitions. State machine is not initialized."); + } + + resp->available_transitions.resize(state_machine_.transition_map.transitions_size); + for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) { + lifecycle_msgs::msg::TransitionDescription & trans_desc = resp->available_transitions[i]; + + auto rcl_transition = state_machine_.transition_map.transitions[i]; + trans_desc.transition.id = static_cast(rcl_transition.id); + trans_desc.transition.label = rcl_transition.label; + trans_desc.start_state.id = static_cast(rcl_transition.start->id); + trans_desc.start_state.label = rcl_transition.start->label; + trans_desc.goal_state.id = static_cast(rcl_transition.goal->id); + trans_desc.goal_state.label = rcl_transition.goal->label; + } +} + +const State & +LifecycleNode::LifecycleNodeInterfaceImpl::get_current_state() +{ + current_state_ = State(state_machine_.current_state); + return current_state_; +} + +std::vector +LifecycleNode::LifecycleNodeInterfaceImpl::get_available_states() const +{ + std::vector states; + states.reserve(state_machine_.transition_map.states_size); + + for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) { + states.emplace_back(&state_machine_.transition_map.states[i]); + } + return states; +} + +std::vector +LifecycleNode::LifecycleNodeInterfaceImpl::get_available_transitions() const +{ + std::vector transitions; + transitions.reserve(state_machine_.current_state->valid_transition_size); + + for (unsigned int i = 0; i < state_machine_.current_state->valid_transition_size; ++i) { + transitions.emplace_back(&state_machine_.current_state->valid_transitions[i]); + } + return transitions; +} + +std::vector +LifecycleNode::LifecycleNodeInterfaceImpl::get_transition_graph() const +{ + std::vector transitions; + transitions.reserve(state_machine_.transition_map.transitions_size); + + for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) { + transitions.emplace_back(&state_machine_.transition_map.transitions[i]); + } + return transitions; +} + +rcl_ret_t +LifecycleNode::LifecycleNodeInterfaceImpl::change_state( + std::uint8_t transition_id, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code) +{ + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR( + "Unable to change state for state machine for %s: %s", + node_base_interface_->get_name(), rcl_get_error_string().str); + return RCL_RET_ERROR; + } + + constexpr bool publish_update = true; + // keep the initial state to pass to a transition callback + State initial_state(state_machine_.current_state); + + if ( + rcl_lifecycle_trigger_transition_by_id( + &state_machine_, transition_id, publish_update) != RCL_RET_OK) + { + RCUTILS_LOG_ERROR( + "Unable to start transition %u from current state %s: %s", + transition_id, state_machine_.current_state->label, rcl_get_error_string().str); + rcutils_reset_error(); + return RCL_RET_ERROR; + } + + auto get_label_for_return_code = + [](node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code) -> const char *{ + auto cb_id = static_cast(cb_return_code); + if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS) { + return rcl_lifecycle_transition_success_label; + } else if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE) { + return rcl_lifecycle_transition_failure_label; + } + return rcl_lifecycle_transition_error_label; + }; + + cb_return_code = execute_callback(state_machine_.current_state->id, initial_state); + auto transition_label = get_label_for_return_code(cb_return_code); + + if ( + rcl_lifecycle_trigger_transition_by_label( + &state_machine_, transition_label, publish_update) != RCL_RET_OK) + { + RCUTILS_LOG_ERROR( + "Failed to finish transition %u. Current state is now: %s (%s)", + transition_id, state_machine_.current_state->label, rcl_get_error_string().str); + rcutils_reset_error(); + return RCL_RET_ERROR; + } + + // error handling ?! + // TODO(karsten1987): iterate over possible ret value + if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) { + RCUTILS_LOG_WARN("Error occurred while doing error handling."); + + auto error_cb_code = execute_callback(state_machine_.current_state->id, initial_state); + auto error_cb_label = get_label_for_return_code(error_cb_code); + if ( + rcl_lifecycle_trigger_transition_by_label( + &state_machine_, error_cb_label, publish_update) != RCL_RET_OK) + { + RCUTILS_LOG_ERROR("Failed to call cleanup on error state: %s", rcl_get_error_string().str); + rcutils_reset_error(); + return RCL_RET_ERROR; + } + } + // This true holds in both cases where the actual callback + // was successful or not, since at this point we have a valid transistion + // to either a new primary state or error state + return RCL_RET_OK; +} + +node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleNode::LifecycleNodeInterfaceImpl::execute_callback( + unsigned int cb_id, const State & previous_state) const +{ + // in case no callback was attached, we forward directly + auto cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + + auto it = cb_map_.find(static_cast(cb_id)); + if (it != cb_map_.end()) { + auto callback = it->second; + try { + cb_success = callback(State(previous_state)); + } catch (const std::exception & e) { + RCUTILS_LOG_ERROR("Caught exception in callback for transition %d", it->first); + RCUTILS_LOG_ERROR("Original error: %s", e.what()); + cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + } + return cb_success; +} + +const State & LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition( + const char * transition_label) +{ + node_interfaces::LifecycleNodeInterface::CallbackReturn error; + return trigger_transition(transition_label, error); +} + +const State & LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition( + const char * transition_label, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code) +{ + auto transition = + rcl_lifecycle_get_transition_by_label(state_machine_.current_state, transition_label); + if (transition) { + change_state(static_cast(transition->id), cb_return_code); + } + return get_current_state(); +} + +const State & +LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition(uint8_t transition_id) +{ + node_interfaces::LifecycleNodeInterface::CallbackReturn error; + change_state(transition_id, error); + (void) error; + return get_current_state(); +} + +const State & +LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition( + uint8_t transition_id, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code) +{ + change_state(transition_id, cb_return_code); + return get_current_state(); +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::add_managed_entity( + std::weak_ptr managed_entity) +{ + weak_managed_entities_.push_back(managed_entity); +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::add_timer_handle( + std::shared_ptr timer) +{ + weak_timers_.push_back(timer); +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_activate() const +{ + for (const auto & weak_entity : weak_managed_entities_) { + auto entity = weak_entity.lock(); + if (entity) { + entity->on_activate(); + } + } +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_deactivate() const +{ + for (const auto & weak_entity : weak_managed_entities_) { + auto entity = weak_entity.lock(); + if (entity) { + entity->on_deactivate(); + } + } +} + +} // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 4b9a3ff5fd..d973278005 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -20,35 +20,28 @@ #include #include #include -#include -#include -#include #include -#include "lifecycle_msgs/msg/transition_description.hpp" -#include "lifecycle_msgs/msg/transition_event.h" // for getting the c-typesupport #include "lifecycle_msgs/msg/transition_event.hpp" #include "lifecycle_msgs/srv/change_state.hpp" #include "lifecycle_msgs/srv/get_state.hpp" #include "lifecycle_msgs/srv/get_available_states.hpp" #include "lifecycle_msgs/srv/get_available_transitions.hpp" -#include "rcl/error_handling.h" - #include "rcl_lifecycle/rcl_lifecycle.h" -#include "rcl_lifecycle/transition_map.h" +#include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" -#include "rcutils/logging_macros.h" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rmw/types.h" namespace rclcpp_lifecycle { -class LifecycleNode::LifecycleNodeInterfaceImpl +class LifecycleNode::LifecycleNodeInterfaceImpl final { using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; using GetStateSrv = lifecycle_msgs::srv::GetState; @@ -59,489 +52,102 @@ class LifecycleNode::LifecycleNodeInterfaceImpl public: LifecycleNodeInterfaceImpl( std::shared_ptr node_base_interface, - std::shared_ptr node_services_interface) - : node_base_interface_(node_base_interface), - node_services_interface_(node_services_interface) - {} - - ~LifecycleNodeInterfaceImpl() - { - rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); - auto ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle); - if (ret != RCL_RET_OK) { - RCUTILS_LOG_FATAL_NAMED( - "rclcpp_lifecycle", - "failed to destroy rcl_state_machine"); - } - } + std::shared_ptr node_services_interface); + + ~LifecycleNodeInterfaceImpl(); void - init(bool enable_communication_interface = true) - { - rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); - const rcl_node_options_t * node_options = - rcl_node_get_options(node_base_interface_->get_rcl_node_handle()); - state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); - auto state_machine_options = rcl_lifecycle_get_default_state_machine_options(); - state_machine_options.enable_com_interface = enable_communication_interface; - state_machine_options.allocator = node_options->allocator; - - // The call to initialize the state machine takes - // currently five different typesupports for all publishers/services - // created within the RCL_LIFECYCLE structure. - // The publisher takes a C-Typesupport since the publishing (i.e. creating - // the message) is done fully in RCL. - // Services are handled in C++, so that it needs a C++ typesupport structure. - rcl_ret_t ret = rcl_lifecycle_state_machine_init( - &state_machine_, - node_handle, - ROSIDL_GET_MSG_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent), - rosidl_typesupport_cpp::get_service_type_support_handle(), - rosidl_typesupport_cpp::get_service_type_support_handle(), - rosidl_typesupport_cpp::get_service_type_support_handle(), - rosidl_typesupport_cpp::get_service_type_support_handle(), - rosidl_typesupport_cpp::get_service_type_support_handle(), - &state_machine_options); - if (ret != RCL_RET_OK) { - throw std::runtime_error( - std::string("Couldn't initialize state machine for node ") + - node_base_interface_->get_name()); - } - - if (enable_communication_interface) { - { // change_state - auto cb = std::bind( - &LifecycleNodeInterfaceImpl::on_change_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_change_state_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_change_state, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_change_state_), - nullptr); - } - - { // get_state - auto cb = std::bind( - &LifecycleNodeInterfaceImpl::on_get_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_get_state_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_get_state, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_state_), - nullptr); - } - - { // get_available_states - auto cb = std::bind( - &LifecycleNodeInterfaceImpl::on_get_available_states, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_get_available_states_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_get_available_states, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_available_states_), - nullptr); - } - - { // get_available_transitions - auto cb = std::bind( - &LifecycleNodeInterfaceImpl::on_get_available_transitions, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_get_available_transitions_ = - std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_get_available_transitions, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_available_transitions_), - nullptr); - } - - { // get_transition_graph - auto cb = std::bind( - &LifecycleNodeInterfaceImpl::on_get_transition_graph, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_get_transition_graph_ = - std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_get_transition_graph, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_transition_graph_), - nullptr); - } - } - } + init(bool enable_communication_interface = true); bool register_callback( std::uint8_t lifecycle_transition, - std::function & cb) - { - cb_map_[lifecycle_transition] = cb; - return true; - } + std::function & cb); + + const State & + get_current_state(); + + std::vector + get_available_states() const; + + std::vector + get_available_transitions() const; + + std::vector + get_transition_graph() const; + + const State & + trigger_transition(uint8_t transition_id); + + const State & + trigger_transition( + uint8_t transition_id, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code); + + const State & trigger_transition(const char * transition_label); + + const State & trigger_transition( + const char * transition_label, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code); + + void + on_activate() const; + + void + on_deactivate() const; + + void + add_managed_entity(std::weak_ptr managed_entity); + + void + add_timer_handle(std::shared_ptr timer); + +private: + RCLCPP_DISABLE_COPY(LifecycleNodeInterfaceImpl) void on_change_state( const std::shared_ptr header, const std::shared_ptr req, - std::shared_ptr resp) - { - (void)header; - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get state. State machine is not initialized."); - } - - auto transition_id = req->transition.id; - // if there's a label attached to the request, - // we check the transition attached to this label. - // we further can't compare the id of the looked up transition - // because ros2 service call defaults all intergers to zero. - // that means if we call ros2 service call ... {transition: {label: shutdown}} - // the id of the request is 0 (zero) whereas the id from the lookup up transition - // can be different. - // the result of this is that the label takes presedence of the id. - if (req->transition.label.size() != 0) { - auto rcl_transition = rcl_lifecycle_get_transition_by_label( - state_machine_.current_state, req->transition.label.c_str()); - if (rcl_transition == nullptr) { - resp->success = false; - return; - } - transition_id = static_cast(rcl_transition->id); - } - - node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code; - auto ret = change_state(transition_id, cb_return_code); - (void) ret; - // TODO(karsten1987): Lifecycle msgs have to be extended to keep both returns - // 1. return is the actual transition - // 2. return is whether an error occurred or not - resp->success = - (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - } + std::shared_ptr resp); void on_get_state( const std::shared_ptr header, const std::shared_ptr req, - std::shared_ptr resp) - { - (void)header; - (void)req; - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get state. State machine is not initialized."); - } - resp->current_state.id = static_cast(state_machine_.current_state->id); - resp->current_state.label = state_machine_.current_state->label; - } + std::shared_ptr resp) const; void on_get_available_states( const std::shared_ptr header, const std::shared_ptr req, - std::shared_ptr resp) - { - (void)header; - (void)req; - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get available states. State machine is not initialized."); - } - - resp->available_states.resize(state_machine_.transition_map.states_size); - for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) { - resp->available_states[i].id = - static_cast(state_machine_.transition_map.states[i].id); - resp->available_states[i].label = - static_cast(state_machine_.transition_map.states[i].label); - } - } + std::shared_ptr resp) const; void on_get_available_transitions( const std::shared_ptr header, const std::shared_ptr req, - std::shared_ptr resp) - { - (void)header; - (void)req; - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get available transitions. State machine is not initialized."); - } - - resp->available_transitions.resize(state_machine_.current_state->valid_transition_size); - for (unsigned int i = 0; i < state_machine_.current_state->valid_transition_size; ++i) { - lifecycle_msgs::msg::TransitionDescription & trans_desc = resp->available_transitions[i]; - - auto rcl_transition = state_machine_.current_state->valid_transitions[i]; - trans_desc.transition.id = static_cast(rcl_transition.id); - trans_desc.transition.label = rcl_transition.label; - trans_desc.start_state.id = static_cast(rcl_transition.start->id); - trans_desc.start_state.label = rcl_transition.start->label; - trans_desc.goal_state.id = static_cast(rcl_transition.goal->id); - trans_desc.goal_state.label = rcl_transition.goal->label; - } - } + std::shared_ptr resp) const; void on_get_transition_graph( const std::shared_ptr header, const std::shared_ptr req, - std::shared_ptr resp) - { - (void)header; - (void)req; - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get available transitions. State machine is not initialized."); - } - - resp->available_transitions.resize(state_machine_.transition_map.transitions_size); - for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) { - lifecycle_msgs::msg::TransitionDescription & trans_desc = resp->available_transitions[i]; - - auto rcl_transition = state_machine_.transition_map.transitions[i]; - trans_desc.transition.id = static_cast(rcl_transition.id); - trans_desc.transition.label = rcl_transition.label; - trans_desc.start_state.id = static_cast(rcl_transition.start->id); - trans_desc.start_state.label = rcl_transition.start->label; - trans_desc.goal_state.id = static_cast(rcl_transition.goal->id); - trans_desc.goal_state.label = rcl_transition.goal->label; - } - } - - const State & - get_current_state() - { - current_state_ = State(state_machine_.current_state); - return current_state_; - } - - std::vector - get_available_states() - { - std::vector states; - states.reserve(state_machine_.transition_map.states_size); - - for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) { - states.emplace_back(&state_machine_.transition_map.states[i]); - } - return states; - } - - std::vector - get_available_transitions() - { - std::vector transitions; - transitions.reserve(state_machine_.current_state->valid_transition_size); - - for (unsigned int i = 0; i < state_machine_.current_state->valid_transition_size; ++i) { - transitions.emplace_back(&state_machine_.current_state->valid_transitions[i]); - } - return transitions; - } - - std::vector - get_transition_graph() - { - std::vector transitions; - transitions.reserve(state_machine_.transition_map.transitions_size); - - for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) { - transitions.emplace_back(&state_machine_.transition_map.transitions[i]); - } - return transitions; - } + std::shared_ptr resp) const; rcl_ret_t - change_state(std::uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code) - { - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( - "Unable to change state for state machine for %s: %s", - node_base_interface_->get_name(), rcl_get_error_string().str); - return RCL_RET_ERROR; - } - - constexpr bool publish_update = true; - // keep the initial state to pass to a transition callback - State initial_state(state_machine_.current_state); - - if ( - rcl_lifecycle_trigger_transition_by_id( - &state_machine_, transition_id, publish_update) != RCL_RET_OK) - { - RCUTILS_LOG_ERROR( - "Unable to start transition %u from current state %s: %s", - transition_id, state_machine_.current_state->label, rcl_get_error_string().str); - rcutils_reset_error(); - return RCL_RET_ERROR; - } - - auto get_label_for_return_code = - [](node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code) -> const char *{ - auto cb_id = static_cast(cb_return_code); - if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS) { - return rcl_lifecycle_transition_success_label; - } else if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE) { - return rcl_lifecycle_transition_failure_label; - } - return rcl_lifecycle_transition_error_label; - }; - - cb_return_code = execute_callback(state_machine_.current_state->id, initial_state); - auto transition_label = get_label_for_return_code(cb_return_code); - - if ( - rcl_lifecycle_trigger_transition_by_label( - &state_machine_, transition_label, publish_update) != RCL_RET_OK) - { - RCUTILS_LOG_ERROR( - "Failed to finish transition %u. Current state is now: %s (%s)", - transition_id, state_machine_.current_state->label, rcl_get_error_string().str); - rcutils_reset_error(); - return RCL_RET_ERROR; - } - - // error handling ?! - // TODO(karsten1987): iterate over possible ret value - if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) { - RCUTILS_LOG_WARN("Error occurred while doing error handling."); - - auto error_cb_code = execute_callback(state_machine_.current_state->id, initial_state); - auto error_cb_label = get_label_for_return_code(error_cb_code); - if ( - rcl_lifecycle_trigger_transition_by_label( - &state_machine_, error_cb_label, publish_update) != RCL_RET_OK) - { - RCUTILS_LOG_ERROR("Failed to call cleanup on error state: %s", rcl_get_error_string().str); - rcutils_reset_error(); - return RCL_RET_ERROR; - } - } - // This true holds in both cases where the actual callback - // was successful or not, since at this point we have a valid transistion - // to either a new primary state or error state - return RCL_RET_OK; - } - - LifecycleNodeInterface::CallbackReturn - execute_callback(unsigned int cb_id, const State & previous_state) - { - // in case no callback was attached, we forward directly - auto cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - - auto it = cb_map_.find(static_cast(cb_id)); - if (it != cb_map_.end()) { - auto callback = it->second; - try { - cb_success = callback(State(previous_state)); - } catch (const std::exception & e) { - RCUTILS_LOG_ERROR("Caught exception in callback for transition %d", it->first); - RCUTILS_LOG_ERROR("Original error: %s", e.what()); - cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - } - } - return cb_success; - } - - const State & trigger_transition(const char * transition_label) - { - LifecycleNodeInterface::CallbackReturn error; - return trigger_transition(transition_label, error); - } + change_state( + std::uint8_t transition_id, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code); - const State & trigger_transition( - const char * transition_label, LifecycleNodeInterface::CallbackReturn & cb_return_code) - { - auto transition = - rcl_lifecycle_get_transition_by_label(state_machine_.current_state, transition_label); - if (transition) { - change_state(static_cast(transition->id), cb_return_code); - } - return get_current_state(); - } - - const State & - trigger_transition(uint8_t transition_id) - { - LifecycleNodeInterface::CallbackReturn error; - change_state(transition_id, error); - (void) error; - return get_current_state(); - } - - const State & - trigger_transition(uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code) - { - change_state(transition_id, cb_return_code); - return get_current_state(); - } - - void - add_managed_entity(std::weak_ptr managed_entity) - { - weak_managed_entities_.push_back(managed_entity); - } - - void - add_timer_handle(std::shared_ptr timer) - { - weak_timers_.push_back(timer); - } - - void - on_activate() - { - for (const auto & weak_entity : weak_managed_entities_) { - auto entity = weak_entity.lock(); - if (entity) { - entity->on_activate(); - } - } - } - - void - on_deactivate() - { - for (const auto & weak_entity : weak_managed_entities_) { - auto entity = weak_entity.lock(); - if (entity) { - entity->on_deactivate(); - } - } - } + node_interfaces::LifecycleNodeInterface::CallbackReturn + execute_callback(unsigned int cb_id, const State & previous_state) const; rcl_lifecycle_state_machine_t state_machine_; State current_state_; std::map< std::uint8_t, - std::function> cb_map_; + std::function> cb_map_; using NodeBasePtr = std::shared_ptr; using NodeServicesPtr = std::shared_ptr; From 2d32d03ba3fc6780818705a8ab2ca0512b64e1c4 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 24 Oct 2022 17:14:12 -0400 Subject: [PATCH 146/455] Make lifecycle impl get_current_state() const. (#2031) We should only need to update the current state when it changes, so we do that in the change_state method (which is not const). Then we can just return the current_state_ object in get_current_state, and then mark it as const. Signed-off-by: Chris Lalancette --- .../include/rclcpp_lifecycle/lifecycle_node.hpp | 2 +- rclcpp_lifecycle/src/lifecycle_node.cpp | 2 +- .../src/lifecycle_node_interface_impl.cpp | 15 +++++++++++++-- .../src/lifecycle_node_interface_impl.hpp | 2 +- 4 files changed, 16 insertions(+), 5 deletions(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 80ec76a769..50d4717ec3 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -839,7 +839,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC const State & - get_current_state(); + get_current_state() const; /// Return a list with the available states. /** diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index f2db5f3950..773febda95 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -535,7 +535,7 @@ LifecycleNode::register_on_error( } const State & -LifecycleNode::get_current_state() +LifecycleNode::get_current_state() const { return impl_->get_current_state(); } diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp index f176cbf535..30c0ba3c0e 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp @@ -182,6 +182,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf nullptr); } } + + current_state_ = State(state_machine_.current_state); } bool @@ -327,9 +329,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::on_get_transition_graph( } const State & -LifecycleNode::LifecycleNodeInterfaceImpl::get_current_state() +LifecycleNode::LifecycleNodeInterfaceImpl::get_current_state() const { - current_state_ = State(state_machine_.current_state); return current_state_; } @@ -396,6 +397,9 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( return RCL_RET_ERROR; } + // Update the internal current_state_ + current_state_ = State(state_machine_.current_state); + auto get_label_for_return_code = [](node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code) -> const char *{ auto cb_id = static_cast(cb_return_code); @@ -421,6 +425,9 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( return RCL_RET_ERROR; } + // Update the internal current_state_ + current_state_ = State(state_machine_.current_state); + // error handling ?! // TODO(karsten1987): iterate over possible ret value if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) { @@ -437,6 +444,10 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( return RCL_RET_ERROR; } } + + // Update the internal current_state_ + current_state_ = State(state_machine_.current_state); + // This true holds in both cases where the actual callback // was successful or not, since at this point we have a valid transistion // to either a new primary state or error state diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index d973278005..9777bbf3a8 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -65,7 +65,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final std::function & cb); const State & - get_current_state(); + get_current_state() const; std::vector get_available_states() const; From 85c0af4fa06264a6b92a519197774c0e10b9606e Mon Sep 17 00:00:00 2001 From: Alexis Paques Date: Tue, 25 Oct 2022 19:33:14 +0200 Subject: [PATCH 147/455] Set the minimum number of threads of the Multithreaded executor to 2 (#2030) * Set the minimum number of threads of the Multithreaded executor to 2 Signed-off-by: Alexis Paques --- .../rclcpp/executors/multi_threaded_executor.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index bb477690be..507d47f913 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -21,6 +21,7 @@ #include "rcpputils/scope_exit.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/utilities.hpp" using rclcpp::executors::MultiThreadedExecutor; @@ -34,9 +35,15 @@ MultiThreadedExecutor::MultiThreadedExecutor( yield_before_execute_(yield_before_execute), next_exec_timeout_(next_exec_timeout) { - number_of_threads_ = number_of_threads ? number_of_threads : std::thread::hardware_concurrency(); - if (number_of_threads_ == 0) { - number_of_threads_ = 1; + number_of_threads_ = number_of_threads > 0 ? + number_of_threads : + std::max(std::thread::hardware_concurrency(), 2U); + + if (number_of_threads_ == 1) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "MultiThreadedExecutor is used with a single thread.\n" + "Use the SingleThreadedExecutor instead."); } } From d119157948720f5888d47898e1c59b56fe1f86c5 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 27 Oct 2022 05:18:21 +0800 Subject: [PATCH 148/455] Fix bug that a callback not reached (#1640) * Add a test case a subscriber on a new executor with a callback group triggered to receive a message Signed-off-by: Chen Lihui * fix flaky and not to use spin_some Signed-off-by: Chen Lihui * update comment Signed-off-by: Chen Lihui * update for not using anti-pattern source code Signed-off-by: Chen Lihui * add a notify guard condition for callback group Co-authored-by: William Woodall Signed-off-by: Chen Lihui * Notify guard condition of Node not to be used in Executor it is only for the waitset of GraphListener Signed-off-by: Chen Lihui * put code in the try catch Signed-off-by: Chen Lihui * defer to create guard condition Signed-off-by: Chen Lihui * use context directly for the create function Signed-off-by: Chen Lihui * cpplint Signed-off-by: Chen Lihui * fix that some case might call add_node after shutdown Signed-off-by: Chen Lihui * nitpick and fix some potential bug Signed-off-by: Chen Lihui * add sanity check as some case might not create notify guard condition after shutdown Signed-off-by: Chen Lihui * Cleanup includes. Signed-off-by: Chris Lalancette * remove destroy method make a callback group can only create one guard condition Signed-off-by: Chen Lihui * remove limitation that guard condition can not be re-created in callback group Signed-off-by: Chen Lihui Signed-off-by: Chen Lihui Signed-off-by: Chris Lalancette Co-authored-by: William Woodall Co-authored-by: Chris Lalancette --- rclcpp/include/rclcpp/callback_group.hpp | 22 ++++++- rclcpp/include/rclcpp/executor.hpp | 12 ++-- rclcpp/src/rclcpp/callback_group.cpp | 45 ++++++++++++- rclcpp/src/rclcpp/executor.cpp | 58 +++++++++-------- .../rclcpp/node_interfaces/node_services.cpp | 12 ++-- .../rclcpp/node_interfaces/node_timers.cpp | 5 +- .../rclcpp/node_interfaces/node_topics.cpp | 2 + .../rclcpp/node_interfaces/node_waitables.cpp | 6 +- rclcpp/test/rclcpp/CMakeLists.txt | 21 +++--- .../test_add_callback_groups_to_executor.cpp | 64 +++++++++++++++++++ 10 files changed, 196 insertions(+), 51 deletions(-) diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index 94bceced81..7d03edf343 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -16,11 +16,14 @@ #define RCLCPP__CALLBACK_GROUP_HPP_ #include +#include +#include #include -#include #include #include "rclcpp/client.hpp" +#include "rclcpp/context.hpp" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/publisher_base.hpp" #include "rclcpp/service.hpp" #include "rclcpp/subscription_base.hpp" @@ -95,6 +98,10 @@ class CallbackGroup CallbackGroupType group_type, bool automatically_add_to_executor_with_node = true); + /// Default destructor. + RCLCPP_PUBLIC + ~CallbackGroup(); + template rclcpp::SubscriptionBase::SharedPtr find_subscription_ptrs_if(Function func) const @@ -171,6 +178,16 @@ class CallbackGroup bool automatically_add_to_executor_with_node() const; + /// Defer creating the notify guard condition and return it. + RCLCPP_PUBLIC + rclcpp::GuardCondition::SharedPtr + get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr); + + /// Trigger the notify guard condition. + RCLCPP_PUBLIC + void + trigger_notify_guard_condition(); + protected: RCLCPP_DISABLE_COPY(CallbackGroup) @@ -213,6 +230,9 @@ class CallbackGroup std::vector waitable_ptrs_; std::atomic_bool can_be_taken_from_; const bool automatically_add_to_executor_with_node_; + // defer the creation of the guard condition + std::shared_ptr notify_guard_condition_ = nullptr; + std::recursive_mutex notify_guard_condition_mutex_; private: template diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index ed2ddc4a0a..65d0a930cb 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -560,14 +560,14 @@ class Executor virtual void spin_once_impl(std::chrono::nanoseconds timeout); - typedef std::map> - WeakNodesToGuardConditionsMap; + std::owner_less> + WeakCallbackGroupsToGuardConditionsMap; - /// maps nodes to guard conditions - WeakNodesToGuardConditionsMap - weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + /// maps callback groups to guard conditions + WeakCallbackGroupsToGuardConditionsMap + weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); /// maps callback groups associated to nodes WeakCallbackGroupsToNodesMap diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index 4b11156cf9..734c781a69 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -12,9 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rclcpp/callback_group.hpp" +#include +#include +#include +#include +#include +#include -#include +#include "rclcpp/callback_group.hpp" +#include "rclcpp/client.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp/waitable.hpp" using rclcpp::CallbackGroup; using rclcpp::CallbackGroupType; @@ -27,6 +37,10 @@ CallbackGroup::CallbackGroup( automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node) {} +CallbackGroup::~CallbackGroup() +{ + trigger_notify_guard_condition(); +} std::atomic_bool & CallbackGroup::can_be_taken_from() @@ -97,6 +111,33 @@ CallbackGroup::automatically_add_to_executor_with_node() const return automatically_add_to_executor_with_node_; } +rclcpp::GuardCondition::SharedPtr +CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr) +{ + std::lock_guard lock(notify_guard_condition_mutex_); + if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) { + if (associated_with_executor_) { + trigger_notify_guard_condition(); + } + notify_guard_condition_ = nullptr; + } + + if (!notify_guard_condition_) { + notify_guard_condition_ = std::make_shared(context_ptr); + } + + return notify_guard_condition_; +} + +void +CallbackGroup::trigger_notify_guard_condition() +{ + std::lock_guard lock(notify_guard_condition_mutex_); + if (notify_guard_condition_) { + notify_guard_condition_->trigger(); + } +} + void CallbackGroup::add_subscription( const rclcpp::SubscriptionBase::SharedPtr subscription_ptr) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 73b7b8d80d..401beb0a73 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -106,11 +106,11 @@ Executor::~Executor() weak_groups_associated_with_executor_to_nodes_.clear(); weak_groups_to_nodes_associated_with_executor_.clear(); weak_groups_to_nodes_.clear(); - for (const auto & pair : weak_nodes_to_guard_conditions_) { + for (const auto & pair : weak_groups_to_guard_conditions_) { auto guard_condition = pair.second; memory_strategy_->remove_guard_condition(guard_condition); } - weak_nodes_to_guard_conditions_.clear(); + weak_groups_to_guard_conditions_.clear(); // Finalize the wait set. if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) { @@ -204,8 +204,7 @@ Executor::add_callback_group_to_map( if (has_executor.exchange(true)) { throw std::runtime_error("Callback group has already been added to an executor."); } - bool is_new_node = !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) && - !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_); + rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr; auto insert_info = weak_groups_to_nodes.insert(std::make_pair(weak_group_ptr, node_ptr)); @@ -215,21 +214,24 @@ Executor::add_callback_group_to_map( } // Also add to the map that contains all callback groups weak_groups_to_nodes_.insert(std::make_pair(weak_group_ptr, node_ptr)); - if (is_new_node) { - const auto & gc = node_ptr->get_notify_guard_condition(); - weak_nodes_to_guard_conditions_[node_ptr] = &gc; - if (notify) { - // Interrupt waiting to handle new node - try { - interrupt_guard_condition_.trigger(); - } catch (const rclcpp::exceptions::RCLError & ex) { - throw std::runtime_error( - std::string( - "Failed to trigger guard condition on callback group add: ") + ex.what()); - } + + if (node_ptr->get_context()->is_valid()) { + auto callback_group_guard_condition = + group_ptr->get_notify_guard_condition(node_ptr->get_context()); + weak_groups_to_guard_conditions_[weak_group_ptr] = callback_group_guard_condition.get(); + // Add the callback_group's notify condition to the guard condition handles + memory_strategy_->add_guard_condition(*callback_group_guard_condition); + } + + if (notify) { + // Interrupt waiting to handle new node + try { + interrupt_guard_condition_.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on callback group add: ") + ex.what()); } - // Add the node's notify condition to the guard condition handles - memory_strategy_->add_guard_condition(gc); } } @@ -300,7 +302,12 @@ Executor::remove_callback_group_from_map( if (!has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) && !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_)) { - weak_nodes_to_guard_conditions_.erase(node_ptr); + auto iter = weak_groups_to_guard_conditions_.find(weak_group_ptr); + if (iter != weak_groups_to_guard_conditions_.end()) { + memory_strategy_->remove_guard_condition(iter->second); + } + weak_groups_to_guard_conditions_.erase(weak_group_ptr); + if (notify) { try { interrupt_guard_condition_.trigger(); @@ -310,7 +317,6 @@ Executor::remove_callback_group_from_map( "Failed to trigger guard condition on callback group remove: ") + ex.what()); } } - memory_strategy_->remove_guard_condition(&node_ptr->get_notify_guard_condition()); } } @@ -700,12 +706,6 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) auto weak_node_ptr = pair.second; if (weak_group_ptr.expired() || weak_node_ptr.expired()) { invalid_group_ptrs.push_back(weak_group_ptr); - auto node_guard_pair = weak_nodes_to_guard_conditions_.find(weak_node_ptr); - if (node_guard_pair != weak_nodes_to_guard_conditions_.end()) { - auto guard_condition = node_guard_pair->second; - weak_nodes_to_guard_conditions_.erase(weak_node_ptr); - memory_strategy_->remove_guard_condition(guard_condition); - } } } std::for_each( @@ -721,6 +721,12 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) { weak_groups_associated_with_executor_to_nodes_.erase(group_ptr); } + auto callback_guard_pair = weak_groups_to_guard_conditions_.find(group_ptr); + if (callback_guard_pair != weak_groups_to_guard_conditions_.end()) { + auto guard_condition = callback_guard_pair->second; + weak_groups_to_guard_conditions_.erase(group_ptr); + memory_strategy_->remove_guard_condition(guard_condition); + } weak_groups_to_nodes_.erase(group_ptr); }); } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp index 14ab1c82c4..2f1afd3224 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp @@ -35,15 +35,17 @@ NodeServices::add_service( // TODO(jacquelinekay): use custom exception throw std::runtime_error("Cannot create service, group not in node."); } - group->add_service(service_base_ptr); } else { - node_base_->get_default_callback_group()->add_service(service_base_ptr); + group = node_base_->get_default_callback_group(); } + group->add_service(service_base_ptr); + // Notify the executor that a new service was created using the parent Node. auto & node_gc = node_base_->get_notify_guard_condition(); try { node_gc.trigger(); + group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string("failed to notify wait set on service creation: ") + ex.what()); @@ -60,15 +62,17 @@ NodeServices::add_client( // TODO(jacquelinekay): use custom exception throw std::runtime_error("Cannot create client, group not in node."); } - group->add_client(client_base_ptr); } else { - node_base_->get_default_callback_group()->add_client(client_base_ptr); + group = node_base_->get_default_callback_group(); } + group->add_client(client_base_ptr); + // Notify the executor that a new client was created using the parent Node. auto & node_gc = node_base_->get_notify_guard_condition(); try { node_gc.trigger(); + group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string("failed to notify wait set on client creation: ") + ex.what()); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp index b463e8a0e7..d2e821a9e6 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp @@ -37,14 +37,15 @@ NodeTimers::add_timer( // TODO(jacquelinekay): use custom exception throw std::runtime_error("Cannot create timer, group not in node."); } - callback_group->add_timer(timer); } else { - node_base_->get_default_callback_group()->add_timer(timer); + callback_group = node_base_->get_default_callback_group(); } + callback_group->add_timer(timer); auto & node_gc = node_base_->get_notify_guard_condition(); try { node_gc.trigger(); + callback_group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string("failed to notify wait set on timer creation: ") + ex.what()); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index 159409528d..167a35f35d 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -73,6 +73,7 @@ NodeTopics::add_publisher( auto & node_gc = node_base_->get_notify_guard_condition(); try { node_gc.trigger(); + callback_group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string("failed to notify wait set on publisher creation: ") + ex.what()); @@ -121,6 +122,7 @@ NodeTopics::add_subscription( auto & node_gc = node_base_->get_notify_guard_condition(); try { node_gc.trigger(); + callback_group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string("failed to notify wait set on subscription creation: ") + ex.what()); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp index 6f243f6025..1d1fe2ce59 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp @@ -35,15 +35,17 @@ NodeWaitables::add_waitable( // TODO(jacobperron): use custom exception throw std::runtime_error("Cannot create waitable, group not in node."); } - group->add_waitable(waitable_ptr); } else { - node_base_->get_default_callback_group()->add_waitable(waitable_ptr); + group = node_base_->get_default_callback_group(); } + group->add_waitable(waitable_ptr); + // Notify the executor that a new waitable was created using the parent Node. auto & node_gc = node_base_->get_notify_guard_condition(); try { node_gc.trigger(); + group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string("failed to notify wait set on waitable creation: ") + ex.what()); diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index d4e497f4bf..75aa9ac63b 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -100,15 +100,20 @@ if(TARGET test_create_subscription) "test_msgs" ) endif() -ament_add_gtest(test_add_callback_groups_to_executor - test_add_callback_groups_to_executor.cpp - TIMEOUT 120) -if(TARGET test_add_callback_groups_to_executor) - target_link_libraries(test_add_callback_groups_to_executor ${PROJECT_NAME}) - ament_target_dependencies(test_add_callback_groups_to_executor - "test_msgs" +function(test_add_callback_groups_to_executor_for_rmw_implementation) + set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) + ament_add_gmock(test_add_callback_groups_to_executor${target_suffix} test_add_callback_groups_to_executor.cpp + ENV ${rmw_implementation_env_var} + TIMEOUT 120 ) -endif() + if(TARGET test_add_callback_groups_to_executor${target_suffix}) + target_link_libraries(test_add_callback_groups_to_executor${target_suffix} ${PROJECT_NAME}) + ament_target_dependencies(test_add_callback_groups_to_executor${target_suffix} + "test_msgs" + ) + endif() +endfunction() +call_for_each_rmw_implementation(test_add_callback_groups_to_executor_for_rmw_implementation) ament_add_gtest(test_expand_topic_or_service_name test_expand_topic_or_service_name.cpp) if(TARGET test_expand_topic_or_service_name) ament_target_dependencies(test_expand_topic_or_service_name diff --git a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp index fa636b7157..07ca1e87d8 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -276,6 +276,70 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, one_node_many_callback_groups_many_e ASSERT_EQ(timer_executor.get_all_callback_groups().size(), 2u); } +/* + * Test callback groups from one node to many executors. + * A subscriber on a new executor with a callback group not received a message + * because the executor can't be triggered while a subscriber created, see + * https://github.com/ros2/rclcpp/issues/1611 +*/ +TYPED_TEST(TestAddCallbackGroupsToExecutor, subscriber_triggered_to_receive_message) +{ + auto node = std::make_shared("my_node", "/ns"); + + // create a thread running an executor with a new callback group for a coming subscriber + rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + rclcpp::executors::SingleThreadedExecutor cb_grp_executor; + + std::promise received_message_promise; + auto received_message_future = received_message_promise.get_future(); + rclcpp::FutureReturnCode return_code = rclcpp::FutureReturnCode::TIMEOUT; + std::thread cb_grp_thread = std::thread( + [&cb_grp, &node, &cb_grp_executor, &received_message_future, &return_code]() { + cb_grp_executor.add_callback_group(cb_grp, node->get_node_base_interface()); + return_code = cb_grp_executor.spin_until_future_complete(received_message_future, 10s); + }); + + // expect the subscriber to receive a message + auto sub_callback = [&received_message_promise](test_msgs::msg::Empty::ConstSharedPtr) { + received_message_promise.set_value(true); + }; + + rclcpp::Subscription::SharedPtr subscription; + rclcpp::Publisher::SharedPtr publisher; + // to create a timer with a callback run on another executor + rclcpp::TimerBase::SharedPtr timer = nullptr; + std::promise timer_promise; + auto timer_callback = + [&subscription, &publisher, &timer, &cb_grp, &node, &sub_callback, &timer_promise]() { + if (timer) { + timer.reset(); + } + + // create a subscription using the `cb_grp` callback group + rclcpp::QoS qos = rclcpp::QoS(1).reliable(); + auto options = rclcpp::SubscriptionOptions(); + options.callback_group = cb_grp; + subscription = + node->create_subscription("topic_name", qos, sub_callback, options); + // create a publisher to send data + publisher = + node->create_publisher("topic_name", qos); + publisher->publish(test_msgs::msg::Empty()); + timer_promise.set_value(); + }; + + rclcpp::executors::SingleThreadedExecutor timer_executor; + timer = node->create_wall_timer(100ms, timer_callback); + timer_executor.add_node(node); + auto future = timer_promise.get_future(); + timer_executor.spin_until_future_complete(future); + cb_grp_thread.join(); + + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + EXPECT_TRUE(received_message_future.get()); +} + /* * Test removing callback group from executor that its not associated with. */ From e5d13a2478154da5fcbc6b9f2ea1d880902647cf Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Fri, 28 Oct 2022 09:15:14 -0700 Subject: [PATCH 149/455] Bugfix 20210810 get current state (#1756) * protect state_machine_ with mutex lock. protect state_handle_ with mutex lock. reconsider mutex lock scope. remove mutex lock from constructors. lock just once during initialization of LifecycleNodeInterfaceImpl. Signed-off-by: Tomoya Fujita * Move updating of current_state to right after initialization. This is slightly more correct in the case that registering one of the services fails. Signed-off-by: Chris Lalancette Signed-off-by: Tomoya Fujita Signed-off-by: Chris Lalancette Co-authored-by: Chris Lalancette --- .../include/rclcpp_lifecycle/state.hpp | 2 + .../src/lifecycle_node_interface_impl.cpp | 139 +++++++++++------- .../src/lifecycle_node_interface_impl.hpp | 1 + rclcpp_lifecycle/src/state.cpp | 6 + 4 files changed, 95 insertions(+), 53 deletions(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp index 86750f1fed..62e1c51fb3 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP_LIFECYCLE__STATE_HPP_ #define RCLCPP_LIFECYCLE__STATE_HPP_ +#include #include #include "rcl_lifecycle/data_types.h" @@ -92,6 +93,7 @@ class State bool owns_rcl_state_handle_; + mutable std::recursive_mutex state_handle_mutex_; rcl_lifecycle_state_t * state_handle_; }; diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp index 30c0ba3c0e..5c5f7797e1 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -58,7 +59,11 @@ LifecycleNode::LifecycleNodeInterfaceImpl::LifecycleNodeInterfaceImpl( LifecycleNode::LifecycleNodeInterfaceImpl::~LifecycleNodeInterfaceImpl() { rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); - auto ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle); + rcl_ret_t ret; + { + std::lock_guard lock(state_machine_mutex_); + ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle); + } if (ret != RCL_RET_OK) { RCUTILS_LOG_FATAL_NAMED( "rclcpp_lifecycle", @@ -72,7 +77,6 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); const rcl_node_options_t * node_options = rcl_node_get_options(node_base_interface_->get_rcl_node_handle()); - state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); auto state_machine_options = rcl_lifecycle_get_default_state_machine_options(); state_machine_options.enable_com_interface = enable_communication_interface; state_machine_options.allocator = node_options->allocator; @@ -83,6 +87,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf // The publisher takes a C-Typesupport since the publishing (i.e. creating // the message) is done fully in RCL. // Services are handled in C++, so that it needs a C++ typesupport structure. + std::lock_guard lock(state_machine_mutex_); + state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); rcl_ret_t ret = rcl_lifecycle_state_machine_init( &state_machine_, node_handle, @@ -99,6 +105,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf node_base_interface_->get_name()); } + current_state_ = State(state_machine_.current_state); + if (enable_communication_interface) { { // change_state auto cb = std::bind( @@ -182,8 +190,6 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf nullptr); } } - - current_state_ = State(state_machine_.current_state); } bool @@ -202,28 +208,31 @@ LifecycleNode::LifecycleNodeInterfaceImpl::on_change_state( std::shared_ptr resp) { (void)header; - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get state. State machine is not initialized."); - } + std::uint8_t transition_id; + { + std::lock_guard lock(state_machine_mutex_); + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error("Can't get state. State machine is not initialized."); + } - auto transition_id = req->transition.id; - // if there's a label attached to the request, - // we check the transition attached to this label. - // we further can't compare the id of the looked up transition - // because ros2 service call defaults all intergers to zero. - // that means if we call ros2 service call ... {transition: {label: shutdown}} - // the id of the request is 0 (zero) whereas the id from the lookup up transition - // can be different. - // the result of this is that the label takes presedence of the id. - if (req->transition.label.size() != 0) { - auto rcl_transition = rcl_lifecycle_get_transition_by_label( - state_machine_.current_state, req->transition.label.c_str()); - if (rcl_transition == nullptr) { - resp->success = false; - return; + transition_id = req->transition.id; + // if there's a label attached to the request, + // we check the transition attached to this label. + // we further can't compare the id of the looked up transition + // because ros2 service call defaults all intergers to zero. + // that means if we call ros2 service call ... {transition: {label: shutdown}} + // the id of the request is 0 (zero) whereas the id from the lookup up transition + // can be different. + // the result of this is that the label takes presedence of the id. + if (req->transition.label.size() != 0) { + auto rcl_transition = rcl_lifecycle_get_transition_by_label( + state_machine_.current_state, req->transition.label.c_str()); + if (rcl_transition == nullptr) { + resp->success = false; + return; + } + transition_id = static_cast(rcl_transition->id); } - transition_id = static_cast(rcl_transition->id); } node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code; @@ -244,6 +253,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::on_get_state( { (void)header; (void)req; + std::lock_guard lock(state_machine_mutex_); if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { throw std::runtime_error( "Can't get state. State machine is not initialized."); @@ -260,6 +270,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_states( { (void)header; (void)req; + std::lock_guard lock(state_machine_mutex_); if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { throw std::runtime_error( "Can't get available states. State machine is not initialized."); @@ -282,6 +293,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_transitions( { (void)header; (void)req; + std::lock_guard lock(state_machine_mutex_); if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { throw std::runtime_error( "Can't get available transitions. State machine is not initialized."); @@ -309,6 +321,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::on_get_transition_graph( { (void)header; (void)req; + std::lock_guard lock(state_machine_mutex_); if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { throw std::runtime_error( "Can't get available transitions. State machine is not initialized."); @@ -338,6 +351,7 @@ std::vector LifecycleNode::LifecycleNodeInterfaceImpl::get_available_states() const { std::vector states; + std::lock_guard lock(state_machine_mutex_); states.reserve(state_machine_.transition_map.states_size); for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) { @@ -350,6 +364,7 @@ std::vector LifecycleNode::LifecycleNodeInterfaceImpl::get_available_transitions() const { std::vector transitions; + std::lock_guard lock(state_machine_mutex_); transitions.reserve(state_machine_.current_state->valid_transition_size); for (unsigned int i = 0; i < state_machine_.current_state->valid_transition_size; ++i) { @@ -362,6 +377,7 @@ std::vector LifecycleNode::LifecycleNodeInterfaceImpl::get_transition_graph() const { std::vector transitions; + std::lock_guard lock(state_machine_mutex_); transitions.reserve(state_machine_.transition_map.transitions_size); for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) { @@ -375,26 +391,33 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( std::uint8_t transition_id, node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code) { - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( - "Unable to change state for state machine for %s: %s", - node_base_interface_->get_name(), rcl_get_error_string().str); - return RCL_RET_ERROR; - } - constexpr bool publish_update = true; - // keep the initial state to pass to a transition callback - State initial_state(state_machine_.current_state); + State initial_state; + unsigned int current_state_id; - if ( - rcl_lifecycle_trigger_transition_by_id( - &state_machine_, transition_id, publish_update) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( - "Unable to start transition %u from current state %s: %s", - transition_id, state_machine_.current_state->label, rcl_get_error_string().str); - rcutils_reset_error(); - return RCL_RET_ERROR; + std::lock_guard lock(state_machine_mutex_); + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR( + "Unable to change state for state machine for %s: %s", + node_base_interface_->get_name(), rcl_get_error_string().str); + return RCL_RET_ERROR; + } + + // keep the initial state to pass to a transition callback + initial_state = State(state_machine_.current_state); + + if ( + rcl_lifecycle_trigger_transition_by_id( + &state_machine_, transition_id, publish_update) != RCL_RET_OK) + { + RCUTILS_LOG_ERROR( + "Unable to start transition %u from current state %s: %s", + transition_id, state_machine_.current_state->label, rcl_get_error_string().str); + rcutils_reset_error(); + return RCL_RET_ERROR; + } + current_state_id = state_machine_.current_state->id; } // Update the internal current_state_ @@ -411,18 +434,22 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( return rcl_lifecycle_transition_error_label; }; - cb_return_code = execute_callback(state_machine_.current_state->id, initial_state); + cb_return_code = execute_callback(current_state_id, initial_state); auto transition_label = get_label_for_return_code(cb_return_code); - if ( - rcl_lifecycle_trigger_transition_by_label( - &state_machine_, transition_label, publish_update) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( - "Failed to finish transition %u. Current state is now: %s (%s)", - transition_id, state_machine_.current_state->label, rcl_get_error_string().str); - rcutils_reset_error(); - return RCL_RET_ERROR; + std::lock_guard lock(state_machine_mutex_); + if ( + rcl_lifecycle_trigger_transition_by_label( + &state_machine_, transition_label, publish_update) != RCL_RET_OK) + { + RCUTILS_LOG_ERROR( + "Failed to finish transition %u. Current state is now: %s (%s)", + transition_id, state_machine_.current_state->label, rcl_get_error_string().str); + rcutils_reset_error(); + return RCL_RET_ERROR; + } + current_state_id = state_machine_.current_state->id; } // Update the internal current_state_ @@ -433,8 +460,9 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) { RCUTILS_LOG_WARN("Error occurred while doing error handling."); - auto error_cb_code = execute_callback(state_machine_.current_state->id, initial_state); + auto error_cb_code = execute_callback(current_state_id, initial_state); auto error_cb_label = get_label_for_return_code(error_cb_code); + std::lock_guard lock(state_machine_mutex_); if ( rcl_lifecycle_trigger_transition_by_label( &state_machine_, error_cb_label, publish_update) != RCL_RET_OK) @@ -486,8 +514,13 @@ const State & LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition( const char * transition_label, node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code) { - auto transition = - rcl_lifecycle_get_transition_by_label(state_machine_.current_state, transition_label); + const rcl_lifecycle_transition_t * transition; + { + std::lock_guard lock(state_machine_mutex_); + + transition = + rcl_lifecycle_get_transition_by_label(state_machine_.current_state, transition_label); + } if (transition) { change_state(static_cast(transition->id), cb_return_code); } diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 9777bbf3a8..d09f44831c 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -143,6 +143,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final node_interfaces::LifecycleNodeInterface::CallbackReturn execute_callback(unsigned int cb_id, const State & previous_state) const; + mutable std::recursive_mutex state_machine_mutex_; rcl_lifecycle_state_machine_t state_machine_; State current_state_; std::map< diff --git a/rclcpp_lifecycle/src/state.cpp b/rclcpp_lifecycle/src/state.cpp index 959b40ed2a..e69078dfba 100644 --- a/rclcpp_lifecycle/src/state.cpp +++ b/rclcpp_lifecycle/src/state.cpp @@ -71,6 +71,7 @@ State::State( if (!rcl_lifecycle_state_handle) { throw std::runtime_error("rcl_lifecycle_state_handle is null"); } + state_handle_ = const_cast(rcl_lifecycle_state_handle); } @@ -94,6 +95,8 @@ State::operator=(const State & rhs) return *this; } + // hold the lock until state_handle_ is reconstructed + std::lock_guard lock(state_handle_mutex_); // reset all currently used resources reset(); @@ -129,6 +132,7 @@ State::operator=(const State & rhs) uint8_t State::id() const { + std::lock_guard lock(state_handle_mutex_); if (!state_handle_) { throw std::runtime_error("Error in state! Internal state_handle is NULL."); } @@ -138,6 +142,7 @@ State::id() const std::string State::label() const { + std::lock_guard lock(state_handle_mutex_); if (!state_handle_) { throw std::runtime_error("Error in state! Internal state_handle is NULL."); } @@ -147,6 +152,7 @@ State::label() const void State::reset() noexcept { + std::lock_guard lock(state_handle_mutex_); if (!owns_rcl_state_handle_) { state_handle_ = nullptr; } From 6f22443513e14eac41f5bb830a566bf820a8e078 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Fri, 28 Oct 2022 09:17:23 -0700 Subject: [PATCH 150/455] MultiThreadExecutor number of threads is at least 2+ in default. (#2032) Signed-off-by: Tomoya Fujita --- rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index c1de8051f2..119013ebfb 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -47,7 +47,7 @@ class MultiThreadedExecutor : public rclcpp::Executor * * \param options common options for all executors * \param number_of_threads number of threads to have in the thread pool, - * the default 0 will use the number of cpu cores found instead + * the default 0 will use the number of cpu cores found (minimum of 2) * \param yield_before_execute if true std::this_thread::yield() is called * \param timeout maximum time to wait */ From edbfe1404b24d0bc85ff88e8ff1f006670788e46 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Fri, 28 Oct 2022 09:17:47 -0700 Subject: [PATCH 151/455] LifecycleNode on_configure doc fix. (#2034) Signed-off-by: Tomoya Fujita --- .../node_interfaces/lifecycle_node_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp index e653ad412d..ed919ca556 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp @@ -53,7 +53,7 @@ class LifecycleNodeInterface /// Callback function for configure transition /* - * \return true by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC virtual CallbackReturn From 586932ebf8f844a8484f91bc618f3e9fdd72dbc1 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 2 Nov 2022 18:06:12 +0000 Subject: [PATCH 152/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 12 ++++++++++++ rclcpp_action/CHANGELOG.rst | 5 +++++ rclcpp_components/CHANGELOG.rst | 5 +++++ rclcpp_lifecycle/CHANGELOG.rst | 9 +++++++++ 4 files changed, 31 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 08914deb08..fc620249fe 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* MultiThreadExecutor number of threads is at least 2+ in default. (`#2032 `_) +* Fix bug that a callback not reached (`#1640 `_) +* Set the minimum number of threads of the Multithreaded executor to 2 (`#2030 `_) +* check thread whether joinable before join (`#2019 `_) +* Set cpplint test timeout to 3 minutes (`#2022 `_) +* Make sure to include-what-you-use in the node_interfaces. (`#2018 `_) +* Do not clear entities callbacks on destruction (`#2002 `_) +* fix mismatched issue if using zero_allocate (`#1995 `_) +* Contributors: Alexis Paques, Chen Lihui, Chris Lalancette, Cristóbal Arroyo, Tomoya Fujita, mauropasse, uupks + 17.0.0 (2022-09-13) ------------------- * Make ParameterService and Sync/AsyncParameterClient accept rclcpp::QoS (`#1978 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index ef4104bcac..0ab833bc3a 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Do not clear entities callbacks on destruction (`#2002 `_) +* Contributors: mauropasse + 17.0.0 (2022-09-13) ------------------- * Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_)" (`#1956 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index c496eb406d..6fa85fbcba 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* use unique ptr and remove unuseful container (`#2013 `_) +* Contributors: Chen Lihui + 17.0.0 (2022-09-13) ------------------- * Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_)" (`#1956 `_) diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 8b88b682b1..f062dd56a9 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,15 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* LifecycleNode on_configure doc fix. (`#2034 `_) +* Bugfix 20210810 get current state (`#1756 `_) +* Make lifecycle impl get_current_state() const. (`#2031 `_) +* Cleanup the lifecycle implementation (`#2027 `_) +* Cleanup the rclcpp_lifecycle dependencies. (`#2021 `_) +* Contributors: Chris Lalancette, Tomoya Fujita + 17.0.0 (2022-09-13) ------------------- * Revert "Revert "Add a create_timer method to Node and `LifecycleNode` classes (`#1975 `_)" (`#2009 `_) (`#2010 `_) From 7c6785176a878cf16eb98995b732f405b9e776ed Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 2 Nov 2022 18:06:21 +0000 Subject: [PATCH 153/455] 17.1.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index fc620249fe..15a1aede88 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +17.1.0 (2022-11-02) +------------------- * MultiThreadExecutor number of threads is at least 2+ in default. (`#2032 `_) * Fix bug that a callback not reached (`#1640 `_) * Set the minimum number of threads of the Multithreaded executor to 2 (`#2030 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index d05253db94..f113613fd3 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 17.0.0 + 17.1.0 The ROS client library in C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 0ab833bc3a..a063bf32f2 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +17.1.0 (2022-11-02) +------------------- * Do not clear entities callbacks on destruction (`#2002 `_) * Contributors: mauropasse diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index f9cbaeb721..758eb0db4e 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 17.0.0 + 17.1.0 Adds action APIs for C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 6fa85fbcba..bc08ed28cd 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +17.1.0 (2022-11-02) +------------------- * use unique ptr and remove unuseful container (`#2013 `_) * Contributors: Chen Lihui diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 2032fc84ee..3557608bc1 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 17.0.0 + 17.1.0 Package containing tools for dynamically loadable components Ivan Paunovic Jacob Perron diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index f062dd56a9..03d02cb2c2 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +17.1.0 (2022-11-02) +------------------- * LifecycleNode on_configure doc fix. (`#2034 `_) * Bugfix 20210810 get current state (`#1756 `_) * Make lifecycle impl get_current_state() const. (`#2031 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index cfd97eafd6..ab94c9fffb 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 17.0.0 + 17.1.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Jacob Perron From 82f1fbff0b29fb13bf93b54fc0fdc996c800f6bc Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Tue, 8 Nov 2022 07:21:43 -0600 Subject: [PATCH 154/455] [rolling] Update maintainers (#2043) * Update maintainers to Ivan Paunovic, Michel Hidalgo, and William Woodall Signed-off-by: Audrow Nash --- CODEOWNERS | 2 ++ rclcpp/package.xml | 6 +++++- rclcpp_action/package.xml | 6 +++++- rclcpp_components/package.xml | 6 +++++- rclcpp_lifecycle/package.xml | 6 +++++- 5 files changed, 22 insertions(+), 4 deletions(-) create mode 100644 CODEOWNERS diff --git a/CODEOWNERS b/CODEOWNERS new file mode 100644 index 0000000000..7ee4b9af4d --- /dev/null +++ b/CODEOWNERS @@ -0,0 +1,2 @@ +# This file was generated by https://github.com/audrow/update-ros2-repos +* @ivanpauno @hidmic @wjwwood diff --git a/rclcpp/package.xml b/rclcpp/package.xml index f113613fd3..6775f4a46d 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -4,11 +4,15 @@ rclcpp 17.1.0 The ROS client library in C++. + Ivan Paunovic - Jacob Perron + Michel Hidalgo William Woodall + Apache License 2.0 + Dirk Thomas + Jacob Perron ament_cmake_ros ament_cmake_gen_version_h diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 758eb0db4e..38e7fdcba1 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -4,11 +4,15 @@ rclcpp_action 17.1.0 Adds action APIs for C++. + Ivan Paunovic - Jacob Perron + Michel Hidalgo William Woodall + Apache License 2.0 + Dirk Thomas + Jacob Perron ament_cmake_ros diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 3557608bc1..cfb88b9e3f 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -4,10 +4,14 @@ rclcpp_components 17.1.0 Package containing tools for dynamically loadable components + Ivan Paunovic - Jacob Perron + Michel Hidalgo William Woodall + Apache License 2.0 + + Jacob Perron Michael Carroll ament_cmake_ros diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index ab94c9fffb..8fa958a0bc 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -4,10 +4,14 @@ rclcpp_lifecycle 17.1.0 Package containing a prototype for lifecycle implementation + Ivan Paunovic - Jacob Perron + Michel Hidalgo William Woodall + Apache License 2.0 + + Jacob Perron Karsten Knese ament_cmake_ros From 91bc312190e61c8915b0152e5c927c987fc22d6c Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Wed, 9 Nov 2022 23:50:01 +0800 Subject: [PATCH 155/455] fix a case that not throw ParameterUninitializedException (#2036) * fix a case that not throw ParameterUninitializedException Signed-off-by: Chen Lihui * catch ParameterUninitializedException exception while calling get_parameters in service callback Signed-off-by: Chen Lihui * update doc Signed-off-by: Chen Lihui * add one more test Signed-off-by: Chen Lihui * explicitly use this to call a method inside the class itself Co-authored-by: Ivan Santiago Paunovic Signed-off-by: Chen Lihui Signed-off-by: Chen Lihui Co-authored-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/node.hpp | 16 ++++++--- .../node_interfaces/node_parameters.cpp | 34 +++++++------------ rclcpp/src/rclcpp/parameter_service.cpp | 2 ++ rclcpp/test/rclcpp/test_node.cpp | 34 +++++++++++++++++++ rclcpp/test/rclcpp/test_parameter_client.cpp | 18 ++++++++++ 5 files changed, 77 insertions(+), 27 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index e514137b51..7ecb67e9b1 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -740,17 +740,21 @@ class Node : public std::enable_shared_from_this /** * If the parameter has not been declared, then this method may throw the * rclcpp::exceptions::ParameterNotDeclaredException exception. + * If the parameter has not been initialized, then this method may throw the + * rclcpp::exceptions::ParameterUninitializedException exception. * * If undeclared parameters are allowed, see the node option * rclcpp::NodeOptions::allow_undeclared_parameters, then this method will - * not throw an exception, and instead return a default initialized - * rclcpp::Parameter, which has a type of + * not throw the rclcpp::exceptions::ParameterNotDeclaredException exception, + * and instead return a default initialized rclcpp::Parameter, which has a type of * rclcpp::ParameterType::PARAMETER_NOT_SET. * * \param[in] name The name of the parameter to get. * \return The requested parameter inside of a rclcpp parameter object. * \throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter * has not been declared and undeclared parameters are not allowed. + * \throws rclcpp::exceptions::ParameterUninitializedException if the parameter + * has not been initialized. */ RCLCPP_PUBLIC rclcpp::Parameter @@ -834,12 +838,12 @@ class Node : public std::enable_shared_from_this /// Return the parameters by the given parameter names. /** - * Like get_parameters(), this method may throw the + * Like get_parameter(const std::string &), this method may throw the * rclcpp::exceptions::ParameterNotDeclaredException exception if the * requested parameter has not been declared and undeclared parameters are - * not allowed. + * not allowed, and may throw the rclcpp::exceptions::ParameterUninitializedException exception. * - * Also like get_parameters(), if undeclared parameters are allowed and the + * Also like get_parameter(const std::string &), if undeclared parameters are allowed and the * parameter has not been declared, then the corresponding rclcpp::Parameter * will be default initialized and therefore have the type * rclcpp::ParameterType::PARAMETER_NOT_SET. @@ -849,6 +853,8 @@ class Node : public std::enable_shared_from_this * \throws rclcpp::exceptions::ParameterNotDeclaredException if any of the * parameters have not been declared and undeclared parameters are not * allowed. + * \throws rclcpp::exceptions::ParameterUninitializedException if any of the + * parameters have not been initialized. */ RCLCPP_PUBLIC std::vector diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index c17eb887b7..9dafcba381 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -904,22 +904,12 @@ NodeParameters::set_parameters_atomically(const std::vector & std::vector NodeParameters::get_parameters(const std::vector & names) const { - std::lock_guard lock(mutex_); std::vector results; results.reserve(names.size()); + std::lock_guard lock(mutex_); for (auto & name : names) { - auto found_parameter = parameters_.find(name); - if (found_parameter != parameters_.cend()) { - // found - results.emplace_back(name, found_parameter->second.value); - } else if (this->allow_undeclared_) { - // not found, but undeclared allowed - results.emplace_back(name, rclcpp::ParameterValue()); - } else { - // not found, and undeclared are not allowed - throw rclcpp::exceptions::ParameterNotDeclaredException(name); - } + results.emplace_back(this->get_parameter(name)); } return results; } @@ -930,18 +920,18 @@ NodeParameters::get_parameter(const std::string & name) const std::lock_guard lock(mutex_); auto param_iter = parameters_.find(name); - if ( - parameters_.end() != param_iter && - (param_iter->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET || - param_iter->second.descriptor.dynamic_typing)) - { - return rclcpp::Parameter{name, param_iter->second.value}; + if (parameters_.end() != param_iter) { + if ( + param_iter->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET || + param_iter->second.descriptor.dynamic_typing) + { + return rclcpp::Parameter{name, param_iter->second.value}; + } + throw rclcpp::exceptions::ParameterUninitializedException(name); } else if (this->allow_undeclared_) { - return rclcpp::Parameter{}; - } else if (parameters_.end() == param_iter) { - throw rclcpp::exceptions::ParameterNotDeclaredException(name); + return rclcpp::Parameter{name}; } else { - throw rclcpp::exceptions::ParameterUninitializedException(name); + throw rclcpp::exceptions::ParameterNotDeclaredException(name); } } diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp index 501ac399f8..0923798339 100644 --- a/rclcpp/src/rclcpp/parameter_service.cpp +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -48,6 +48,8 @@ ParameterService::ParameterService( } } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameters: %s", ex.what()); + } catch (const rclcpp::exceptions::ParameterUninitializedException & ex) { + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameters: %s", ex.what()); } }, qos_profile, nullptr); diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index c40811a4e4..19c87b3684 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -3346,6 +3346,9 @@ TEST_F(TestNode, static_and_dynamic_typing) { EXPECT_THROW( node->get_parameter("integer_override_not_given"), rclcpp::exceptions::ParameterUninitializedException); + EXPECT_THROW( + node->get_parameters({"integer_override_not_given"}), + rclcpp::exceptions::ParameterUninitializedException); } { auto param = node->declare_parameter("integer_set_after_declare", rclcpp::PARAMETER_INTEGER); @@ -3367,3 +3370,34 @@ TEST_F(TestNode, static_and_dynamic_typing) { rclcpp::exceptions::InvalidParameterTypeException); } } + +TEST_F(TestNode, parameter_uninitialized_exception_even_if_allow_undeclared) { + rclcpp::NodeOptions no; + no.allow_undeclared_parameters(true); + auto node = std::make_shared("node", "ns", no); + { + const std::string param_name = "integer_override_not_given"; + auto param_value = node->declare_parameter(param_name, rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, param_value.get_type()); + // Throws if not set before access + EXPECT_THROW( + node->get_parameter(param_name), + rclcpp::exceptions::ParameterUninitializedException); + EXPECT_THROW( + node->get_parameters({param_name}), + rclcpp::exceptions::ParameterUninitializedException); + } +} + +TEST_F(TestNode, get_parameter_with_node_allow_undeclared) { + rclcpp::NodeOptions no; + no.allow_undeclared_parameters(true); + auto node = std::make_shared("node", "ns", no); + { + const std::string param_name = "allow_undeclared_param"; + auto param = node->get_parameter(param_name); + EXPECT_EQ(param_name, param.get_name()); + EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, param.get_type()); + EXPECT_EQ(rclcpp::ParameterValue{}, param.get_parameter_value()); + } +} diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index 64ef2d903b..2ce414d327 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -440,6 +440,7 @@ TEST_F(TestParameterClient, sync_parameter_get_parameter_types_allow_undeclared) TEST_F(TestParameterClient, sync_parameter_get_parameters) { node->declare_parameter("foo", 4); node->declare_parameter("bar", "this is bar"); + node->declare_parameter("baz", rclcpp::PARAMETER_INTEGER); auto synchronous_client = std::make_shared(node); { @@ -448,6 +449,14 @@ TEST_F(TestParameterClient, sync_parameter_get_parameters) { ASSERT_EQ(0u, parameters.size()); } + { + // not throw ParameterUninitializedException while getting parameter from service + // even if the parameter is not initialized in the node + std::vector names{"baz"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(0u, parameters.size()); + } + { std::vector names{"none", "foo", "bar"}; std::vector parameters = synchronous_client->get_parameters(names, 10s); @@ -487,6 +496,7 @@ TEST_F(TestParameterClient, sync_parameter_get_parameters) { TEST_F(TestParameterClient, sync_parameter_get_parameters_allow_undeclared) { node_with_option->declare_parameter("foo", 4); node_with_option->declare_parameter("bar", "this is bar"); + node_with_option->declare_parameter("baz", rclcpp::PARAMETER_INTEGER); auto synchronous_client = std::make_shared(node_with_option); { @@ -495,6 +505,14 @@ TEST_F(TestParameterClient, sync_parameter_get_parameters_allow_undeclared) { ASSERT_EQ(1u, parameters.size()); } + { + // not throw ParameterUninitializedException while getting parameter from service + // even if the parameter is not initialized in the node + std::vector names{"baz"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(0u, parameters.size()); + } + { std::vector names{"none", "foo", "bar"}; std::vector parameters = synchronous_client->get_parameters(names, 10s); From e5d20474da86f2835e2704d2e3bd78ada5a37d86 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 23 Nov 2022 08:44:34 -0800 Subject: [PATCH 156/455] Mark rclcpp::Clock::now() as const (#2050) Signed-off-by: Shane Loretz Signed-off-by: Shane Loretz --- rclcpp/include/rclcpp/clock.hpp | 2 +- rclcpp/src/rclcpp/clock.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 42d3e01e7e..1e08e1d8fd 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -77,7 +77,7 @@ class Clock */ RCLCPP_PUBLIC Time - now(); + now() const; /** * Sleep until a specified Time, according to clock type. diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index 66c8db70e1..b34f328690 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -67,7 +67,7 @@ Clock::Clock(rcl_clock_type_t clock_type) Clock::~Clock() {} Time -Clock::now() +Clock::now() const { Time now(0, 0, impl_->rcl_clock_.type); From a00ef22d6d3a5b3ce67c60459642665e2fdcd914 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 30 Nov 2022 08:31:53 -0500 Subject: [PATCH 157/455] Add in a warning for a KeepLast depth of 0. (#2048) * Add in a warning for a KeepLast depth of 0. It really doesn't make much sense to have a KeepLast depth of 0; no data could possibly be stored. Indeed, the underlying DDS implementations don't actually support this. It currently "works" because a KeepLast depth of 0 is assumed to be system default, so the RMW layer typically chooses "1" instead. But this isn't something we should be encouraging users to do, so add a warning. Signed-off-by: Chris Lalancette --- rclcpp/src/rclcpp/qos.cpp | 29 +++++++++++++++++++++- rclcpp/test/rclcpp/test_qos_parameters.cpp | 6 +++++ 2 files changed, 34 insertions(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/qos.cpp b/rclcpp/src/rclcpp/qos.cpp index 70b9976db6..2d0277bfb5 100644 --- a/rclcpp/src/rclcpp/qos.cpp +++ b/rclcpp/src/rclcpp/qos.cpp @@ -16,6 +16,8 @@ #include +#include "rclcpp/logging.hpp" + #include "rmw/error_handling.h" #include "rmw/types.h" #include "rmw/qos_profiles.h" @@ -67,13 +69,30 @@ KeepAll::KeepAll() KeepLast::KeepLast(size_t depth) : QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth) -{} +{ + if (depth == 0) { + RCLCPP_WARN_ONCE( + rclcpp::get_logger( + "rclcpp"), + "A zero depth with KEEP_LAST doesn't make sense; no data could be stored." + "This will be interpreted as SYSTEM_DEFAULT"); + } +} QoS::QoS( const QoSInitialization & qos_initialization, const rmw_qos_profile_t & initial_profile) : rmw_qos_profile_(initial_profile) { + if (qos_initialization.history_policy == RMW_QOS_POLICY_HISTORY_KEEP_LAST && + qos_initialization.depth == 0) + { + RCLCPP_WARN_ONCE( + rclcpp::get_logger( + "rclcpp"), + "A zero depth with KEEP_LAST doesn't make sense; no data could be stored." + "This will be interpreted as SYSTEM_DEFAULT"); + } rmw_qos_profile_.history = qos_initialization.history_policy; rmw_qos_profile_.depth = qos_initialization.depth; } @@ -111,6 +130,14 @@ QoS::history(HistoryPolicy history) QoS & QoS::keep_last(size_t depth) { + if (depth == 0) { + RCLCPP_WARN_ONCE( + rclcpp::get_logger( + "rclcpp"), + "A zero depth with KEEP_LAST doesn't make sense; no data could be stored." + "This will be interpreted as SYSTEM_DEFAULT"); + } + rmw_qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; rmw_qos_profile_.depth = depth; return *this; diff --git a/rclcpp/test/rclcpp/test_qos_parameters.cpp b/rclcpp/test/rclcpp/test_qos_parameters.cpp index 97d524d176..be9a12cb5e 100644 --- a/rclcpp/test/rclcpp/test_qos_parameters.cpp +++ b/rclcpp/test/rclcpp/test_qos_parameters.cpp @@ -255,3 +255,9 @@ TEST(TestQosParameters, internal_functions_failure_modes) { nullptr, rclcpp::QosPolicyKind::Reliability), std::invalid_argument); } + +TEST(TestQosParameters, keep_last_zero) { + rclcpp::KeepLast keep_last(0); + + EXPECT_EQ(keep_last.depth, RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT); +} From 66b19448b0520b15a9e6c28483863b2a4351c2f6 Mon Sep 17 00:00:00 2001 From: Lei Liu <64953129+llapx@users.noreply.github.com> Date: Fri, 2 Dec 2022 10:17:43 +0800 Subject: [PATCH 158/455] Fix SharedFuture from async_send_request never becomes valid (#2044) Signed-off-by: Lei Liu --- rclcpp/include/rclcpp/client.hpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 04e0ce9a33..b10e3e1593 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -792,16 +792,14 @@ class Client : public ClientBase async_send_request_impl(const Request & request, CallbackInfoVariant value) { int64_t sequence_number; + std::lock_guard lock(pending_requests_mutex_); rcl_ret_t ret = rcl_send_request(get_client_handle().get(), &request, &sequence_number); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request"); } - { - std::lock_guard lock(pending_requests_mutex_); - pending_requests_.try_emplace( - sequence_number, - std::make_pair(std::chrono::system_clock::now(), std::move(value))); - } + pending_requests_.try_emplace( + sequence_number, + std::make_pair(std::chrono::system_clock::now(), std::move(value))); return sequence_number; } From 338eed0c060ab58608fc6e44c6936bde2c4e061e Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 6 Dec 2022 11:19:40 -0800 Subject: [PATCH 159/455] Remove templating on to_rcl_subscription_options (#2056) --- rclcpp/include/rclcpp/generic_subscription.hpp | 2 +- rclcpp/include/rclcpp/subscription.hpp | 2 +- rclcpp/include/rclcpp/subscription_options.hpp | 1 - rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp | 4 ++-- 4 files changed, 4 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp index 673712eedb..16c8fe6fbf 100644 --- a/rclcpp/include/rclcpp/generic_subscription.hpp +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -81,7 +81,7 @@ class GenericSubscription : public rclcpp::SubscriptionBase node_base, *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), topic_name, - options.template to_rcl_subscription_options(qos), + options.to_rcl_subscription_options(qos), true), callback_(callback), ts_lib_(ts_lib) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 11bf9c6e43..74e6b5c05d 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -140,7 +140,7 @@ class Subscription : public SubscriptionBase node_base, type_support_handle, topic_name, - options.template to_rcl_subscription_options(qos), + options.to_rcl_subscription_options(qos), callback.is_serialized_message_callback()), any_callback_(callback), options_(options), diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 2b819da399..c5c3e21eb1 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -110,7 +110,6 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase * \param qos QoS profile for subcription. * \return rcl_subscription_options_t structure based on the rclcpp::QoS */ - template rcl_subscription_options_t to_rcl_subscription_options(const rclcpp::QoS & qos) const { diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp index a99633bec6..264577a739 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp @@ -44,8 +44,8 @@ const rcl_publisher_options_t PublisherOptions() const rcl_subscription_options_t SubscriptionOptions() { - return rclcpp::SubscriptionOptionsWithAllocator>().template - to_rcl_subscription_options(rclcpp::QoS(10)); + return rclcpp::SubscriptionOptionsWithAllocator>() + .to_rcl_subscription_options(rclcpp::QoS(10)); } } // namespace From 1ac37b692c4cce54f0ffeaad1f4fe3d5688322bd Mon Sep 17 00:00:00 2001 From: andrei Date: Tue, 13 Dec 2022 22:31:14 +0900 Subject: [PATCH 160/455] fix nullptr dereference in prune_requests_older_than (#2008) * fix nullptr dereference in prune_requests_older_than Signed-off-by: akela1101 * add tests for prune_requests_older_than Signed-off-by: akela1101 * Update rclcpp/test/rclcpp/test_client.cpp Co-authored-by: Chen Lihui Signed-off-by: akela1101 Signed-off-by: akela1101 Co-authored-by: Chen Lihui --- rclcpp/include/rclcpp/client.hpp | 4 +++- rclcpp/test/rclcpp/test_client.cpp | 21 +++++++++++++++++++++ 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index b10e3e1593..d84ee089b5 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -769,7 +769,9 @@ class Client : public ClientBase auto old_size = pending_requests_.size(); for (auto it = pending_requests_.begin(), last = pending_requests_.end(); it != last; ) { if (it->second.first < time_point) { - pruned_requests->push_back(it->first); + if (pruned_requests) { + pruned_requests->push_back(it->first); + } it = pending_requests_.erase(it); } else { ++it; diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp index a9d81d9176..9070e1caa9 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -327,6 +327,27 @@ TEST_F(TestClientWithServer, test_client_remove_pending_request) { EXPECT_TRUE(client->remove_pending_request(future)); } +TEST_F(TestClientWithServer, prune_requests_older_than_no_pruned) { + auto client = node->create_client(service_name); + auto request = std::make_shared(); + auto future = client->async_send_request(request); + auto time = std::chrono::system_clock::now() + 1s; + + EXPECT_EQ(1u, client->prune_requests_older_than(time)); +} + +TEST_F(TestClientWithServer, prune_requests_older_than_with_pruned) { + auto client = node->create_client(service_name); + auto request = std::make_shared(); + auto future = client->async_send_request(request); + auto time = std::chrono::system_clock::now() + 1s; + + std::vector pruned_requests; + EXPECT_EQ(1u, client->prune_requests_older_than(time, &pruned_requests)); + ASSERT_EQ(1u, pruned_requests.size()); + EXPECT_EQ(future.request_id, pruned_requests[0]); +} + TEST_F(TestClientWithServer, async_send_request_rcl_send_request_error) { // Checking rcl_send_request in rclcpp::Client::async_send_request() auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_request, RCL_RET_ERROR); From 432bf21f0261bab209b37ccfe6da550f02751a22 Mon Sep 17 00:00:00 2001 From: Jeffery Hsu Date: Tue, 13 Dec 2022 11:11:09 -0600 Subject: [PATCH 161/455] Add clock type to node_options (#1982) * add clock type to node_options and change node/lifecycle_node constructor accordingly Signed-off-by: Jeffery Hsu * Modify TimeSource::NodeState class to work with different clock types. Add test cases. Signed-off-by: Jeffery Hsu * Change on_parameter_event to output RCLCPP_ERROR and check ros_time_active_ in ClocksState::attachClock() Signed-off-by: Jeffery Hsu * Remove a redundant include Signed-off-by: Jeffery Hsu * Disallow setting use_sim_time to true if a clock_type can't support it. Following the reasoning in c54a6f1cd2b5bfbe530c362ad7e8fa22f753e325, on_set_parameters doesn't try to enforce use_sim_time to be of boolean type explicitly. Signed-off-by: Jeffery Hsu * minior style change Signed-off-by: Jeffery Hsu * remove reason string for successful result Signed-off-by: Jeffery Hsu Signed-off-by: Jeffery Hsu --- .../rclcpp/node_interfaces/node_clock.hpp | 6 +- rclcpp/include/rclcpp/node_options.hpp | 17 ++++ rclcpp/src/rclcpp/node.cpp | 3 +- .../src/rclcpp/node_interfaces/node_clock.cpp | 9 +- rclcpp/src/rclcpp/node_options.cpp | 14 +++ rclcpp/src/rclcpp/time_source.cpp | 92 +++++++++++++++---- rclcpp/test/rclcpp/test_node.cpp | 68 ++++++++++++++ rclcpp/test/rclcpp/test_node_options.cpp | 9 ++ rclcpp/test/rclcpp/test_time_source.cpp | 29 ++++++ rclcpp_lifecycle/src/lifecycle_node.cpp | 3 +- 10 files changed, 222 insertions(+), 28 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp b/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp index fd9a34a907..fb2e0670a7 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_ +#include "rcl/time.h" #include "rclcpp/clock.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" @@ -42,7 +43,8 @@ class NodeClock : public NodeClockInterface rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging); + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rcl_clock_type_t clock_type); RCLCPP_PUBLIC virtual @@ -67,7 +69,7 @@ class NodeClock : public NodeClockInterface rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; - rclcpp::Clock::SharedPtr ros_clock_; + rclcpp::Clock::SharedPtr clock_; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_options.hpp b/rclcpp/include/rclcpp/node_options.hpp index d9735357dd..47a97fd3c4 100644 --- a/rclcpp/include/rclcpp/node_options.hpp +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -19,6 +19,7 @@ #include #include +#include "rcl/time.h" #include "rcl/node_options.h" #include "rclcpp/context.hpp" #include "rclcpp/contexts/default_context.hpp" @@ -46,6 +47,7 @@ class NodeOptions * - enable_topic_statistics = false * - start_parameter_services = true * - start_parameter_event_publisher = true + * - clock_type = RCL_ROS_TIME * - clock_qos = rclcpp::ClockQoS() * - use_clock_thread = true * - rosout_qos = rclcpp::RosoutQoS() @@ -246,6 +248,19 @@ class NodeOptions NodeOptions & start_parameter_event_publisher(bool start_parameter_event_publisher); + /// Return a reference to the clock type. + RCLCPP_PUBLIC + const rcl_clock_type_t & + clock_type() const; + + /// Set the clock type. + /** + * The clock type to be used by the node. + */ + RCLCPP_PUBLIC + NodeOptions & + clock_type(const rcl_clock_type_t & clock_type); + /// Return a reference to the clock QoS. RCLCPP_PUBLIC const rclcpp::QoS & @@ -400,6 +415,8 @@ class NodeOptions bool start_parameter_event_publisher_ {true}; + rcl_clock_type_t clock_type_ {RCL_ROS_TIME}; + rclcpp::QoS clock_qos_ = rclcpp::ClockQoS(); bool use_clock_thread_ {true}; diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 7dba3e51ad..c1fbdb1f98 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -176,7 +176,8 @@ Node::Node( node_topics_, node_graph_, node_services_, - node_logging_ + node_logging_, + options.clock_type() )), node_parameters_(new rclcpp::node_interfaces::NodeParameters( node_base_, diff --git a/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp b/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp index 734d7bdb37..a37c65b71b 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp @@ -24,13 +24,14 @@ NodeClock::NodeClock( rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging) + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rcl_clock_type_t clock_type) : node_base_(node_base), node_topics_(node_topics), node_graph_(node_graph), node_services_(node_services), node_logging_(node_logging), - ros_clock_(std::make_shared(RCL_ROS_TIME)) + clock_(std::make_shared(clock_type)) {} NodeClock::~NodeClock() @@ -39,11 +40,11 @@ NodeClock::~NodeClock() rclcpp::Clock::SharedPtr NodeClock::get_clock() { - return ros_clock_; + return clock_; } rclcpp::Clock::ConstSharedPtr NodeClock::get_clock() const { - return ros_clock_; + return clock_; } diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index 06feffd1e6..b90a4b27e7 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -77,6 +77,7 @@ NodeOptions::operator=(const NodeOptions & other) this->enable_topic_statistics_ = other.enable_topic_statistics_; this->start_parameter_services_ = other.start_parameter_services_; this->start_parameter_event_publisher_ = other.start_parameter_event_publisher_; + this->clock_type_ = other.clock_type_; this->clock_qos_ = other.clock_qos_; this->use_clock_thread_ = other.use_clock_thread_; this->parameter_event_qos_ = other.parameter_event_qos_; @@ -260,6 +261,19 @@ NodeOptions::start_parameter_event_publisher(bool start_parameter_event_publishe return *this; } +const rcl_clock_type_t & +NodeOptions::clock_type() const +{ + return this->clock_type_; +} + +NodeOptions & +NodeOptions::clock_type(const rcl_clock_type_t & clock_type) +{ + this->clock_type_ = clock_type; + return *this; +} + const rclcpp::QoS & NodeOptions::clock_qos() const { diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 42a9208251..bf1f9b3daa 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -91,10 +91,13 @@ class ClocksState final // Attach a clock void attachClock(rclcpp::Clock::SharedPtr clock) { - if (clock->get_clock_type() != RCL_ROS_TIME) { - throw std::invalid_argument("Cannot attach clock to a time source that's not a ROS clock"); + { + std::lock_guard clock_guard(clock->get_clock_mutex()); + if (clock->get_clock_type() != RCL_ROS_TIME && ros_time_active_) { + throw std::invalid_argument( + "ros_time_active_ can't be true while clock is not of RCL_ROS_TIME type"); + } } - std::lock_guard guard(clock_list_lock_); associated_clocks_.push_back(clock); // Set the clock to zero unless there's a recently received message @@ -125,27 +128,32 @@ class ClocksState final { std::lock_guard clock_guard(clock->get_clock_mutex()); - // Do change - if (!set_ros_time_enabled && clock->ros_time_is_active()) { - auto ret = rcl_disable_ros_time_override(clock->get_clock_handle()); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "Failed to disable ros_time_override_status"); + if (clock->get_clock_type() == RCL_ROS_TIME) { + // Do change + if (!set_ros_time_enabled && clock->ros_time_is_active()) { + auto ret = rcl_disable_ros_time_override(clock->get_clock_handle()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Failed to disable ros_time_override_status"); + } + } else if (set_ros_time_enabled && !clock->ros_time_is_active()) { + auto ret = rcl_enable_ros_time_override(clock->get_clock_handle()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Failed to enable ros_time_override_status"); + } } - } else if (set_ros_time_enabled && !clock->ros_time_is_active()) { - auto ret = rcl_enable_ros_time_override(clock->get_clock_handle()); + + auto ret = rcl_set_ros_time_override( + clock->get_clock_handle(), + rclcpp::Time(*msg).nanoseconds()); if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error( - ret, "Failed to enable ros_time_override_status"); + ret, "Failed to set ros_time_override_status"); } - } - - auto ret = rcl_set_ros_time_override( - clock->get_clock_handle(), - rclcpp::Time(*msg).nanoseconds()); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "Failed to set ros_time_override_status"); + } else if (set_ros_time_enabled) { + throw std::invalid_argument( + "set_ros_time_enabled can't be true while clock is not of RCL_ROS_TIME type"); } } @@ -166,6 +174,18 @@ class ClocksState final last_msg_set_ = msg; } + bool are_all_clocks_rcl_ros_time() + { + std::lock_guard guard(clock_list_lock_); + for (auto & clock : associated_clocks_) { + std::lock_guard clock_guard(clock->get_clock_mutex()); + if (clock->get_clock_type() != RCL_ROS_TIME) { + return false; + } + } + return true; + } + private: // Store (and update on node attach) logger for logging. Logger logger_; @@ -266,6 +286,10 @@ class TimeSource::NodeState final throw std::invalid_argument("Invalid type for parameter 'use_sim_time', should be 'bool'"); } + on_set_parameters_callback_ = node_parameters_->add_on_set_parameters_callback( + std::bind(&TimeSource::NodeState::on_set_parameters, this, std::placeholders::_1)); + + // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609 parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event( node_topics_, @@ -285,6 +309,10 @@ class TimeSource::NodeState final // can't possibly call any of the callbacks as we are cleaning up. destroy_clock_sub(); clocks_state_.disable_ros_time(); + if (on_set_parameters_callback_) { + node_parameters_->remove_on_set_parameters_callback(on_set_parameters_callback_.get()); + } + on_set_parameters_callback_.reset(); parameter_subscription_.reset(); node_base_.reset(); node_topics_.reset(); @@ -419,10 +447,34 @@ class TimeSource::NodeState final clock_subscription_.reset(); } + // On set Parameters callback handle + node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_{nullptr}; + // Parameter Event subscription using ParamSubscriptionT = rclcpp::Subscription; std::shared_ptr parameter_subscription_; + // Callback for parameter settings + rcl_interfaces::msg::SetParametersResult on_set_parameters( + const std::vector & parameters) + { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto & param : parameters) { + if (param.get_name() == "use_sim_time" && param.get_type() == rclcpp::PARAMETER_BOOL) { + if (param.as_bool() && !(clocks_state_.are_all_clocks_rcl_ros_time())) { + result.successful = false; + result.reason = + "use_sim_time parameter can't be true while clocks are not all of RCL_ROS_TIME type"; + RCLCPP_ERROR( + logger_, + "use_sim_time parameter can't be true while clocks are not all of RCL_ROS_TIME type"); + } + } + } + return result; + } + // Callback for parameter updates void on_parameter_event(std::shared_ptr event) { diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index 19c87b3684..512c2c1384 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -95,6 +95,74 @@ TEST_F(TestNode, construction_and_destruction) { (void)node; }, rclcpp::exceptions::InvalidNamespaceError); } + + { + rclcpp::NodeOptions options; + ASSERT_NO_THROW( + { + const auto node = std::make_shared("my_node", "/ns", options); + EXPECT_EQ(RCL_ROS_TIME, node->get_clock()->get_clock_type()); + }); + } + + { + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"use_sim_time", true}, + }); + ASSERT_NO_THROW( + { + const auto node = std::make_shared("my_node", "/ns", options); + EXPECT_EQ(RCL_ROS_TIME, node->get_clock()->get_clock_type()); + }); + } + + { + rclcpp::NodeOptions options; + options.clock_type(RCL_SYSTEM_TIME); + ASSERT_NO_THROW( + { + const auto node = std::make_shared("my_node", "/ns", options); + EXPECT_EQ(RCL_SYSTEM_TIME, node->get_clock()->get_clock_type()); + }); + } + + { + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"use_sim_time", true}, + }); + options.clock_type(RCL_SYSTEM_TIME); + ASSERT_THROW( + const auto node = std::make_shared( + "my_node", "/ns", + options), std::invalid_argument); + } + + { + rclcpp::NodeOptions options; + options.clock_type(RCL_STEADY_TIME); + ASSERT_NO_THROW( + { + const auto node = std::make_shared("my_node", "/ns", options); + EXPECT_EQ(RCL_STEADY_TIME, node->get_clock()->get_clock_type()); + }); + } + + { + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"use_sim_time", true}, + }); + options.clock_type(RCL_STEADY_TIME); + ASSERT_THROW( + const auto node = std::make_shared( + "my_node", "/ns", + options), std::invalid_argument); + } } /* diff --git a/rclcpp/test/rclcpp/test_node_options.cpp b/rclcpp/test/rclcpp/test_node_options.cpp index 3fd0a0d52b..2468923229 100644 --- a/rclcpp/test/rclcpp/test_node_options.cpp +++ b/rclcpp/test/rclcpp/test_node_options.cpp @@ -316,3 +316,12 @@ TEST(TestNodeOptions, set_get_allocator) { // Check invalid allocator EXPECT_THROW(options.get_rcl_node_options(), std::bad_alloc); } + +TEST(TestNodeOptions, clock_type) { + rclcpp::NodeOptions options; + EXPECT_EQ(RCL_ROS_TIME, options.clock_type()); + options.clock_type(RCL_SYSTEM_TIME); + EXPECT_EQ(RCL_SYSTEM_TIME, options.clock_type()); + options.clock_type(RCL_STEADY_TIME); + EXPECT_EQ(RCL_STEADY_TIME, options.clock_type()); +} diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index a5aad9a498..22aefcda3b 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -267,6 +267,35 @@ TEST(TimeSource, invalid_sim_time_parameter_override) rclcpp::shutdown(); } +TEST(TimeSource, valid_clock_type_for_sim_time) +{ + rclcpp::init(0, nullptr); + + rclcpp::NodeOptions options; + auto node = std::make_shared("my_node", options); + EXPECT_TRUE( + node->set_parameter( + rclcpp::Parameter( + "use_sim_time", rclcpp::ParameterValue( + true))).successful); + rclcpp::shutdown(); +} + +TEST(TimeSource, invalid_clock_type_for_sim_time) +{ + rclcpp::init(0, nullptr); + + rclcpp::NodeOptions options; + options.clock_type(RCL_STEADY_TIME); + auto node = std::make_shared("my_node", options); + EXPECT_FALSE( + node->set_parameter( + rclcpp::Parameter( + "use_sim_time", rclcpp::ParameterValue( + true))).successful); + rclcpp::shutdown(); +} + TEST_F(TestTimeSource, clock) { rclcpp::TimeSource ts(node); auto ros_clock = std::make_shared(RCL_ROS_TIME); diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 773febda95..723c7bc8c1 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -85,7 +85,8 @@ LifecycleNode::LifecycleNode( node_topics_, node_graph_, node_services_, - node_logging_ + node_logging_, + options.clock_type() )), node_parameters_(new rclcpp::node_interfaces::NodeParameters( node_base_, From 86335dd4acd91d5dd973c4e4e97014e5e8a916bc Mon Sep 17 00:00:00 2001 From: Mateusz Szczygielski <112629916+msz-rai@users.noreply.github.com> Date: Tue, 13 Dec 2022 19:40:38 +0100 Subject: [PATCH 162/455] Fix logging macros to build with msvc and cpp20 (#2063) Signed-off-by: Mateusz Szczygielski Signed-off-by: Mateusz Szczygielski --- rclcpp/resource/logging.hpp.em | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/resource/logging.hpp.em b/rclcpp/resource/logging.hpp.em index 63581ceb54..01087e5dd0 100644 --- a/rclcpp/resource/logging.hpp.em +++ b/rclcpp/resource/logging.hpp.em @@ -124,7 +124,7 @@ def get_rclcpp_suffix_from_features(features): ) \ do { \ static_assert( \ - ::std::is_same::type>::type, \ + ::std::is_same>, \ typename ::rclcpp::Logger>::value, \ "First argument to logging macros must be an rclcpp::Logger"); \ @[ if 'throttle' in feature_combination]@ \ From a20a295a3bd78c1b27217de477a6ad17e0ba0df8 Mon Sep 17 00:00:00 2001 From: mauropasse Date: Fri, 16 Dec 2022 09:44:59 +0000 Subject: [PATCH 163/455] Set explicitly callback type (#2059) * Set explicitly callback type Signed-off-by: Mauro Passerino * Revert "Set explicitly callback type" This reverts commit dfb4c54adb0580d976199738a8a6dcdb261a87b0. Signed-off-by: Mauro Passerino * Add type info to cpp_callback_trampoline Signed-off-by: Mauro Passerino Signed-off-by: Mauro Passerino Co-authored-by: Mauro Passerino --- rclcpp/include/rclcpp/client.hpp | 5 +++-- rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp | 4 +++- rclcpp/include/rclcpp/qos_event.hpp | 5 +++-- rclcpp/include/rclcpp/service.hpp | 5 +++-- rclcpp/include/rclcpp/subscription_base.hpp | 5 +++-- rclcpp_action/src/client.cpp | 4 ++-- rclcpp_action/src/server.cpp | 4 ++-- 7 files changed, 19 insertions(+), 13 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index d84ee089b5..c3a2210a71 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -312,7 +312,7 @@ class ClientBase // 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, + rclcpp::detail::cpp_callback_trampoline, static_cast(&new_callback)); // Store the std::function to keep it in scope, also overwrites the existing one. @@ -320,7 +320,8 @@ class ClientBase // Set it again, now using the permanent storage. set_on_new_response_callback( - rclcpp::detail::cpp_callback_trampoline, + rclcpp::detail::cpp_callback_trampoline< + decltype(on_new_response_callback_), const void *, size_t>, static_cast(&on_new_response_callback_)); } diff --git a/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp b/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp index 4f46dfc939..e9c07b71e0 100644 --- a/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp +++ b/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp @@ -41,6 +41,7 @@ namespace detail * so no exceptions should be thrown at this point, and doing so results in * undefined behavior. * + * \tparam UserDataRealT Declared type of the passed function * \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 @@ -50,6 +51,7 @@ namespace detail * \returns whatever the callback returns, if anything */ template< + typename UserDataRealT, typename UserDataT, typename ... Args, typename ReturnT = void @@ -57,7 +59,7 @@ template< ReturnT cpp_callback_trampoline(UserDataT user_data, Args ... args) noexcept { - auto & actual_callback = *reinterpret_cast *>(user_data); + auto & actual_callback = *static_cast(user_data); return actual_callback(args ...); } diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index f258c5d5ba..b48854c4bd 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -185,7 +185,7 @@ class QOSEventHandlerBase : public Waitable // 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, + rclcpp::detail::cpp_callback_trampoline, static_cast(&new_callback)); // Store the std::function to keep it in scope, also overwrites the existing one. @@ -193,7 +193,8 @@ class QOSEventHandlerBase : public Waitable // Set it again, now using the permanent storage. set_on_new_event_callback( - rclcpp::detail::cpp_callback_trampoline, + rclcpp::detail::cpp_callback_trampoline< + decltype(on_new_event_callback_), const void *, size_t>, static_cast(&on_new_event_callback_)); } diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 65b457ffda..9e258a0223 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -222,7 +222,7 @@ class ServiceBase // 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, + rclcpp::detail::cpp_callback_trampoline, static_cast(&new_callback)); // Store the std::function to keep it in scope, also overwrites the existing one. @@ -230,7 +230,8 @@ class ServiceBase // Set it again, now using the permanent storage. set_on_new_request_callback( - rclcpp::detail::cpp_callback_trampoline, + rclcpp::detail::cpp_callback_trampoline< + decltype(on_new_request_callback_), const void *, size_t>, static_cast(&on_new_request_callback_)); } diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index f21f27e864..09fabe79e5 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -348,7 +348,7 @@ class SubscriptionBase : public std::enable_shared_from_this // 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, + rclcpp::detail::cpp_callback_trampoline, static_cast(&new_callback)); // Store the std::function to keep it in scope, also overwrites the existing one. @@ -356,7 +356,8 @@ class SubscriptionBase : public std::enable_shared_from_this // Set it again, now using the permanent storage. set_on_new_message_callback( - rclcpp::detail::cpp_callback_trampoline, + rclcpp::detail::cpp_callback_trampoline< + decltype(on_new_message_callback_), const void *, size_t>, static_cast(&on_new_message_callback_)); } diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 2266b7cee6..e687ab3400 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -433,7 +433,7 @@ ClientBase::set_callback_to_entity( // 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, + rclcpp::detail::cpp_callback_trampoline, static_cast(&new_callback)); std::lock_guard lock(listener_mutex_); @@ -453,7 +453,7 @@ ClientBase::set_callback_to_entity( auto & cb = it->second; set_on_ready_callback( entity_type, - rclcpp::detail::cpp_callback_trampoline, + rclcpp::detail::cpp_callback_trampolinesecond), const void *, size_t>, static_cast(&cb)); } diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 42c4074495..c0fa96a88e 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -745,7 +745,7 @@ ServerBase::set_callback_to_entity( // 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, + rclcpp::detail::cpp_callback_trampoline, static_cast(&new_callback)); std::lock_guard lock(listener_mutex_); @@ -765,7 +765,7 @@ ServerBase::set_callback_to_entity( auto & cb = it->second; set_on_ready_callback( entity_type, - rclcpp::detail::cpp_callback_trampoline, + rclcpp::detail::cpp_callback_trampolinesecond), const void *, size_t>, static_cast(&cb)); } From c091fe1a4538dbb370a31d0e590bd44ae4194483 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 19 Dec 2022 18:18:54 -0800 Subject: [PATCH 164/455] Implement validity checks for rclcpp::Clock (#2040) --- rclcpp/include/rclcpp/clock.hpp | 45 ++++++++++++++++++++ rclcpp/src/rclcpp/clock.cpp | 65 +++++++++++++++++++++++++++++ rclcpp/test/rclcpp/test_time.cpp | 70 ++++++++++++++++++++++++++++++++ 3 files changed, 180 insertions(+) diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 1e08e1d8fd..702b224d53 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -137,6 +137,51 @@ class Clock Duration rel_time, Context::SharedPtr context = contexts::get_global_default_context()); + /** + * Check if the clock is started. + * + * A started clock is a clock that reflects non-zero time. + * Typically a clock will be unstarted if it is using RCL_ROS_TIME with ROS time and + * nothing has been published on the clock topic yet. + * + * \return true if clock is started + * \throws std::runtime_error if the clock is not rcl_clock_valid + */ + RCLCPP_PUBLIC + bool + started(); + + /** + * Wait until clock to start. + * + * \rclcpp::Clock::started + * \param context the context to wait in + * \return true if clock was already started or became started + * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid + */ + RCLCPP_PUBLIC + bool + wait_until_started(Context::SharedPtr context = contexts::get_global_default_context()); + + /** + * Wait for clock to start, with timeout. + * + * The timeout is waited in steady time. + * + * \rclcpp::Clock::started + * \param timeout the maximum time to wait for. + * \param context the context to wait in. + * \param wait_tick_ns the time to wait between each iteration of the wait loop (in nanoseconds). + * \return true if clock was or became valid + * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid + */ + RCLCPP_PUBLIC + bool + wait_until_started( + const rclcpp::Duration & timeout, + Context::SharedPtr context = contexts::get_global_default_context(), + const rclcpp::Duration & wait_tick_ns = rclcpp::Duration(0, static_cast(1e7))); + /** * Returns the clock of the type `RCL_ROS_TIME` is active. * diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index b34f328690..f46649a77d 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -182,6 +182,71 @@ Clock::sleep_for(Duration rel_time, Context::SharedPtr context) return sleep_until(now() + rel_time, context); } +bool +Clock::started() +{ + if (!rcl_clock_valid(get_clock_handle())) { + throw std::runtime_error("clock is not rcl_clock_valid"); + } + return rcl_clock_time_started(get_clock_handle()); +} + +bool +Clock::wait_until_started(Context::SharedPtr context) +{ + if (!context || !context->is_valid()) { + throw std::runtime_error("context cannot be slept with because it's invalid"); + } + if (!rcl_clock_valid(get_clock_handle())) { + throw std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid"); + } + + if (started()) { + return true; + } else { + // Wait until the first non-zero time + return sleep_until(rclcpp::Time(0, 1, get_clock_type()), context); + } +} + +bool +Clock::wait_until_started( + const Duration & timeout, + Context::SharedPtr context, + const Duration & wait_tick_ns) +{ + if (!context || !context->is_valid()) { + throw std::runtime_error("context cannot be slept with because it's invalid"); + } + if (!rcl_clock_valid(get_clock_handle())) { + throw std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid"); + } + + Clock timeout_clock = Clock(RCL_STEADY_TIME); + Time start = timeout_clock.now(); + + // Check if the clock has started every wait_tick_ns nanoseconds + // Context check checks for rclcpp::shutdown() + while (!started() && context->is_valid()) { + if (timeout < wait_tick_ns) { + timeout_clock.sleep_for(timeout); + } else { + Duration time_left = start + timeout - timeout_clock.now(); + if (time_left > wait_tick_ns) { + timeout_clock.sleep_for(Duration(wait_tick_ns)); + } else { + timeout_clock.sleep_for(time_left); + } + } + + if (timeout_clock.now() - start > timeout) { + return started(); + } + } + return started(); +} + + bool Clock::ros_time_is_active() { diff --git a/rclcpp/test/rclcpp/test_time.cpp b/rclcpp/test/rclcpp/test_time.cpp index 2f188b2d7c..f3969d3886 100644 --- a/rclcpp/test/rclcpp/test_time.cpp +++ b/rclcpp/test/rclcpp/test_time.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "rcl/error_handling.h" #include "rcl/time.h" @@ -848,3 +849,72 @@ TEST_F(TestClockSleep, sleep_for_basic_ros) { sleep_thread.join(); EXPECT_TRUE(sleep_succeeded); } + +class TestClockStarted : public ::testing::Test +{ +protected: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestClockStarted, started) { + // rclcpp::Clock ros_clock(RCL_ROS_TIME); + // auto ros_clock_handle = ros_clock.get_clock_handle(); + // + // // At this point, the ROS clock is reading system time since the ROS time override isn't on + // // So we expect it to be started (it's extremely unlikely that system time is at epoch start) + // EXPECT_TRUE(ros_clock.started()); + // EXPECT_TRUE(ros_clock.wait_until_started()); + // EXPECT_TRUE(ros_clock.wait_until_started(rclcpp::Duration(0, static_cast(1e7)))); + // EXPECT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(ros_clock_handle)); + // EXPECT_TRUE(ros_clock.ros_time_is_active()); + // EXPECT_FALSE(ros_clock.started()); + // EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(ros_clock_handle, 1)); + // EXPECT_TRUE(ros_clock.started()); + // + // rclcpp::Clock system_clock(RCL_SYSTEM_TIME); + // EXPECT_TRUE(system_clock.started()); + // EXPECT_TRUE(system_clock.wait_until_started()); + // EXPECT_TRUE(system_clock.wait_until_started(rclcpp::Duration(0, static_cast(1e7)))); + // + // rclcpp::Clock steady_clock(RCL_STEADY_TIME); + // EXPECT_TRUE(steady_clock.started()); + // EXPECT_TRUE(steady_clock.wait_until_started()); + // EXPECT_TRUE(steady_clock.wait_until_started(rclcpp::Duration(0, static_cast(1e7)))); + // + // rclcpp::Clock uninit_clock(RCL_CLOCK_UNINITIALIZED); + // RCLCPP_EXPECT_THROW_EQ( + // uninit_clock.started(), std::runtime_error("clock is not rcl_clock_valid")); + // RCLCPP_EXPECT_THROW_EQ( + // uninit_clock.wait_until_started(rclcpp::Duration(0, static_cast(1e7))), + // std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid")); +} + +TEST_F(TestClockStarted, started_timeout) { + rclcpp::Clock ros_clock(RCL_ROS_TIME); + auto ros_clock_handle = ros_clock.get_clock_handle(); + + EXPECT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(ros_clock_handle)); + EXPECT_TRUE(ros_clock.ros_time_is_active()); + + EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(ros_clock_handle, 0)); + + EXPECT_FALSE(ros_clock.started()); + EXPECT_FALSE(ros_clock.wait_until_started(rclcpp::Duration(0, static_cast(1e7)))); + + std::thread t([]() { + std::this_thread::sleep_for(std::chrono::seconds(1)); + rclcpp::shutdown(); + }); + + // Test rclcpp shutdown escape hatch (otherwise this waits indefinitely) + EXPECT_FALSE(ros_clock.wait_until_started()); + t.join(); +} From 9c1c9896a39047faadec0e784b692c5292d3c1dd Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 19 Dec 2022 18:21:40 -0800 Subject: [PATCH 165/455] Move event callback binding to PublisherBase and SubscriptionBase (#2066) --- rclcpp/include/rclcpp/generic_publisher.hpp | 36 +++------------- .../include/rclcpp/generic_subscription.hpp | 38 ++-------------- rclcpp/include/rclcpp/publisher.hpp | 32 ++------------ rclcpp/include/rclcpp/publisher_base.hpp | 11 ++++- rclcpp/include/rclcpp/subscription.hpp | 35 ++------------- rclcpp/include/rclcpp/subscription_base.hpp | 10 +++++ rclcpp/src/rclcpp/publisher_base.cpp | 41 +++++++++++++++++- rclcpp/src/rclcpp/subscription_base.cpp | 43 ++++++++++++++++++- .../node_interfaces/test_node_topics.cpp | 18 ++++---- rclcpp/test/rclcpp/test_publisher.cpp | 9 ++-- rclcpp/test/rclcpp/test_type_support.cpp | 17 +++++--- 11 files changed, 141 insertions(+), 149 deletions(-) diff --git a/rclcpp/include/rclcpp/generic_publisher.hpp b/rclcpp/include/rclcpp/generic_publisher.hpp index e1b46002bc..7cd2d8bc39 100644 --- a/rclcpp/include/rclcpp/generic_publisher.hpp +++ b/rclcpp/include/rclcpp/generic_publisher.hpp @@ -78,38 +78,12 @@ class GenericPublisher : public rclcpp::PublisherBase node_base, topic_name, *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), - options.template to_rcl_publisher_options(qos)), + options.template to_rcl_publisher_options(qos), + // NOTE(methylDragon): Passing these args separately is necessary for event binding + options.event_callbacks, + options.use_default_callbacks), ts_lib_(ts_lib) - { - // This is unfortunately duplicated with the code in publisher.hpp. - // TODO(nnmm): Deduplicate by moving this into PublisherBase. - if (options.event_callbacks.deadline_callback) { - this->add_event_handler( - options.event_callbacks.deadline_callback, - RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); - } - if (options.event_callbacks.liveliness_callback) { - this->add_event_handler( - options.event_callbacks.liveliness_callback, - RCL_PUBLISHER_LIVELINESS_LOST); - } - if (options.event_callbacks.incompatible_qos_callback) { - this->add_event_handler( - options.event_callbacks.incompatible_qos_callback, - RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); - } else if (options.use_default_callbacks) { - // Register default callback when not specified - try { - this->add_event_handler( - [this](QOSOfferedIncompatibleQoSInfo & info) { - this->default_incompatible_qos_callback(info); - }, - RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); - } catch (UnsupportedEventTypeException & /*exc*/) { - // pass - } - } - } + {} RCLCPP_PUBLIC virtual ~GenericPublisher() = default; diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp index 16c8fe6fbf..12a1c79f8f 100644 --- a/rclcpp/include/rclcpp/generic_subscription.hpp +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -82,44 +82,12 @@ class GenericSubscription : public rclcpp::SubscriptionBase *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), topic_name, options.to_rcl_subscription_options(qos), + options.event_callbacks, + options.use_default_callbacks, true), callback_(callback), ts_lib_(ts_lib) - { - // This is unfortunately duplicated with the code in subscription.hpp. - // TODO(nnmm): Deduplicate by moving this into SubscriptionBase. - if (options.event_callbacks.deadline_callback) { - this->add_event_handler( - options.event_callbacks.deadline_callback, - RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); - } - if (options.event_callbacks.liveliness_callback) { - this->add_event_handler( - options.event_callbacks.liveliness_callback, - RCL_SUBSCRIPTION_LIVELINESS_CHANGED); - } - if (options.event_callbacks.incompatible_qos_callback) { - this->add_event_handler( - options.event_callbacks.incompatible_qos_callback, - RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); - } else if (options.use_default_callbacks) { - // Register default callback when not specified - try { - this->add_event_handler( - [this](QOSRequestedIncompatibleQoSInfo & info) { - this->default_incompatible_qos_callback(info); - }, - RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); - } catch (UnsupportedEventTypeException & /*exc*/) { - // pass - } - } - if (options.event_callbacks.message_lost_callback) { - this->add_event_handler( - options.event_callbacks.message_lost_callback, - RCL_SUBSCRIPTION_MESSAGE_LOST); - } - } + {} RCLCPP_PUBLIC virtual ~GenericSubscription() = default; diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 78b5700611..43fa4ef6a2 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -131,40 +131,16 @@ class Publisher : public PublisherBase node_base, topic, rclcpp::get_message_type_support_handle(), - options.template to_rcl_publisher_options(qos)), + options.template to_rcl_publisher_options(qos), + // NOTE(methylDragon): Passing these args separately is necessary for event binding + options.event_callbacks, + options.use_default_callbacks), options_(options), published_type_allocator_(*options.get_allocator()), ros_message_type_allocator_(*options.get_allocator()) { allocator::set_allocator_for_deleter(&published_type_deleter_, &published_type_allocator_); allocator::set_allocator_for_deleter(&ros_message_type_deleter_, &ros_message_type_allocator_); - - if (options_.event_callbacks.deadline_callback) { - this->add_event_handler( - options_.event_callbacks.deadline_callback, - RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); - } - if (options_.event_callbacks.liveliness_callback) { - this->add_event_handler( - options_.event_callbacks.liveliness_callback, - RCL_PUBLISHER_LIVELINESS_LOST); - } - if (options_.event_callbacks.incompatible_qos_callback) { - this->add_event_handler( - options_.event_callbacks.incompatible_qos_callback, - RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); - } else if (options_.use_default_callbacks) { - // Register default callback when not specified - try { - this->add_event_handler( - [this](QOSOfferedIncompatibleQoSInfo & info) { - this->default_incompatible_qos_callback(info); - }, - RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); - } catch (UnsupportedEventTypeException & /*exc*/) { - // pass - } - } // Setup continues in the post construction method, post_init_setup(). } diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index 8416757a53..153d5a6ebe 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -78,11 +78,18 @@ class PublisherBase : public std::enable_shared_from_this rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic, const rosidl_message_type_support_t & type_support, - const rcl_publisher_options_t & publisher_options); + const rcl_publisher_options_t & publisher_options, + const PublisherEventCallbacks & event_callbacks, + bool use_default_callbacks); RCLCPP_PUBLIC virtual ~PublisherBase(); + /// Add event handlers for passed in event_callbacks. + RCLCPP_PUBLIC + void + bind_event_callbacks(const PublisherEventCallbacks & event_callbacks, bool use_default_callbacks); + /// Get the topic that this publisher publishes on. /** \return The topic name. */ RCLCPP_PUBLIC @@ -348,6 +355,8 @@ class PublisherBase : public std::enable_shared_from_this rmw_gid_t rmw_gid_; const rosidl_message_type_support_t type_support_; + + const PublisherEventCallbacks event_callbacks_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 74e6b5c05d..d9e84b29f8 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -141,43 +141,14 @@ class Subscription : public SubscriptionBase type_support_handle, topic_name, options.to_rcl_subscription_options(qos), + // NOTE(methylDragon): Passing these args separately is necessary for event binding + options.event_callbacks, + options.use_default_callbacks, callback.is_serialized_message_callback()), any_callback_(callback), options_(options), message_memory_strategy_(message_memory_strategy) { - if (options_.event_callbacks.deadline_callback) { - this->add_event_handler( - options_.event_callbacks.deadline_callback, - RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); - } - if (options_.event_callbacks.liveliness_callback) { - this->add_event_handler( - options_.event_callbacks.liveliness_callback, - RCL_SUBSCRIPTION_LIVELINESS_CHANGED); - } - if (options_.event_callbacks.incompatible_qos_callback) { - this->add_event_handler( - options_.event_callbacks.incompatible_qos_callback, - RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); - } else if (options_.use_default_callbacks) { - // Register default callback when not specified - try { - this->add_event_handler( - [this](QOSRequestedIncompatibleQoSInfo & info) { - this->default_incompatible_qos_callback(info); - }, - RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); - } catch (UnsupportedEventTypeException & /*exc*/) { - // pass - } - } - if (options_.event_callbacks.message_lost_callback) { - this->add_event_handler( - options_.event_callbacks.message_lost_callback, - RCL_SUBSCRIPTION_MESSAGE_LOST); - } - // Setup intra process publishing if requested. if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) { using rclcpp::detail::resolve_intra_process_buffer_type; diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 09fabe79e5..b1aeb4eb7d 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -84,12 +84,20 @@ class SubscriptionBase : public std::enable_shared_from_this const rosidl_message_type_support_t & type_support_handle, const std::string & topic_name, const rcl_subscription_options_t & subscription_options, + const SubscriptionEventCallbacks & event_callbacks, + bool use_default_callbacks, bool is_serialized = false); /// Destructor. RCLCPP_PUBLIC virtual ~SubscriptionBase(); + /// Add event handlers for passed in event_callbacks. + RCLCPP_PUBLIC + void + bind_event_callbacks( + const SubscriptionEventCallbacks & event_callbacks, bool use_default_callbacks); + /// Get the topic that this subscription is subscribed on. RCLCPP_PUBLIC const char * @@ -570,6 +578,8 @@ class SubscriptionBase : public std::enable_shared_from_this uint64_t intra_process_subscription_id_; std::shared_ptr subscription_intra_process_; + const SubscriptionEventCallbacks event_callbacks_; + private: RCLCPP_DISABLE_COPY(SubscriptionBase) diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index 9517af6d3c..ce0540659a 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -45,11 +45,14 @@ PublisherBase::PublisherBase( rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic, const rosidl_message_type_support_t & type_support, - const rcl_publisher_options_t & publisher_options) + const rcl_publisher_options_t & publisher_options, + const PublisherEventCallbacks & event_callbacks, + bool use_default_callbacks) : rcl_node_handle_(node_base->get_shared_rcl_node_handle()), intra_process_is_enabled_(false), intra_process_publisher_id_(0), - type_support_(type_support) + type_support_(type_support), + event_callbacks_(event_callbacks) { auto custom_deleter = [node_handle = this->rcl_node_handle_](rcl_publisher_t * rcl_pub) { @@ -98,6 +101,8 @@ PublisherBase::PublisherBase( rmw_reset_error(); throw std::runtime_error(msg); } + + bind_event_callbacks(event_callbacks_, use_default_callbacks); } PublisherBase::~PublisherBase() @@ -126,6 +131,38 @@ PublisherBase::get_topic_name() const return rcl_publisher_get_topic_name(publisher_handle_.get()); } +void +PublisherBase::bind_event_callbacks( + const PublisherEventCallbacks & event_callbacks, bool use_default_callbacks) +{ + if (event_callbacks.deadline_callback) { + this->add_event_handler( + event_callbacks.deadline_callback, + RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); + } + if (event_callbacks.liveliness_callback) { + this->add_event_handler( + event_callbacks.liveliness_callback, + RCL_PUBLISHER_LIVELINESS_LOST); + } + if (event_callbacks.incompatible_qos_callback) { + this->add_event_handler( + event_callbacks.incompatible_qos_callback, + RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + } else if (use_default_callbacks) { + // Register default callback when not specified + try { + this->add_event_handler( + [this](QOSOfferedIncompatibleQoSInfo & info) { + this->default_incompatible_qos_callback(info); + }, + RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + } catch (UnsupportedEventTypeException & /*exc*/) { + // pass + } + } +} + size_t PublisherBase::get_queue_size() const { diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 871321fbe8..0225747b3a 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -39,12 +39,15 @@ SubscriptionBase::SubscriptionBase( const rosidl_message_type_support_t & type_support_handle, const std::string & topic_name, const rcl_subscription_options_t & subscription_options, + const SubscriptionEventCallbacks & event_callbacks, + bool use_default_callbacks, 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), + event_callbacks_(event_callbacks), type_support_(type_support_handle), is_serialized_(is_serialized) { @@ -80,9 +83,10 @@ SubscriptionBase::SubscriptionBase( rcl_node_get_name(rcl_node_handle), rcl_node_get_namespace(rcl_node_handle)); } - rclcpp::exceptions::throw_from_rcl_error(ret, "could not create subscription"); } + + bind_event_callbacks(event_callbacks_, use_default_callbacks); } SubscriptionBase::~SubscriptionBase() @@ -101,6 +105,43 @@ SubscriptionBase::~SubscriptionBase() ipm->remove_subscription(intra_process_subscription_id_); } +void +SubscriptionBase::bind_event_callbacks( + const SubscriptionEventCallbacks & event_callbacks, bool use_default_callbacks) +{ + if (event_callbacks.deadline_callback) { + this->add_event_handler( + event_callbacks.deadline_callback, + RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); + } + if (event_callbacks.liveliness_callback) { + this->add_event_handler( + event_callbacks.liveliness_callback, + RCL_SUBSCRIPTION_LIVELINESS_CHANGED); + } + if (event_callbacks.incompatible_qos_callback) { + this->add_event_handler( + event_callbacks.incompatible_qos_callback, + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); + } else if (use_default_callbacks) { + // Register default callback when not specified + try { + this->add_event_handler( + [this](QOSRequestedIncompatibleQoSInfo & info) { + this->default_incompatible_qos_callback(info); + }, + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); + } catch (UnsupportedEventTypeException & /*exc*/) { + // pass + } + } + if (event_callbacks.message_lost_callback) { + this->add_event_handler( + event_callbacks.message_lost_callback, + RCL_SUBSCRIPTION_MESSAGE_LOST); + } +} + const char * SubscriptionBase::get_topic_name() const { diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp index 264577a739..ecfc89a6aa 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp @@ -36,16 +36,14 @@ const rosidl_message_type_support_t EmptyTypeSupport() return *rosidl_typesupport_cpp::get_message_type_support_handle(); } -const rcl_publisher_options_t PublisherOptions() +const rclcpp::PublisherOptionsWithAllocator> PublisherOptions() { - return rclcpp::PublisherOptionsWithAllocator>().template - to_rcl_publisher_options(rclcpp::QoS(10)); + return rclcpp::PublisherOptionsWithAllocator>(); } -const rcl_subscription_options_t SubscriptionOptions() +const rclcpp::SubscriptionOptionsWithAllocator> SubscriptionOptions() { - return rclcpp::SubscriptionOptionsWithAllocator>() - .to_rcl_subscription_options(rclcpp::QoS(10)); + return rclcpp::SubscriptionOptionsWithAllocator>(); } } // namespace @@ -55,7 +53,9 @@ class TestPublisher : public rclcpp::PublisherBase public: explicit TestPublisher(rclcpp::Node * node) : rclcpp::PublisherBase( - node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), PublisherOptions()) {} + node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} }; class TestSubscription : public rclcpp::SubscriptionBase @@ -63,7 +63,9 @@ class TestSubscription : public rclcpp::SubscriptionBase public: explicit TestSubscription(rclcpp::Node * node) : rclcpp::SubscriptionBase( - node->get_node_base_interface().get(), EmptyTypeSupport(), "topic", SubscriptionOptions()) {} + node->get_node_base_interface().get(), EmptyTypeSupport(), "topic", + SubscriptionOptions().to_rcl_subscription_options(rclcpp::QoS(10)), + SubscriptionOptions().event_callbacks, SubscriptionOptions().use_default_callbacks) {} std::shared_ptr create_message() override {return nullptr;} std::shared_ptr diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 35605230d9..5cffe1d5e1 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -245,10 +245,9 @@ const rosidl_message_type_support_t EmptyTypeSupport() return *rosidl_typesupport_cpp::get_message_type_support_handle(); } -const rcl_publisher_options_t PublisherOptions() +const rclcpp::PublisherOptionsWithAllocator> PublisherOptions() { - return rclcpp::PublisherOptionsWithAllocator>().template - to_rcl_publisher_options(rclcpp::QoS(10)); + return rclcpp::PublisherOptionsWithAllocator>(); } class TestPublisherBase : public rclcpp::PublisherBase @@ -256,7 +255,9 @@ class TestPublisherBase : public rclcpp::PublisherBase public: explicit TestPublisherBase(rclcpp::Node * node) : rclcpp::PublisherBase( - node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), PublisherOptions()) {} + node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} }; /* diff --git a/rclcpp/test/rclcpp/test_type_support.cpp b/rclcpp/test/rclcpp/test_type_support.cpp index 1732031029..682907e2a9 100644 --- a/rclcpp/test/rclcpp/test_type_support.cpp +++ b/rclcpp/test/rclcpp/test_type_support.cpp @@ -58,10 +58,9 @@ class TestTypeSupport : public ::testing::Test rclcpp::Node::SharedPtr node = std::make_shared("my_node", "/ns"); }; -const rcl_publisher_options_t PublisherOptions() +const rclcpp::PublisherOptionsWithAllocator> PublisherOptions() { - return rclcpp::PublisherOptionsWithAllocator>().template - to_rcl_publisher_options(rclcpp::QoS(10)); + return rclcpp::PublisherOptionsWithAllocator>(); } // Auxiliary classes used to test rosidl_message_type_support_t getters @@ -77,7 +76,8 @@ class TestTSParameterEvent : public rclcpp::PublisherBase node->get_node_base_interface().get(), "topicTSParameterEvent", *ts_parameter_event, - PublisherOptions()) {} + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} }; const rosidl_message_type_support_t * ts_set_parameter_result = @@ -91,7 +91,8 @@ class TestTSSetParameterResult : public rclcpp::PublisherBase node->get_node_base_interface().get(), "topicTSSetParameterResult", *ts_set_parameter_result, - PublisherOptions()) {} + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} }; const rosidl_message_type_support_t * ts_parameter_descriptor = @@ -105,7 +106,8 @@ class TestTSParameterDescriptor : public rclcpp::PublisherBase node->get_node_base_interface().get(), "topicTSParameterDescriptor", *ts_parameter_descriptor, - PublisherOptions()) {} + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} }; const rosidl_message_type_support_t * ts_list_parameter_result = @@ -119,7 +121,8 @@ class TestTSListParametersResult : public rclcpp::PublisherBase node->get_node_base_interface().get(), "topicTSListParametersResult", *ts_list_parameter_result, - PublisherOptions()) {} + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} }; /* From 3fb012e2e979475f5044ab0e0f9b91d336db5f46 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Wed, 21 Dec 2022 10:11:24 +0000 Subject: [PATCH 166/455] do not throw exception if trying to dequeue an empty intra-process buffer (#2061) Signed-off-by: Alberto Soragna Signed-off-by: Alberto Soragna --- .../experimental/buffers/ring_buffer_implementation.hpp | 3 +-- .../rclcpp/experimental/subscription_intra_process.hpp | 8 +++++++- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp index c01240b429..245d417d86 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp @@ -86,8 +86,7 @@ class RingBufferImplementation : public BufferImplementationBase std::lock_guard lock(mutex_); if (!has_data_()) { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Calling dequeue on empty intra-process buffer"); - throw std::runtime_error("Calling dequeue on empty intra-process buffer"); + return BufferT(); } auto request = std::move(ring_buffer_[read_index_]); diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 803d940086..91ea91a7c3 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -109,8 +109,14 @@ class SubscriptionIntraProcess if (any_callback_.use_take_shared_method()) { shared_msg = this->buffer_->consume_shared(); + if (!shared_msg) { + return nullptr; + } } else { unique_msg = this->buffer_->consume_unique(); + if (!unique_msg) { + return nullptr; + } } return std::static_pointer_cast( std::make_shared>( @@ -138,7 +144,7 @@ class SubscriptionIntraProcess execute_impl(std::shared_ptr & data) { if (!data) { - throw std::runtime_error("'data' is empty"); + return; } rmw_message_info_t msg_info; From c5491a4e58a6d4ee41ee52b2feaeb67182da8a4a Mon Sep 17 00:00:00 2001 From: jrutgeer <74099146+jrutgeer@users.noreply.github.com> Date: Fri, 23 Dec 2022 14:27:42 +0100 Subject: [PATCH 167/455] API url info fix (#2071) * Fixed API url info. Signed-off-by: jrutgeer * Fix api url info in README Signed-off-by: jrutgeer Signed-off-by: jrutgeer --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 0226a75f0a..f4fe5837aa 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,8 @@ rclcpp provides the standard C++ API for interacting with ROS 2. `#include "rclcpp/rclcpp.hpp"` allows use of the most common elements of the ROS 2 system. -Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components. +The link to the latest API documentation can be found on the rclcpp package info page, at the [ROS Index](https://index.ros.org/p/rclcpp/). + ### Examples From a73e0bd23b17c9dd20395586bf9872fa189c4292 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Wed, 28 Dec 2022 17:59:02 -0800 Subject: [PATCH 168/455] Implement Unified Node Interface (NodeInterfaces class) (#2041) Co-authored-by: William Woodall Co-authored-by: methylDragon --- .../rclcpp/detail/template_contains.hpp | 47 ++++ .../include/rclcpp/detail/template_unique.hpp | 49 ++++ .../detail/node_interfaces_helpers.hpp | 204 +++++++++++++++ .../node_interfaces/node_base_interface.hpp | 3 + .../node_interfaces/node_clock_interface.hpp | 3 + .../node_interfaces/node_graph_interface.hpp | 3 + .../node_interfaces/node_interfaces.hpp | 165 ++++++++++++ .../node_logging_interface.hpp | 3 + .../node_parameters_interface.hpp | 3 + .../node_services_interface.hpp | 3 + .../node_time_source_interface.hpp | 3 + .../node_interfaces/node_timers_interface.hpp | 3 + .../node_interfaces/node_topics_interface.hpp | 3 + .../node_waitables_interface.hpp | 3 + rclcpp/test/rclcpp/CMakeLists.txt | 10 + .../detail/test_template_utils.cpp | 56 ++++ .../node_interfaces/test_node_interfaces.cpp | 245 ++++++++++++++++++ .../lifecycle_node_interface.hpp | 5 + 18 files changed, 811 insertions(+) create mode 100644 rclcpp/include/rclcpp/detail/template_contains.hpp create mode 100644 rclcpp/include/rclcpp/detail/template_unique.hpp create mode 100644 rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp create mode 100644 rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/detail/test_template_utils.cpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp diff --git a/rclcpp/include/rclcpp/detail/template_contains.hpp b/rclcpp/include/rclcpp/detail/template_contains.hpp new file mode 100644 index 0000000000..b60a75f36d --- /dev/null +++ b/rclcpp/include/rclcpp/detail/template_contains.hpp @@ -0,0 +1,47 @@ +// Copyright 2022 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__TEMPLATE_CONTAINS_HPP_ +#define RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_ + +#include + +namespace rclcpp +{ +namespace detail +{ + +/// Template meta-function that checks if a given T is contained in the list Us. +template +struct template_contains; + +template +inline constexpr bool template_contains_v = template_contains::value; + +template +struct template_contains +{ + enum { value = (std::is_same_v|| template_contains_v)}; +}; + +template +struct template_contains +{ + enum { value = false }; +}; + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_ diff --git a/rclcpp/include/rclcpp/detail/template_unique.hpp b/rclcpp/include/rclcpp/detail/template_unique.hpp new file mode 100644 index 0000000000..4986102d78 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/template_unique.hpp @@ -0,0 +1,49 @@ +// Copyright 2022 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__TEMPLATE_UNIQUE_HPP_ +#define RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_ + +#include + +#include "rclcpp/detail/template_contains.hpp" + +namespace rclcpp +{ +namespace detail +{ + +/// Template meta-function that checks if a given list Ts contains unique types. +template +struct template_unique; + +template +inline constexpr bool template_unique_v = template_unique::value; + +template +struct template_unique +{ + enum { value = !template_contains_v&& template_unique_v}; +}; + +template +struct template_unique +{ + enum { value = true }; +}; + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp b/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp new file mode 100644 index 0000000000..8f7b452f5f --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp @@ -0,0 +1,204 @@ +// Copyright 2022 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__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_ +#define RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ +namespace detail +{ + +// Support and Helper template classes for the NodeInterfaces class. + +template +std::tuple...> +init_tuple(NodeT & n); + +/// Stores the interfaces in a tuple, provides constructors, and getters. +template +struct NodeInterfacesStorage +{ + template + NodeInterfacesStorage(NodeT & node) // NOLINT(runtime/explicit) + : interfaces_(init_tuple(node)) + {} + + explicit NodeInterfacesStorage(std::shared_ptr... args) + : interfaces_(args ...) + {} + + /// Individual Node Interface non-const getter. + template + std::shared_ptr + get() + { + static_assert( + (std::is_same_v|| ...), + "NodeInterfaces class does not contain given NodeInterfaceT"); + return std::get>(interfaces_); + } + + /// Individual Node Interface const getter. + template + std::shared_ptr + get() const + { + static_assert( + (std::is_same_v|| ...), + "NodeInterfaces class does not contain given NodeInterfaceT"); + return std::get>(interfaces_); + } + +protected: + std::tuple...> interfaces_; +}; + +/// Prototype of NodeInterfacesSupports. +/** + * Should read NodeInterfacesSupports<..., T, ...> as "NodeInterfaces supports T", and + * if NodeInterfacesSupport is specialized for T, the is_supported should be + * set to std::true_type, but by default it is std::false_type, which will + * lead to a compiler error when trying to use T with NodeInterfaces. + */ +template +struct NodeInterfacesSupports; + +/// Prototype of NodeInterfacesSupportCheck template meta-function. +/** + * This meta-function checks that all the types given are supported, + * throwing a more human-readable error if an unsupported type is used. + */ +template +struct NodeInterfacesSupportCheck; + +/// Iterating specialization that ensures classes are supported and inherited. +template +struct NodeInterfacesSupportCheck + : public NodeInterfacesSupportCheck +{ + static_assert( + NodeInterfacesSupports::is_supported::value, + "given NodeInterfaceT is not supported by rclcpp::node_interfaces::NodeInterfaces"); +}; + +/// Terminating case when there are no more "RemainingInterfaceTs". +template +struct NodeInterfacesSupportCheck +{}; + +/// Default specialization, needs to be specialized for each supported interface. +template +struct NodeInterfacesSupports +{ + // Specializations need to set this to std::true_type in addition to other interfaces. + using is_supported = std::false_type; +}; + +/// Terminating specialization of NodeInterfacesSupports. +template +struct NodeInterfacesSupports + : public StorageClassT +{ + /// Perfect forwarding constructor to get arguments down to StorageClassT. + template + explicit NodeInterfacesSupports(ArgsT && ... args) + : StorageClassT(std::forward(args) ...) + {} +}; + +// Helper functions to initialize the tuple in NodeInterfaces. + +template +void +init_element(TupleT & t, NodeT & n) +{ + std::get>(t) = + NodeInterfacesSupports::get_from_node_like(n); +} + +template +std::tuple...> +init_tuple(NodeT & n) +{ + using StorageClassT = NodeInterfacesStorage; + std::tuple...> t; + (init_element(t, n), ...); + return t; +} + +/// Macro for creating specializations with less boilerplate. +/** + * You can use this macro to add support for your interface class if: + * + * - The standard getter is get_node_{NodeInterfaceName}_interface(), and + * - the getter returns a non-const shared_ptr<{NodeInterfaceType}> + * + * Examples of using this can be seen in the standard node interface headers + * in rclcpp, e.g. rclcpp/node_interfaces/node_base_interface.hpp has: + * + * RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base) + * + * If your interface has a non-standard getter, or you want to instrument it or + * something like that, then you'll need to create your own specialization of + * the NodeInterfacesSupports struct without this macro. + */ +#define RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(NodeInterfaceType, NodeInterfaceName) \ + namespace rclcpp::node_interfaces::detail { \ + template \ + struct NodeInterfacesSupports< \ + StorageClassT, \ + NodeInterfaceType, \ + RemainingInterfaceTs ...> \ + : public NodeInterfacesSupports \ + { \ + using is_supported = std::true_type; \ + \ + template \ + static \ + std::shared_ptr \ + get_from_node_like(NodeT & node_like) \ + { \ + return node_like.get_node_ ## NodeInterfaceName ## _interface(); \ + } \ + \ + /* Perfect forwarding constructor to get arguments down to StorageClassT (eventually). */ \ + template \ + explicit NodeInterfacesSupports(ArgsT && ... args) \ + : NodeInterfacesSupports( \ + std::forward(args) ...) \ + {} \ + \ + std::shared_ptr \ + get_node_ ## NodeInterfaceName ## _interface() \ + { \ + return StorageClassT::template get(); \ + } \ + }; \ + } // namespace rclcpp::node_interfaces::detail + +} // namespace detail +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index 2e94b80d9c..fd4f64b22b 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -26,6 +26,7 @@ #include "rclcpp/context.hpp" #include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -177,4 +178,6 @@ class NodeBaseInterface } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base) + #endif // RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp index 8965a371d8..744eef4ce6 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp @@ -17,6 +17,7 @@ #include "rclcpp/clock.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -50,4 +51,6 @@ class NodeClockInterface } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeClockInterface, clock) + #endif // RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp index 3958f5b2d9..fefda0da69 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -29,6 +29,7 @@ #include "rclcpp/event.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/visibility_control.hpp" @@ -382,4 +383,6 @@ class NodeGraphInterface } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeGraphInterface, graph) + #endif // RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp new file mode 100644 index 0000000000..2f40380c75 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp @@ -0,0 +1,165 @@ +// Copyright 2022 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__NODE_INTERFACES__NODE_INTERFACES_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_ + +#include + +#include "rclcpp/detail/template_unique.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" + +#define ALL_RCLCPP_NODE_INTERFACES \ + rclcpp::node_interfaces::NodeBaseInterface, \ + rclcpp::node_interfaces::NodeClockInterface, \ + rclcpp::node_interfaces::NodeGraphInterface, \ + rclcpp::node_interfaces::NodeLoggingInterface, \ + rclcpp::node_interfaces::NodeParametersInterface, \ + rclcpp::node_interfaces::NodeServicesInterface, \ + rclcpp::node_interfaces::NodeTimeSourceInterface, \ + rclcpp::node_interfaces::NodeTimersInterface, \ + rclcpp::node_interfaces::NodeTopicsInterface, \ + rclcpp::node_interfaces::NodeWaitablesInterface + + +namespace rclcpp +{ +namespace node_interfaces +{ + + +/// A helper class for aggregating node interfaces. +template +class NodeInterfaces + : public detail::NodeInterfacesSupportCheck< + detail::NodeInterfacesStorage, + InterfaceTs ... + >, + public detail::NodeInterfacesSupports< + detail::NodeInterfacesStorage, + InterfaceTs ... + > +{ + static_assert( + 0 != sizeof ...(InterfaceTs), + "must provide at least one interface as a template argument"); + static_assert( + rclcpp::detail::template_unique_v, + "must provide unique template parameters"); + + using NodeInterfacesSupportsT = detail::NodeInterfacesSupports< + detail::NodeInterfacesStorage, + InterfaceTs ... + >; + +public: + /// Create a new NodeInterfaces object using the given node-like object's interfaces. + /** + * Specify which interfaces you need by passing them as template parameters. + * + * This allows you to aggregate interfaces from different sources together to pass as a single + * aggregate object to any functions that take node interfaces or node-likes, without needing to + * templatize that function. + * + * You may also use this constructor to create a NodeInterfaces that contains a subset of + * another NodeInterfaces' interfaces. + * + * Finally, this class supports implicit conversion from node-like objects, allowing you to + * directly pass a node-like to a function that takes a NodeInterfaces object. + * + * Usage examples: + * ```cpp + * // Suppose we have some function: + * void fn(NodeInterfaces interfaces); + * + * // Then we can, explicitly: + * rclcpp::Node node("some_node"); + * auto ni = NodeInterfaces(node); + * fn(ni); + * + * // But also: + * fn(node); + * + * // Subsetting a NodeInterfaces object also works! + * auto ni_base = NodeInterfaces(ni); + * + * // Or aggregate them (you could aggregate interfaces from disparate node-likes) + * auto ni_aggregated = NodeInterfaces( + * node->get_node_base_interface(), + * node->get_node_clock_interface() + * ) + * + * // And then to access the interfaces: + * // Get with get<> + * auto base = ni.get(); + * + * // Or the appropriate getter + * auto clock = ni.get_clock_interface(); + * ``` + * + * You may use any of the standard node interfaces that come with rclcpp: + * - rclcpp::node_interfaces::NodeBaseInterface + * - rclcpp::node_interfaces::NodeClockInterface + * - rclcpp::node_interfaces::NodeGraphInterface + * - rclcpp::node_interfaces::NodeLoggingInterface + * - rclcpp::node_interfaces::NodeParametersInterface + * - rclcpp::node_interfaces::NodeServicesInterface + * - rclcpp::node_interfaces::NodeTimeSourceInterface + * - rclcpp::node_interfaces::NodeTimersInterface + * - rclcpp::node_interfaces::NodeTopicsInterface + * - rclcpp::node_interfaces::NodeWaitablesInterface + * + * Or you use custom interfaces as long as you make a template specialization + * of the rclcpp::node_interfaces::detail::NodeInterfacesSupport struct using + * the RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT macro. + * + * Usage example: + * ```RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)``` + * + * If you choose not to use the helper macro, then you can specialize the + * template yourself, but you must: + * + * - Provide a template specialization of the get_from_node_like method that gets the interface + * from any node-like that stores the interface, using the node-like's getter + * - Designate the is_supported type as std::true_type using a using directive + * - Provide any number of getter methods to be used to obtain the interface with the + * NodeInterface object, noting that the getters of the storage class will apply to all + * supported interfaces. + * - The getter method names should not clash in name with any other interface getter + * specializations if those other interfaces are meant to be aggregated in the same + * NodeInterfaces object. + * + * \param[in] node Node-like object from which to get the node interfaces + */ + template + NodeInterfaces(NodeT & node) // NOLINT(runtime/explicit) + : NodeInterfacesSupportsT(node) + {} + + /// NodeT::SharedPtr Constructor + template + NodeInterfaces(std::shared_ptr node) // NOLINT(runtime/explicit) + : NodeInterfaces(node ? *node : throw std::invalid_argument("given node pointer is nullptr")) + {} + + explicit NodeInterfaces(std::shared_ptr... args) + : NodeInterfacesSupportsT(args ...) + {} +}; + + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp index c2add566b9..870a81a53b 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp @@ -19,6 +19,7 @@ #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -58,4 +59,6 @@ class NodeLoggingInterface } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeLoggingInterface, logging) + #endif // RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index 3156668a73..301c1e3214 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -25,6 +25,7 @@ #include "rcl_interfaces/msg/set_parameters_result.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/visibility_control.hpp" @@ -276,4 +277,6 @@ class NodeParametersInterface } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeParametersInterface, parameters) + #endif // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp index ab6cdab42e..ff58ef36b6 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp @@ -20,6 +20,7 @@ #include "rclcpp/callback_group.hpp" #include "rclcpp/client.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/service.hpp" #include "rclcpp/visibility_control.hpp" @@ -62,4 +63,6 @@ class NodeServicesInterface } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeServicesInterface, services) + #endif // RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp index 5b7065193e..3783e5d83a 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp @@ -16,6 +16,7 @@ #define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_ #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -37,4 +38,6 @@ class NodeTimeSourceInterface } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTimeSourceInterface, time_source) + #endif // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp index 4573d3d02b..2f1aaef51e 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp @@ -17,6 +17,7 @@ #include "rclcpp/callback_group.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/timer.hpp" #include "rclcpp/visibility_control.hpp" @@ -47,4 +48,6 @@ class NodeTimersInterface } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTimersInterface, timers) + #endif // RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp index 7aa40a3d5e..aa5530f8dd 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp @@ -22,6 +22,7 @@ #include "rclcpp/callback_group.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/publisher.hpp" @@ -95,4 +96,6 @@ class NodeTopicsInterface } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTopicsInterface, topics) + #endif // RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp index 0faae1da62..fd0029a89b 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp @@ -17,6 +17,7 @@ #include "rclcpp/callback_group.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/visibility_control.hpp" #include "rclcpp/waitable.hpp" @@ -54,4 +55,6 @@ class NodeWaitablesInterface } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeWaitablesInterface, waitables) + #endif // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_ diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 75aa9ac63b..6dffb543e9 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -234,6 +234,11 @@ if(TARGET test_node_interfaces__node_graph) "test_msgs") target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME} mimick) endif() +ament_add_gtest(test_node_interfaces__node_interfaces + node_interfaces/test_node_interfaces.cpp) +if(TARGET test_node_interfaces__node_interfaces) + target_link_libraries(test_node_interfaces__node_interfaces ${PROJECT_NAME} mimick) +endif() ament_add_gtest(test_node_interfaces__node_parameters node_interfaces/test_node_parameters.cpp) if(TARGET test_node_interfaces__node_parameters) @@ -262,6 +267,11 @@ ament_add_gtest(test_node_interfaces__node_waitables if(TARGET test_node_interfaces__node_waitables) target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME} mimick) endif() +ament_add_gtest(test_node_interfaces__test_template_utils # Compile time test + node_interfaces/detail/test_template_utils.cpp) +if(TARGET test_node_interfaces__test_template_utils) + target_link_libraries(test_node_interfaces__test_template_utils ${PROJECT_NAME}) +endif() # TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node diff --git a/rclcpp/test/rclcpp/node_interfaces/detail/test_template_utils.cpp b/rclcpp/test/rclcpp/node_interfaces/detail/test_template_utils.cpp new file mode 100644 index 0000000000..9ae715ebce --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/detail/test_template_utils.cpp @@ -0,0 +1,56 @@ +// Copyright 2022 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. + +#include + +#include "rclcpp/detail/template_contains.hpp" +#include "rclcpp/detail/template_unique.hpp" + + +TEST(NoOpTests, test_node_interfaces_template_utils) { +} // This is just to let gtest work + +namespace rclcpp +{ +namespace detail +{ + +// This tests template logic at compile time +namespace +{ + +struct test_template_unique +{ + static_assert(template_unique_v, "failed"); + static_assert(template_unique_v, "failed"); + static_assert(!template_unique_v, "failed"); + static_assert(!template_unique_v, "failed"); + static_assert(!template_unique_v, "failed"); +}; + + +struct test_template_contains +{ + static_assert(template_contains_v, "failed"); + static_assert(template_contains_v, "failed"); + static_assert(template_contains_v, "failed"); + static_assert(!template_contains_v, "failed"); + static_assert(!template_contains_v, "failed"); + static_assert(!template_contains_v, "failed"); +}; + +} // namespace + +} // namespace detail +} // namespace rclcpp diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp new file mode 100644 index 0000000000..96596dea37 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp @@ -0,0 +1,245 @@ +// Copyright 2022 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. + +#include + +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_interfaces.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" + +class TestNodeInterfaces : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +/* + Testing NodeInterfaces construction from nodes. + */ +TEST_F(TestNodeInterfaces, node_interfaces_nominal) { + auto node = std::make_shared("my_node"); + + // Create a NodeInterfaces for base and graph using a rclcpp::Node. + { + using rclcpp::node_interfaces::NodeInterfaces; + using rclcpp::node_interfaces::NodeBaseInterface; + using rclcpp::node_interfaces::NodeGraphInterface; + auto node_interfaces = NodeInterfaces(node); + } + + // Implicit conversion of rclcpp::Node into function that uses NodeInterfaces of base. + { + using rclcpp::node_interfaces::NodeInterfaces; + using rclcpp::node_interfaces::NodeBaseInterface; + auto some_func = [](NodeInterfaces ni) { + auto base_interface = ni.get(); + }; + + some_func(node); + } + + // Implicit narrowing of NodeInterfaces into a new interface NodeInterfaces with fewer interfaces. + { + using rclcpp::node_interfaces::NodeInterfaces; + using rclcpp::node_interfaces::NodeBaseInterface; + using rclcpp::node_interfaces::NodeGraphInterface; + auto some_func = [](NodeInterfaces ni_with_one) { + auto base_interface = ni_with_one.get(); + }; + + NodeInterfaces ni_with_two(node); + + some_func(ni_with_two); + } + + // Create a NodeInterfaces via aggregation of interfaces in constructor. + { + using rclcpp::node_interfaces::NodeInterfaces; + using rclcpp::node_interfaces::NodeBaseInterface; + using rclcpp::node_interfaces::NodeGraphInterface; + auto loose_node_base = node->get_node_base_interface(); + auto loose_node_graph = node->get_node_graph_interface(); + auto ni = NodeInterfaces( + loose_node_base, + loose_node_graph); + } +} + +/* + Test construction with all standard rclcpp::node_interfaces::Node*Interfaces. + */ +TEST_F(TestNodeInterfaces, node_interfaces_standard_interfaces) { + auto node = std::make_shared("my_node", "/ns"); + + auto ni = rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeBaseInterface, + rclcpp::node_interfaces::NodeClockInterface, + rclcpp::node_interfaces::NodeGraphInterface, + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeTimersInterface, + rclcpp::node_interfaces::NodeTopicsInterface, + rclcpp::node_interfaces::NodeServicesInterface, + rclcpp::node_interfaces::NodeWaitablesInterface, + rclcpp::node_interfaces::NodeParametersInterface, + rclcpp::node_interfaces::NodeTimeSourceInterface + >(node); +} + +/* + Testing getters. + */ +TEST_F(TestNodeInterfaces, ni_init) { + auto node = std::make_shared("my_node", "/ns"); + + using rclcpp::node_interfaces::NodeInterfaces; + using rclcpp::node_interfaces::NodeBaseInterface; + using rclcpp::node_interfaces::NodeClockInterface; + using rclcpp::node_interfaces::NodeGraphInterface; + using rclcpp::node_interfaces::NodeLoggingInterface; + using rclcpp::node_interfaces::NodeTimersInterface; + using rclcpp::node_interfaces::NodeTopicsInterface; + using rclcpp::node_interfaces::NodeServicesInterface; + using rclcpp::node_interfaces::NodeWaitablesInterface; + using rclcpp::node_interfaces::NodeParametersInterface; + using rclcpp::node_interfaces::NodeTimeSourceInterface; + + auto ni = NodeInterfaces< + NodeBaseInterface, + NodeClockInterface, + NodeGraphInterface, + NodeLoggingInterface, + NodeTimersInterface, + NodeTopicsInterface, + NodeServicesInterface, + NodeWaitablesInterface, + NodeParametersInterface, + NodeTimeSourceInterface + >(node); + + { + auto base = ni.get(); + base = ni.get_node_base_interface(); + EXPECT_STREQ(base->get_name(), "my_node"); // Test for functionality + } + { + auto clock = ni.get(); + clock = ni.get_node_clock_interface(); + clock->get_clock(); + } + { + auto graph = ni.get(); + graph = ni.get_node_graph_interface(); + } + { + auto logging = ni.get(); + logging = ni.get_node_logging_interface(); + } + { + auto timers = ni.get(); + timers = ni.get_node_timers_interface(); + } + { + auto topics = ni.get(); + topics = ni.get_node_topics_interface(); + } + { + auto services = ni.get(); + services = ni.get_node_services_interface(); + } + { + auto waitables = ni.get(); + waitables = ni.get_node_waitables_interface(); + } + { + auto parameters = ni.get(); + parameters = ni.get_node_parameters_interface(); + } + { + auto time_source = ni.get(); + time_source = ni.get_node_time_source_interface(); + } +} + +/* + Testing macro'ed getters. + */ +TEST_F(TestNodeInterfaces, ni_all_init) { + auto node = std::make_shared("my_node", "/ns"); + + using rclcpp::node_interfaces::NodeInterfaces; + using rclcpp::node_interfaces::NodeBaseInterface; + using rclcpp::node_interfaces::NodeClockInterface; + using rclcpp::node_interfaces::NodeGraphInterface; + using rclcpp::node_interfaces::NodeLoggingInterface; + using rclcpp::node_interfaces::NodeTimersInterface; + using rclcpp::node_interfaces::NodeTopicsInterface; + using rclcpp::node_interfaces::NodeServicesInterface; + using rclcpp::node_interfaces::NodeWaitablesInterface; + using rclcpp::node_interfaces::NodeParametersInterface; + using rclcpp::node_interfaces::NodeTimeSourceInterface; + + auto ni = rclcpp::node_interfaces::NodeInterfaces(node); + + { + auto base = ni.get(); + base = ni.get_node_base_interface(); + EXPECT_STREQ(base->get_name(), "my_node"); // Test for functionality + } + { + auto clock = ni.get(); + clock = ni.get_node_clock_interface(); + clock->get_clock(); + } + { + auto graph = ni.get(); + graph = ni.get_node_graph_interface(); + } + { + auto logging = ni.get(); + logging = ni.get_node_logging_interface(); + } + { + auto timers = ni.get(); + timers = ni.get_node_timers_interface(); + } + { + auto topics = ni.get(); + topics = ni.get_node_topics_interface(); + } + { + auto services = ni.get(); + services = ni.get_node_services_interface(); + } + { + auto waitables = ni.get(); + waitables = ni.get_node_waitables_interface(); + } + { + auto parameters = ni.get(); + parameters = ni.get_node_parameters_interface(); + } + { + auto time_source = ni.get(); + time_source = ni.get_node_time_source_interface(); + } +} diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp index ed919ca556..272c4def27 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp @@ -19,6 +19,7 @@ #include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/visibility_control.h" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" namespace rclcpp_lifecycle { @@ -106,4 +107,8 @@ class LifecycleNodeInterface } // namespace node_interfaces } // namespace rclcpp_lifecycle + +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT( + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface, lifecycle_node) + #endif // RCLCPP_LIFECYCLE__NODE_INTERFACES__LIFECYCLE_NODE_INTERFACE_HPP_ From c63f9eae0f16504512829917d1c53d515a592485 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 29 Dec 2022 10:30:32 -0800 Subject: [PATCH 169/455] 18.0.0 Signed-off-by: Shane Loretz --- rclcpp/CHANGELOG.rst | 18 ++++++++++++++++++ rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 6 ++++++ rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 5 +++++ rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 7 +++++++ rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 40 insertions(+), 4 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 15a1aede88..0c8201217c 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +18.0.0 (2022-12-29) +------------------- +* Implement Unified Node Interface (NodeInterfaces class) (`#2041 `_) +* Do not throw exception if trying to dequeue an empty intra-process buffer (`#2061 `_) +* Move event callback binding to PublisherBase and SubscriptionBase (`#2066 `_) +* Implement validity checks for rclcpp::Clock (`#2040 `_) +* Explicitly set callback type (`#2059 `_) +* Fix logging macros to build with msvc and cpp20 (`#2063 `_) +* Add clock type to node_options (`#1982 `_) +* Fix nullptr dereference in prune_requests_older_than (`#2008 `_) +* Remove templating on to_rcl_subscription_options (`#2056 `_) +* Fix SharedFuture from async_send_request never becoming valid (`#2044 `_) +* Add in a warning for a KeepLast depth of 0. (`#2048 `_) +* Mark rclcpp::Clock::now() as const (`#2050 `_) +* Fix a case that did not throw ParameterUninitializedException (`#2036 `_) +* Update maintainers (`#2043 `_) +* Contributors: Alberto Soragna, Audrow Nash, Chen Lihui, Chris Lalancette, Jeffery Hsu, Lei Liu, Mateusz Szczygielski, Shane Loretz, andrei, mauropasse, methylDragon + 17.1.0 (2022-11-02) ------------------- * MultiThreadExecutor number of threads is at least 2+ in default. (`#2032 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 6775f4a46d..a18cd7ae1d 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 17.1.0 + 18.0.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index a063bf32f2..2a08ebe63f 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,12 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +18.0.0 (2022-12-29) +------------------- +* Explicitly set callback type (`#2059 `_) +* Update maintainers (`#2043 `_) +* Contributors: Audrow Nash, mauropasse + 17.1.0 (2022-11-02) ------------------- * Do not clear entities callbacks on destruction (`#2002 `_) diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 38e7fdcba1..375e978be1 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 17.1.0 + 18.0.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index bc08ed28cd..820be8b817 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +18.0.0 (2022-12-29) +------------------- +* Update maintainers (`#2043 `_) +* Contributors: Audrow Nash + 17.1.0 (2022-11-02) ------------------- * use unique ptr and remove unuseful container (`#2013 `_) diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index cfb88b9e3f..de1b88e242 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 17.1.0 + 18.0.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 03d02cb2c2..0285505488 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,13 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +18.0.0 (2022-12-29) +------------------- +* Implement Unified Node Interface (NodeInterfaces class) (`#2041 `_) +* Add clock type to node_options (`#1982 `_) +* Update maintainers (`#2043 `_) +* Contributors: Audrow Nash, Jeffery Hsu, methylDragon + 17.1.0 (2022-11-02) ------------------- * LifecycleNode on_configure doc fix. (`#2034 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 8fa958a0bc..092105238b 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 17.1.0 + 18.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 1bbb03302a0eb36955e7ecc19189b9f6bbadcd6f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 4 Jan 2023 08:49:57 -0500 Subject: [PATCH 170/455] Add in a fix for older compilers. (#2075) * Add in a fix for older compilers. The addition of the NodeInterfaces class made it stop compiling with older compilers (such as gcc 9.4.0 on Ubuntu 20.04). The error has to do with calling the copy constructor on rclcpp::Node, which is deleted. Work around this by removing the NodeInterfaces shared_ptr constructor, which isn't technically needed. Signed-off-by: Chris Lalancette --- .../rclcpp/node_interfaces/node_interfaces.hpp | 6 ------ .../rclcpp/node_interfaces/test_node_interfaces.cpp | 12 ++++++------ 2 files changed, 6 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp index 2f40380c75..97959145b3 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp @@ -147,12 +147,6 @@ class NodeInterfaces : NodeInterfacesSupportsT(node) {} - /// NodeT::SharedPtr Constructor - template - NodeInterfaces(std::shared_ptr node) // NOLINT(runtime/explicit) - : NodeInterfaces(node ? *node : throw std::invalid_argument("given node pointer is nullptr")) - {} - explicit NodeInterfaces(std::shared_ptr... args) : NodeInterfacesSupportsT(args ...) {} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp index 96596dea37..ebb91c4770 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp @@ -44,7 +44,7 @@ TEST_F(TestNodeInterfaces, node_interfaces_nominal) { using rclcpp::node_interfaces::NodeInterfaces; using rclcpp::node_interfaces::NodeBaseInterface; using rclcpp::node_interfaces::NodeGraphInterface; - auto node_interfaces = NodeInterfaces(node); + auto node_interfaces = NodeInterfaces(*node); } // Implicit conversion of rclcpp::Node into function that uses NodeInterfaces of base. @@ -55,7 +55,7 @@ TEST_F(TestNodeInterfaces, node_interfaces_nominal) { auto base_interface = ni.get(); }; - some_func(node); + some_func(*node); } // Implicit narrowing of NodeInterfaces into a new interface NodeInterfaces with fewer interfaces. @@ -67,7 +67,7 @@ TEST_F(TestNodeInterfaces, node_interfaces_nominal) { auto base_interface = ni_with_one.get(); }; - NodeInterfaces ni_with_two(node); + NodeInterfaces ni_with_two(*node); some_func(ni_with_two); } @@ -102,7 +102,7 @@ TEST_F(TestNodeInterfaces, node_interfaces_standard_interfaces) { rclcpp::node_interfaces::NodeWaitablesInterface, rclcpp::node_interfaces::NodeParametersInterface, rclcpp::node_interfaces::NodeTimeSourceInterface - >(node); + >(*node); } /* @@ -134,7 +134,7 @@ TEST_F(TestNodeInterfaces, ni_init) { NodeWaitablesInterface, NodeParametersInterface, NodeTimeSourceInterface - >(node); + >(*node); { auto base = ni.get(); @@ -198,7 +198,7 @@ TEST_F(TestNodeInterfaces, ni_all_init) { using rclcpp::node_interfaces::NodeParametersInterface; using rclcpp::node_interfaces::NodeTimeSourceInterface; - auto ni = rclcpp::node_interfaces::NodeInterfaces(node); + auto ni = rclcpp::node_interfaces::NodeInterfaces(*node); { auto base = ni.get(); From 3db2ece14545ade3ab9cec0146be57c1a3ae83f8 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 13 Jan 2023 16:47:13 -0500 Subject: [PATCH 171/455] Fix the keep_last warning when using system defaults. (#2082) If the user creates SystemDefaultsQoS setting, they are explicitly asking for SystemDefaults. In that case, we should *not* warn, and just let the underlying RMW choose what it wants. Implement that here by passing a boolean parameter through that decides when we should print out the warning, and then skip printing that warning when SystemDefaultsQoS is created. Signed-off-by: Chris Lalancette Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/qos.hpp | 6 ++++-- rclcpp/src/rclcpp/qos.cpp | 37 +++++++++++++++-------------------- 2 files changed, 20 insertions(+), 23 deletions(-) diff --git a/rclcpp/include/rclcpp/qos.hpp b/rclcpp/include/rclcpp/qos.hpp index 4946d37889..2ad49487c5 100644 --- a/rclcpp/include/rclcpp/qos.hpp +++ b/rclcpp/include/rclcpp/qos.hpp @@ -80,7 +80,9 @@ struct RCLCPP_PUBLIC QoSInitialization size_t depth; /// Constructor which takes both a history policy and a depth (even if it would be unused). - QoSInitialization(rmw_qos_history_policy_t history_policy_arg, size_t depth_arg); + QoSInitialization( + rmw_qos_history_policy_t history_policy_arg, size_t depth_arg, + bool print_depth_warning = true); /// Create a QoSInitialization from an existing rmw_qos_profile_t, using its history and depth. static @@ -97,7 +99,7 @@ struct RCLCPP_PUBLIC KeepAll : public rclcpp::QoSInitialization /// Use to initialize the QoS with the keep_last history setting and the given depth. struct RCLCPP_PUBLIC KeepLast : public rclcpp::QoSInitialization { - explicit KeepLast(size_t depth); + explicit KeepLast(size_t depth, bool print_depth_warning = true); }; /// Encapsulation of Quality of Service settings. diff --git a/rclcpp/src/rclcpp/qos.cpp b/rclcpp/src/rclcpp/qos.cpp index 2d0277bfb5..2453149aa4 100644 --- a/rclcpp/src/rclcpp/qos.cpp +++ b/rclcpp/src/rclcpp/qos.cpp @@ -45,9 +45,19 @@ std::string qos_policy_name_from_kind(rmw_qos_policy_kind_t policy_kind) } } -QoSInitialization::QoSInitialization(rmw_qos_history_policy_t history_policy_arg, size_t depth_arg) +QoSInitialization::QoSInitialization( + rmw_qos_history_policy_t history_policy_arg, size_t depth_arg, + bool print_depth_warning) : history_policy(history_policy_arg), depth(depth_arg) -{} +{ + if (history_policy == RMW_QOS_POLICY_HISTORY_KEEP_LAST && depth == 0 && print_depth_warning) { + RCLCPP_WARN_ONCE( + rclcpp::get_logger( + "rclcpp"), + "A zero depth with KEEP_LAST doesn't make sense; no data could be stored. " + "This will be interpreted as SYSTEM_DEFAULT"); + } +} QoSInitialization QoSInitialization::from_rmw(const rmw_qos_profile_t & rmw_qos) @@ -55,8 +65,9 @@ QoSInitialization::from_rmw(const rmw_qos_profile_t & rmw_qos) switch (rmw_qos.history) { case RMW_QOS_POLICY_HISTORY_KEEP_ALL: return KeepAll(); - case RMW_QOS_POLICY_HISTORY_KEEP_LAST: case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: + return KeepLast(rmw_qos.depth, false); + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: case RMW_QOS_POLICY_HISTORY_UNKNOWN: default: return KeepLast(rmw_qos.depth); @@ -67,16 +78,9 @@ KeepAll::KeepAll() : QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 0) {} -KeepLast::KeepLast(size_t depth) -: QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth) +KeepLast::KeepLast(size_t depth, bool print_depth_warning) +: QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth, print_depth_warning) { - if (depth == 0) { - RCLCPP_WARN_ONCE( - rclcpp::get_logger( - "rclcpp"), - "A zero depth with KEEP_LAST doesn't make sense; no data could be stored." - "This will be interpreted as SYSTEM_DEFAULT"); - } } QoS::QoS( @@ -84,15 +88,6 @@ QoS::QoS( const rmw_qos_profile_t & initial_profile) : rmw_qos_profile_(initial_profile) { - if (qos_initialization.history_policy == RMW_QOS_POLICY_HISTORY_KEEP_LAST && - qos_initialization.depth == 0) - { - RCLCPP_WARN_ONCE( - rclcpp::get_logger( - "rclcpp"), - "A zero depth with KEEP_LAST doesn't make sense; no data could be stored." - "This will be interpreted as SYSTEM_DEFAULT"); - } rmw_qos_profile_.history = qos_initialization.history_policy; rmw_qos_profile_.depth = qos_initialization.depth; } From 37adc03c11171f67c213ee72f7ce43fdcbda7b9e Mon Sep 17 00:00:00 2001 From: Alexander Hans Date: Tue, 17 Jan 2023 20:50:44 +0100 Subject: [PATCH 172/455] Fix -Wmaybe-uninitialized warning (#2081) * Fix -Wmaybe-uninitialized warning gcc 12 warned about `callback_list_ptr` potentially being uninitialized when compiling in release mode (i.e., with `-O2`). Since `shutdown_type` is a compile-time parameter, we fix the warning by enforcing the decision at compile time. Signed-off-by: Alexander Hans --- rclcpp/include/rclcpp/context.hpp | 8 ++- rclcpp/src/rclcpp/context.cpp | 111 +++++++++++++----------------- 2 files changed, 54 insertions(+), 65 deletions(-) diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 3add3f6d46..2917ce3363 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -398,20 +398,22 @@ class Context : public std::enable_shared_from_this using ShutdownCallback = ShutdownCallbackHandle::ShutdownCallbackType; + template RCLCPP_LOCAL ShutdownCallbackHandle add_shutdown_callback( - ShutdownType shutdown_type, ShutdownCallback callback); + template RCLCPP_LOCAL bool remove_shutdown_callback( - ShutdownType shutdown_type, const ShutdownCallbackHandle & callback_handle); + template + RCLCPP_LOCAL std::vector - get_shutdown_callback(ShutdownType shutdown_type) const; + get_shutdown_callback() const; }; /// Return a copy of the list of context shared pointers. diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 1599172e2e..971c0ee73a 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -365,49 +365,45 @@ Context::on_shutdown(OnShutdownCallback callback) rclcpp::OnShutdownCallbackHandle Context::add_on_shutdown_callback(OnShutdownCallback callback) { - return add_shutdown_callback(ShutdownType::on_shutdown, callback); + return add_shutdown_callback(callback); } bool Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle) { - return remove_shutdown_callback(ShutdownType::on_shutdown, callback_handle); + return remove_shutdown_callback(callback_handle); } rclcpp::PreShutdownCallbackHandle Context::add_pre_shutdown_callback(PreShutdownCallback callback) { - return add_shutdown_callback(ShutdownType::pre_shutdown, callback); + return add_shutdown_callback(callback); } bool Context::remove_pre_shutdown_callback( const PreShutdownCallbackHandle & callback_handle) { - return remove_shutdown_callback(ShutdownType::pre_shutdown, callback_handle); + return remove_shutdown_callback(callback_handle); } +template rclcpp::ShutdownCallbackHandle Context::add_shutdown_callback( - ShutdownType shutdown_type, ShutdownCallback callback) { auto callback_shared_ptr = std::make_shared(callback); - switch (shutdown_type) { - case ShutdownType::pre_shutdown: - { - std::lock_guard lock(pre_shutdown_callbacks_mutex_); - pre_shutdown_callbacks_.emplace(callback_shared_ptr); - } - break; - case ShutdownType::on_shutdown: - { - std::lock_guard lock(on_shutdown_callbacks_mutex_); - on_shutdown_callbacks_.emplace(callback_shared_ptr); - } - break; + static_assert( + shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown); + + if constexpr (shutdown_type == ShutdownType::pre_shutdown) { + std::lock_guard lock(pre_shutdown_callbacks_mutex_); + pre_shutdown_callbacks_.emplace(callback_shared_ptr); + } else { + std::lock_guard lock(on_shutdown_callbacks_mutex_); + on_shutdown_callbacks_.emplace(callback_shared_ptr); } ShutdownCallbackHandle callback_handle; @@ -415,73 +411,64 @@ Context::add_shutdown_callback( return callback_handle; } +template bool Context::remove_shutdown_callback( - ShutdownType shutdown_type, const ShutdownCallbackHandle & callback_handle) { - std::mutex * mutex_ptr = nullptr; - std::unordered_set< - std::shared_ptr> * callback_list_ptr; - - switch (shutdown_type) { - case ShutdownType::pre_shutdown: - mutex_ptr = &pre_shutdown_callbacks_mutex_; - callback_list_ptr = &pre_shutdown_callbacks_; - break; - case ShutdownType::on_shutdown: - mutex_ptr = &on_shutdown_callbacks_mutex_; - callback_list_ptr = &on_shutdown_callbacks_; - break; - } - - std::lock_guard lock(*mutex_ptr); - auto callback_shared_ptr = callback_handle.callback.lock(); + const auto callback_shared_ptr = callback_handle.callback.lock(); if (callback_shared_ptr == nullptr) { return false; } - return callback_list_ptr->erase(callback_shared_ptr) == 1; + + const auto remove_callback = [this, &callback_shared_ptr](auto & mutex, auto & callback_set) { + const std::lock_guard lock(mutex); + return callback_set.erase(callback_shared_ptr) == 1; + }; + + static_assert( + shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown); + + if constexpr (shutdown_type == ShutdownType::pre_shutdown) { + return remove_callback(pre_shutdown_callbacks_mutex_, pre_shutdown_callbacks_); + } else { + return remove_callback(on_shutdown_callbacks_mutex_, on_shutdown_callbacks_); + } } std::vector Context::get_on_shutdown_callbacks() const { - return get_shutdown_callback(ShutdownType::on_shutdown); + return get_shutdown_callback(); } std::vector Context::get_pre_shutdown_callbacks() const { - return get_shutdown_callback(ShutdownType::pre_shutdown); + return get_shutdown_callback(); } +template std::vector -Context::get_shutdown_callback(ShutdownType shutdown_type) const +Context::get_shutdown_callback() const { - std::mutex * mutex_ptr = nullptr; - const std::unordered_set< - std::shared_ptr> * callback_list_ptr; - - switch (shutdown_type) { - case ShutdownType::pre_shutdown: - mutex_ptr = &pre_shutdown_callbacks_mutex_; - callback_list_ptr = &pre_shutdown_callbacks_; - break; - case ShutdownType::on_shutdown: - mutex_ptr = &on_shutdown_callbacks_mutex_; - callback_list_ptr = &on_shutdown_callbacks_; - break; - } + const auto get_callback_vector = [this](auto & mutex, auto & callback_set) { + const std::lock_guard lock(mutex); + std::vector callbacks; + for (auto & callback : callback_set) { + callbacks.push_back(*callback); + } + return callbacks; + }; - std::vector callbacks; - { - std::lock_guard lock(*mutex_ptr); - for (auto & iter : *callback_list_ptr) { - callbacks.emplace_back(*iter); - } - } + static_assert( + shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown); - return callbacks; + if constexpr (shutdown_type == ShutdownType::pre_shutdown) { + return get_callback_vector(pre_shutdown_callbacks_mutex_, pre_shutdown_callbacks_); + } else { + return get_callback_vector(on_shutdown_callbacks_mutex_, on_shutdown_callbacks_); + } } std::shared_ptr From ab71df3ce1d8993e8424e7d0934b2e205284c682 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 23 Jan 2023 09:46:50 -0600 Subject: [PATCH 173/455] Improve component_manager_isolated shutdown (#2085) This eliminates a potential hang when the isolated container is being shutdown via the rclcpp SignalHandler. Signed-off-by: Michael Carroll Co-authored-by: Chris Lalancette --- .../component_manager_isolated.hpp | 48 +++++++++++++------ 1 file changed, 34 insertions(+), 14 deletions(-) diff --git a/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp b/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp index ea77532312..e2061e4da2 100644 --- a/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp +++ b/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp @@ -38,6 +38,16 @@ class ComponentManagerIsolated : public rclcpp_components::ComponentManager { std::shared_ptr executor; std::thread thread; + std::atomic_bool thread_initialized; + + /// Constructor for the wrapper. + /// This is necessary as atomic variables don't have copy/move operators + /// implemented so this structure is not copyable/movable by default + explicit DedicatedExecutorWrapper(std::shared_ptr exec) + : executor(exec), + thread_initialized(false) + { + } }; public: @@ -59,15 +69,19 @@ class ComponentManagerIsolated : public rclcpp_components::ComponentManager void add_node_to_executor(uint64_t node_id) override { - DedicatedExecutorWrapper executor_wrapper; auto exec = std::make_shared(); exec->add_node(node_wrappers_[node_id].get_node_base_interface()); - executor_wrapper.executor = exec; - executor_wrapper.thread = std::thread( - [exec]() { + + // Emplace rather than std::move since move operations are deleted for atomics + auto result = dedicated_executor_wrappers_.emplace(std::make_pair(node_id, exec)); + DedicatedExecutorWrapper & wrapper = result.first->second; + wrapper.executor = exec; + auto & thread_initialized = wrapper.thread_initialized; + wrapper.thread = std::thread( + [exec, &thread_initialized]() { + thread_initialized = true; exec->spin(); }); - dedicated_executor_wrappers_[node_id] = std::move(executor_wrapper); } /// Remove component node from executor model, it's invoked in on_unload_node() /** @@ -90,15 +104,21 @@ class ComponentManagerIsolated : public rclcpp_components::ComponentManager */ void cancel_executor(DedicatedExecutorWrapper & executor_wrapper) { - // We can't immediately call the cancel() API on an executor because if it is not - // already spinning, this operation will have no effect. - // We rely on the assumption that this class creates executors and then immediately makes them - // spin in a separate thread, i.e. the time gap between when the executor is created and when - // it starts to spin is small (although it's not negligible). - - while (!executor_wrapper.executor->is_spinning()) { - // This is an arbitrarily small delay to avoid busy looping - rclcpp::sleep_for(std::chrono::milliseconds(1)); + // Verify that the executor thread has begun spinning. + // If it has not, then wait until the thread starts to ensure + // that cancel() will fully stop the execution + // + // This prevents a previous race condition that occurs between the + // creation of the executor spin thread and cancelling an executor + + if (!executor_wrapper.thread_initialized) { + auto context = this->get_node_base_interface()->get_context(); + + // Guarantee that either the executor is spinning or we are shutting down. + while (!executor_wrapper.executor->is_spinning() && rclcpp::ok(context)) { + // This is an arbitrarily small delay to avoid busy looping + rclcpp::sleep_for(std::chrono::milliseconds(1)); + } } // After the while loop we are sure that the executor is now spinning, so From 7d660acc05df6fc480d2b05e09d487d8c027d2c1 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 27 Jan 2023 14:16:31 -0500 Subject: [PATCH 174/455] Fix clock state cached time to be a copy, not a reference. (#2092) That is, in cache_last_msg(), we were just taking a shared_ptr reference. While this is pretty fast, it also means that any changes to that message would be reflected internally. Instead, make a new shared pointer out of that message when it comes in, which essentially causes this to be a copy. Signed-off-by: Chris Lalancette --- rclcpp/src/rclcpp/time_source.cpp | 20 ++++++-------------- 1 file changed, 6 insertions(+), 14 deletions(-) diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index bf1f9b3daa..7f3b3b9536 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -37,7 +37,8 @@ class ClocksState final { public: ClocksState() - : logger_(rclcpp::get_logger("rclcpp")) + : logger_(rclcpp::get_logger("rclcpp")), + last_time_msg_(std::make_shared()) { } @@ -53,13 +54,8 @@ class ClocksState final ros_time_active_ = true; // Update all attached clocks to zero or last recorded time - std::lock_guard guard(clock_list_lock_); - auto time_msg = std::make_shared(); - if (last_msg_set_) { - time_msg = std::make_shared(last_msg_set_->clock); - } for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { - set_clock(time_msg, true, *it); + set_clock(last_time_msg_, true, *it); } } @@ -101,11 +97,7 @@ class ClocksState final std::lock_guard guard(clock_list_lock_); associated_clocks_.push_back(clock); // Set the clock to zero unless there's a recently received message - auto time_msg = std::make_shared(); - if (last_msg_set_) { - time_msg = std::make_shared(last_msg_set_->clock); - } - set_clock(time_msg, ros_time_active_, clock); + set_clock(last_time_msg_, ros_time_active_, clock); } // Detach a clock @@ -171,7 +163,7 @@ class ClocksState final // Cache the last clock message received void cache_last_msg(std::shared_ptr msg) { - last_msg_set_ = msg; + last_time_msg_ = std::make_shared(msg->clock); } bool are_all_clocks_rcl_ros_time() @@ -199,7 +191,7 @@ class ClocksState final // This is needed when new clocks are added. bool ros_time_active_{false}; // Last set message to be passed to newly registered clocks - std::shared_ptr last_msg_set_; + std::shared_ptr last_time_msg_{nullptr}; }; class TimeSource::NodeState final From 97c5c11c25794ab5aedab60b1ba7494cb34e25e6 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 30 Jan 2023 13:47:11 -0800 Subject: [PATCH 175/455] Add default constructor to NodeInterfaces (#2094) * Add default constructor to NodeInterfaces Signed-off-by: Shane Loretz * Remove unnecessary NOLINT Signed-off-by: Shane Loretz --------- Signed-off-by: Shane Loretz Signed-off-by: Shane Loretz --- .../node_interfaces/detail/node_interfaces_helpers.hpp | 4 ++++ .../include/rclcpp/node_interfaces/node_interfaces.hpp | 5 +++++ .../test/rclcpp/node_interfaces/test_node_interfaces.cpp | 9 +++++++++ 3 files changed, 18 insertions(+) diff --git a/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp b/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp index 8f7b452f5f..b999344ca9 100644 --- a/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp @@ -44,6 +44,10 @@ struct NodeInterfacesStorage : interfaces_(init_tuple(node)) {} + NodeInterfacesStorage() + : interfaces_() + {} + explicit NodeInterfacesStorage(std::shared_ptr... args) : interfaces_(args ...) {} diff --git a/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp index 97959145b3..bb4886d592 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp @@ -147,6 +147,11 @@ class NodeInterfaces : NodeInterfacesSupportsT(node) {} + // Create a NodeInterfaces object with no bound interfaces + NodeInterfaces() + : NodeInterfacesSupportsT() + {} + explicit NodeInterfaces(std::shared_ptr... args) : NodeInterfacesSupportsT(args ...) {} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp index ebb91c4770..fd6d6173d6 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp @@ -33,6 +33,15 @@ class TestNodeInterfaces : public ::testing::Test } }; +TEST_F(TestNodeInterfaces, default_constructor) { + auto node = std::make_shared("my_node"); + using rclcpp::node_interfaces::NodeInterfaces; + using rclcpp::node_interfaces::NodeBaseInterface; + using rclcpp::node_interfaces::NodeGraphInterface; + NodeInterfaces interfaces; + interfaces = NodeInterfaces(*node); +} + /* Testing NodeInterfaces construction from nodes. */ From be8c5d01c683ea381ffb77f4fc246411d8d31639 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 30 Jan 2023 18:26:56 -0800 Subject: [PATCH 176/455] 19.0.0 Signed-off-by: Shane Loretz --- rclcpp/CHANGELOG.rst | 9 +++++++++ rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 5 +++++ rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 24 insertions(+), 4 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 0c8201217c..8a95b94793 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +19.0.0 (2023-01-30) +------------------- +* Add default constructor to NodeInterfaces (`#2094 `_) +* Fix clock state cached time to be a copy, not a reference. (`#2092 `_) +* Fix -Wmaybe-uninitialized warning (`#2081 `_) +* Fix the keep_last warning when using system defaults. (`#2082 `_) +* Add in a fix for older compilers. (`#2075 `_) +* Contributors: Alexander Hans, Chris Lalancette, Shane Loretz + 18.0.0 (2022-12-29) ------------------- * Implement Unified Node Interface (NodeInterfaces class) (`#2041 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index a18cd7ae1d..490cefd818 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 18.0.0 + 19.0.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 2a08ebe63f..3c69ff0122 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +19.0.0 (2023-01-30) +------------------- + 18.0.0 (2022-12-29) ------------------- * Explicitly set callback type (`#2059 `_) diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 375e978be1..0ce45cc08b 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 18.0.0 + 19.0.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 820be8b817..a1b44a1472 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +19.0.0 (2023-01-30) +------------------- +* Improve component_manager_isolated shutdown (`#2085 `_) +* Contributors: Michael Carroll + 18.0.0 (2022-12-29) ------------------- * Update maintainers (`#2043 `_) diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index de1b88e242..743edb3ee1 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 18.0.0 + 19.0.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 0285505488..6ea3085d35 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +19.0.0 (2023-01-30) +------------------- + 18.0.0 (2022-12-29) ------------------- * Implement Unified Node Interface (NodeInterfaces class) (`#2041 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 092105238b..3b2ad29db6 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 18.0.0 + 19.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 1fd5a96561166ad2ad557bbc1a297cd652883cb2 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Tue, 7 Feb 2023 01:46:47 +0800 Subject: [PATCH 177/455] Fix bug on the disorder of calling shutdown callback (#2097) * Fix bug on the disorder of calling shutdown callback Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/context.hpp | 4 +- rclcpp/src/rclcpp/context.cpp | 18 ++- rclcpp/test/rclcpp/CMakeLists.txt | 3 + rclcpp/test/rclcpp/test_context.cpp | 216 ++++++++++++++++++++++++++++ 4 files changed, 235 insertions(+), 6 deletions(-) create mode 100644 rclcpp/test/rclcpp/test_context.cpp diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 2917ce3363..7b1429abb2 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -376,10 +376,10 @@ class Context : public std::enable_shared_from_this // attempt to acquire another sub context. std::recursive_mutex sub_contexts_mutex_; - std::unordered_set> on_shutdown_callbacks_; + std::vector> on_shutdown_callbacks_; mutable std::mutex on_shutdown_callbacks_mutex_; - std::unordered_set> pre_shutdown_callbacks_; + std::vector> pre_shutdown_callbacks_; mutable std::mutex pre_shutdown_callbacks_mutex_; /// Condition variable for timed sleep (see sleep_for). diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 971c0ee73a..0fec6d066c 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -400,10 +400,10 @@ Context::add_shutdown_callback( if constexpr (shutdown_type == ShutdownType::pre_shutdown) { std::lock_guard lock(pre_shutdown_callbacks_mutex_); - pre_shutdown_callbacks_.emplace(callback_shared_ptr); + pre_shutdown_callbacks_.emplace_back(callback_shared_ptr); } else { std::lock_guard lock(on_shutdown_callbacks_mutex_); - on_shutdown_callbacks_.emplace(callback_shared_ptr); + on_shutdown_callbacks_.emplace_back(callback_shared_ptr); } ShutdownCallbackHandle callback_handle; @@ -421,9 +421,19 @@ Context::remove_shutdown_callback( return false; } - const auto remove_callback = [this, &callback_shared_ptr](auto & mutex, auto & callback_set) { + const auto remove_callback = [&callback_shared_ptr](auto & mutex, auto & callback_vector) { const std::lock_guard lock(mutex); - return callback_set.erase(callback_shared_ptr) == 1; + auto iter = callback_vector.begin(); + for (; iter != callback_vector.end(); iter++) { + if ((*iter).get() == callback_shared_ptr.get()) { + break; + } + } + if (iter == callback_vector.end()) { + return false; + } + callback_vector.erase(iter); + return true; }; static_assert( diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 6dffb543e9..f2f204c582 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -582,6 +582,9 @@ target_link_libraries(test_logger ${PROJECT_NAME}) ament_add_gmock(test_logging test_logging.cpp) target_link_libraries(test_logging ${PROJECT_NAME}) +ament_add_gmock(test_context test_context.cpp) +target_link_libraries(test_context ${PROJECT_NAME}) + ament_add_gtest(test_time test_time.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_time) diff --git a/rclcpp/test/rclcpp/test_context.cpp b/rclcpp/test/rclcpp/test_context.cpp new file mode 100644 index 0000000000..9d736f9db2 --- /dev/null +++ b/rclcpp/test/rclcpp/test_context.cpp @@ -0,0 +1,216 @@ +// Copyright 2023 Sony Group Corporation. +// +// 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. +#include + +#include "rclcpp/context.hpp" +#include "rclcpp/rclcpp.hpp" + +TEST(TestContext, check_pre_shutdown_callback_order) { + auto context = std::make_shared(); + context->init(0, nullptr); + + int result[4] = {0, 0, 0, 0}; + int index = 0; + + auto callback1 = [&result, &index]() { + result[index] = 1; + index++; + }; + auto callback2 = [&result, &index]() { + result[index] = 2; + index++; + }; + auto callback3 = [&result, &index]() { + result[index] = 3; + index++; + }; + auto callback4 = [&result, &index]() { + result[index] = 4; + index++; + }; + + context->add_pre_shutdown_callback(callback1); + context->add_pre_shutdown_callback(callback2); + context->add_pre_shutdown_callback(callback3); + context->add_pre_shutdown_callback(callback4); + + context->shutdown("for test"); + + EXPECT_TRUE(result[0] == 1 && result[1] == 2 && result[2] == 3 && result[3] == 4); +} + +TEST(TestContext, check_on_shutdown_callback_order) { + auto context = std::make_shared(); + context->init(0, nullptr); + + int result[4] = {0, 0, 0, 0}; + int index = 0; + + auto callback1 = [&result, &index]() { + result[index] = 1; + index++; + }; + auto callback2 = [&result, &index]() { + result[index] = 2; + index++; + }; + auto callback3 = [&result, &index]() { + result[index] = 3; + index++; + }; + auto callback4 = [&result, &index]() { + result[index] = 4; + index++; + }; + + context->add_on_shutdown_callback(callback1); + context->add_on_shutdown_callback(callback2); + context->add_on_shutdown_callback(callback3); + context->add_on_shutdown_callback(callback4); + + context->shutdown("for test"); + + EXPECT_TRUE(result[0] == 1 && result[1] == 2 && result[2] == 3 && result[3] == 4); +} + +TEST(TestContext, check_mixed_register_shutdown_callback_order) { + auto context = std::make_shared(); + context->init(0, nullptr); + + int result[8] = {0, 0, 0, 0, 0, 0, 0, 0}; + int index = 0; + + auto callback1 = [&result, &index]() { + result[index] = 1; + index++; + }; + auto callback2 = [&result, &index]() { + result[index] = 2; + index++; + }; + auto callback3 = [&result, &index]() { + result[index] = 3; + index++; + }; + auto callback4 = [&result, &index]() { + result[index] = 4; + index++; + }; + auto callback5 = [&result, &index]() { + result[index] = 5; + index++; + }; + auto callback6 = [&result, &index]() { + result[index] = 6; + index++; + }; + auto callback7 = [&result, &index]() { + result[index] = 7; + index++; + }; + auto callback8 = [&result, &index]() { + result[index] = 8; + index++; + }; + + // Mixed register + context->add_pre_shutdown_callback(callback1); + context->add_on_shutdown_callback(callback5); + context->add_pre_shutdown_callback(callback2); + context->add_on_shutdown_callback(callback6); + context->add_pre_shutdown_callback(callback3); + context->add_on_shutdown_callback(callback7); + context->add_pre_shutdown_callback(callback4); + context->add_on_shutdown_callback(callback8); + + context->shutdown("for test"); + + EXPECT_TRUE( + result[0] == 1 && result[1] == 2 && result[2] == 3 && result[3] == 4 && + result[4] == 5 && result[5] == 6 && result[6] == 7 && result[7] == 8); +} + +TEST(TestContext, check_pre_shutdown_callback_order_after_del) { + auto context = std::make_shared(); + context->init(0, nullptr); + + int result[4] = {0, 0, 0, 0}; + int index = 0; + + auto callback1 = [&result, &index]() { + result[index] = 1; + index++; + }; + auto callback2 = [&result, &index]() { + result[index] = 2; + index++; + }; + auto callback3 = [&result, &index]() { + result[index] = 3; + index++; + }; + auto callback4 = [&result, &index]() { + result[index] = 4; + index++; + }; + + context->add_pre_shutdown_callback(callback1); + auto callback_handle = context->add_pre_shutdown_callback(callback2); + context->add_pre_shutdown_callback(callback3); + context->add_pre_shutdown_callback(callback4); + + EXPECT_TRUE(context->remove_pre_shutdown_callback(callback_handle)); + EXPECT_FALSE(context->remove_pre_shutdown_callback(callback_handle)); + + context->shutdown("for test"); + + EXPECT_TRUE(result[0] == 1 && result[1] == 3 && result[2] == 4 && result[3] == 0); +} + +TEST(TestContext, check_on_shutdown_callback_order_after_del) { + auto context = std::make_shared(); + context->init(0, nullptr); + + int result[4] = {0, 0, 0, 0}; + int index = 0; + + auto callback1 = [&result, &index]() { + result[index] = 1; + index++; + }; + auto callback2 = [&result, &index]() { + result[index] = 2; + index++; + }; + auto callback3 = [&result, &index]() { + result[index] = 3; + index++; + }; + auto callback4 = [&result, &index]() { + result[index] = 4; + index++; + }; + + context->add_on_shutdown_callback(callback1); + auto callback_handle = context->add_on_shutdown_callback(callback2); + context->add_on_shutdown_callback(callback3); + context->add_on_shutdown_callback(callback4); + + EXPECT_TRUE(context->remove_on_shutdown_callback(callback_handle)); + EXPECT_FALSE(context->remove_on_shutdown_callback(callback_handle)); + + context->shutdown("for test"); + + EXPECT_TRUE(result[0] == 1 && result[1] == 3 && result[2] == 4 && result[3] == 0); +} From beda0966db945ee3ba56e378183a7b7bfdab9526 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 10 Feb 2023 19:39:27 +0800 Subject: [PATCH 178/455] Topic node guard condition in executor (#2074) * add a test Signed-off-by: Chen Lihui * rollback the node guard condition for exectuor Signed-off-by: Chen Lihui --------- Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/executor.hpp | 9 ++++ rclcpp/src/rclcpp/executor.cpp | 19 ++++++++ .../test_add_callback_groups_to_executor.cpp | 47 +++++++++++++++++++ 3 files changed, 75 insertions(+) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 65d0a930cb..7f2071cded 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -560,11 +560,20 @@ class Executor virtual void spin_once_impl(std::chrono::nanoseconds timeout); + typedef std::map> + WeakNodesToGuardConditionsMap; + typedef std::map> WeakCallbackGroupsToGuardConditionsMap; + /// maps nodes to guard conditions + WeakNodesToGuardConditionsMap + weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + /// maps callback groups to guard conditions WeakCallbackGroupsToGuardConditionsMap weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 401beb0a73..32b895c1e3 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -112,6 +112,12 @@ Executor::~Executor() } weak_groups_to_guard_conditions_.clear(); + for (const auto & pair : weak_nodes_to_guard_conditions_) { + auto guard_condition = pair.second; + memory_strategy_->remove_guard_condition(guard_condition); + } + weak_nodes_to_guard_conditions_.clear(); + // Finalize the wait set. if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( @@ -274,6 +280,10 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt } }); + const auto & gc = node_ptr->get_notify_guard_condition(); + weak_nodes_to_guard_conditions_[node_ptr] = &gc; + // Add the node's notify condition to the guard condition handles + memory_strategy_->add_guard_condition(gc); weak_nodes_.push_back(node_ptr); } @@ -378,6 +388,9 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node } } + memory_strategy_->remove_guard_condition(&node_ptr->get_notify_guard_condition()); + weak_nodes_to_guard_conditions_.erase(node_ptr); + std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); has_executor.store(false); } @@ -706,6 +719,12 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) auto weak_node_ptr = pair.second; if (weak_group_ptr.expired() || weak_node_ptr.expired()) { invalid_group_ptrs.push_back(weak_group_ptr); + auto node_guard_pair = weak_nodes_to_guard_conditions_.find(weak_node_ptr); + if (node_guard_pair != weak_nodes_to_guard_conditions_.end()) { + auto guard_condition = node_guard_pair->second; + weak_nodes_to_guard_conditions_.erase(weak_node_ptr); + memory_strategy_->remove_guard_condition(guard_condition); + } } } std::for_each( diff --git a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp index 07ca1e87d8..02fa0b7a94 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -340,6 +340,53 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, subscriber_triggered_to_receive_mess EXPECT_TRUE(received_message_future.get()); } +/* + * Test callback group created after spin. + * A subscriber with a new callback group that created after executor spin not received a message + * because the executor can't be triggered while a subscriber created, see + * https://github.com/ros2/rclcpp/issues/2067 +*/ +TYPED_TEST(TestAddCallbackGroupsToExecutor, callback_group_create_after_spin) +{ + auto node = std::make_shared("my_node", "/ns"); + + // create a publisher to send data + rclcpp::QoS qos = rclcpp::QoS(1).reliable().transient_local(); + rclcpp::Publisher::SharedPtr publisher = + node->create_publisher("topic_name", qos); + publisher->publish(test_msgs::msg::Empty()); + + // create a thread running an executor + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + std::promise received_message_promise; + auto received_message_future = received_message_promise.get_future(); + rclcpp::FutureReturnCode return_code = rclcpp::FutureReturnCode::TIMEOUT; + std::thread executor_thread = std::thread( + [&executor, &received_message_future, &return_code]() { + return_code = executor.spin_until_future_complete(received_message_future, 5s); + }); + + // to create a callback group after spin + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + // expect the subscriber to receive a message + auto sub_callback = [&received_message_promise](test_msgs::msg::Empty::ConstSharedPtr) { + received_message_promise.set_value(true); + }; + // create a subscription using the `cb_grp` callback group + auto options = rclcpp::SubscriptionOptions(); + options.callback_group = cb_grp; + rclcpp::Subscription::SharedPtr subscription = + node->create_subscription("topic_name", qos, sub_callback, options); + + executor_thread.join(); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + EXPECT_TRUE(received_message_future.get()); +} + /* * Test removing callback group from executor that its not associated with. */ From ce5a2614fac2a86545629d18cb1bee168b5ee066 Mon Sep 17 00:00:00 2001 From: mauropasse Date: Fri, 10 Feb 2023 16:25:52 +0000 Subject: [PATCH 179/455] Add support for timers on reset callback (#1979) Signed-off-by: Mauro Passerino Co-authored-by: Alberto Soragna --- rclcpp/include/rclcpp/timer.hpp | 37 ++++++++++++++ rclcpp/src/rclcpp/timer.cpp | 85 +++++++++++++++++++++++++++++++-- 2 files changed, 119 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index e31c9fe4bf..8af0c6fb68 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -149,11 +149,48 @@ class TimerBase bool exchange_in_use_by_wait_set_state(bool in_use_state); + /// Set a callback to be called when the timer is reset + /** + * You should aim to make this callback 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. + * + * Calling it again will override 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, + * you may use a lambda with captures or std::bind. + * + * \param[in] callback functor to be called whenever timer is reset + */ + RCLCPP_PUBLIC + void + set_on_reset_callback(std::function callback); + + /// Unset the callback registered for reset timer + RCLCPP_PUBLIC + void + clear_on_reset_callback(); + protected: + std::recursive_mutex callback_mutex_; + // Declare callback before timer_handle_, so on destruction + // the callback is destroyed last. Otherwise, the rcl timer + // callback would point briefly to a destroyed function. + // Clearing the callback on timer destructor also makes sure + // the rcl callback is cleared before on_reset_callback_. + std::function on_reset_callback_{nullptr}; + Clock::SharedPtr clock_; std::shared_ptr timer_handle_; std::atomic in_use_by_wait_set_{false}; + + RCLCPP_PUBLIC + void + set_on_reset_callback(rcl_event_callback_t callback, const void * user_data); }; diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 788cdf8dce..59ddd5e8dd 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -19,9 +19,12 @@ #include #include +#include "rmw/impl/cpp/demangle.hpp" + #include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/detail/cpp_callback_trampoline.hpp" #include "rclcpp/exceptions.hpp" - +#include "rclcpp/logging.hpp" #include "rcutils/logging_macros.h" using rclcpp::TimerBase; @@ -71,7 +74,9 @@ TimerBase::TimerBase( } TimerBase::~TimerBase() -{} +{ + clear_on_reset_callback(); +} void TimerBase::cancel() @@ -96,7 +101,11 @@ TimerBase::is_canceled() void TimerBase::reset() { - rcl_ret_t ret = rcl_timer_reset(timer_handle_.get()); + rcl_ret_t ret = RCL_RET_OK; + { + std::lock_guard lock(callback_mutex_); + ret = rcl_timer_reset(timer_handle_.get()); + } if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't reset timer"); } @@ -138,3 +147,73 @@ TimerBase::exchange_in_use_by_wait_set_state(bool in_use_state) { return in_use_by_wait_set_.exchange(in_use_state); } + +void +TimerBase::set_on_reset_callback(std::function callback) +{ + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_reset_callback " + "is not callable."); + } + + auto new_callback = + [callback, this](size_t reset_calls) { + try { + callback(reset_calls); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("rclcpp"), + "rclcpp::TimerBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on reset' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("rclcpp"), + "rclcpp::TimerBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on reset' callback"); + } + }; + + 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 + // been replaced but rcl hasn't been told about the new one yet. + set_on_reset_callback( + rclcpp::detail::cpp_callback_trampoline< + decltype(new_callback), const void *, size_t>, + static_cast(&new_callback)); + + // Store the std::function to keep it in scope, also overwrites the existing one. + on_reset_callback_ = new_callback; + + // Set it again, now using the permanent storage. + set_on_reset_callback( + rclcpp::detail::cpp_callback_trampoline< + decltype(on_reset_callback_), const void *, size_t>, + static_cast(&on_reset_callback_)); +} + +void +TimerBase::clear_on_reset_callback() +{ + std::lock_guard lock(callback_mutex_); + + if (on_reset_callback_) { + set_on_reset_callback(nullptr, nullptr); + on_reset_callback_ = nullptr; + } +} + +void +TimerBase::set_on_reset_callback(rcl_event_callback_t callback, const void * user_data) +{ + rcl_ret_t ret = rcl_timer_set_on_reset_callback(timer_handle_.get(), callback, user_data); + + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to set timer on reset callback"); + } +} From 15ea024d482d767abbaf4b8e5e9e0412e2053604 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 14 Feb 2023 14:35:58 +0000 Subject: [PATCH 180/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 7 +++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 3 +++ 4 files changed, 16 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 8a95b94793..1a0e33029c 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add support for timers on reset callback (`#1979 `_) +* Topic node guard condition in executor (`#2074 `_) +* Fix bug on the disorder of calling shutdown callback (`#2097 `_) +* Contributors: Barry Xu, Chen Lihui, mauropasse + 19.0.0 (2023-01-30) ------------------- * Add default constructor to NodeInterfaces (`#2094 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 3c69ff0122..a3ad7f9b1e 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 19.0.0 (2023-01-30) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index a1b44a1472..69341c59f8 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 19.0.0 (2023-01-30) ------------------- * Improve component_manager_isolated shutdown (`#2085 `_) diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 6ea3085d35..e8012e9d0c 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 19.0.0 (2023-01-30) ------------------- From 01b19247f10d1743d4ca96839283fdd40ac5fe4c Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 14 Feb 2023 14:36:12 +0000 Subject: [PATCH 181/455] 19.1.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 1a0e33029c..43fadcfdb9 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +19.1.0 (2023-02-14) +------------------- * Add support for timers on reset callback (`#1979 `_) * Topic node guard condition in executor (`#2074 `_) * Fix bug on the disorder of calling shutdown callback (`#2097 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 490cefd818..de4790fa96 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 19.0.0 + 19.1.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index a3ad7f9b1e..74a908512a 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +19.1.0 (2023-02-14) +------------------- 19.0.0 (2023-01-30) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 0ce45cc08b..e3b5d52fc5 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 19.0.0 + 19.1.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 69341c59f8..bdb9c04fff 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +19.1.0 (2023-02-14) +------------------- 19.0.0 (2023-01-30) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 743edb3ee1..cc7c604c0b 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 19.0.0 + 19.1.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index e8012e9d0c..e878313af6 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +19.1.0 (2023-02-14) +------------------- 19.0.0 (2023-01-30) ------------------- diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 3b2ad29db6..f4e01246bc 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 19.0.0 + 19.1.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 35a5d6a66ce30c9e5c75cc6cefe6aa52e87c94e0 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Thu, 16 Feb 2023 12:56:47 +0000 Subject: [PATCH 182/455] fixes for rmw callbacks in qos_event class (#2102) Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/qos_event.hpp | 5 +++-- rclcpp/src/rclcpp/qos_event.cpp | 4 +--- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index b48854c4bd..926cc2c75e 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -214,10 +214,11 @@ class QOSEventHandlerBase : public Waitable 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::recursive_mutex callback_mutex_; std::function on_new_event_callback_{nullptr}; + + rcl_event_t event_handle_; + size_t wait_set_event_index_; }; template diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index 91df5bb346..96e2066e91 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -40,9 +40,7 @@ QOSEventHandlerBase::~QOSEventHandlerBase() // This clearing is not needed for other rclcpp entities like pub/subs, since // they do own the underlying rmw entities, which are destroyed // on their rclcpp destructors, thus no risk of dangling pointers. - if (on_new_event_callback_) { - clear_on_ready_callback(); - } + clear_on_ready_callback(); if (rcl_event_fini(&event_handle_) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( From 28e4b1bd738c23e3ede2c70bf35786ce829ae910 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Thu, 16 Feb 2023 22:32:10 +0100 Subject: [PATCH 183/455] Fix documentation of Context class (#2107) Signed-off-by: Silvio Traversaro --- rclcpp/include/rclcpp/context.hpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 7b1429abb2..45c70b9af2 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -65,8 +65,11 @@ using PreShutdownCallbackHandle = ShutdownCallbackHandle; /// Context which encapsulates shared state between nodes and other similar entities. /** * A context also represents the lifecycle between init and shutdown of rclcpp. - * It is often used in conjunction with rclcpp::init, or rclcpp::init_local, - * and rclcpp::shutdown. + * Nodes may be attached to a particular context by passing to the rclcpp::Node + * constructor a rclcpp::NodeOptions instance in which the Context is set via + * rclcpp::NodeOptions::context. + * Nodes will be automatically removed from the context when destructed. + * Contexts may be shutdown by calling rclcpp::shutdown. */ class Context : public std::enable_shared_from_this { From b1e834a8dfb0e2058fbf0e3adcc06cd8eda86243 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Sat, 25 Feb 2023 00:03:10 +0800 Subject: [PATCH 184/455] to create a sublogger while getting child of Logger (#1717) * to create a sublogger while getting child of Logger Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/logger.hpp | 10 +- rclcpp/src/rclcpp/logger.cpp | 45 +++++ rclcpp/test/rclcpp/CMakeLists.txt | 6 + .../test/rclcpp/test_rosout_subscription.cpp | 180 ++++++++++++++++++ 4 files changed, 234 insertions(+), 7 deletions(-) create mode 100644 rclcpp/test/rclcpp/test_rosout_subscription.cpp diff --git a/rclcpp/include/rclcpp/logger.hpp b/rclcpp/include/rclcpp/logger.hpp index c5e248028e..622ddd27da 100644 --- a/rclcpp/include/rclcpp/logger.hpp +++ b/rclcpp/include/rclcpp/logger.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "rclcpp/visibility_control.hpp" @@ -122,6 +123,7 @@ class Logger : name_(new std::string(name)) {} std::shared_ptr name_; + std::shared_ptr> logger_sublogger_pairname_ = nullptr; public: RCLCPP_PUBLIC @@ -157,13 +159,7 @@ class Logger */ RCLCPP_PUBLIC Logger - get_child(const std::string & suffix) - { - if (!name_) { - return Logger(); - } - return Logger(*name_ + "." + suffix); - } + get_child(const std::string & suffix); /// Set level for current logger. /** diff --git a/rclcpp/src/rclcpp/logger.cpp b/rclcpp/src/rclcpp/logger.cpp index b58edd4c80..d1a7640215 100644 --- a/rclcpp/src/rclcpp/logger.cpp +++ b/rclcpp/src/rclcpp/logger.cpp @@ -12,14 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include +#include #include "rcl_logging_interface/rcl_logging_interface.h" +#include "rcl/logging_rosout.h" #include "rclcpp/exceptions.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" +#include "./logging_mutex.hpp" + namespace rclcpp { @@ -62,6 +67,46 @@ get_logging_directory() return path; } +Logger +Logger::get_child(const std::string & suffix) +{ + if (!name_) { + return Logger(); + } + + rcl_ret_t rcl_ret = RCL_RET_OK; + std::shared_ptr logging_mutex; + logging_mutex = get_global_logging_mutex(); + { + std::lock_guard guard(*logging_mutex); + rcl_ret = rcl_logging_rosout_add_sublogger((*name_).c_str(), suffix.c_str()); + if (RCL_RET_OK != rcl_ret) { + exceptions::throw_from_rcl_error( + rcl_ret, "failed to call rcl_logging_rosout_add_sublogger", + rcutils_get_error_state(), rcutils_reset_error); + } + } + + Logger logger(*name_ + RCUTILS_LOGGING_SEPARATOR_STRING + suffix); + if (RCL_RET_OK == rcl_ret) { + logger.logger_sublogger_pairname_.reset( + new std::pair({*name_, suffix}), + [logging_mutex](std::pair * logger_sublogger_pairname_ptr) { + std::lock_guard guard(*logging_mutex); + rcl_ret_t rcl_ret = rcl_logging_rosout_remove_sublogger( + logger_sublogger_pairname_ptr->first.c_str(), + logger_sublogger_pairname_ptr->second.c_str()); + delete logger_sublogger_pairname_ptr; + if (RCL_RET_OK != rcl_ret) { + exceptions::throw_from_rcl_error( + rcl_ret, "failed to call rcl_logging_rosout_remove_sublogger", + rcutils_get_error_state(), rcutils_reset_error); + } + }); + } + return logger; +} + void Logger::set_level(Level level) { diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index f2f204c582..1e3b7a65ca 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -739,6 +739,12 @@ if(TARGET test_rosout_qos) target_link_libraries(test_rosout_qos ${PROJECT_NAME}) endif() +ament_add_gtest(test_rosout_subscription test_rosout_subscription.cpp) +if(TARGET test_rosout_subscription) + ament_target_dependencies(test_rosout_subscription "rcl") + target_link_libraries(test_rosout_subscription ${PROJECT_NAME}) +endif() + ament_add_gtest(test_executor test_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) diff --git a/rclcpp/test/rclcpp/test_rosout_subscription.cpp b/rclcpp/test/rclcpp/test_rosout_subscription.cpp new file mode 100644 index 0000000000..ea761fdc0c --- /dev/null +++ b/rclcpp/test/rclcpp/test_rosout_subscription.cpp @@ -0,0 +1,180 @@ +// 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. + +#include + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "rcl_interfaces/msg/log.hpp" + +using namespace std::chrono_literals; + +class TestRosoutSubscription : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_rosout_subscription", "/ns"); + sub = node->create_subscription( + "/rosout", 10, [this](rcl_interfaces::msg::Log::ConstSharedPtr msg) { + if (msg->msg == this->rosout_msg_data && + msg->name == this->rosout_msg_name) + { + received_msg_promise.set_value(true); + } + }); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::Subscription::SharedPtr sub; + std::promise received_msg_promise; + std::string rosout_msg_data; + std::string rosout_msg_name; +}; + +TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild) { + std::string logger_name = "ns.test_rosout_subscription.child"; + this->rosout_msg_data = "SOMETHING"; + this->rosout_msg_name = logger_name; + { + // before calling get_child of Logger + { + RCLCPP_INFO( + rclcpp::get_logger(logger_name), this->rosout_msg_data.c_str()); + auto future = received_msg_promise.get_future(); + auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); + ASSERT_EQ(rclcpp::FutureReturnCode::TIMEOUT, return_code); + received_msg_promise = {}; + } + + rclcpp::Logger child_logger = this->node->get_logger().get_child("child"); + ASSERT_EQ(child_logger.get_name(), logger_name); + + // after calling get_child of Logger + // 1. use child_logger directly + { + RCLCPP_INFO(child_logger, this->rosout_msg_data.c_str()); + auto future = received_msg_promise.get_future(); + auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + EXPECT_TRUE(future.get()); + received_msg_promise = {}; + } + + // 2. use rclcpp::get_logger + { + RCLCPP_INFO(rclcpp::get_logger(logger_name), this->rosout_msg_data.c_str()); + auto future = received_msg_promise.get_future(); + auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + EXPECT_TRUE(future.get()); + received_msg_promise = {}; + } + } + + // `child_logger` is end of life, there is no sublogger + { + RCLCPP_INFO(rclcpp::get_logger(logger_name), this->rosout_msg_data.c_str()); + auto future = received_msg_promise.get_future(); + auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); + ASSERT_EQ(rclcpp::FutureReturnCode::TIMEOUT, return_code); + received_msg_promise = {}; + } +} + +TEST_F(TestRosoutSubscription, test_rosoutsubscription_parent_log) { + std::string logger_name = "ns.test_rosout_subscription"; + this->rosout_msg_data = "SOMETHING"; + this->rosout_msg_name = logger_name; + + rclcpp::Logger logger = this->node->get_logger(); + ASSERT_EQ(logger.get_name(), logger_name); + RCLCPP_INFO(logger, this->rosout_msg_data.c_str()); + auto future = received_msg_promise.get_future(); + auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + EXPECT_TRUE(future.get()); + received_msg_promise = {}; +} + +TEST_F(TestRosoutSubscription, test_rosoutsubscription_child_log) { + std::string logger_name = "ns.test_rosout_subscription.child1"; + this->rosout_msg_data = "SOMETHING"; + this->rosout_msg_name = logger_name; + + rclcpp::Logger logger = this->node->get_logger(); + RCLCPP_INFO(logger, this->rosout_msg_data.c_str()); + auto future = received_msg_promise.get_future(); + auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); + ASSERT_EQ(rclcpp::FutureReturnCode::TIMEOUT, return_code); + received_msg_promise = {}; + + logger = this->node->get_logger().get_child("child1"); + RCLCPP_INFO(logger, this->rosout_msg_data.c_str()); + future = received_msg_promise.get_future(); + return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + EXPECT_TRUE(future.get()); + received_msg_promise = {}; + + logger = this->node->get_logger().get_child("child2"); + RCLCPP_INFO(logger, this->rosout_msg_data.c_str()); + future = received_msg_promise.get_future(); + return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); + ASSERT_EQ(rclcpp::FutureReturnCode::TIMEOUT, return_code); + received_msg_promise = {}; + + this->rosout_msg_name = "ns.test_rosout_subscription.child2"; + RCLCPP_INFO(logger, this->rosout_msg_data.c_str()); + future = received_msg_promise.get_future(); + return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + EXPECT_TRUE(future.get()); + received_msg_promise = {}; +} + +TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild_hierarchy) { + std::string logger_name = "ns.test_rosout_subscription.child.grandchild"; + this->rosout_msg_data = "SOMETHING"; + this->rosout_msg_name = logger_name; + + rclcpp::Logger grandchild_logger = + this->node->get_logger().get_child("child").get_child("grandchild"); + ASSERT_EQ(grandchild_logger.get_name(), logger_name); + RCLCPP_INFO(grandchild_logger, this->rosout_msg_data.c_str()); + auto future = received_msg_promise.get_future(); + auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + EXPECT_TRUE(future.get()); + received_msg_promise = {}; +} From 006d1fa1df37b417923d2fe5c653adfaae77260c Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 24 Feb 2023 18:27:32 +0000 Subject: [PATCH 185/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 7 +++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 3 +++ 4 files changed, 16 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 43fadcfdb9..2e4fc755bf 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* to create a sublogger while getting child of Logger (`#1717 `_) +* Fix documentation of Context class (`#2107 `_) +* fixes for rmw callbacks in qos_event class (`#2102 `_) +* Contributors: Alberto Soragna, Chen Lihui, Silvio Traversaro + 19.1.0 (2023-02-14) ------------------- * Add support for timers on reset callback (`#1979 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 74a908512a..8b4c60e738 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 19.1.0 (2023-02-14) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index bdb9c04fff..96cd96980a 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 19.1.0 (2023-02-14) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index e878313af6..e887cacdd2 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 19.1.0 (2023-02-14) ------------------- From f57f4077fdfe75f6c916eba1c33fe5ee4918d9cc Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 24 Feb 2023 18:27:42 +0000 Subject: [PATCH 186/455] 19.2.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 2e4fc755bf..f7788aea69 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +19.2.0 (2023-02-24) +------------------- * to create a sublogger while getting child of Logger (`#1717 `_) * Fix documentation of Context class (`#2107 `_) * fixes for rmw callbacks in qos_event class (`#2102 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index de4790fa96..c339f48435 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 19.1.0 + 19.2.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 8b4c60e738..1e7a39a2d3 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +19.2.0 (2023-02-24) +------------------- 19.1.0 (2023-02-14) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index e3b5d52fc5..f4594f83ad 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 19.1.0 + 19.2.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 96cd96980a..00d8920ad3 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +19.2.0 (2023-02-24) +------------------- 19.1.0 (2023-02-14) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index cc7c604c0b..d1d37829a9 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 19.1.0 + 19.2.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index e887cacdd2..bd39d5029e 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +19.2.0 (2023-02-24) +------------------- 19.1.0 (2023-02-14) ------------------- diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index f4e01246bc..8c67bd908b 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 19.1.0 + 19.2.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 9ea55ba620f13a09d67ea8439d75fb78574e2087 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Sat, 25 Feb 2023 04:16:02 +0800 Subject: [PATCH 187/455] to fix flaky test about TestTimeSource.callbacks (#2111) Signed-off-by: Chen Lihui --- rclcpp/test/rclcpp/test_time_source.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index 22aefcda3b..26d7cb2192 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -81,7 +81,7 @@ void spin_until_time( executor.spin_once(10ms); - if (clock->now().nanoseconds() >= end_time.count()) { + if (clock->now().nanoseconds() == end_time.count()) { return; } } From 3062dec77e4695dc0dc8a2efe764dd18c2ad47da Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Fri, 24 Feb 2023 23:48:23 +0100 Subject: [PATCH 188/455] Allow publishing borrowed messages with intra-process enabled (#2108) Signed-off-by: Miguel Company --- rclcpp/include/rclcpp/publisher.hpp | 6 +----- rclcpp/src/rclcpp/publisher_base.cpp | 2 +- rclcpp/test/rclcpp/test_publisher.cpp | 4 +--- 3 files changed, 3 insertions(+), 9 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 43fa4ef6a2..f4627cc96c 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -381,10 +381,6 @@ class Publisher : public PublisherBase if (!loaned_msg.is_valid()) { throw std::runtime_error("loaned message is not valid"); } - if (intra_process_is_enabled_) { - // TODO(Karsten1987): support loaned message passed by intraprocess - throw std::runtime_error("storing loaned messages in intra process is not supported yet"); - } // verify that publisher supports loaned messages // TODO(Karsten1987): This case separation has to be done in rclcpp @@ -398,7 +394,7 @@ class Publisher : public PublisherBase } else { // we don't release the ownership, let the middleware copy the ros message // and thus the destructor of rclcpp::LoanedMessage cleans up the memory. - this->do_inter_process_publish(loaned_msg.get()); + this->publish(loaned_msg.get()); } } diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index ce0540659a..93dbcf814c 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -265,7 +265,7 @@ PublisherBase::assert_liveliness() const bool PublisherBase::can_loan_messages() const { - return rcl_publisher_can_loan_messages(publisher_handle_.get()); + return !intra_process_is_enabled_ && rcl_publisher_can_loan_messages(publisher_handle_.get()); } bool diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 5cffe1d5e1..20a46194fc 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -409,9 +409,7 @@ TEST_F(TestPublisher, intra_process_publish_failures) { std::allocator allocator; { rclcpp::LoanedMessage loaned_msg(*publisher, allocator); - RCLCPP_EXPECT_THROW_EQ( - publisher->publish(std::move(loaned_msg)), - std::runtime_error("storing loaned messages in intra process is not supported yet")); + EXPECT_NO_THROW(publisher->publish(std::move(loaned_msg))); } { From 968ce0a03fdc92bc3c3b1b360421675ba3bbc191 Mon Sep 17 00:00:00 2001 From: Brian Date: Tue, 28 Feb 2023 13:43:39 -0500 Subject: [PATCH 189/455] Service introspection (#1985) * Implementation of service introspection. To do this, we add a new method on the Client and Service classes that allows the user to change the introspection method at runtime. These end up calling into the rcl layer to do the actual configuration, at which point service introspection messages will be sent as configured. Signed-off-by: Chris Lalancette Signed-off-by: Brian Chen --- rclcpp/include/rclcpp/client.hpp | 43 ++- rclcpp/include/rclcpp/service.hpp | 47 ++- rclcpp/src/rclcpp/client.cpp | 3 +- rclcpp/src/rclcpp/service.cpp | 5 +- rclcpp/test/rclcpp/CMakeLists.txt | 11 + .../rclcpp/test_service_introspection.cpp | 339 ++++++++++++++++++ 6 files changed, 430 insertions(+), 18 deletions(-) create mode 100644 rclcpp/test/rclcpp/test_service_introspection.cpp diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index c3a2210a71..f3346a067c 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -16,14 +16,15 @@ #define RCLCPP__CLIENT_HPP_ #include +#include #include -#include #include #include #include // NOLINT, cpplint doesn't think this is a cpp std header #include #include #include +#include #include #include // NOLINT #include @@ -31,8 +32,10 @@ #include "rcl/client.h" #include "rcl/error_handling.h" #include "rcl/event_callback.h" +#include "rcl/service_introspection.h" #include "rcl/wait.h" +#include "rclcpp/clock.hpp" #include "rclcpp/detail/cpp_callback_trampoline.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" @@ -467,15 +470,13 @@ class Client : public ClientBase rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, const std::string & service_name, rcl_client_options_t & client_options) - : ClientBase(node_base, node_graph) + : ClientBase(node_base, node_graph), + srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle()) { - using rosidl_typesupport_cpp::get_service_type_support_handle; - auto service_type_support_handle = - get_service_type_support_handle(); rcl_ret_t ret = rcl_client_init( this->get_client_handle().get(), this->get_rcl_node_handle(), - service_type_support_handle, + srv_type_support_handle_, service_name.c_str(), &client_options); if (ret != RCL_RET_OK) { @@ -781,6 +782,33 @@ class Client : public ClientBase return old_size - pending_requests_.size(); } + /// Configure client introspection. + /** + * \param[in] clock clock to use to generate introspection timestamps + * \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher + * \param[in] introspection_state the state to set introspection to + */ + void + configure_introspection( + Clock::SharedPtr clock, const QoS & qos_service_event_pub, + rcl_service_introspection_state_t introspection_state) + { + rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options(); + pub_opts.qos = qos_service_event_pub.get_rmw_qos_profile(); + + rcl_ret_t ret = rcl_client_configure_service_introspection( + client_handle_.get(), + node_handle_.get(), + clock->get_clock_handle(), + srv_type_support_handle_, + pub_opts, + introspection_state); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure client introspection"); + } + } + protected: using CallbackTypeValueVariant = std::tuple; using CallbackWithRequestTypeValueVariant = std::tuple< @@ -831,6 +859,9 @@ class Client : public ClientBase CallbackInfoVariant>> pending_requests_; std::mutex pending_requests_mutex_; + +private: + const rosidl_service_type_support_t * srv_type_support_handle_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 9e258a0223..3cfc11e9ca 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -26,6 +26,7 @@ #include "rcl/error_handling.h" #include "rcl/event_callback.h" #include "rcl/service.h" +#include "rcl/service_introspection.h" #include "rmw/error_handling.h" #include "rmw/impl/cpp/demangle.hpp" @@ -34,6 +35,7 @@ #include "tracetools/tracetools.h" #include "rclcpp/any_service_callback.hpp" +#include "rclcpp/clock.hpp" #include "rclcpp/detail/cpp_callback_trampoline.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" @@ -308,11 +310,9 @@ class Service const std::string & service_name, AnyServiceCallback any_callback, rcl_service_options_t & service_options) - : ServiceBase(node_handle), any_callback_(any_callback) + : ServiceBase(node_handle), any_callback_(any_callback), + srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle()) { - using rosidl_typesupport_cpp::get_service_type_support_handle; - auto service_type_support_handle = get_service_type_support_handle(); - // rcl does the static memory allocation here service_handle_ = std::shared_ptr( new rcl_service_t, [handle = node_handle_, service_name](rcl_service_t * service) @@ -331,7 +331,7 @@ class Service rcl_ret_t ret = rcl_service_init( service_handle_.get(), node_handle.get(), - service_type_support_handle, + srv_type_support_handle_, service_name.c_str(), &service_options); if (ret != RCL_RET_OK) { @@ -371,8 +371,8 @@ class Service std::shared_ptr node_handle, std::shared_ptr service_handle, AnyServiceCallback any_callback) - : ServiceBase(node_handle), - any_callback_(any_callback) + : ServiceBase(node_handle), any_callback_(any_callback), + srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle()) { // check if service handle was initialized if (!rcl_service_is_valid(service_handle.get())) { @@ -406,8 +406,8 @@ class Service std::shared_ptr node_handle, rcl_service_t * service_handle, AnyServiceCallback any_callback) - : ServiceBase(node_handle), - any_callback_(any_callback) + : ServiceBase(node_handle), any_callback_(any_callback), + srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle()) { // check if service handle was initialized if (!rcl_service_is_valid(service_handle)) { @@ -487,10 +487,39 @@ class Service } } + /// Configure client introspection. + /** + * \param[in] clock clock to use to generate introspection timestamps + * \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher + * \param[in] introspection_state the state to set introspection to + */ + void + configure_introspection( + Clock::SharedPtr clock, const QoS & qos_service_event_pub, + rcl_service_introspection_state_t introspection_state) + { + rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options(); + pub_opts.qos = qos_service_event_pub.get_rmw_qos_profile(); + + rcl_ret_t ret = rcl_service_configure_service_introspection( + service_handle_.get(), + node_handle_.get(), + clock->get_clock_handle(), + srv_type_support_handle_, + pub_opts, + introspection_state); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure service introspection"); + } + } + private: RCLCPP_DISABLE_COPY(Service) AnyServiceCallback any_callback_; + + const rosidl_service_type_support_t * srv_type_support_handle_; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 4adc6d4a96..e33db71ce2 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -23,9 +23,11 @@ #include "rcl/graph.h" #include "rcl/node.h" #include "rcl/wait.h" + #include "rclcpp/exceptions.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/logging.hpp" @@ -241,7 +243,6 @@ ClientBase::set_on_new_response_callback(rcl_event_callback_t callback, const vo user_data); if (RCL_RET_OK != ret) { - 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/service.cpp b/rclcpp/src/rclcpp/service.cpp index ea1a83a2a6..9c246e4b6b 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -22,6 +22,7 @@ #include "rclcpp/any_service_callback.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/qos.hpp" #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -131,7 +132,7 @@ ServiceBase::set_on_new_request_callback(rcl_event_callback_t callback, const vo user_data); if (RCL_RET_OK != ret) { - using rclcpp::exceptions::throw_from_rcl_error; - throw_from_rcl_error(ret, "failed to set the on new request callback for service"); + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to set the on new request callback for service"); } } diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 1e3b7a65ca..0399f3ae11 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -508,6 +508,17 @@ if(TARGET test_service) ) target_link_libraries(test_service ${PROJECT_NAME} mimick) endif() +ament_add_gmock(test_service_introspection test_service_introspection.cpp) +if(TARGET test_service) + ament_target_dependencies(test_service_introspection + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + target_link_libraries(test_service_introspection ${PROJECT_NAME} mimick) +endif() # Creating and destroying nodes is slow with Connext, so this needs larger timeout. ament_add_gtest(test_subscription test_subscription.cpp TIMEOUT 120) if(TARGET test_subscription) diff --git a/rclcpp/test/rclcpp/test_service_introspection.cpp b/rclcpp/test/rclcpp/test_service_introspection.cpp new file mode 100644 index 0000000000..94c94a58ce --- /dev/null +++ b/rclcpp/test/rclcpp/test_service_introspection.cpp @@ -0,0 +1,339 @@ +// Copyright 2022 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. + +#include +#include + +#include +#include + +#include "gmock/gmock.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/node_options.hpp" +#include "rclcpp/parameter.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/srv/basic_types.hpp" +#include "service_msgs/msg/service_event_info.hpp" + +using namespace std::chrono_literals; +using test_msgs::srv::BasicTypes; +using service_msgs::msg::ServiceEventInfo; + + +class TestServiceIntrospection : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared( + "my_node", "/ns"); + + auto srv_callback = + [](const BasicTypes::Request::SharedPtr & req, const BasicTypes::Response::SharedPtr & resp) { + resp->set__bool_value(!req->bool_value); + resp->set__int64_value(req->int64_value); + return resp; + }; + + auto callback = [this](const std::shared_ptr & msg) { + events.push_back(msg); + (void)msg; + }; + + client = node->create_client("service"); + service = node->create_service("service", srv_callback); + sub = node->create_subscription("service/_service_event", 10, callback); + events.clear(); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::Client::SharedPtr client; + rclcpp::Service::SharedPtr service; + rclcpp::Subscription::SharedPtr sub; + std::vector> events; + std::chrono::milliseconds timeout = std::chrono::milliseconds(1000); +}; + +TEST_F(TestServiceIntrospection, service_introspection_nominal) +{ + auto request = std::make_shared(); + request->set__bool_value(true); + request->set__int64_value(42); + + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS); + + auto future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + + BasicTypes::Response::SharedPtr response = future.get(); + ASSERT_EQ(response->bool_value, false); + ASSERT_EQ(response->int64_value, 42); + + // wrap up work to get all the service_event messages + auto start = std::chrono::steady_clock::now(); + while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + + std::map> event_map; + for (auto & event : events) { + event_map[event->info.event_type] = event; + } + ASSERT_EQ(event_map.size(), 4U); + + rmw_gid_t client_gid; + rmw_get_gid_for_client(rcl_client_get_rmw_handle(client->get_client_handle().get()), &client_gid); + std::array client_gid_arr; + std::move(std::begin(client_gid.data), std::end(client_gid.data), client_gid_arr.begin()); + ASSERT_THAT( + client_gid_arr, + testing::Eq(event_map[ServiceEventInfo::REQUEST_SENT]->info.client_gid)); + + ASSERT_EQ( + event_map[ServiceEventInfo::REQUEST_SENT]->info.sequence_number, + event_map[ServiceEventInfo::REQUEST_RECEIVED]->info.sequence_number); + ASSERT_EQ( + event_map[ServiceEventInfo::RESPONSE_SENT]->info.sequence_number, + event_map[ServiceEventInfo::RESPONSE_RECEIVED]->info.sequence_number); + ASSERT_EQ( + event_map[ServiceEventInfo::REQUEST_SENT]->info.sequence_number, + event_map[ServiceEventInfo::RESPONSE_SENT]->info.sequence_number); + ASSERT_EQ( + event_map[ServiceEventInfo::REQUEST_RECEIVED]->info.sequence_number, + event_map[ServiceEventInfo::RESPONSE_RECEIVED]->info.sequence_number); + + ASSERT_EQ(event_map[ServiceEventInfo::REQUEST_SENT]->request[0].int64_value, 42); + ASSERT_EQ(event_map[ServiceEventInfo::REQUEST_SENT]->request[0].bool_value, true); + ASSERT_EQ(event_map[ServiceEventInfo::RESPONSE_SENT]->response[0].int64_value, 42); + ASSERT_EQ(event_map[ServiceEventInfo::RESPONSE_SENT]->response[0].bool_value, false); + ASSERT_EQ(event_map[ServiceEventInfo::RESPONSE_SENT]->request.size(), 0U); + ASSERT_EQ(event_map[ServiceEventInfo::REQUEST_RECEIVED]->response.size(), 0U); +} + +TEST_F(TestServiceIntrospection, service_introspection_enable_disable_events) +{ + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_OFF); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_OFF); + + auto request = std::make_shared(); + request->set__bool_value(true); + request->set__int64_value(42); + auto future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + auto start = std::chrono::steady_clock::now(); + while ((std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 0U); + + events.clear(); + + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_OFF); + + future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + start = std::chrono::steady_clock::now(); + while (events.size() < 2 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 2U); + + events.clear(); + + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_OFF); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + + future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + start = std::chrono::steady_clock::now(); + while (events.size() < 2 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 2U); + + events.clear(); + + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + + future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + start = std::chrono::steady_clock::now(); + while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 4U); +} + +TEST_F(TestServiceIntrospection, service_introspection_enable_disable_event_content) +{ + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + + auto request = std::make_shared(); + request->set__bool_value(true); + request->set__int64_value(42); + auto future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + auto start = std::chrono::steady_clock::now(); + while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 4U); + for (const auto & event : events) { + EXPECT_EQ(event->request.size(), 0U); + EXPECT_EQ(event->response.size(), 0U); + } + + events.clear(); + + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + + future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + start = std::chrono::steady_clock::now(); + while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 4U); + for (const auto & event : events) { + switch (event->info.event_type) { + case ServiceEventInfo::REQUEST_SENT: + EXPECT_EQ(event->request.size(), 1U); + break; + case ServiceEventInfo::REQUEST_RECEIVED: + EXPECT_EQ(event->request.size(), 0U); + break; + case ServiceEventInfo::RESPONSE_SENT: + EXPECT_EQ(event->response.size(), 0U); + break; + case ServiceEventInfo::RESPONSE_RECEIVED: + EXPECT_EQ(event->response.size(), 1U); + break; + } + } + + events.clear(); + + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS); + + future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + start = std::chrono::steady_clock::now(); + while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 4U); + for (const auto & event : events) { + switch (event->info.event_type) { + case ServiceEventInfo::REQUEST_SENT: + EXPECT_EQ(event->request.size(), 0U); + break; + case ServiceEventInfo::REQUEST_RECEIVED: + EXPECT_EQ(event->request.size(), 1U); + break; + case ServiceEventInfo::RESPONSE_SENT: + EXPECT_EQ(event->response.size(), 1U); + break; + case ServiceEventInfo::RESPONSE_RECEIVED: + EXPECT_EQ(event->response.size(), 0U); + break; + } + } + + events.clear(); + + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS); + + future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + start = std::chrono::steady_clock::now(); + while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 4U); + for (const auto & event : events) { + switch (event->info.event_type) { + case ServiceEventInfo::REQUEST_SENT: + case ServiceEventInfo::REQUEST_RECEIVED: + EXPECT_EQ(event->request.size(), 1U); + break; + case ServiceEventInfo::RESPONSE_SENT: + case ServiceEventInfo::RESPONSE_RECEIVED: + EXPECT_EQ(event->response.size(), 1U); + break; + } + } +} From 72c05ecee023b73f8bd687349c879d1d86df9f9e Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Tue, 28 Feb 2023 14:42:39 -0800 Subject: [PATCH 190/455] Fix memory leak in tracetools::get_symbol() (#2104) Signed-off-by: Christophe Bedard --- rclcpp/include/rclcpp/any_service_callback.hpp | 12 ++++++++---- .../include/rclcpp/any_subscription_callback.hpp | 12 ++++++++---- rclcpp/include/rclcpp/timer.hpp | 14 ++++++++++---- 3 files changed, 26 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp/include/rclcpp/any_service_callback.hpp index 10aef7d0ee..f8c2592fc5 100644 --- a/rclcpp/include/rclcpp/any_service_callback.hpp +++ b/rclcpp/include/rclcpp/any_service_callback.hpp @@ -191,10 +191,14 @@ class AnyServiceCallback #ifndef TRACETOOLS_DISABLED std::visit( [this](auto && arg) { - TRACEPOINT( - rclcpp_callback_register, - static_cast(this), - tracetools::get_symbol(arg)); + if (TRACEPOINT_ENABLED(rclcpp_callback_register)) { + char * symbol = tracetools::get_symbol(arg); + DO_TRACEPOINT( + rclcpp_callback_register, + static_cast(this), + symbol); + std::free(symbol); + } }, callback_); #endif // TRACETOOLS_DISABLED } diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index d4f5fc309b..65b29d8535 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -965,10 +965,14 @@ class AnySubscriptionCallback #ifndef TRACETOOLS_DISABLED std::visit( [this](auto && callback) { - TRACEPOINT( - rclcpp_callback_register, - static_cast(this), - tracetools::get_symbol(callback)); + if (TRACEPOINT_ENABLED(rclcpp_callback_register)) { + char * symbol = tracetools::get_symbol(callback); + DO_TRACEPOINT( + rclcpp_callback_register, + static_cast(this), + symbol); + std::free(symbol); + } }, callback_variant_); #endif // TRACETOOLS_DISABLED } diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 8af0c6fb68..91b1705985 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -227,10 +227,16 @@ class GenericTimer : public TimerBase rclcpp_timer_callback_added, static_cast(get_timer_handle().get()), reinterpret_cast(&callback_)); - TRACEPOINT( - rclcpp_callback_register, - reinterpret_cast(&callback_), - tracetools::get_symbol(callback_)); +#ifndef TRACETOOLS_DISABLED + if (TRACEPOINT_ENABLED(rclcpp_callback_register)) { + char * symbol = tracetools::get_symbol(callback_); + DO_TRACEPOINT( + rclcpp_callback_register, + reinterpret_cast(&callback_), + symbol); + std::free(symbol); + } +#endif } /// Default destructor. From b589b490c3864f1359dddd0d1c0338ee0778becd Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 1 Mar 2023 14:27:01 +0000 Subject: [PATCH 191/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 8 ++++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 3 +++ 4 files changed, 17 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index f7788aea69..90f1e10b5c 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix memory leak in tracetools::get_symbol() (`#2104 `_) +* Service introspection (`#1985 `_) +* Allow publishing borrowed messages with intra-process enabled (`#2108 `_) +* to fix flaky test about TestTimeSource.callbacks (`#2111 `_) +* Contributors: Brian, Chen Lihui, Christophe Bedard, Miguel Company + 19.2.0 (2023-02-24) ------------------- * to create a sublogger while getting child of Logger (`#1717 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 1e7a39a2d3..8b2c5a3086 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 19.2.0 (2023-02-24) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 00d8920ad3..d0baa80fa0 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 19.2.0 (2023-02-24) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index bd39d5029e..3824e65ee4 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 19.2.0 (2023-02-24) ------------------- From e7890b7c621717fe53207a181fb5cc034cb304a4 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 1 Mar 2023 14:27:21 +0000 Subject: [PATCH 192/455] 19.3.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 90f1e10b5c..746eddda33 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +19.3.0 (2023-03-01) +------------------- * Fix memory leak in tracetools::get_symbol() (`#2104 `_) * Service introspection (`#1985 `_) * Allow publishing borrowed messages with intra-process enabled (`#2108 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index c339f48435..38773e2ec2 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 19.2.0 + 19.3.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 8b2c5a3086..ca4799f69c 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +19.3.0 (2023-03-01) +------------------- 19.2.0 (2023-02-24) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index f4594f83ad..d2b51621e5 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 19.2.0 + 19.3.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index d0baa80fa0..77c1c2e390 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +19.3.0 (2023-03-01) +------------------- 19.2.0 (2023-02-24) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index d1d37829a9..ef51696cb3 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 19.2.0 + 19.3.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 3824e65ee4..33ec6af902 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +19.3.0 (2023-03-01) +------------------- 19.2.0 (2023-02-24) ------------------- diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 8c67bd908b..dc52a2a723 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 19.2.0 + 19.3.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 399f4df7396d67b42ccaea651bbd87d66b0d62b3 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Thu, 2 Mar 2023 07:45:22 -0500 Subject: [PATCH 193/455] Fix the GoalUUID to_string representation (#1999) * Fix expected results of the goal_uuid_to_string test Signed-off-by: Nathan Wiebe Neufeldt Co-authored-by: Chris Lalancette --- rclcpp_action/include/rclcpp_action/types.hpp | 2 +- rclcpp_action/src/types.cpp | 24 ++++++++++++------- rclcpp_action/test/test_types.cpp | 8 +++---- 3 files changed, 21 insertions(+), 13 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index 4f8548ff76..829b6cd2c1 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -34,7 +34,7 @@ using GoalUUID = std::array; using GoalStatus = action_msgs::msg::GoalStatus; using GoalInfo = action_msgs::msg::GoalInfo; -/// Convert a goal id to a human readable string. +/// Convert a goal id to a human readable RFC-4122 compliant string. RCLCPP_ACTION_PUBLIC std::string to_string(const GoalUUID & goal_id); diff --git a/rclcpp_action/src/types.cpp b/rclcpp_action/src/types.cpp index 773702789e..7f2a71d688 100644 --- a/rclcpp_action/src/types.cpp +++ b/rclcpp_action/src/types.cpp @@ -15,25 +15,33 @@ #include "rclcpp_action/types.hpp" #include -#include namespace rclcpp_action { std::string to_string(const GoalUUID & goal_id) { - std::stringstream stream; - stream << std::hex; - for (const auto & element : goal_id) { - stream << static_cast(element); + constexpr char HEX[] = "0123456789abcdef"; + std::string result; + result.resize(36); + size_t i = 0; + for (uint8_t byte : goal_id) { + result[i++] = HEX[byte >> 4]; + result[i++] = HEX[byte & 0x0f]; + // A RFC-4122 compliant UUID looks like: + // 00000000-0000-0000-0000-000000000000 + // That means that there is a '-' at offset 8, 13, 18, and 23 + if (i == 8 || i == 13 || i == 18 || i == 23) { + result[i++] = '-'; + } } - return stream.str(); + return result; } void convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info) { - for (size_t i = 0; i < 16; ++i) { + for (size_t i = 0; i < UUID_SIZE; ++i) { info->goal_id.uuid[i] = goal_id[i]; } } @@ -41,7 +49,7 @@ convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info) void convert(const rcl_action_goal_info_t & info, GoalUUID * goal_id) { - for (size_t i = 0; i < 16; ++i) { + for (size_t i = 0; i < UUID_SIZE; ++i) { (*goal_id)[i] = info.goal_id.uuid[i]; } } diff --git a/rclcpp_action/test/test_types.cpp b/rclcpp_action/test/test_types.cpp index 7c652aaad6..4922c52930 100644 --- a/rclcpp_action/test/test_types.cpp +++ b/rclcpp_action/test/test_types.cpp @@ -22,17 +22,17 @@ TEST(TestActionTypes, goal_uuid_to_string) { for (uint8_t i = 0; i < UUID_SIZE; ++i) { goal_id[i] = i; } - EXPECT_STREQ("0123456789abcdef", rclcpp_action::to_string(goal_id).c_str()); + EXPECT_STREQ("00010203-0405-0607-0809-0a0b0c0d0e0f", rclcpp_action::to_string(goal_id).c_str()); for (uint8_t i = 0; i < UUID_SIZE; ++i) { goal_id[i] = static_cast(16u + i); } - EXPECT_STREQ("101112131415161718191a1b1c1d1e1f", rclcpp_action::to_string(goal_id).c_str()); + EXPECT_STREQ("10111213-1415-1617-1819-1a1b1c1d1e1f", rclcpp_action::to_string(goal_id).c_str()); for (uint8_t i = 0; i < UUID_SIZE; ++i) { goal_id[i] = static_cast(std::numeric_limits::max() - i); } - EXPECT_STREQ("fffefdfcfbfaf9f8f7f6f5f4f3f2f1f0", rclcpp_action::to_string(goal_id).c_str()); + EXPECT_STREQ("fffefdfc-fbfa-f9f8-f7f6-f5f4f3f2f1f0", rclcpp_action::to_string(goal_id).c_str()); } TEST(TestActionTypes, goal_uuid_to_rcl_action_goal_info) { @@ -54,7 +54,7 @@ TEST(TestActionTypes, rcl_action_goal_info_to_goal_uuid) { } rclcpp_action::GoalUUID goal_id; - rclcpp_action::convert(goal_id, &goal_info); + rclcpp_action::convert(goal_info, &goal_id); for (uint8_t i = 0; i < UUID_SIZE; ++i) { EXPECT_EQ(goal_info.goal_id.uuid[i], goal_id[i]); } From 11778f5048dc0d533953dae02b1ec252f36bb578 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Sat, 4 Mar 2023 13:13:56 -0800 Subject: [PATCH 194/455] add get_fully_qualified_name to rclcpp_lifecycle (#2115) Signed-off-by: stevemacenski --- .../include/rclcpp_lifecycle/lifecycle_node.hpp | 9 +++++++++ rclcpp_lifecycle/src/lifecycle_node.cpp | 6 ++++++ rclcpp_lifecycle/test/test_lifecycle_node.cpp | 1 + 3 files changed, 16 insertions(+) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 50d4717ec3..9fec474710 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -172,6 +172,15 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, const char * get_namespace() const; + /// Get the fully-qualified name of the node. + /** + * The fully-qualified name includes the local namespace and name of the node. + * \return fully-qualified name of the node. + */ + RCLCPP_PUBLIC + const char * + get_fully_qualified_name() const; + /// Get the logger of the node. /** * \return The logger of the node. diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 723c7bc8c1..a57a95e6be 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -165,6 +165,12 @@ LifecycleNode::get_namespace() const return node_base_->get_namespace(); } +const char * +LifecycleNode::get_fully_qualified_name() const +{ + return node_base_->get_fully_qualified_name(); +} + rclcpp::Logger LifecycleNode::get_logger() const { diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index e1863a4d39..31850c589b 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -229,6 +229,7 @@ TEST_F(TestDefaultStateMachine, empty_initializer) { auto test_node = std::make_shared("testnode"); EXPECT_STREQ("testnode", test_node->get_name()); EXPECT_STREQ("/", test_node->get_namespace()); + EXPECT_STREQ("/testnode", test_node->get_fully_qualified_name()); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); } From 1a9b117d5357f4d37ecd457843ee27e8ba56b65c Mon Sep 17 00:00:00 2001 From: mauropasse Date: Sun, 5 Mar 2023 17:53:14 +0000 Subject: [PATCH 195/455] Fix clang warning: bugprone-use-after-move (#2116) Signed-off-by: Mauro Passerino Co-authored-by: Mauro Passerino --- rclcpp/include/rclcpp/experimental/intra_process_manager.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index d8054be2d4..11f2dda6a4 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -454,6 +454,8 @@ class IntraProcessManager if (std::next(it) == subscription_ids.end()) { // If this is the last subscription, give up ownership subscription->provide_intra_process_data(std::move(message)); + // Last message delivered, break from for loop + break; } else { // Copy the message since we have additional subscriptions to serve Deleter deleter = message.get_deleter(); @@ -493,6 +495,8 @@ class IntraProcessManager if (std::next(it) == subscription_ids.end()) { // If this is the last subscription, give up ownership ros_message_subscription->provide_intra_process_message(std::move(message)); + // Last message delivered, break from for loop + break; } else { // Copy the message since we have additional subscriptions to serve Deleter deleter = message.get_deleter(); From dbe555a3c3470b051c5854881d464965a1d6c8a7 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 6 Mar 2023 12:45:24 -0500 Subject: [PATCH 196/455] Use the correct macro for LifecycleNode::get_fully_qualified_name (#2117) Signed-off-by: Chris Lalancette --- rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 9fec474710..197ecf5c19 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -177,7 +177,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, * The fully-qualified name includes the local namespace and name of the node. * \return fully-qualified name of the node. */ - RCLCPP_PUBLIC + RCLCPP_LIFECYCLE_PUBLIC const char * get_fully_qualified_name() const; From b6a803f48ce4c372363596afd8555ccfaa2c40b8 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 7 Mar 2023 14:43:58 -0500 Subject: [PATCH 197/455] Update all rclcpp packages to C++17. (#2121) The main reason to do this is so that we can compile rclcpp with the clang static analyzer. As of clang++-14 (what is in Ubuntu 22.04), the default still seems to be C++14, so we need to specify C++17 so that new things in the rclcpp headers work properly. Further, due to reasons I don't fully understand, I needed to set CMAKE_CXX_STANDARD_REQUIRED in order for clang to really use that version. So set this as well. Signed-off-by: Chris Lalancette --- rclcpp/CMakeLists.txt | 1 + rclcpp_action/CMakeLists.txt | 5 +++-- rclcpp_components/CMakeLists.txt | 5 +++-- rclcpp_lifecycle/CMakeLists.txt | 5 +++-- 4 files changed, 10 insertions(+), 6 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 3abd999f3d..8d186ad27a 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -25,6 +25,7 @@ find_package(tracetools REQUIRED) # Default to C++17 if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") # About -Wno-sign-conversion: With Clang, -Wconversion implies -Wsign-conversion. There are a number of diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index 353e179244..785ed0e5e1 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -9,9 +9,10 @@ find_package(rcl_action REQUIRED) find_package(rcpputils REQUIRED) find_package(rosidl_runtime_c REQUIRED) -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options( diff --git a/rclcpp_components/CMakeLists.txt b/rclcpp_components/CMakeLists.txt index 0b9cda7803..121ef434c2 100644 --- a/rclcpp_components/CMakeLists.txt +++ b/rclcpp_components/CMakeLists.txt @@ -2,9 +2,10 @@ cmake_minimum_required(VERSION 3.5) project(rclcpp_components) -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options( diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index b3461a7333..f46f993b21 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -2,9 +2,10 @@ cmake_minimum_required(VERSION 3.5) project(rclcpp_lifecycle) -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual) From 6c4afb3a702617d263079bf9a8c304fc4e0f34b3 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 13 Mar 2023 09:33:30 -0400 Subject: [PATCH 198/455] Hook up the incompatible type event inside of rclcpp (#2069) * Rename QOSEventHandler* to EventHandler. We are going to be using it for more than just QOS events, so rename it to just EventHandler and EventHandlerBase for now. Leave the old names in place but deprecated. * Rename qos_event.hpp -> event_handler.hpp * Revamp incompatible_qos callback setting. * Add in incompatible type implementation. Signed-off-by: Chris Lalancette --- rclcpp/CMakeLists.txt | 2 +- rclcpp/include/rclcpp/event_handler.hpp | 306 ++++++++++++++++++ rclcpp/include/rclcpp/publisher_base.hpp | 13 +- rclcpp/include/rclcpp/publisher_options.hpp | 2 +- rclcpp/include/rclcpp/qos_event.hpp | 275 +--------------- rclcpp/include/rclcpp/subscription_base.hpp | 15 +- .../include/rclcpp/subscription_options.hpp | 2 +- rclcpp/include/rclcpp/wait_set_template.hpp | 2 +- .../{qos_event.cpp => event_handler.cpp} | 18 +- rclcpp/src/rclcpp/publisher_base.cpp | 59 +++- rclcpp/src/rclcpp/subscription_base.cpp | 58 +++- .../test_allocator_memory_strategy.cpp | 4 +- rclcpp/test/rclcpp/test_qos_event.cpp | 8 +- 13 files changed, 437 insertions(+), 327 deletions(-) create mode 100644 rclcpp/include/rclcpp/event_handler.hpp rename rclcpp/src/rclcpp/{qos_event.cpp => event_handler.cpp} (86%) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 8d186ad27a..6979d980f8 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -93,7 +93,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/parameter_value.cpp src/rclcpp/publisher_base.cpp src/rclcpp/qos.cpp - src/rclcpp/qos_event.cpp + src/rclcpp/event_handler.cpp src/rclcpp/qos_overriding_options.cpp src/rclcpp/serialization.cpp src/rclcpp/serialized_message.cpp diff --git a/rclcpp/include/rclcpp/event_handler.hpp b/rclcpp/include/rclcpp/event_handler.hpp new file mode 100644 index 0000000000..af5d8c08dc --- /dev/null +++ b/rclcpp/include/rclcpp/event_handler.hpp @@ -0,0 +1,306 @@ +// Copyright 2019 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__EVENT_HANDLER_HPP_ +#define RCLCPP__EVENT_HANDLER_HPP_ + +#include +#include +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/event_callback.h" +#include "rmw/impl/cpp/demangle.hpp" +#include "rmw/incompatible_qos_events_statuses.h" +#include "rmw/events_statuses/incompatible_type.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 +{ + +using QOSDeadlineRequestedInfo = rmw_requested_deadline_missed_status_t; +using QOSDeadlineOfferedInfo = rmw_offered_deadline_missed_status_t; +using QOSLivelinessChangedInfo = rmw_liveliness_changed_status_t; +using QOSLivelinessLostInfo = rmw_liveliness_lost_status_t; +using QOSMessageLostInfo = rmw_message_lost_status_t; +using QOSOfferedIncompatibleQoSInfo = rmw_offered_qos_incompatible_event_status_t; +using QOSRequestedIncompatibleQoSInfo = rmw_requested_qos_incompatible_event_status_t; + +using IncompatibleTypeInfo = rmw_incompatible_type_status_t; + +using QOSDeadlineRequestedCallbackType = std::function; +using QOSDeadlineOfferedCallbackType = std::function; +using QOSLivelinessChangedCallbackType = std::function; +using QOSLivelinessLostCallbackType = std::function; +using QOSMessageLostCallbackType = std::function; +using QOSOfferedIncompatibleQoSCallbackType = std::function; +using QOSRequestedIncompatibleQoSCallbackType = + std::function; + +using IncompatibleTypeCallbackType = std::function; + +/// Contains callbacks for various types of events a Publisher can receive from the middleware. +struct PublisherEventCallbacks +{ + QOSDeadlineOfferedCallbackType deadline_callback; + QOSLivelinessLostCallbackType liveliness_callback; + QOSOfferedIncompatibleQoSCallbackType incompatible_qos_callback; + IncompatibleTypeCallbackType incompatible_type_callback; +}; + +/// Contains callbacks for non-message events that a Subscription can receive from the middleware. +struct SubscriptionEventCallbacks +{ + QOSDeadlineRequestedCallbackType deadline_callback; + QOSLivelinessChangedCallbackType liveliness_callback; + QOSRequestedIncompatibleQoSCallbackType incompatible_qos_callback; + QOSMessageLostCallbackType message_lost_callback; + IncompatibleTypeCallbackType incompatible_type_callback; +}; + +class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public std::runtime_error +{ +public: + RCLCPP_PUBLIC + UnsupportedEventTypeException( + rcl_ret_t ret, + const rcl_error_state_t * error_state, + const std::string & prefix); + + RCLCPP_PUBLIC + UnsupportedEventTypeException( + const exceptions::RCLErrorBase & base_exc, + const std::string & prefix); +}; + +class EventHandlerBase : public Waitable +{ +public: + enum class EntityType : std::size_t + { + Event, + }; + + RCLCPP_PUBLIC + virtual ~EventHandlerBase(); + + /// Get the number of ready events + RCLCPP_PUBLIC + size_t + get_number_of_ready_events() override; + + /// Add the Waitable to a wait set. + RCLCPP_PUBLIC + void + 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; + + /// 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. + * + * 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 + * 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. + * + * 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 rmw_event_set_callback + * \sa rcl_event_set_callback + * + * \param[in] callback functor to be called when a new event occurs + */ + 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::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::EventHandlerBase@" << 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::EventHandlerBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on ready' callback"); + } + }; + + 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 + // 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< + decltype(on_new_event_callback_), const void *, size_t>, + static_cast(&on_new_event_callback_)); + } + + /// Unset the callback registered for new events, if any. + void + clear_on_ready_callback() override + { + std::lock_guard lock(callback_mutex_); + if (on_new_event_callback_) { + set_on_new_event_callback(nullptr, nullptr); + on_new_event_callback_ = nullptr; + } + } + +protected: + RCLCPP_PUBLIC + void + set_on_new_event_callback(rcl_event_callback_t callback, const void * user_data); + + std::recursive_mutex callback_mutex_; + std::function on_new_event_callback_{nullptr}; + + rcl_event_t event_handle_; + size_t wait_set_event_index_; +}; + +using QOSEventHandlerBase [[deprecated("Use rclcpp::EventHandlerBase")]] = EventHandlerBase; + +template +class EventHandler : public EventHandlerBase +{ +public: + template + EventHandler( + const EventCallbackT & callback, + InitFuncT init_func, + ParentHandleT parent_handle, + EventTypeEnum event_type) + : parent_handle_(parent_handle), event_callback_(callback) + { + 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) { + if (ret == RCL_RET_UNSUPPORTED) { + UnsupportedEventTypeException exc(ret, rcl_get_error_state(), "Failed to initialize event"); + rcl_reset_error(); + throw exc; + } else { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to initialize event"); + } + } + } + + /// Take data so that the callback cannot be scheduled again + std::shared_ptr + take_data() override + { + EventCallbackInfoT callback_info; + rcl_ret_t ret = rcl_take_event(&event_handle_, &callback_info); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't take event info: %s", rcl_get_error_string().str); + return nullptr; + } + return std::static_pointer_cast(std::make_shared(callback_info)); + } + + std::shared_ptr + take_data_by_entity_id(size_t id) override + { + (void)id; + return take_data(); + } + + /// Execute any entities of the Waitable that are ready. + void + execute(std::shared_ptr & data) override + { + if (!data) { + throw std::runtime_error("'data' is empty"); + } + auto callback_ptr = std::static_pointer_cast(data); + event_callback_(*callback_ptr); + callback_ptr.reset(); + } + +private: + using EventCallbackInfoT = typename std::remove_reference::template argument_type<0>>::type; + + ParentHandleT parent_handle_; + EventCallbackT event_callback_; +}; + +template +using QOSEventHandler [[deprecated("Use rclcpp::EventHandler")]] = EventHandler; + +} // namespace rclcpp + +#endif // RCLCPP__EVENT_HANDLER_HPP_ diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index 153d5a6ebe..d9181bea43 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -33,7 +33,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/network_flow_endpoint.hpp" #include "rclcpp/qos.hpp" -#include "rclcpp/qos_event.hpp" +#include "rclcpp/event_handler.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" #include "rcpputils/time.hpp" @@ -124,7 +124,7 @@ class PublisherBase : public std::enable_shared_from_this /** \return The map of QoS event handlers. */ RCLCPP_PUBLIC const - std::unordered_map> & + std::unordered_map> & get_event_handlers() const; /// Get subscription count @@ -276,7 +276,7 @@ class PublisherBase : public std::enable_shared_from_this * 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 + * \sa rclcpp::EventHandlerBase::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 @@ -327,7 +327,7 @@ class PublisherBase : public std::enable_shared_from_this const EventCallbackT & callback, const rcl_publisher_event_type_t event_type) { - auto handler = std::make_shared>>( callback, rcl_publisher_event_init, @@ -339,12 +339,15 @@ class PublisherBase : public std::enable_shared_from_this RCLCPP_PUBLIC void default_incompatible_qos_callback(QOSOfferedIncompatibleQoSInfo & info) const; + RCLCPP_PUBLIC + void default_incompatible_type_callback(IncompatibleTypeInfo & info) const; + std::shared_ptr rcl_node_handle_; std::shared_ptr publisher_handle_; std::unordered_map> event_handlers_; + std::shared_ptr> event_handlers_; using IntraProcessManagerWeakPtr = std::weak_ptr; diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index 3c88ebccd1..3501834dd1 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -26,7 +26,7 @@ #include "rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp" #include "rclcpp/intra_process_setting.hpp" #include "rclcpp/qos.hpp" -#include "rclcpp/qos_event.hpp" +#include "rclcpp/event_handler.hpp" #include "rclcpp/qos_overriding_options.hpp" namespace rclcpp diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 926cc2c75e..efc0aa5739 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -15,279 +15,8 @@ #ifndef RCLCPP__QOS_EVENT_HPP_ #define RCLCPP__QOS_EVENT_HPP_ -#include -#include -#include -#include -#include +#warning This header is obsolete, please include rclcpp/event_handler.hpp instead -#include "rcl/error_handling.h" -#include "rcl/event_callback.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 -{ - -using QOSDeadlineRequestedInfo = rmw_requested_deadline_missed_status_t; -using QOSDeadlineOfferedInfo = rmw_offered_deadline_missed_status_t; -using QOSLivelinessChangedInfo = rmw_liveliness_changed_status_t; -using QOSLivelinessLostInfo = rmw_liveliness_lost_status_t; -using QOSMessageLostInfo = rmw_message_lost_status_t; -using QOSOfferedIncompatibleQoSInfo = rmw_offered_qos_incompatible_event_status_t; -using QOSRequestedIncompatibleQoSInfo = rmw_requested_qos_incompatible_event_status_t; - -using QOSDeadlineRequestedCallbackType = std::function; -using QOSDeadlineOfferedCallbackType = std::function; -using QOSLivelinessChangedCallbackType = std::function; -using QOSLivelinessLostCallbackType = std::function; -using QOSMessageLostCallbackType = std::function; -using QOSOfferedIncompatibleQoSCallbackType = std::function; -using QOSRequestedIncompatibleQoSCallbackType = - std::function; - -/// Contains callbacks for various types of events a Publisher can receive from the middleware. -struct PublisherEventCallbacks -{ - QOSDeadlineOfferedCallbackType deadline_callback; - QOSLivelinessLostCallbackType liveliness_callback; - QOSOfferedIncompatibleQoSCallbackType incompatible_qos_callback; -}; - -/// Contains callbacks for non-message events that a Subscription can receive from the middleware. -struct SubscriptionEventCallbacks -{ - QOSDeadlineRequestedCallbackType deadline_callback; - QOSLivelinessChangedCallbackType liveliness_callback; - QOSRequestedIncompatibleQoSCallbackType incompatible_qos_callback; - QOSMessageLostCallbackType message_lost_callback; -}; - -class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public std::runtime_error -{ -public: - RCLCPP_PUBLIC - UnsupportedEventTypeException( - rcl_ret_t ret, - const rcl_error_state_t * error_state, - const std::string & prefix); - - RCLCPP_PUBLIC - UnsupportedEventTypeException( - const exceptions::RCLErrorBase & base_exc, - const std::string & prefix); -}; - -class QOSEventHandlerBase : public Waitable -{ -public: - enum class EntityType : std::size_t - { - Event, - }; - - RCLCPP_PUBLIC - virtual ~QOSEventHandlerBase(); - - /// Get the number of ready events - RCLCPP_PUBLIC - size_t - get_number_of_ready_events() override; - - /// Add the Waitable to a wait set. - RCLCPP_PUBLIC - void - 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; - - /// 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. - * - * 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 - * 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. - * - * 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 rmw_event_set_callback - * \sa rcl_event_set_callback - * - * \param[in] callback functor to be called when a new event occurs - */ - 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::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 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 ready' callback"); - } - }; - - 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 - // 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< - decltype(on_new_event_callback_), const void *, size_t>, - static_cast(&on_new_event_callback_)); - } - - /// Unset the callback registered for new events, if any. - void - clear_on_ready_callback() override - { - std::lock_guard lock(callback_mutex_); - if (on_new_event_callback_) { - set_on_new_event_callback(nullptr, nullptr); - on_new_event_callback_ = nullptr; - } - } - -protected: - RCLCPP_PUBLIC - void - set_on_new_event_callback(rcl_event_callback_t callback, const void * user_data); - - std::recursive_mutex callback_mutex_; - std::function on_new_event_callback_{nullptr}; - - rcl_event_t event_handle_; - size_t wait_set_event_index_; -}; - -template -class QOSEventHandler : public QOSEventHandlerBase -{ -public: - template - QOSEventHandler( - const EventCallbackT & callback, - InitFuncT init_func, - ParentHandleT parent_handle, - EventTypeEnum event_type) - : parent_handle_(parent_handle), event_callback_(callback) - { - 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) { - if (ret == RCL_RET_UNSUPPORTED) { - UnsupportedEventTypeException exc(ret, rcl_get_error_state(), "Failed to initialize event"); - rcl_reset_error(); - throw exc; - } else { - rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to initialize event"); - } - } - } - - /// Take data so that the callback cannot be scheduled again - std::shared_ptr - take_data() override - { - EventCallbackInfoT callback_info; - rcl_ret_t ret = rcl_take_event(&event_handle_, &callback_info); - if (ret != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't take event info: %s", rcl_get_error_string().str); - return nullptr; - } - return std::static_pointer_cast(std::make_shared(callback_info)); - } - - std::shared_ptr - take_data_by_entity_id(size_t id) override - { - (void)id; - return take_data(); - } - - /// Execute any entities of the Waitable that are ready. - void - execute(std::shared_ptr & data) override - { - if (!data) { - throw std::runtime_error("'data' is empty"); - } - auto callback_ptr = std::static_pointer_cast(data); - event_callback_(*callback_ptr); - callback_ptr.reset(); - } - -private: - using EventCallbackInfoT = typename std::remove_reference::template argument_type<0>>::type; - - ParentHandleT parent_handle_; - EventCallbackT event_callback_; -}; - -} // namespace rclcpp +#include "rclcpp/event_handler.hpp" #endif // RCLCPP__QOS_EVENT_HPP_ diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index b1aeb4eb7d..52057a39d2 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -37,7 +37,7 @@ #include "rclcpp/message_info.hpp" #include "rclcpp/network_flow_endpoint.hpp" #include "rclcpp/qos.hpp" -#include "rclcpp/qos_event.hpp" +#include "rclcpp/event_handler.hpp" #include "rclcpp/serialized_message.hpp" #include "rclcpp/subscription_content_filter_options.hpp" #include "rclcpp/type_support_decl.hpp" @@ -115,7 +115,7 @@ class SubscriptionBase : public std::enable_shared_from_this /** \return The map of QoS event handlers. */ RCLCPP_PUBLIC const - std::unordered_map> & + std::unordered_map> & get_event_handlers() const; /// Get the actual QoS settings, after the defaults have been determined. @@ -457,7 +457,7 @@ class SubscriptionBase : public std::enable_shared_from_this * 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 + * \sa rclcpp::EventHandlerBase::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 @@ -542,7 +542,7 @@ class SubscriptionBase : public std::enable_shared_from_this const EventCallbackT & callback, const rcl_subscription_event_type_t event_type) { - auto handler = std::make_shared>>( callback, rcl_subscription_event_init, @@ -555,6 +555,9 @@ class SubscriptionBase : public std::enable_shared_from_this RCLCPP_PUBLIC void default_incompatible_qos_callback(QOSRequestedIncompatibleQoSInfo & info) const; + RCLCPP_PUBLIC + void default_incompatible_type_callback(IncompatibleTypeInfo & info) const; + RCLCPP_PUBLIC bool matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const; @@ -571,7 +574,7 @@ class SubscriptionBase : public std::enable_shared_from_this rclcpp::Logger node_logger_; std::unordered_map> event_handlers_; + std::shared_ptr> event_handlers_; bool use_intra_process_; IntraProcessManagerWeakPtr weak_ipm_; @@ -588,7 +591,7 @@ class SubscriptionBase : public std::enable_shared_from_this std::atomic subscription_in_use_by_wait_set_{false}; std::atomic intra_process_subscription_waitable_in_use_by_wait_set_{false}; - std::unordered_map> qos_events_in_use_by_wait_set_; std::recursive_mutex callback_mutex_; diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index c5c3e21eb1..43b9ec034c 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -26,7 +26,7 @@ #include "rclcpp/intra_process_buffer_type.hpp" #include "rclcpp/intra_process_setting.hpp" #include "rclcpp/qos.hpp" -#include "rclcpp/qos_event.hpp" +#include "rclcpp/event_handler.hpp" #include "rclcpp/qos_overriding_options.hpp" #include "rclcpp/subscription_content_filter_options.hpp" #include "rclcpp/topic_statistics_state.hpp" diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index 3654801c91..b3f41f8ed5 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -516,7 +516,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli * the waitable to be removed, but it will cause the associated entity pointer * to be nullptr when introspecting this waitable after waiting. * - * Note that rclcpp::QOSEventHandlerBase are just a special case of + * Note that rclcpp::EventHandlerBase is just a special case of * rclcpp::Waitable and can be added with this function. * * \param[in] waitable Waitable to be added. diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/event_handler.cpp similarity index 86% rename from rclcpp/src/rclcpp/qos_event.cpp rename to rclcpp/src/rclcpp/event_handler.cpp index 96e2066e91..40ae6c030d 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/event_handler.cpp @@ -12,9 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include "rclcpp/qos_event.hpp" +#include "rcl/event.h" + +#include "rclcpp/event_handler.hpp" +#include "rclcpp/exceptions/exceptions.hpp" namespace rclcpp { @@ -33,7 +37,7 @@ UnsupportedEventTypeException::UnsupportedEventTypeException( std::runtime_error(prefix + (prefix.empty() ? "" : ": ") + base_exc.formatted_message) {} -QOSEventHandlerBase::~QOSEventHandlerBase() +EventHandlerBase::~EventHandlerBase() { // Since the rmw event listener holds a reference to // this callback, we need to clear it on destruction of this class. @@ -52,14 +56,14 @@ QOSEventHandlerBase::~QOSEventHandlerBase() /// Get the number of ready events. size_t -QOSEventHandlerBase::get_number_of_ready_events() +EventHandlerBase::get_number_of_ready_events() { return 1; } /// Add the Waitable to a wait set. void -QOSEventHandlerBase::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_); if (RCL_RET_OK != ret) { @@ -69,13 +73,13 @@ QOSEventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set) /// Check if the Waitable is ready. bool -QOSEventHandlerBase::is_ready(rcl_wait_set_t * wait_set) +EventHandlerBase::is_ready(rcl_wait_set_t * wait_set) { return wait_set->events[wait_set_event_index_] == &event_handle_; } void -QOSEventHandlerBase::set_on_new_event_callback( +EventHandlerBase::set_on_new_event_callback( rcl_event_callback_t callback, const void * user_data) { @@ -86,7 +90,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 QOS Event"); + throw_from_rcl_error(ret, "failed to set the on new message callback for Event"); } } diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index 93dbcf814c..a96a6ee791 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -37,7 +37,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/network_flow_endpoint.hpp" #include "rclcpp/node.hpp" -#include "rclcpp/qos_event.hpp" +#include "rclcpp/event_handler.hpp" using rclcpp::PublisherBase; @@ -145,21 +145,43 @@ PublisherBase::bind_event_callbacks( event_callbacks.liveliness_callback, RCL_PUBLISHER_LIVELINESS_LOST); } + + QOSOfferedIncompatibleQoSCallbackType incompatible_qos_cb; if (event_callbacks.incompatible_qos_callback) { - this->add_event_handler( - event_callbacks.incompatible_qos_callback, - RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + incompatible_qos_cb = event_callbacks.incompatible_qos_callback; + } else if (use_default_callbacks) { + // Register default callback when not specified + incompatible_qos_cb = [this](QOSOfferedIncompatibleQoSInfo & info) { + this->default_incompatible_qos_callback(info); + }; + } + try { + if (incompatible_qos_cb) { + this->add_event_handler(incompatible_qos_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + } + } catch (const UnsupportedEventTypeException & /*exc*/) { + RCLCPP_DEBUG( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for incompatible qos; wrong callback type"); + } + + IncompatibleTypeCallbackType incompatible_type_cb; + if (event_callbacks.incompatible_type_callback) { + incompatible_type_cb = event_callbacks.incompatible_type_callback; } else if (use_default_callbacks) { // Register default callback when not specified - try { - this->add_event_handler( - [this](QOSOfferedIncompatibleQoSInfo & info) { - this->default_incompatible_qos_callback(info); - }, - RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); - } catch (UnsupportedEventTypeException & /*exc*/) { - // pass + incompatible_type_cb = [this](IncompatibleTypeInfo & info) { + this->default_incompatible_type_callback(info); + }; + } + try { + if (incompatible_type_cb) { + this->add_event_handler(incompatible_type_cb, RCL_PUBLISHER_INCOMPATIBLE_TYPE); } + } catch (UnsupportedEventTypeException & /*exc*/) { + RCLCPP_DEBUG( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for incompatible type; wrong callback type"); } } @@ -195,7 +217,7 @@ PublisherBase::get_publisher_handle() const } const -std::unordered_map> & +std::unordered_map> & PublisherBase::get_event_handlers() const { return event_handlers_; @@ -311,6 +333,17 @@ PublisherBase::default_incompatible_qos_callback( policy_name.c_str()); } +void +PublisherBase::default_incompatible_type_callback( + rclcpp::IncompatibleTypeInfo & event) const +{ + (void)event; + + RCLCPP_WARN( + rclcpp::get_logger(rcl_node_get_logger_name(rcl_node_handle_.get())), + "Incompatible type on topic '%s', no messages will be sent to it.", get_topic_name()); +} + std::vector PublisherBase::get_network_flow_endpoints() const { rcutils_allocator_t allocator = rcutils_get_default_allocator(); diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 0225747b3a..2302876338 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -27,7 +27,7 @@ #include "rclcpp/experimental/intra_process_manager.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" -#include "rclcpp/qos_event.hpp" +#include "rclcpp/event_handler.hpp" #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -114,27 +114,48 @@ SubscriptionBase::bind_event_callbacks( event_callbacks.deadline_callback, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); } + if (event_callbacks.liveliness_callback) { this->add_event_handler( event_callbacks.liveliness_callback, RCL_SUBSCRIPTION_LIVELINESS_CHANGED); } + + QOSRequestedIncompatibleQoSCallbackType incompatible_qos_cb; if (event_callbacks.incompatible_qos_callback) { - this->add_event_handler( - event_callbacks.incompatible_qos_callback, - RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); + incompatible_qos_cb = event_callbacks.incompatible_qos_callback; + } else if (use_default_callbacks) { + // Register default callback when not specified + incompatible_qos_cb = [this](QOSRequestedIncompatibleQoSInfo & info) { + this->default_incompatible_qos_callback(info); + }; + } + // Register default callback when not specified + try { + if (incompatible_qos_cb) { + this->add_event_handler(incompatible_qos_cb, RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); + } + } catch (const UnsupportedEventTypeException & /*exc*/) { + // pass + } + + IncompatibleTypeCallbackType incompatible_type_cb; + if (event_callbacks.incompatible_type_callback) { + incompatible_type_cb = event_callbacks.incompatible_type_callback; } else if (use_default_callbacks) { // Register default callback when not specified - try { - this->add_event_handler( - [this](QOSRequestedIncompatibleQoSInfo & info) { - this->default_incompatible_qos_callback(info); - }, - RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); - } catch (UnsupportedEventTypeException & /*exc*/) { - // pass + incompatible_type_cb = [this](IncompatibleTypeInfo & info) { + this->default_incompatible_type_callback(info); + }; + } + try { + if (incompatible_type_cb) { + this->add_event_handler(incompatible_type_cb, RCL_SUBSCRIPTION_INCOMPATIBLE_TYPE); } + } catch (UnsupportedEventTypeException & /*exc*/) { + // pass } + if (event_callbacks.message_lost_callback) { this->add_event_handler( event_callbacks.message_lost_callback, @@ -161,7 +182,7 @@ SubscriptionBase::get_subscription_handle() const } const -std::unordered_map> & +std::unordered_map> & SubscriptionBase::get_event_handlers() const { return event_handlers_; @@ -299,6 +320,17 @@ SubscriptionBase::default_incompatible_qos_callback( policy_name.c_str()); } +void +SubscriptionBase::default_incompatible_type_callback( + rclcpp::IncompatibleTypeInfo & event) const +{ + (void)event; + + RCLCPP_WARN( + rclcpp::get_logger(rcl_node_get_logger_name(node_handle_.get())), + "Incompatible type on topic '%s', no messages will be sent to it.", get_topic_name()); +} + bool SubscriptionBase::matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const { diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index 1779d5d88e..0e53deec70 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -497,8 +497,8 @@ TEST_F(TestAllocatorMemoryStrategy, add_remove_waitables) { TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_subscription) { RclWaitSetSizes expected_sizes = {}; expected_sizes.size_of_subscriptions = 1; - expected_sizes.size_of_events = 1; - expected_sizes.size_of_waitables = 1; + expected_sizes.size_of_events = 2; + expected_sizes.size_of_waitables = 2; auto node_with_subscription = create_node_with_subscription("subscription_node"); EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_subscription, expected_sizes)); } diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index 221e2bdf0a..9c4c839a86 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -232,7 +232,7 @@ TEST_F(TestQosEvent, construct_destruct_rcl_error) { auto throwing_statement = [callback, rcl_handle, event_type]() { // reset() is not needed for the exception, but it handles unused return value warning std::make_shared< - rclcpp::QOSEventHandler>>( + rclcpp::EventHandler>>( callback, rcl_publisher_event_init, rcl_handle, event_type).reset(); }; // This is done through a lambda because the compiler is having trouble parsing the templated @@ -248,7 +248,7 @@ TEST_F(TestQosEvent, construct_destruct_rcl_error) { auto throwing_statement = [callback, rcl_handle, event_type]() { // reset() is needed for this exception std::make_shared< - rclcpp::QOSEventHandler>>( + rclcpp::EventHandler>>( callback, rcl_publisher_event_init, rcl_handle, event_type).reset(); }; @@ -267,7 +267,7 @@ TEST_F(TestQosEvent, execute) { auto callback = [&handler_callback_executed](int) {handler_callback_executed = true;}; rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; - rclcpp::QOSEventHandler handler( + rclcpp::EventHandler handler( callback, rcl_publisher_event_init, rcl_handle, event_type); std::shared_ptr data = handler.take_data(); @@ -292,7 +292,7 @@ TEST_F(TestQosEvent, add_to_wait_set) { auto callback = [](int) {}; rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; - rclcpp::QOSEventHandler handler( + rclcpp::EventHandler handler( callback, rcl_publisher_event_init, rcl_handle, event_type); EXPECT_EQ(1u, handler.get_number_of_ready_events()); From 232262c02a1265830c7785b7547bd51e1124fcd8 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Tue, 14 Mar 2023 01:10:24 +0800 Subject: [PATCH 199/455] Avoid losing waitable handles while using MultiThreadedExecutor (#2109) Signed-off-by: Barry Xu --- .../strategies/allocator_memory_strategy.hpp | 23 +-- .../test_allocator_memory_strategy.cpp | 176 +++++++++++++++++- 2 files changed, 180 insertions(+), 19 deletions(-) diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 88698179d4..46379744a1 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "rcl/allocator.h" @@ -120,8 +121,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } for (size_t i = 0; i < waitable_handles_.size(); ++i) { - if (!waitable_handles_[i]->is_ready(wait_set)) { - waitable_handles_[i].reset(); + if (waitable_handles_[i]->is_ready(wait_set)) { + waitable_triggered_handles_.emplace_back(std::move(waitable_handles_[i])); } } @@ -145,10 +146,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy timer_handles_.end() ); - waitable_handles_.erase( - std::remove(waitable_handles_.begin(), waitable_handles_.end(), nullptr), - waitable_handles_.end() - ); + waitable_handles_.clear(); } bool collect_entities(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override @@ -392,8 +390,9 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy rclcpp::AnyExecutable & any_exec, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override { - auto it = waitable_handles_.begin(); - while (it != waitable_handles_.end()) { + auto & waitable_handles = waitable_triggered_handles_; + auto it = waitable_handles.begin(); + while (it != waitable_handles.end()) { std::shared_ptr & waitable = *it; if (waitable) { // Find the group for this handle and see if it can be serviced @@ -401,7 +400,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy if (!group) { // Group was not found, meaning the waitable is not valid... // Remove it from the ready list and continue looking - it = waitable_handles_.erase(it); + it = waitable_handles.erase(it); continue; } if (!group->can_be_taken_from().load()) { @@ -414,11 +413,11 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy any_exec.waitable = waitable; any_exec.callback_group = group; any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes); - waitable_handles_.erase(it); + waitable_handles.erase(it); return; } // Else, the waitable is no longer valid, remove it and continue - it = waitable_handles_.erase(it); + it = waitable_handles.erase(it); } } @@ -499,6 +498,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy VectorRebind> timer_handles_; VectorRebind> waitable_handles_; + VectorRebind> waitable_triggered_handles_; + std::shared_ptr allocator_; }; diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index 0e53deec70..2adf907bc5 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -63,6 +63,57 @@ class TestWaitable : public rclcpp::Waitable } }; +static bool test_waitable_result2 = false; + +class TestWaitable2 : public rclcpp::Waitable +{ +public: + explicit TestWaitable2(rcl_publisher_t * pub_ptr) + : pub_ptr_(pub_ptr), + pub_event_(rcl_get_zero_initialized_event()) + { + EXPECT_EQ( + rcl_publisher_event_init(&pub_event_, pub_ptr_, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED), + RCL_RET_OK); + } + + ~TestWaitable2() + { + EXPECT_EQ(rcl_event_fini(&pub_event_), RCL_RET_OK); + } + + 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); + } + + bool is_ready(rcl_wait_set_t *) override + { + return test_waitable_result2; + } + + std::shared_ptr + take_data() override + { + return nullptr; + } + + void execute(std::shared_ptr & data) override + { + (void) data; + } + + size_t get_number_of_ready_events() override + { + return 1; + } + +private: + rcl_publisher_t * pub_ptr_; + rcl_event_t pub_event_; + size_t wait_set_event_index_; +}; + struct RclWaitSetSizes { size_t size_of_subscriptions = 0; @@ -657,20 +708,129 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_timer) { } TEST_F(TestAllocatorMemoryStrategy, get_next_waitable) { - auto node1 = std::make_shared("waitable_node", "ns"); - auto node2 = std::make_shared("waitable_node2", "ns"); - rclcpp::Waitable::SharedPtr waitable1 = std::make_shared(); - rclcpp::Waitable::SharedPtr waitable2 = std::make_shared(); - node1->get_node_waitables_interface()->add_waitable(waitable1, nullptr); - node2->get_node_waitables_interface()->add_waitable(waitable2, nullptr); - auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { rclcpp::AnyExecutable result; allocator_memory_strategy()->get_next_waitable(result, weak_groups_to_nodes); return result; }; - EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity)); + { + auto node1 = std::make_shared( + "waitable_node", "ns", + rclcpp::NodeOptions() + .start_parameter_event_publisher(false) + .start_parameter_services(false)); + + rclcpp::PublisherOptions pub_options; + pub_options.use_default_callbacks = false; + + auto pub1 = node1->create_publisher( + "test_topic_1", rclcpp::QoS(10), pub_options); + + auto waitable1 = + std::make_shared(pub1->get_publisher_handle().get()); + node1->get_node_waitables_interface()->add_waitable(waitable1, nullptr); + + auto basic_node = create_node_with_disabled_callback_groups("basic_node"); + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + basic_node->for_each_callback_group( + [basic_node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + basic_node->get_node_base_interface())); + }); + node1->for_each_callback_group( + [node1, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node1->get_node_base_interface())); + }); + allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + ASSERT_EQ( + rcl_wait_set_init( + &wait_set, + allocator_memory_strategy()->number_of_ready_subscriptions(), + allocator_memory_strategy()->number_of_guard_conditions(), + allocator_memory_strategy()->number_of_ready_timers(), + allocator_memory_strategy()->number_of_ready_clients(), + allocator_memory_strategy()->number_of_ready_services(), + allocator_memory_strategy()->number_of_ready_events(), + rclcpp::contexts::get_global_default_context()->get_rcl_context().get(), + allocator_memory_strategy()->get_allocator()), + RCL_RET_OK); + + ASSERT_TRUE(allocator_memory_strategy()->add_handles_to_wait_set(&wait_set)); + + ASSERT_EQ( + rcl_wait( + &wait_set, + std::chrono::duration_cast(std::chrono::milliseconds(100)) + .count()), + RCL_RET_OK); + test_waitable_result2 = true; + allocator_memory_strategy()->remove_null_handles(&wait_set); + + rclcpp::AnyExecutable result = get_next_entity(weak_groups_to_nodes); + EXPECT_EQ(result.node_base, node1->get_node_base_interface()); + test_waitable_result2 = false; + + EXPECT_EQ(rcl_wait_set_fini(&wait_set), RCL_RET_OK); + } + + { + auto node2 = std::make_shared( + "waitable_node2", "ns", + rclcpp::NodeOptions() + .start_parameter_services(false) + .start_parameter_event_publisher(false)); + + rclcpp::PublisherOptions pub_options; + pub_options.use_default_callbacks = false; + + auto pub2 = node2->create_publisher( + "test_topic_2", rclcpp::QoS(10), pub_options); + + auto waitable2 = + std::make_shared(pub2->get_publisher_handle().get()); + node2->get_node_waitables_interface()->add_waitable(waitable2, nullptr); + + auto basic_node2 = std::make_shared( + "basic_node2", "ns", + rclcpp::NodeOptions() + .start_parameter_services(false) + .start_parameter_event_publisher(false)); + WeakCallbackGroupsToNodesMap weak_groups_to_uncollected_nodes; + basic_node2->for_each_callback_group( + [basic_node2, &weak_groups_to_uncollected_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_uncollected_nodes.insert( + std::pair( + group_ptr, + basic_node2->get_node_base_interface())); + }); + node2->for_each_callback_group( + [node2, + &weak_groups_to_uncollected_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_uncollected_nodes.insert( + std::pair( + group_ptr, + node2->get_node_base_interface())); + }); + + rclcpp::AnyExecutable failed_result = get_next_entity(weak_groups_to_uncollected_nodes); + EXPECT_EQ(failed_result.node_base, nullptr); + } } TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_mutually_exclusive) { From 18dd05fba59179da1144477ecc7e31af73e53599 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 13 Mar 2023 17:23:43 -0500 Subject: [PATCH 200/455] Documentation improvements on the executor (#2125) Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/executor.hpp | 100 +++++++++++++++++++++++++++++ 1 file changed, 100 insertions(+) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 7f2071cded..94a8488557 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -315,6 +315,16 @@ class Executor virtual void spin_all(std::chrono::nanoseconds max_duration); + + /// Collect work once and execute the next available work, optionally within a duration. + /** + * This function can be overridden. The default implementation is suitable for + * a single-thread model of execution. + * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function + * to block (which may have unintended consequences). + * \param[in] timeout The maximum amount of time to spend waiting for work. + * `-1` is potentially block forever waiting for work. + */ RCLCPP_PUBLIC virtual void spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); @@ -413,12 +423,29 @@ class Executor is_spinning(); protected: + /// Add a node to executor, execute the next available unit of work, and remove the node. + /** + * Implementation of spin_node_once using std::chrono::nanoseconds + * \param[in] node Shared pointer to the node to add. + * \param[in] timeout How long to wait for work to become available. Negative values cause + * spin_node_once to block indefinitely (the default behavior). A timeout of 0 causes this + * function to be non-blocking. + */ RCLCPP_PUBLIC void spin_node_once_nanoseconds( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, std::chrono::nanoseconds timeout); + /// Collect work and execute available work, optionally within a duration. + /** + * Implementation of spin_some and spin_all. + * The exhaustive flag controls if the function will re-collect available work within the duration. + * + * \param[in] max_duration The maximum amount of time to spend executing work, or 0 for no limit. + * \param[in] exhaustive when set to true, continue to collect work and execute (spin_all) + * when set to false, return when all collected work is executed (spin_some) + */ RCLCPP_PUBLIC void spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive); @@ -433,30 +460,60 @@ class Executor void execute_any_executable(AnyExecutable & any_exec); + /// Run subscription executable. + /** + * Do necessary setup and tear-down as well as executing the subscription. + * \param[in] subscription Subscription to execute + */ RCLCPP_PUBLIC static void execute_subscription( rclcpp::SubscriptionBase::SharedPtr subscription); + /// Run timer executable. + /** + * Do necessary setup and tear-down as well as executing the timer callback. + * \param[in] timer Timer to execute + */ RCLCPP_PUBLIC static void execute_timer(rclcpp::TimerBase::SharedPtr timer); + /// Run service server executable. + /** + * Do necessary setup and tear-down as well as executing the service server callback. + * \param[in] service Service to execute + */ RCLCPP_PUBLIC static void execute_service(rclcpp::ServiceBase::SharedPtr service); + /// Run service client executable. + /** + * Do necessary setup and tear-down as well as executing the service client callback. + * \param[in] service Service to execute + */ RCLCPP_PUBLIC static void execute_client(rclcpp::ClientBase::SharedPtr client); + /// Block until more work becomes avilable or timeout is reached. /** + * Builds a set of waitable entities, which are passed to the middleware. + * After building wait set, waits on middleware to notify. + * \param[in] timeout duration to wait for new work to become available. * \throws std::runtime_error if the wait set can be cleared */ RCLCPP_PUBLIC void wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); + /// Find node associated with a callback group + /** + * \param[in] weak_groups_to_nodes map of callback groups to nodes + * \param[in] group callback group to find assocatiated node + * \return Pointer to associated node if found, else nullptr + */ RCLCPP_PUBLIC rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_by_group( @@ -475,6 +532,11 @@ class Executor const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const; + /// Find the callback group associated with a timer + /** + * \param[in] timer Timer to find associated callback group + * \return Pointer to callback group node if found, else nullptr + */ RCLCPP_PUBLIC rclcpp::CallbackGroup::SharedPtr get_group_by_timer(rclcpp::TimerBase::SharedPtr timer); @@ -502,16 +564,54 @@ class Executor WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_); + /// Check for executable in ready state and populate union structure. + /** + * \param[out] any_executable populated union structure of ready executable + * \return true if an executable was ready and any_executable was populated, + * otherwise false + */ RCLCPP_PUBLIC bool get_next_ready_executable(AnyExecutable & any_executable); + /// Check for executable in ready state and populate union structure. + /** + * This is the implementation of get_next_ready_executable that takes into + * account the current state of callback groups' association with nodes and + * executors. + * + * This checks in a particular order for available work: + * * Timers + * * Subscriptions + * * Services + * * Clients + * * Waitable + * + * If the next executable is not associated with this executor/node pair, + * then this method will return false. + * + * \param[out] any_executable populated union structure of ready executable + * \param[in] weak_groups_to_nodes mapping of callback groups to nodes + * \return true if an executable was ready and any_executable was populated, + * otherwise false + */ RCLCPP_PUBLIC bool get_next_ready_executable_from_map( AnyExecutable & any_executable, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); + /// Wait for executable in ready state and populate union structure. + /** + * If an executable is ready, it will return immediately, otherwise + * block based on the timeout for work to become ready. + * + * \param[out] any_executable populated union structure of ready executable + * \param[in] timeout duration of time to wait for work, a negative value + * (the defualt behavior), will make this function block indefinitely + * \return true if an executable was ready and any_executable was populated, + * otherwise false + */ RCLCPP_PUBLIC bool get_next_executable( From cbd48c0eb481cdf5d17b2ca056d4596df8b0af1c Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 14 Mar 2023 15:38:20 -0400 Subject: [PATCH 201/455] Fixes to silence some clang warnings. (#2127) This does 2 separate things: * Adds (void)unused_variable things where needed. * Stops doing some checks on moved-from items in tests. With this in place, most of the remaining clang static analysis warnings are gone. Signed-off-by: Chris Lalancette --- rclcpp/test/rclcpp/test_function_traits.cpp | 13 +++++++++++++ rclcpp/test/rclcpp/test_loaned_message.cpp | 1 - rclcpp/test/rclcpp/test_serialized_message.cpp | 2 -- rclcpp_lifecycle/test/test_lifecycle_publisher.cpp | 1 + 4 files changed, 14 insertions(+), 3 deletions(-) diff --git a/rclcpp/test/rclcpp/test_function_traits.cpp b/rclcpp/test/rclcpp/test_function_traits.cpp index a9e6e8d261..20c9521c4e 100644 --- a/rclcpp/test/rclcpp/test_function_traits.cpp +++ b/rclcpp/test/rclcpp/test_function_traits.cpp @@ -393,6 +393,7 @@ TEST(TestFunctionTraits, argument_types) { auto bind_one_bool = std::bind( &ObjectMember::callback_one_bool, &object_member, std::placeholders::_1); + (void)bind_one_bool; // to quiet clang static_assert( std::is_same< @@ -402,6 +403,7 @@ TEST(TestFunctionTraits, argument_types) { auto bind_one_bool_const = std::bind( &ObjectMember::callback_one_bool_const, &object_member, std::placeholders::_1); + (void)bind_one_bool_const; // to quiet clang static_assert( std::is_same< @@ -413,6 +415,7 @@ TEST(TestFunctionTraits, argument_types) { auto bind_two_bools = std::bind( &ObjectMember::callback_two_bools, &object_member, std::placeholders::_1, std::placeholders::_2); + (void)bind_two_bools; // to quiet clang static_assert( std::is_same< @@ -429,6 +432,7 @@ TEST(TestFunctionTraits, argument_types) { auto bind_one_bool_one_float = std::bind( &ObjectMember::callback_one_bool_one_float, &object_member, std::placeholders::_1, std::placeholders::_2); + (void)bind_one_bool_one_float; // to quiet clang static_assert( std::is_same< @@ -447,6 +451,7 @@ TEST(TestFunctionTraits, argument_types) { >::value, "Functor accepts a float as second argument"); auto bind_one_int = std::bind(func_one_int, std::placeholders::_1); + (void)bind_one_int; // to quiet clang static_assert( std::is_same< @@ -455,6 +460,7 @@ TEST(TestFunctionTraits, argument_types) { >::value, "Functor accepts an int as first argument"); auto bind_two_ints = std::bind(func_two_ints, std::placeholders::_1, std::placeholders::_2); + (void)bind_two_ints; // to quiet clang static_assert( std::is_same< @@ -470,6 +476,7 @@ TEST(TestFunctionTraits, argument_types) { auto bind_one_int_one_char = std::bind( func_one_int_one_char, std::placeholders::_1, std::placeholders::_2); + (void)bind_one_int_one_char; // to quiet clang static_assert( std::is_same< @@ -530,18 +537,21 @@ TEST(TestFunctionTraits, check_arguments) { (void)one; return 1; }; + (void)lambda_one_int; // to quiet clang auto lambda_two_ints = [](int one, int two) { (void)one; (void)two; return 2; }; + (void)lambda_two_ints; // to quiet clang auto lambda_one_int_one_char = [](int one, char two) { (void)one; (void)two; return 3; }; + (void)lambda_one_int_one_char; // to quiet clang static_assert( rclcpp::function_traits::check_arguments::value, @@ -572,6 +582,7 @@ TEST(TestFunctionTraits, check_arguments) { auto bind_one_bool = std::bind( &ObjectMember::callback_one_bool, &object_member, std::placeholders::_1); + (void)bind_one_bool; // to quiet clang // Test std::bind functions static_assert( @@ -580,6 +591,7 @@ TEST(TestFunctionTraits, check_arguments) { auto bind_one_bool_const = std::bind( &ObjectMember::callback_one_bool_const, &object_member, std::placeholders::_1); + (void)bind_one_bool_const; // to quiet clang // Test std::bind functions static_assert( @@ -745,6 +757,7 @@ TEST_F(TestMember, bind_member_functor) { auto bind_member_functor = std::bind( &TestMember::MemberFunctor, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + (void)bind_member_functor; // to quiet clang static_assert( rclcpp::function_traits::check_argumentspublisher()->is_activated()); { auto msg_ptr = std::make_unique(); From 1a796b5515cde3d8b6a64f8c53c7f49d8a742d32 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Fri, 17 Mar 2023 12:35:34 -0700 Subject: [PATCH 202/455] use allocator via init_options argument. (#2129) Signed-off-by: Tomoya Fujita --- rclcpp/src/rclcpp/context.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 0fec6d066c..35a11730ab 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -219,7 +219,7 @@ Context::init( if (0u == count) { ret = rcl_logging_configure_with_output_handler( &rcl_context_->global_arguments, - rcl_init_options_get_allocator(init_options_.get_rcl_init_options()), + rcl_init_options_get_allocator(init_options.get_rcl_init_options()), rclcpp_logging_output_handler); if (RCL_RET_OK != ret) { rcl_context_.reset(); From bff59925de111b43b179e64661b0708061afa4e4 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Tue, 21 Mar 2023 14:11:48 -0700 Subject: [PATCH 203/455] extract the result response before the callback is issued. (#2132) Signed-off-by: Tomoya Fujita --- rclcpp_action/src/client.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index e687ab3400..2d5018d5af 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -318,14 +318,19 @@ ClientBase::handle_result_response( const rmw_request_id_t & response_header, std::shared_ptr response) { - std::lock_guard guard(pimpl_->result_requests_mutex); - const int64_t & sequence_number = response_header.sequence_number; - if (pimpl_->pending_result_responses.count(sequence_number) == 0) { - RCLCPP_ERROR(pimpl_->logger, "unknown result response, ignoring..."); - return; + std::map::node_type pending_result_response; + { + std::lock_guard guard(pimpl_->result_requests_mutex); + const int64_t & sequence_number = response_header.sequence_number; + if (pimpl_->pending_result_responses.count(sequence_number) == 0) { + RCLCPP_ERROR(pimpl_->logger, "unknown result response, ignoring..."); + return; + } + pending_result_response = + pimpl_->pending_result_responses.extract(sequence_number); } - pimpl_->pending_result_responses[sequence_number](response); - pimpl_->pending_result_responses.erase(sequence_number); + auto & response_callback = pending_result_response.mapped(); + response_callback(response); } void From cb08c79a0a8843706ae52fcfeec784f4f569b41b Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Wed, 22 Mar 2023 21:36:47 +0800 Subject: [PATCH 204/455] Implement matched event (#2105) Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/event_handler.hpp | 5 + rclcpp/src/rclcpp/publisher_base.cpp | 5 + rclcpp/src/rclcpp/subscription_base.cpp | 5 + rclcpp/test/rclcpp/CMakeLists.txt | 26 ++-- rclcpp/test/rclcpp/test_qos_event.cpp | 190 ++++++++++++++++++++++++ 5 files changed, 221 insertions(+), 10 deletions(-) diff --git a/rclcpp/include/rclcpp/event_handler.hpp b/rclcpp/include/rclcpp/event_handler.hpp index af5d8c08dc..3f41de469c 100644 --- a/rclcpp/include/rclcpp/event_handler.hpp +++ b/rclcpp/include/rclcpp/event_handler.hpp @@ -47,6 +47,7 @@ using QOSOfferedIncompatibleQoSInfo = rmw_offered_qos_incompatible_event_status_ using QOSRequestedIncompatibleQoSInfo = rmw_requested_qos_incompatible_event_status_t; using IncompatibleTypeInfo = rmw_incompatible_type_status_t; +using MatchedInfo = rmw_matched_status_t; using QOSDeadlineRequestedCallbackType = std::function; using QOSDeadlineOfferedCallbackType = std::function; @@ -58,6 +59,8 @@ using QOSRequestedIncompatibleQoSCallbackType = std::function; using IncompatibleTypeCallbackType = std::function; +using PublisherMatchedCallbackType = std::function; +using SubscriptionMatchedCallbackType = std::function; /// Contains callbacks for various types of events a Publisher can receive from the middleware. struct PublisherEventCallbacks @@ -66,6 +69,7 @@ struct PublisherEventCallbacks QOSLivelinessLostCallbackType liveliness_callback; QOSOfferedIncompatibleQoSCallbackType incompatible_qos_callback; IncompatibleTypeCallbackType incompatible_type_callback; + PublisherMatchedCallbackType matched_callback; }; /// Contains callbacks for non-message events that a Subscription can receive from the middleware. @@ -76,6 +80,7 @@ struct SubscriptionEventCallbacks QOSRequestedIncompatibleQoSCallbackType incompatible_qos_callback; QOSMessageLostCallbackType message_lost_callback; IncompatibleTypeCallbackType incompatible_type_callback; + SubscriptionMatchedCallbackType matched_callback; }; class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public std::runtime_error diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index a96a6ee791..698db2d559 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -183,6 +183,11 @@ PublisherBase::bind_event_callbacks( rclcpp::get_logger("rclcpp"), "Failed to add event handler for incompatible type; wrong callback type"); } + if (event_callbacks.matched_callback) { + this->add_event_handler( + event_callbacks.matched_callback, + RCL_PUBLISHER_MATCHED); + } } size_t diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 2302876338..e95cb4ac49 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -161,6 +161,11 @@ SubscriptionBase::bind_event_callbacks( event_callbacks.message_lost_callback, RCL_SUBSCRIPTION_MESSAGE_LOST); } + if (event_callbacks.matched_callback) { + this->add_event_handler( + event_callbacks.matched_callback, + RCL_SUBSCRIPTION_MATCHED); + } } const char * diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 0399f3ae11..d4da759c02 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -444,17 +444,23 @@ function(test_generic_pubsub_for_rmw_implementation) endif() endfunction() call_for_each_rmw_implementation(test_generic_pubsub_for_rmw_implementation) -ament_add_gtest(test_qos_event test_qos_event.cpp) -if(TARGET test_qos_event) - ament_target_dependencies(test_qos_event - "rmw" - "test_msgs" - ) - target_link_libraries(test_qos_event - ${PROJECT_NAME} - mimick + +function(test_qos_event_for_rmw_implementation) + set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) + ament_add_gmock(test_qos_event${target_suffix} test_qos_event.cpp + ENV ${rmw_implementation_env_var} ) -endif() + if(TARGET test_qos_event${target_suffix}) + target_link_libraries(test_qos_event${target_suffix} ${PROJECT_NAME} mimick) + ament_target_dependencies(test_qos_event${target_suffix} + "rmw" + "rosidl_typesupport_cpp" + "test_msgs" + ) + endif() +endfunction() +call_for_each_rmw_implementation(test_qos_event_for_rmw_implementation) + ament_add_gmock(test_qos_overriding_options test_qos_overriding_options.cpp) if(TARGET test_qos_overriding_options) target_link_libraries(test_qos_overriding_options diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index 9c4c839a86..6b522d7ea2 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -14,6 +14,7 @@ #include +#include #include #include #include @@ -313,6 +314,11 @@ TEST_F(TestQosEvent, add_to_wait_set) { TEST_F(TestQosEvent, test_on_new_event_callback) { + // rmw_connextdds doesn't support rmw_event_set_callback() interface + if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { + GTEST_SKIP(); + } + auto offered_deadline = rclcpp::Duration(std::chrono::milliseconds(1)); auto requested_deadline = rclcpp::Duration(std::chrono::milliseconds(2)); @@ -354,6 +360,11 @@ TEST_F(TestQosEvent, test_on_new_event_callback) TEST_F(TestQosEvent, test_invalid_on_new_event_callback) { + // rmw_connextdds doesn't support rmw_event_set_callback() interface + if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { + GTEST_SKIP(); + } + 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;}; @@ -376,6 +387,12 @@ 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( + pub->set_on_new_qos_event_callback(dummy_cb, RCL_PUBLISHER_MATCHED)); + + EXPECT_NO_THROW( + pub->clear_on_new_qos_event_callback(RCL_PUBLISHER_MATCHED)); + EXPECT_NO_THROW( sub->set_on_new_qos_event_callback(dummy_cb, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED)); @@ -394,6 +411,12 @@ TEST_F(TestQosEvent, test_invalid_on_new_event_callback) EXPECT_NO_THROW( sub->clear_on_new_qos_event_callback(RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS)); + EXPECT_NO_THROW( + sub->set_on_new_qos_event_callback(dummy_cb, RCL_SUBSCRIPTION_MATCHED)); + + EXPECT_NO_THROW( + sub->clear_on_new_qos_event_callback(RCL_SUBSCRIPTION_MATCHED)); + std::function invalid_cb; rclcpp::SubscriptionOptions sub_options; @@ -413,3 +436,170 @@ TEST_F(TestQosEvent, test_invalid_on_new_event_callback) pub->set_on_new_qos_event_callback(invalid_cb, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED), std::invalid_argument); } + +TEST_F(TestQosEvent, test_pub_matched_event_by_set_event_callback) +{ + // rmw_connextdds doesn't support rmw_event_set_callback() interface + if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { + GTEST_SKIP(); + } + + std::atomic_size_t matched_count = 0; + + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.matched_callback = [](auto) {}; + auto pub = node->create_publisher( + topic_name, 10, pub_options); + + auto matched_event_callback = [&matched_count](size_t count) { + matched_count += count; + }; + + pub->set_on_new_qos_event_callback(matched_event_callback, RCL_PUBLISHER_MATCHED); + + rclcpp::executors::SingleThreadedExecutor ex; + ex.add_node(node->get_node_base_interface()); + + const auto timeout = std::chrono::milliseconds(200); + + { + auto sub1 = node->create_subscription(topic_name, 10, message_callback); + ex.spin_some(timeout); + EXPECT_EQ(matched_count, static_cast(1)); + + { + auto sub2 = node->create_subscription( + topic_name, 10, message_callback); + ex.spin_some(timeout); + EXPECT_EQ(matched_count, static_cast(2)); + } + ex.spin_some(timeout); + EXPECT_EQ(matched_count, static_cast(3)); + } + + ex.spin_some(timeout); + EXPECT_EQ(matched_count, static_cast(4)); +} + +TEST_F(TestQosEvent, test_sub_matched_event_by_set_event_callback) +{ + // rmw_connextdds doesn't support rmw_event_set_callback() interface + if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { + GTEST_SKIP(); + } + + std::atomic_size_t matched_count = 0; + + rclcpp::SubscriptionOptions sub_options; + sub_options.event_callbacks.matched_callback = [](auto) {}; + auto sub = node->create_subscription( + topic_name, 10, message_callback, sub_options); + + auto matched_event_callback = [&matched_count](size_t count) { + matched_count += count; + }; + + sub->set_on_new_qos_event_callback(matched_event_callback, RCL_SUBSCRIPTION_MATCHED); + + rclcpp::executors::SingleThreadedExecutor ex; + ex.add_node(node->get_node_base_interface()); + + const auto timeout = std::chrono::milliseconds(200); + + { + auto pub1 = node->create_publisher(topic_name, 10); + + ex.spin_some(timeout); + EXPECT_EQ(matched_count, static_cast(1)); + + { + auto pub2 = node->create_publisher(topic_name, 10); + ex.spin_some(timeout); + EXPECT_EQ(matched_count, static_cast(2)); + } + + ex.spin_some(timeout); + EXPECT_EQ(matched_count, static_cast(3)); + } + + ex.spin_some(timeout); + EXPECT_EQ(matched_count, static_cast(4)); +} + +TEST_F(TestQosEvent, test_pub_matched_event_by_option_event_callback) +{ + rmw_matched_status_t matched_expected_result; + + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.matched_callback = + [&matched_expected_result](rmw_matched_status_t & s) { + EXPECT_EQ(s.total_count, matched_expected_result.total_count); + EXPECT_EQ(s.total_count_change, matched_expected_result.total_count_change); + EXPECT_EQ(s.current_count, matched_expected_result.current_count); + EXPECT_EQ(s.current_count_change, matched_expected_result.current_count_change); + }; + + auto pub = node->create_publisher( + topic_name, 10, pub_options); + + rclcpp::executors::SingleThreadedExecutor ex; + ex.add_node(node->get_node_base_interface()); + + // Create a connected subscription + matched_expected_result.total_count = 1; + matched_expected_result.total_count_change = 1; + matched_expected_result.current_count = 1; + matched_expected_result.current_count_change = 1; + + const auto timeout = std::chrono::milliseconds(200); + + { + auto sub = node->create_subscription(topic_name, 10, message_callback); + ex.spin_some(timeout); + + // destroy a connected subscription + matched_expected_result.total_count = 1; + matched_expected_result.total_count_change = 0; + matched_expected_result.current_count = 0; + matched_expected_result.current_count_change = -1; + } + ex.spin_some(timeout); +} + +TEST_F(TestQosEvent, test_sub_matched_event_by_option_event_callback) +{ + rmw_matched_status_t matched_expected_result; + + rclcpp::SubscriptionOptions sub_options; + sub_options.event_callbacks.matched_callback = + [&matched_expected_result](rmw_matched_status_t & s) { + EXPECT_EQ(s.total_count, matched_expected_result.total_count); + EXPECT_EQ(s.total_count_change, matched_expected_result.total_count_change); + EXPECT_EQ(s.current_count, matched_expected_result.current_count); + EXPECT_EQ(s.current_count_change, matched_expected_result.current_count_change); + }; + auto sub = node->create_subscription( + topic_name, 10, message_callback, sub_options); + + rclcpp::executors::SingleThreadedExecutor ex; + ex.add_node(node->get_node_base_interface()); + + // Create a connected publisher + matched_expected_result.total_count = 1; + matched_expected_result.total_count_change = 1; + matched_expected_result.current_count = 1; + matched_expected_result.current_count_change = 1; + + const auto timeout = std::chrono::milliseconds(200); + { + auto pub1 = node->create_publisher(topic_name, 10); + ex.spin_some(timeout); + + // destroy a connected publisher + matched_expected_result.total_count = 1; + matched_expected_result.total_count_change = 0; + matched_expected_result.current_count = 0; + matched_expected_result.current_count_change = -1; + } + ex.spin_some(timeout); +} From 20e9cd17f6723337dc0915f4e2a966f9585c309c Mon Sep 17 00:00:00 2001 From: Emerson Knapp <537409+emersonknapp@users.noreply.github.com> Date: Sun, 26 Mar 2023 05:45:36 -0700 Subject: [PATCH 205/455] Remove deprecated header (#2139) Signed-off-by: Emerson Knapp --- rclcpp/include/rclcpp/scope_exit.hpp | 56 ---------------------------- 1 file changed, 56 deletions(-) delete mode 100644 rclcpp/include/rclcpp/scope_exit.hpp diff --git a/rclcpp/include/rclcpp/scope_exit.hpp b/rclcpp/include/rclcpp/scope_exit.hpp deleted file mode 100644 index 8b8e3ea49b..0000000000 --- a/rclcpp/include/rclcpp/scope_exit.hpp +++ /dev/null @@ -1,56 +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_ - -// 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_ From a5368e6fe43112eaefdd805003ba9d2e9f45bbee Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Wed, 29 Mar 2023 21:50:17 -0700 Subject: [PATCH 206/455] add Logger::get_effective_level(). (#2141) Signed-off-by: Tomoya Fujita --- rclcpp/include/rclcpp/logger.hpp | 18 ++++++++++++ rclcpp/src/rclcpp/logger.cpp | 14 ++++++++++ rclcpp/test/rclcpp/test_logger.cpp | 44 ++++++++++++++++++++++++++++++ 3 files changed, 76 insertions(+) diff --git a/rclcpp/include/rclcpp/logger.hpp b/rclcpp/include/rclcpp/logger.hpp index 622ddd27da..77f7f8d670 100644 --- a/rclcpp/include/rclcpp/logger.hpp +++ b/rclcpp/include/rclcpp/logger.hpp @@ -170,6 +170,24 @@ class Logger RCLCPP_PUBLIC void set_level(Level level); + + /// Get effective level for current logger. + /** + * The effective level is determined as the severity level of + * the logger if it is set, otherwise it is the first specified severity + * level of the logger's ancestors, starting with its closest ancestor. + * The ancestor hierarchy is signified by logger names being separated by dots: + * a logger named `x` is an ancestor of `x.y`, and both `x` and `x.y` are + * ancestors of `x.y.z`, etc. + * If the level has not been set for the logger nor any of its + * ancestors, the default level is used. + * + * \throws rclcpp::exceptions::RCLError if any error happens. + * \return Level for the current logger. + */ + RCLCPP_PUBLIC + Level + get_effective_level() const; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/logger.cpp b/rclcpp/src/rclcpp/logger.cpp index d1a7640215..874858b180 100644 --- a/rclcpp/src/rclcpp/logger.cpp +++ b/rclcpp/src/rclcpp/logger.cpp @@ -125,4 +125,18 @@ Logger::set_level(Level level) } } +Logger::Level +Logger::get_effective_level() const +{ + int logger_level = rcutils_logging_get_logger_effective_level(get_name()); + + if (logger_level < 0) { + exceptions::throw_from_rcl_error( + RCL_RET_ERROR, "Couldn't get logger level", + rcutils_get_error_state(), rcutils_reset_error); + } + + return static_cast(logger_level); +} + } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/test_logger.cpp b/rclcpp/test/rclcpp/test_logger.cpp index 1869b8f478..83d42f0ce8 100644 --- a/rclcpp/test/rclcpp/test_logger.cpp +++ b/rclcpp/test/rclcpp/test_logger.cpp @@ -160,6 +160,50 @@ TEST(TestLogger, set_level) { EXPECT_EQ(RCUTILS_RET_OK, rcutils_logging_shutdown()); } +TEST(TestLogger, get_effective_level) { + ASSERT_EQ(RCUTILS_RET_OK, rcutils_logging_initialize()); + + rclcpp::Logger logger = rclcpp::get_logger("test_logger"); + rclcpp::Logger child_logger = rclcpp::get_logger("test_logger.child"); + + // set child logger level unset to test effective level + child_logger.set_level(rclcpp::Logger::Level::Unset); + + // default + EXPECT_EQ(rclcpp::Logger::Level::Info, logger.get_effective_level()); + EXPECT_EQ(rclcpp::Logger::Level::Info, child_logger.get_effective_level()); + + // unset + logger.set_level(rclcpp::Logger::Level::Unset); + EXPECT_EQ(rclcpp::Logger::Level::Info, logger.get_effective_level()); + EXPECT_EQ(rclcpp::Logger::Level::Info, child_logger.get_effective_level()); + + // debug + logger.set_level(rclcpp::Logger::Level::Debug); + EXPECT_EQ(rclcpp::Logger::Level::Debug, logger.get_effective_level()); + EXPECT_EQ(rclcpp::Logger::Level::Debug, child_logger.get_effective_level()); + + // info + logger.set_level(rclcpp::Logger::Level::Info); + EXPECT_EQ(rclcpp::Logger::Level::Info, logger.get_effective_level()); + EXPECT_EQ(rclcpp::Logger::Level::Info, child_logger.get_effective_level()); + + // warn + logger.set_level(rclcpp::Logger::Level::Warn); + EXPECT_EQ(rclcpp::Logger::Level::Warn, logger.get_effective_level()); + EXPECT_EQ(rclcpp::Logger::Level::Warn, child_logger.get_effective_level()); + + // error + logger.set_level(rclcpp::Logger::Level::Error); + EXPECT_EQ(rclcpp::Logger::Level::Error, logger.get_effective_level()); + EXPECT_EQ(rclcpp::Logger::Level::Error, child_logger.get_effective_level()); + + // fatal + logger.set_level(rclcpp::Logger::Level::Fatal); + EXPECT_EQ(rclcpp::Logger::Level::Fatal, logger.get_effective_level()); + EXPECT_EQ(rclcpp::Logger::Level::Fatal, child_logger.get_effective_level()); +} + TEST(TestLogger, get_logging_directory) { ASSERT_EQ(true, rcutils_set_env("HOME", "/fake_home_dir")); ASSERT_EQ(true, rcutils_set_env("USERPROFILE", nullptr)); From 73d555b4026fec0c4350e8589571fb58f0e58651 Mon Sep 17 00:00:00 2001 From: Christopher Wecht Date: Mon, 3 Apr 2023 16:01:39 +0200 Subject: [PATCH 207/455] Fix unnecessary allocations in executor.cpp (#2135) Signed-off-by: Christopher Wecht --- rclcpp/src/rclcpp/executor.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 32b895c1e3..9bafbe3106 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -558,13 +558,14 @@ Executor::execute_any_executable(AnyExecutable & any_exec) } } +template static void take_and_do_error_handling( const char * action_description, const char * topic_or_service_name, - std::function take_action, - std::function handle_action) + Taker take_action, + Handler handle_action) { bool taken = false; try { From 71a06985af7b4b143025dd73938fdb61f1b1ab3a Mon Sep 17 00:00:00 2001 From: Yadu Date: Tue, 4 Apr 2023 00:53:49 +0800 Subject: [PATCH 208/455] Minor grammar fix (#2149) Signed-off-by: Yadunund --- rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 9dafcba381..c8bb62e238 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -651,7 +651,7 @@ NodeParameters::undeclare_parameter(const std::string & name) } if (!parameter_info->second.descriptor.dynamic_typing) { throw rclcpp::exceptions::InvalidParameterTypeException{ - name, "cannot undeclare an statically typed parameter"}; + name, "cannot undeclare a statically typed parameter"}; } parameters_.erase(parameter_info); @@ -824,7 +824,7 @@ NodeParameters::set_parameters_atomically(const std::vector & auto it = parameters_.find(parameter.get_name()); if (it != parameters_.end() && rclcpp::PARAMETER_NOT_SET != it->second.value.get_type()) { if (!it->second.descriptor.dynamic_typing) { - result.reason = "cannot undeclare an statically typed parameter"; + result.reason = "cannot undeclare a statically typed parameter"; result.successful = false; return result; } From 5f9695afb02f178ec739fa1591bb018a9f9b2be0 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 11 Apr 2023 19:20:21 -0500 Subject: [PATCH 209/455] Trigger the intraprocess guard condition with data (#2164) If the intraprocess buffer still has data after taking, re-trigger the guard condition to ensure that the executor will continue to service it, even if incoming publications stop. Signed-off-by: Michael Carroll --- .../subscription_intra_process.hpp | 7 ++ .../test/rclcpp/executors/test_executors.cpp | 103 ++++++++++++++++++ 2 files changed, 110 insertions(+) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 91ea91a7c3..ec89ebc5ef 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -118,6 +118,13 @@ class SubscriptionIntraProcess return nullptr; } } + + if (this->buffer_->has_data()) { + // If there is data still to be processed, indicate to the + // executor or waitset by triggering the guard condition. + this->trigger_guard_condition(); + } + return std::static_pointer_cast( std::make_shared>( std::pair( diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 1fa2cbb4dd..eb6652f19b 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -593,3 +593,106 @@ TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) { rclcpp::shutdown(); } + +template +class TestIntraprocessExecutors : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); + std::stringstream test_name; + test_name << test_info->test_case_name() << "_" << test_info->name(); + node = std::make_shared("node", test_name.str()); + + callback_count = 0; + + const std::string topic_name = std::string("topic_") + test_name.str(); + + rclcpp::PublisherOptions po; + po.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + publisher = node->create_publisher(topic_name, rclcpp::QoS(1), po); + + auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) { + this->callback_count.fetch_add(1); + }; + + rclcpp::SubscriptionOptions so; + so.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + subscription = + node->create_subscription( + topic_name, rclcpp::QoS(kNumMessages), std::move(callback), so); + } + + void TearDown() + { + publisher.reset(); + subscription.reset(); + node.reset(); + } + + const size_t kNumMessages = 100; + + rclcpp::Node::SharedPtr node; + rclcpp::Publisher::SharedPtr publisher; + rclcpp::Subscription::SharedPtr subscription; + std::atomic_int callback_count; +}; + +TYPED_TEST_SUITE(TestIntraprocessExecutors, ExecutorTypes, ExecutorTypeNames); + +TYPED_TEST(TestIntraprocessExecutors, testIntraprocessRetrigger) { + // This tests that executors will continue to service intraprocess subscriptions in the case + // that publishers aren't continuing to publish. + // This was previously broken in that intraprocess guard conditions were only triggered on + // publish and the test was added to prevent future regressions. + const size_t kNumMessages = 100; + + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + EXPECT_EQ(0, this->callback_count.load()); + this->publisher->publish(test_msgs::msg::Empty()); + + // Wait for up to 5 seconds for the first message to come available. + const std::chrono::milliseconds sleep_per_loop(10); + int loops = 0; + while (1u != this->callback_count.load() && loops < 500) { + rclcpp::sleep_for(sleep_per_loop); + executor.spin_some(); + loops++; + } + EXPECT_EQ(1u, this->callback_count.load()); + + // reset counter + this->callback_count.store(0); + + for (size_t ii = 0; ii < kNumMessages; ++ii) { + this->publisher->publish(test_msgs::msg::Empty()); + } + + // Fire a timer every 10ms up to 5 seconds waiting for subscriptions to be read. + loops = 0; + auto timer = this->node->create_wall_timer( + std::chrono::milliseconds(10), [this, &executor, &loops, &kNumMessages]() { + loops++; + if (kNumMessages == this->callback_count.load() || + loops == 500) + { + executor.cancel(); + } + }); + executor.spin(); + EXPECT_EQ(kNumMessages, this->callback_count.load()); +} From 3088b536cc02f0e5d01aa4bccda0cc2d2d8c1ebf Mon Sep 17 00:00:00 2001 From: Emerson Knapp <537409+emersonknapp@users.noreply.github.com> Date: Wed, 12 Apr 2023 05:57:57 -0700 Subject: [PATCH 210/455] Add type_hash to cpp TopicEndpointInfo (#2137) * Add type_hash to cpp TopicEndpointInfo Signed-off-by: Emerson Knapp --- .../node_interfaces/node_graph_interface.hpp | 14 +++++++++++++- rclcpp/src/rclcpp/node_interfaces/node_graph.cpp | 12 ++++++++++++ .../rclcpp/node_interfaces/test_node_graph.cpp | 13 +++++++++++++ 3 files changed, 38 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp index fefda0da69..5967997ac7 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -57,7 +57,8 @@ class TopicEndpointInfo node_namespace_(info.node_namespace), topic_type_(info.topic_type), endpoint_type_(static_cast(info.endpoint_type)), - qos_profile_({info.qos_profile.history, info.qos_profile.depth}, info.qos_profile) + qos_profile_({info.qos_profile.history, info.qos_profile.depth}, info.qos_profile), + topic_type_hash_(info.topic_type_hash) { std::copy(info.endpoint_gid, info.endpoint_gid + RMW_GID_STORAGE_SIZE, endpoint_gid_.begin()); } @@ -122,6 +123,16 @@ class TopicEndpointInfo const rclcpp::QoS & qos_profile() const; + /// Get a mutable reference to the type hash of the topic endpoint. + RCLCPP_PUBLIC + rosidl_type_hash_t & + topic_type_hash(); + + /// Get a const reference to the type hash of the topic endpoint. + RCLCPP_PUBLIC + const rosidl_type_hash_t & + topic_type_hash() const; + private: std::string node_name_; std::string node_namespace_; @@ -129,6 +140,7 @@ class TopicEndpointInfo rclcpp::EndpointType endpoint_type_; std::array endpoint_gid_; rclcpp::QoS qos_profile_; + rosidl_type_hash_t topic_type_hash_; }; namespace node_interfaces diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index e13ec7cd4c..6f6ed13be7 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -789,3 +789,15 @@ rclcpp::TopicEndpointInfo::qos_profile() const { return qos_profile_; } + +rosidl_type_hash_t & +rclcpp::TopicEndpointInfo::topic_type_hash() +{ + return topic_type_hash_; +} + +const rosidl_type_hash_t & +rclcpp::TopicEndpointInfo::topic_type_hash() const +{ + return topic_type_hash_; +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp index c25af15422..70ccb5622d 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -28,6 +28,7 @@ #include "rclcpp/node_interfaces/node_graph.hpp" #include "rclcpp/rclcpp.hpp" #include "rcutils/strdup.h" +#include "test_msgs/msg/empty.h" #include "test_msgs/msg/empty.hpp" #include "test_msgs/srv/empty.hpp" @@ -599,6 +600,18 @@ TEST_F(TestNodeGraph, get_info_by_topic) rclcpp::QoS const_actual_qos = const_publisher_endpoint_info.qos_profile(); EXPECT_EQ(const_actual_qos.reliability(), rclcpp::ReliabilityPolicy::Reliable); + const rosidl_type_hash_t expected_type_hash = *test_msgs__msg__Empty__get_type_hash(nullptr); + EXPECT_EQ( + 0, memcmp( + &publisher_endpoint_info.topic_type_hash(), + &expected_type_hash, + sizeof(rosidl_type_hash_t))); + EXPECT_EQ( + 0, memcmp( + &const_publisher_endpoint_info.topic_type_hash(), + &expected_type_hash, + sizeof(rosidl_type_hash_t))); + auto endpoint_gid = publisher_endpoint_info.endpoint_gid(); auto const_endpoint_gid = const_publisher_endpoint_info.endpoint_gid(); bool endpoint_gid_is_all_zeros = true; From b8173e28c6382ba7c6bf163631f9e7de1e368d1f Mon Sep 17 00:00:00 2001 From: methylDragon Date: Thu, 13 Apr 2023 14:32:33 +0000 Subject: [PATCH 211/455] Dynamic Subscription (REP-2011 Subset): Stubs for rclcpp (#2165) * Implement subscription base changes * Add stubbed out classes Signed-off-by: methylDragon --- rclcpp/CMakeLists.txt | 5 + .../dynamic_typesupport/dynamic_message.hpp | 70 ++++++++ .../dynamic_message_type.hpp | 64 ++++++++ .../dynamic_message_type_builder.hpp | 65 ++++++++ .../dynamic_message_type_support.hpp | 67 ++++++++ .../dynamic_serialization_support.hpp | 60 +++++++ .../include/rclcpp/generic_subscription.hpp | 27 +++- rclcpp/include/rclcpp/rclcpp.hpp | 12 ++ rclcpp/include/rclcpp/subscription.hpp | 53 ++++++- rclcpp/include/rclcpp/subscription_base.hpp | 69 +++++++- rclcpp/package.xml | 1 + .../dynamic_typesupport/dynamic_message.cpp | 40 +++++ .../dynamic_message_type.cpp | 38 +++++ .../dynamic_message_type_builder.cpp | 37 +++++ .../dynamic_message_type_support.cpp | 49 ++++++ .../dynamic_serialization_support.cpp | 46 ++++++ rclcpp/src/rclcpp/executor.cpp | 149 +++++++++++------- rclcpp/src/rclcpp/generic_subscription.cpp | 69 +++++++- rclcpp/src/rclcpp/subscription_base.cpp | 37 ++++- .../node_interfaces/test_node_topics.cpp | 17 ++ 20 files changed, 902 insertions(+), 73 deletions(-) create mode 100644 rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp create mode 100644 rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp create mode 100644 rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp create mode 100644 rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp create mode 100644 rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp create mode 100644 rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp create mode 100644 rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp create mode 100644 rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp create mode 100644 rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp create mode 100644 rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 6979d980f8..96351f7528 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -49,6 +49,11 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp src/rclcpp/detail/utilities.cpp src/rclcpp/duration.cpp + src/rclcpp/dynamic_typesupport/dynamic_message.cpp + src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp + src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp + src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp + src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp src/rclcpp/event.cpp src/rclcpp/exceptions/exceptions.cpp src/rclcpp/executable_list.cpp diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp new file mode 100644 index 0000000000..f9586aabb7 --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp @@ -0,0 +1,70 @@ +// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_ + +#include +#include +#include + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +/// Utility wrapper class for rosidl_dynamic_typesupport_dynamic_data_t +/// STUBBED OUT +class DynamicMessage : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessage) + + RCLCPP_PUBLIC + virtual ~DynamicMessage(); + +protected: + // NOTE(methylDragon): + // This is just here to extend the lifetime of the serialization support + // It isn't actually used by the builder since the builder should compose its own support + // + // ... Though ideally it should be the exact same support as the one stored in the + // DynamicSerializationSupport + DynamicSerializationSupport::SharedPtr serialization_support_; + + rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data_; + bool is_loaned_; + + // Used for returning the loaned value, and lifetime management + DynamicMessage::SharedPtr parent_data_; + +private: + RCLCPP_DISABLE_COPY(DynamicMessage) + + RCLCPP_PUBLIC + DynamicMessage(); +}; + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp new file mode 100644 index 0000000000..93cbabdade --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp @@ -0,0 +1,64 @@ +// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_ + +#include +#include + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +/// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_t` +/// STUBBED OUT +class DynamicMessageType : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageType) + + RCLCPP_PUBLIC + virtual ~DynamicMessageType(); + +protected: + // NOTE(methylDragon): + // This is just here to extend the lifetime of the serialization support + // It isn't actually used by the builder since the builder should compose its own support + // + // ... Though ideally it should be the exact same support as the one stored in the + // `DynamicSerializationSupport` + DynamicSerializationSupport::SharedPtr serialization_support_; + + rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type_; + +private: + RCLCPP_DISABLE_COPY(DynamicMessageType) + + RCLCPP_PUBLIC + DynamicMessageType(); +}; + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp new file mode 100644 index 0000000000..90a768ead9 --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp @@ -0,0 +1,65 @@ +// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_ + +#include +#include + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +/// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_builder_t *` +/// STUBBED OUT +class DynamicMessageTypeBuilder : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeBuilder) + + RCLCPP_PUBLIC + virtual ~DynamicMessageTypeBuilder(); + +protected: + // NOTE(methylDragon): + // This is just here to extend the lifetime of the serialization support + // It isn't actually used by the builder since the builder should compose its own support + // + // ... Though ideally it should be the exact same support as the one stored in the + // `DynamicSerializationSupport` + DynamicSerializationSupport::SharedPtr serialization_support_; + + rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder_; + +private: + RCLCPP_DISABLE_COPY(DynamicMessageTypeBuilder) + + RCLCPP_PUBLIC + DynamicMessageTypeBuilder(); +}; + +} // namespace dynamic_typesupport +} // namespace rclcpp + + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp new file mode 100644 index 0000000000..5b9f68532c --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp @@ -0,0 +1,67 @@ +// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" + +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +/// Utility wrapper class for `rosidl_message_type_support_t` containing managed +/// STUBBED OUT +class DynamicMessageTypeSupport : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeSupport) + + RCLCPP_PUBLIC + virtual ~DynamicMessageTypeSupport(); + +protected: + DynamicSerializationSupport::SharedPtr serialization_support_; + DynamicMessageType::SharedPtr dynamic_message_type_; + DynamicMessage::SharedPtr dynamic_message_; + + rosidl_message_type_support_t rosidl_message_type_support_; + +private: + RCLCPP_DISABLE_COPY(DynamicMessageTypeSupport) + + RCLCPP_PUBLIC + DynamicMessageTypeSupport(); +}; + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp new file mode 100644 index 0000000000..dde0710d25 --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp @@ -0,0 +1,60 @@ +// Copyright 2023 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__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_ + +#include +#include +#include + +#include +#include + +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +/// Utility wrapper class for rosidl_dynamic_typesupport_serialization_support_t +class DynamicSerializationSupport : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicSerializationSupport) + + RCLCPP_PUBLIC + explicit DynamicSerializationSupport(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicSerializationSupport( + const std::string & serialization_library_name, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + virtual ~DynamicSerializationSupport(); + +protected: + rosidl_dynamic_typesupport_serialization_support_t rosidl_serialization_support_; + +private: + RCLCPP_DISABLE_COPY(DynamicSerializationSupport) +}; + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_ diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp index 12a1c79f8f..dd25e9b919 100644 --- a/rclcpp/include/rclcpp/generic_subscription.hpp +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -84,7 +84,7 @@ class GenericSubscription : public rclcpp::SubscriptionBase options.to_rcl_subscription_options(qos), options.event_callbacks, options.use_default_callbacks, - true), + SubscriptionType::SERIALIZED_MESSAGE), callback_(callback), ts_lib_(ts_lib) {} @@ -123,6 +123,31 @@ class GenericSubscription : public rclcpp::SubscriptionBase RCLCPP_PUBLIC void return_serialized_message(std::shared_ptr & message) override; + + // DYNAMIC TYPE ================================================================================== + RCLCPP_PUBLIC + rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr get_shared_dynamic_message_type() + override; + + RCLCPP_PUBLIC + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr get_shared_dynamic_message() override; + + RCLCPP_PUBLIC + rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support() override; + + RCLCPP_PUBLIC + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr create_dynamic_message() override; + + RCLCPP_PUBLIC + void return_dynamic_message( + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override; + + RCLCPP_PUBLIC + void handle_dynamic_message( + const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message, + const rclcpp::MessageInfo & message_info) override; + private: RCLCPP_DISABLE_COPY(GenericSubscription) diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index f1d751ff3f..ef587578e2 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -117,6 +117,18 @@ * - Allocator related items: * - rclcpp/allocator/allocator_common.hpp * - rclcpp/allocator/allocator_deleter.hpp + * - Dynamic typesupport wrappers + * - rclcpp::dynamic_typesupport::DynamicMessage + * - rclcpp::dynamic_typesupport::DynamicMessageType + * - rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder + * - rclcpp::dynamic_typesupport::DynamicSerializationSupport + * - rclcpp/dynamic_typesupport/dynamic_message.hpp + * - rclcpp/dynamic_typesupport/dynamic_message_type.hpp + * - rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp + * - rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp + * - Dynamic typesupport + * - rclcpp::dynamic_typesupport::DynamicMessageTypeSupport + * - rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp * - Generic publisher * - rclcpp::Node::create_generic_publisher() * - rclcpp::GenericPublisher diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index d9e84b29f8..4111d17af9 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -144,7 +144,7 @@ class Subscription : public SubscriptionBase // NOTE(methylDragon): Passing these args separately is necessary for event binding options.event_callbacks, options.use_default_callbacks, - callback.is_serialized_message_callback()), + callback.is_serialized_message_callback() ? SubscriptionType::SERIALIZED_MESSAGE : SubscriptionType::ROS_MESSAGE), // NOLINT any_callback_(callback), options_(options), message_memory_strategy_(message_memory_strategy) @@ -388,6 +388,57 @@ class Subscription : public SubscriptionBase return any_callback_.use_take_shared_method(); } + // DYNAMIC TYPE ================================================================================== + // TODO(methylDragon): Reorder later + // TODO(methylDragon): Implement later... + rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr + get_shared_dynamic_message_type() override + { + throw rclcpp::exceptions::UnimplementedError( + "get_shared_dynamic_message_type is not implemented for Subscription"); + } + + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr + get_shared_dynamic_message() override + { + throw rclcpp::exceptions::UnimplementedError( + "get_shared_dynamic_message is not implemented for Subscription"); + } + + rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support() override + { + throw rclcpp::exceptions::UnimplementedError( + "get_shared_dynamic_serialization_support is not implemented for Subscription"); + } + + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr + create_dynamic_message() override + { + throw rclcpp::exceptions::UnimplementedError( + "create_dynamic_message is not implemented for Subscription"); + } + + void + return_dynamic_message( + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override + { + (void) message; + throw rclcpp::exceptions::UnimplementedError( + "return_dynamic_message is not implemented for Subscription"); + } + + void + handle_dynamic_message( + const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message, + const rclcpp::MessageInfo & message_info) override + { + (void) message; + (void) message_info; + throw rclcpp::exceptions::UnimplementedError( + "handle_dynamic_message is not implemented for Subscription"); + } + private: RCLCPP_DISABLE_COPY(Subscription) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 52057a39d2..a304e69187 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -31,6 +31,9 @@ #include "rclcpp/any_subscription_callback.hpp" #include "rclcpp/detail/cpp_callback_trampoline.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" #include "rclcpp/experimental/intra_process_manager.hpp" #include "rclcpp/experimental/subscription_intra_process_base.hpp" #include "rclcpp/macros.hpp" @@ -60,6 +63,15 @@ namespace experimental class IntraProcessManager; } // namespace experimental +enum class SubscriptionType : uint8_t +{ + INVALID = 0, // The subscription type is most likely uninitialized + ROS_MESSAGE = 1, // take message as ROS message and handle as ROS message + SERIALIZED_MESSAGE = 2, // take message as serialized and handle as serialized + DYNAMIC_MESSAGE_DIRECT = 3, // take message as DynamicMessage and handle as DynamicMessage + DYNAMIC_MESSAGE_FROM_SERIALIZED = 4 // take message as serialized and handle as DynamicMessage +}; + /// Virtual base class for subscriptions. This pattern allows us to iterate over different template /// specializations of Subscription, among other things. class SubscriptionBase : public std::enable_shared_from_this @@ -76,7 +88,7 @@ class SubscriptionBase : public std::enable_shared_from_this * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic. * \param[in] topic_name Name of the topic to subscribe to. * \param[in] subscription_options Options for the subscription. - * \param[in] is_serialized is true if the message will be delivered still serialized + * \param[in] subscription_type Enum flag to change how the message will be received and delivered */ RCLCPP_PUBLIC SubscriptionBase( @@ -86,7 +98,7 @@ class SubscriptionBase : public std::enable_shared_from_this const rcl_subscription_options_t & subscription_options, const SubscriptionEventCallbacks & event_callbacks, bool use_default_callbacks, - bool is_serialized = false); + SubscriptionType subscription_type = SubscriptionType::ROS_MESSAGE); /// Destructor. RCLCPP_PUBLIC @@ -235,6 +247,14 @@ class SubscriptionBase : public std::enable_shared_from_this bool is_serialized() const; + /// Return the type of the subscription. + /** + * \return `SubscriptionType`, which adjusts how messages are received and delivered. + */ + RCLCPP_PUBLIC + SubscriptionType + get_subscription_type() const; + /// Get matching publisher count. /** \return The number of publishers on this topic. */ RCLCPP_PUBLIC @@ -535,6 +555,49 @@ class SubscriptionBase : public std::enable_shared_from_this rclcpp::ContentFilterOptions get_content_filter() const; + // DYNAMIC TYPE ================================================================================== + // TODO(methylDragon): Reorder later + RCLCPP_PUBLIC + virtual + rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr + get_shared_dynamic_message_type() = 0; + + RCLCPP_PUBLIC + virtual + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr + get_shared_dynamic_message() = 0; + + RCLCPP_PUBLIC + virtual + rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support() = 0; + + /// Borrow a new serialized message (this clones!) + /** \return Shared pointer to a rclcpp::dynamic_typesupport::DynamicMessage. */ + RCLCPP_PUBLIC + virtual + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr + create_dynamic_message() = 0; + + RCLCPP_PUBLIC + virtual + void + return_dynamic_message(rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) = 0; + + RCLCPP_PUBLIC + virtual + void + handle_dynamic_message( + const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message, + const rclcpp::MessageInfo & message_info) = 0; + + RCLCPP_PUBLIC + bool + take_dynamic_message( + rclcpp::dynamic_typesupport::DynamicMessage & message_out, + rclcpp::MessageInfo & message_info_out); + // =============================================================================================== + protected: template void @@ -587,7 +650,7 @@ class SubscriptionBase : public std::enable_shared_from_this RCLCPP_DISABLE_COPY(SubscriptionBase) rosidl_message_type_support_t type_support_; - bool is_serialized_; + SubscriptionType subscription_type_; std::atomic subscription_in_use_by_wait_set_{false}; std::atomic intra_process_subscription_waitable_in_use_by_wait_set_{false}; diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 38773e2ec2..f7c89260bc 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -39,6 +39,7 @@ rcpputils rcutils rmw + rosidl_dynamic_typesupport statistics_msgs tracetools diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp new file mode 100644 index 0000000000..c340b107b6 --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp @@ -0,0 +1,40 @@ +// Copyright 2023 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. + +#include +#include +#include +#include + +#include +#include + +#include "rcl/allocator.h" +#include "rcl/types.h" +#include "rcutils/logging_macros.h" + +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/exceptions.hpp" + + +using rclcpp::dynamic_typesupport::DynamicMessage; +using rclcpp::dynamic_typesupport::DynamicMessageType; +using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder; +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + +DynamicMessage::~DynamicMessage() +{} // STUBBED diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp new file mode 100644 index 0000000000..069aff03aa --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp @@ -0,0 +1,38 @@ +// Copyright 2023 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. + +#include +#include +#include +#include + +#include +#include + +#include "rcutils/logging_macros.h" + +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/exceptions.hpp" + + +using rclcpp::dynamic_typesupport::DynamicMessage; +using rclcpp::dynamic_typesupport::DynamicMessageType; +using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder; +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + +DynamicMessageType::~DynamicMessageType() +{} // STUBBED diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp new file mode 100644 index 0000000000..1c5d3708a1 --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp @@ -0,0 +1,37 @@ +// Copyright 2023 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. + +#include +#include +#include +#include + +#include +#include + +#include "rcutils/logging_macros.h" + +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/exceptions.hpp" + +using rclcpp::dynamic_typesupport::DynamicMessage; +using rclcpp::dynamic_typesupport::DynamicMessageType; +using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder; +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + +DynamicMessageTypeBuilder::~DynamicMessageTypeBuilder() +{} // STUBBED diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp new file mode 100644 index 0000000000..3a920c2805 --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp @@ -0,0 +1,49 @@ +// Copyright 2023 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. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "rcl/allocator.h" +#include "rcl/dynamic_message_type_support.h" +#include "rcl/type_hash.h" +#include "rcl/types.h" +#include "rcutils/logging_macros.h" +#include "rcutils/types/rcutils_ret.h" +#include "rmw/dynamic_message_type_support.h" + +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +using rclcpp::dynamic_typesupport::DynamicMessage; +using rclcpp::dynamic_typesupport::DynamicMessageType; +using rclcpp::dynamic_typesupport::DynamicMessageTypeSupport; +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + +DynamicMessageTypeSupport::~DynamicMessageTypeSupport() +{} // STUBBED diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp new file mode 100644 index 0000000000..769f5fb8cf --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp @@ -0,0 +1,46 @@ +// Copyright 2023 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. + +#include +#include +#include +#include +#include + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/exceptions.hpp" + +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + +// CONSTRUCTION ==================================================================================== +DynamicSerializationSupport::DynamicSerializationSupport(rcl_allocator_t allocator) +: DynamicSerializationSupport::DynamicSerializationSupport("", allocator) +{ + throw std::runtime_error("Unimplemented"); +} + +DynamicSerializationSupport::DynamicSerializationSupport( + const std::string & /*serialization_library_name*/, + rcl_allocator_t /*allocator*/) +: rosidl_serialization_support_( + rosidl_dynamic_typesupport_get_zero_initialized_serialization_support()) +{ + throw std::runtime_error("Unimplemented"); +} + +DynamicSerializationSupport::~DynamicSerializationSupport() +{} // STUBBED diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 9bafbe3106..45059bb953 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -24,6 +24,7 @@ #include "rcl/error_handling.h" #include "rcpputils/scope_exit.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/executor.hpp" #include "rclcpp/guard_condition.hpp" @@ -598,70 +599,104 @@ take_and_do_error_handling( void Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) { + using rclcpp::dynamic_typesupport::DynamicMessage; + rclcpp::MessageInfo message_info; message_info.get_rmw_message_info().from_intra_process = false; - if (subscription->is_serialized()) { - // This is the case where a copy of the serialized message is taken from - // the middleware via inter-process communication. - std::shared_ptr serialized_msg = subscription->create_serialized_message(); - take_and_do_error_handling( - "taking a serialized message from topic", - subscription->get_topic_name(), - [&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);}, - [&]() - { - subscription->handle_serialized_message(serialized_msg, message_info); - }); - subscription->return_serialized_message(serialized_msg); - } else if (subscription->can_loan_messages()) { - // This is the case where a loaned message is taken from the middleware via - // inter-process communication, given to the user for their callback, - // and then returned. - void * loaned_msg = nullptr; - // TODO(wjwwood): refactor this into methods on subscription when LoanedMessage - // is extened to support subscriptions as well. - take_and_do_error_handling( - "taking a loaned message from topic", - subscription->get_topic_name(), - [&]() + switch (subscription->get_subscription_type()) { + // Take ROS message + case rclcpp::SubscriptionType::ROS_MESSAGE: { - rcl_ret_t ret = rcl_take_loaned_message( - subscription->get_subscription_handle().get(), - &loaned_msg, - &message_info.get_rmw_message_info(), - nullptr); - if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { - return false; - } else if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + if (subscription->can_loan_messages()) { + // This is the case where a loaned message is taken from the middleware via + // inter-process communication, given to the user for their callback, + // and then returned. + void * loaned_msg = nullptr; + // TODO(wjwwood): refactor this into methods on subscription when LoanedMessage + // is extened to support subscriptions as well. + take_and_do_error_handling( + "taking a loaned message from topic", + subscription->get_topic_name(), + [&]() + { + rcl_ret_t ret = rcl_take_loaned_message( + subscription->get_subscription_handle().get(), + &loaned_msg, + &message_info.get_rmw_message_info(), + nullptr); + if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { + return false; + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + return true; + }, + [&]() {subscription->handle_loaned_message(loaned_msg, message_info);}); + if (nullptr != loaned_msg) { + rcl_ret_t ret = rcl_return_loaned_message_from_subscription( + subscription->get_subscription_handle().get(), loaned_msg); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "rcl_return_loaned_message_from_subscription() failed for subscription on topic " + "'%s': %s", + subscription->get_topic_name(), rcl_get_error_string().str); + } + loaned_msg = nullptr; + } + } else { + // This case is taking a copy of the message data from the middleware via + // inter-process communication. + std::shared_ptr message = subscription->create_message(); + take_and_do_error_handling( + "taking a message from topic", + subscription->get_topic_name(), + [&]() {return subscription->take_type_erased(message.get(), message_info);}, + [&]() {subscription->handle_message(message, message_info);}); + subscription->return_message(message); } - return true; - }, - [&]() {subscription->handle_loaned_message(loaned_msg, message_info);}); - if (nullptr != loaned_msg) { - rcl_ret_t ret = rcl_return_loaned_message_from_subscription( - subscription->get_subscription_handle().get(), - loaned_msg); - if (RCL_RET_OK != ret) { - RCLCPP_ERROR( - rclcpp::get_logger("rclcpp"), - "rcl_return_loaned_message_from_subscription() failed for subscription on topic '%s': %s", - subscription->get_topic_name(), rcl_get_error_string().str); + break; + } + + // Take serialized message + case rclcpp::SubscriptionType::SERIALIZED_MESSAGE: + { + // This is the case where a copy of the serialized message is taken from + // the middleware via inter-process communication. + std::shared_ptr serialized_msg = + subscription->create_serialized_message(); + take_and_do_error_handling( + "taking a serialized message from topic", + subscription->get_topic_name(), + [&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);}, + [&]() + { + subscription->handle_serialized_message(serialized_msg, message_info); + }); + subscription->return_serialized_message(serialized_msg); + break; + } + + // DYNAMIC SUBSCRIPTION ======================================================================== + // Take dynamic message directly from the middleware + case rclcpp::SubscriptionType::DYNAMIC_MESSAGE_DIRECT: + { + throw std::runtime_error("Unimplemented"); + } + + // Take serialized and then convert to dynamic message + case rclcpp::SubscriptionType::DYNAMIC_MESSAGE_FROM_SERIALIZED: + { + throw std::runtime_error("Unimplemented"); + } + + default: + { + throw std::runtime_error("Subscription type is not supported"); } - loaned_msg = nullptr; - } - } else { - // This case is taking a copy of the message data from the middleware via - // inter-process communication. - std::shared_ptr message = subscription->create_message(); - take_and_do_error_handling( - "taking a message from topic", - subscription->get_topic_name(), - [&]() {return subscription->take_type_erased(message.get(), message_info);}, - [&]() {subscription->handle_message(message, message_info);}); - subscription->return_message(message); } + return; } void diff --git a/rclcpp/src/rclcpp/generic_subscription.cpp b/rclcpp/src/rclcpp/generic_subscription.cpp index cc50955773..e6e61add24 100644 --- a/rclcpp/src/rclcpp/generic_subscription.cpp +++ b/rclcpp/src/rclcpp/generic_subscription.cpp @@ -25,17 +25,20 @@ namespace rclcpp { -std::shared_ptr GenericSubscription::create_message() +std::shared_ptr +GenericSubscription::create_message() { return create_serialized_message(); } -std::shared_ptr GenericSubscription::create_serialized_message() +std::shared_ptr +GenericSubscription::create_serialized_message() { return std::make_shared(0); } -void GenericSubscription::handle_message( +void +GenericSubscription::handle_message( std::shared_ptr &, const rclcpp::MessageInfo &) { @@ -51,7 +54,8 @@ GenericSubscription::handle_serialized_message( callback_(message); } -void GenericSubscription::handle_loaned_message( +void +GenericSubscription::handle_loaned_message( void * message, const rclcpp::MessageInfo & message_info) { (void) message; @@ -60,16 +64,69 @@ void GenericSubscription::handle_loaned_message( "handle_loaned_message is not implemented for GenericSubscription"); } -void GenericSubscription::return_message(std::shared_ptr & message) +void +GenericSubscription::return_message(std::shared_ptr & message) { auto typed_message = std::static_pointer_cast(message); return_serialized_message(typed_message); } -void GenericSubscription::return_serialized_message( +void +GenericSubscription::return_serialized_message( std::shared_ptr & message) { message.reset(); } + +// DYNAMIC TYPE ==================================================================================== +// TODO(methylDragon): Reorder later +rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr +GenericSubscription::get_shared_dynamic_message_type() +{ + throw rclcpp::exceptions::UnimplementedError( + "get_shared_dynamic_message_type is not implemented for GenericSubscription"); +} + +rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr +GenericSubscription::get_shared_dynamic_message() +{ + throw rclcpp::exceptions::UnimplementedError( + "get_shared_dynamic_message is not implemented for GenericSubscription"); +} + +rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr +GenericSubscription::get_shared_dynamic_serialization_support() +{ + throw rclcpp::exceptions::UnimplementedError( + "get_shared_dynamic_serialization_support is not implemented for GenericSubscription"); +} + +rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr +GenericSubscription::create_dynamic_message() +{ + throw rclcpp::exceptions::UnimplementedError( + "create_dynamic_message is not implemented for GenericSubscription"); +} + +void +GenericSubscription::return_dynamic_message( + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) +{ + (void) message; + throw rclcpp::exceptions::UnimplementedError( + "return_dynamic_message is not implemented for GenericSubscription"); +} + +void +GenericSubscription::handle_dynamic_message( + const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message, + const rclcpp::MessageInfo & message_info) +{ + (void) message; + (void) message_info; + throw rclcpp::exceptions::UnimplementedError( + "handle_dynamic_message is not implemented for GenericSubscription"); +} + } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index e95cb4ac49..38271cde09 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -22,6 +22,7 @@ #include "rcpputils/scope_exit.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/experimental/intra_process_manager.hpp" @@ -32,6 +33,8 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" +#include "rosidl_dynamic_typesupport/types.h" + using rclcpp::SubscriptionBase; SubscriptionBase::SubscriptionBase( @@ -41,7 +44,7 @@ SubscriptionBase::SubscriptionBase( const rcl_subscription_options_t & subscription_options, const SubscriptionEventCallbacks & event_callbacks, bool use_default_callbacks, - bool is_serialized) + SubscriptionType subscription_type) : node_base_(node_base), node_handle_(node_base_->get_shared_rcl_node_handle()), node_logger_(rclcpp::get_node_logger(node_handle_.get())), @@ -49,8 +52,16 @@ SubscriptionBase::SubscriptionBase( intra_process_subscription_id_(0), event_callbacks_(event_callbacks), type_support_(type_support_handle), - is_serialized_(is_serialized) + subscription_type_(subscription_type) { + if (!rmw_feature_supported(RMW_MIDDLEWARE_CAN_TAKE_DYNAMIC_MESSAGE) && + subscription_type == rclcpp::SubscriptionType::DYNAMIC_MESSAGE_DIRECT) + { + throw std::runtime_error( + "Cannot set subscription to take dynamic message directly, feature not supported in rmw" + ); + } + auto custom_deletor = [node_handle = this->node_handle_](rcl_subscription_t * rcl_subs) { if (rcl_subscription_fini(rcl_subs, node_handle.get()) != RCL_RET_OK) { @@ -258,7 +269,13 @@ SubscriptionBase::get_message_type_support_handle() const bool SubscriptionBase::is_serialized() const { - return is_serialized_; + return subscription_type_ == rclcpp::SubscriptionType::SERIALIZED_MESSAGE; +} + +rclcpp::SubscriptionType +SubscriptionBase::get_subscription_type() const +{ + return subscription_type_; } size_t @@ -442,8 +459,7 @@ SubscriptionBase::set_content_filter( rcl_subscription_content_filter_options_t options = rcl_get_zero_initialized_subscription_content_filter_options(); - std::vector cstrings = - get_c_vector_string(expression_parameters); + std::vector cstrings = get_c_vector_string(expression_parameters); rcl_ret_t ret = rcl_subscription_content_filter_options_init( subscription_handle_.get(), get_c_string(filter_expression), @@ -515,3 +531,14 @@ SubscriptionBase::get_content_filter() const return ret_options; } + + +// DYNAMIC TYPE ================================================================================== +bool +SubscriptionBase::take_dynamic_message( + rclcpp::dynamic_typesupport::DynamicMessage & /*message_out*/, + rclcpp::MessageInfo & /*message_info_out*/) +{ + throw std::runtime_error("Unimplemented"); + return false; +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp index ecfc89a6aa..e8f873f693 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp @@ -28,6 +28,11 @@ #include "../../mocking_utils/patch.hpp" #include "../../utils/rclcpp_gtest_macros.hpp" +using rclcpp::dynamic_typesupport::DynamicMessage; +using rclcpp::dynamic_typesupport::DynamicMessageType; +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + + namespace { @@ -77,6 +82,18 @@ class TestSubscription : public rclcpp::SubscriptionBase const std::shared_ptr &, const rclcpp::MessageInfo &) override {} void return_message(std::shared_ptr &) override {} void return_serialized_message(std::shared_ptr &) override {} + + DynamicMessageType::SharedPtr get_shared_dynamic_message_type() override {return nullptr;} + DynamicMessage::SharedPtr get_shared_dynamic_message() override {return nullptr;} + DynamicSerializationSupport::SharedPtr get_shared_dynamic_serialization_support() override + { + return nullptr; + } + DynamicMessage::SharedPtr create_dynamic_message() override {return nullptr;} + void return_dynamic_message(DynamicMessage::SharedPtr &) override {} + void handle_dynamic_message( + const DynamicMessage::SharedPtr &, + const rclcpp::MessageInfo &) override {} }; class TestNodeTopics : public ::testing::Test From 82a693e02862eb9aa6642e5607a723b222fb76c9 Mon Sep 17 00:00:00 2001 From: ymski <114902604+ymski@users.noreply.github.com> Date: Fri, 14 Apr 2023 04:44:02 +0900 Subject: [PATCH 212/455] applied tracepoints for ring_buffer (#2091) applied tracepoints for intra_publish add tracepoints for linking buffer and subscription Signed-off-by: Kodai Yamasaki <114902604+ymski@users.noreply.github.com> --- .../buffers/intra_process_buffer.hpp | 5 +++++ .../buffers/ring_buffer_implementation.hpp | 18 +++++++++++++++++- .../subscription_intra_process_buffer.hpp | 6 ++++++ rclcpp/include/rclcpp/publisher.hpp | 12 ++++++++++++ 4 files changed, 40 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp index c2e747e774..05092bb23b 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp @@ -24,6 +24,7 @@ #include "rclcpp/allocator/allocator_deleter.hpp" #include "rclcpp/experimental/buffers/buffer_implementation_base.hpp" #include "rclcpp/macros.hpp" +#include "tracetools/tracetools.h" namespace rclcpp { @@ -94,6 +95,10 @@ class TypedIntraProcessBuffer : public IntraProcessBuffer(buffer_.get()), + static_cast(this)); if (!allocator) { message_allocator_ = std::make_shared(); } else { diff --git a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp index 245d417d86..2c06ea6cbe 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp @@ -25,6 +25,7 @@ #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" +#include "tracetools/tracetools.h" namespace rclcpp { @@ -51,6 +52,7 @@ class RingBufferImplementation : public BufferImplementationBase if (capacity == 0) { throw std::invalid_argument("capacity must be a positive, non-zero value"); } + TRACEPOINT(rclcpp_construct_ring_buffer, static_cast(this), capacity_); } virtual ~RingBufferImplementation() {} @@ -67,6 +69,12 @@ class RingBufferImplementation : public BufferImplementationBase write_index_ = next_(write_index_); ring_buffer_[write_index_] = std::move(request); + TRACEPOINT( + rclcpp_ring_buffer_enqueue, + static_cast(this), + write_index_, + size_ + 1, + is_full_()); if (is_full_()) { read_index_ = next_(read_index_); @@ -90,6 +98,11 @@ class RingBufferImplementation : public BufferImplementationBase } auto request = std::move(ring_buffer_[read_index_]); + TRACEPOINT( + rclcpp_ring_buffer_dequeue, + static_cast(this), + read_index_, + size_ - 1); read_index_ = next_(read_index_); size_--; @@ -135,7 +148,10 @@ class RingBufferImplementation : public BufferImplementationBase return is_full_(); } - void clear() {} + void clear() + { + TRACEPOINT(rclcpp_ring_buffer_clear, static_cast(this)); + } private: /// Get the next index value for the ring buffer diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 3c71512677..30debed83a 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -31,6 +31,8 @@ #include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" +#include "tracetools/tracetools.h" + namespace rclcpp { namespace experimental @@ -91,6 +93,10 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff buffer_type, qos_profile, std::make_shared(subscribed_type_allocator_)); + TRACEPOINT( + rclcpp_ipb_to_subscription, + static_cast(buffer_.get()), + static_cast(this)); } bool diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index f4627cc96c..f474a67192 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -484,6 +484,10 @@ class Publisher : public PublisherBase if (!msg) { throw std::runtime_error("cannot publish msg which is a null pointer"); } + TRACEPOINT( + rclcpp_intra_publish, + static_cast(publisher_handle_.get()), + msg.get()); ipm->template do_intra_process_publish( intra_process_publisher_id_, @@ -502,6 +506,10 @@ class Publisher : public PublisherBase if (!msg) { throw std::runtime_error("cannot publish msg which is a null pointer"); } + TRACEPOINT( + rclcpp_intra_publish, + static_cast(publisher_handle_.get()), + msg.get()); ipm->template do_intra_process_publish( intra_process_publisher_id_, @@ -521,6 +529,10 @@ class Publisher : public PublisherBase if (!msg) { throw std::runtime_error("cannot publish msg which is a null pointer"); } + TRACEPOINT( + rclcpp_intra_publish, + static_cast(publisher_handle_.get()), + msg.get()); return ipm->template do_intra_process_publish_and_return_shared( From d4785257782fe3f576646e6d0f3372429d869109 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 13 Apr 2023 19:48:00 +0000 Subject: [PATCH 213/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 20 ++++++++++++++++++++ rclcpp_action/CHANGELOG.rst | 7 +++++++ rclcpp_components/CHANGELOG.rst | 5 +++++ rclcpp_lifecycle/CHANGELOG.rst | 8 ++++++++ 4 files changed, 40 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 746eddda33..fed7eee66e 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* applied tracepoints for ring_buffer (`#2091 `_) +* Dynamic Subscription (REP-2011 Subset): Stubs for rclcpp (`#2165 `_) +* Add type_hash to cpp TopicEndpointInfo (`#2137 `_) +* Trigger the intraprocess guard condition with data (`#2164 `_) +* Minor grammar fix (`#2149 `_) +* Fix unnecessary allocations in executor.cpp (`#2135 `_) +* add Logger::get_effective_level(). (`#2141 `_) +* Remove deprecated header (`#2139 `_) +* Implement matched event (`#2105 `_) +* use allocator via init_options argument. (`#2129 `_) +* Fixes to silence some clang warnings. (`#2127 `_) +* Documentation improvements on the executor (`#2125 `_) +* Avoid losing waitable handles while using MultiThreadedExecutor (`#2109 `_) +* Hook up the incompatible type event inside of rclcpp (`#2069 `_) +* Update all rclcpp packages to C++17. (`#2121 `_) +* Fix clang warning: bugprone-use-after-move (`#2116 `_) +* Contributors: Barry Xu, Chris Lalancette, Christopher Wecht, Emerson Knapp, Michael Carroll, Tomoya Fujita, Yadu, mauropasse, methylDragon, ymski + 19.3.0 (2023-03-01) ------------------- * Fix memory leak in tracetools::get_symbol() (`#2104 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index ca4799f69c..ffd57ee2a1 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,13 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* extract the result response before the callback is issued. (`#2132 `_) +* Update all rclcpp packages to C++17. (`#2121 `_) +* Fix the GoalUUID to_string representation (`#1999 `_) +* Contributors: Chris Lalancette, Nathan Wiebe Neufeldt, Tomoya Fujita + 19.3.0 (2023-03-01) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 77c1c2e390..06b61f9a4f 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update all rclcpp packages to C++17. (`#2121 `_) +* Contributors: Chris Lalancette + 19.3.0 (2023-03-01) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 33ec6af902..e093b917ea 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,14 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fixes to silence some clang warnings. (`#2127 `_) +* Update all rclcpp packages to C++17. (`#2121 `_) +* Use the correct macro for LifecycleNode::get_fully_qualified_name (`#2117 `_) +* add get_fully_qualified_name to rclcpp_lifecycle (`#2115 `_) +* Contributors: Chris Lalancette, Steve Macenski + 19.3.0 (2023-03-01) ------------------- From eaf6edd6b2926370f0561fcf527251f2db011673 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 13 Apr 2023 19:48:07 +0000 Subject: [PATCH 214/455] 20.0.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index fed7eee66e..cb6be1fd3f 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +20.0.0 (2023-04-13) +------------------- * applied tracepoints for ring_buffer (`#2091 `_) * Dynamic Subscription (REP-2011 Subset): Stubs for rclcpp (`#2165 `_) * Add type_hash to cpp TopicEndpointInfo (`#2137 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index f7c89260bc..f940e43472 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 19.3.0 + 20.0.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index ffd57ee2a1..f4e1e540da 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +20.0.0 (2023-04-13) +------------------- * extract the result response before the callback is issued. (`#2132 `_) * Update all rclcpp packages to C++17. (`#2121 `_) * Fix the GoalUUID to_string representation (`#1999 `_) diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index d2b51621e5..35d22fed39 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 19.3.0 + 20.0.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 06b61f9a4f..aaa389eef0 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +20.0.0 (2023-04-13) +------------------- * Update all rclcpp packages to C++17. (`#2121 `_) * Contributors: Chris Lalancette diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index ef51696cb3..8308ce7630 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 19.3.0 + 20.0.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index e093b917ea..e5a66b031c 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +20.0.0 (2023-04-13) +------------------- * Fixes to silence some clang warnings. (`#2127 `_) * Update all rclcpp packages to C++17. (`#2121 `_) * Use the correct macro for LifecycleNode::get_fully_qualified_name (`#2117 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index dc52a2a723..1df53fdfdd 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 19.3.0 + 20.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From fd7a0dc21961227c2ef09d0b0bf46436ba68ea34 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Fri, 14 Apr 2023 03:08:27 +0000 Subject: [PATCH 215/455] Implement deliver message kind (#2168) * Implement deliver message kind Signed-off-by: methylDragon * Add examples to docstring Signed-off-by: methylDragon --------- Signed-off-by: methylDragon --- .../include/rclcpp/generic_subscription.hpp | 2 +- rclcpp/include/rclcpp/subscription.hpp | 2 +- rclcpp/include/rclcpp/subscription_base.hpp | 35 +++++++++++++------ rclcpp/src/rclcpp/executor.cpp | 20 ++++------- rclcpp/src/rclcpp/subscription_base.cpp | 18 +++------- 5 files changed, 38 insertions(+), 39 deletions(-) diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp index dd25e9b919..975a9d0d0d 100644 --- a/rclcpp/include/rclcpp/generic_subscription.hpp +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -84,7 +84,7 @@ class GenericSubscription : public rclcpp::SubscriptionBase options.to_rcl_subscription_options(qos), options.event_callbacks, options.use_default_callbacks, - SubscriptionType::SERIALIZED_MESSAGE), + DeliveredMessageKind::SERIALIZED_MESSAGE), callback_(callback), ts_lib_(ts_lib) {} diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 4111d17af9..eb1a980dd3 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -144,7 +144,7 @@ class Subscription : public SubscriptionBase // NOTE(methylDragon): Passing these args separately is necessary for event binding options.event_callbacks, options.use_default_callbacks, - callback.is_serialized_message_callback() ? SubscriptionType::SERIALIZED_MESSAGE : SubscriptionType::ROS_MESSAGE), // NOLINT + callback.is_serialized_message_callback() ? DeliveredMessageKind::SERIALIZED_MESSAGE : DeliveredMessageKind::ROS_MESSAGE), // NOLINT any_callback_(callback), options_(options), message_memory_strategy_(message_memory_strategy) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index a304e69187..f4957824a5 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -63,13 +63,25 @@ namespace experimental class IntraProcessManager; } // namespace experimental -enum class SubscriptionType : uint8_t +/// The kind of message that the subscription delivers in its callback, used by the executor +/** + * This enum needs to exist because the callback handle is not accessible to the executor's scope. + * + * "Kind" is used since what is being delivered is a category of messages, for example, there are + * different ROS message types that can be delivered, but they're all ROS messages. + * + * As a concrete example, all of the following callbacks will be considered ROS_MESSAGE for + * DeliveredMessageKind: + * - void callback(const std_msgs::msg::String &) + * - void callback(const std::string &) // type adaption + * - void callback(std::unique_ptr) + */ +enum class DeliveredMessageKind : uint8_t { - INVALID = 0, // The subscription type is most likely uninitialized - ROS_MESSAGE = 1, // take message as ROS message and handle as ROS message - SERIALIZED_MESSAGE = 2, // take message as serialized and handle as serialized - DYNAMIC_MESSAGE_DIRECT = 3, // take message as DynamicMessage and handle as DynamicMessage - DYNAMIC_MESSAGE_FROM_SERIALIZED = 4 // take message as serialized and handle as DynamicMessage + INVALID = 0, + ROS_MESSAGE = 1, // The subscription delivers a ROS message to its callback + SERIALIZED_MESSAGE = 2, // The subscription delivers a serialized message to its callback + DYNAMIC_MESSAGE = 3, // The subscription delivers a dynamic message to its callback }; /// Virtual base class for subscriptions. This pattern allows us to iterate over different template @@ -88,7 +100,8 @@ class SubscriptionBase : public std::enable_shared_from_this * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic. * \param[in] topic_name Name of the topic to subscribe to. * \param[in] subscription_options Options for the subscription. - * \param[in] subscription_type Enum flag to change how the message will be received and delivered + * \param[in] delivered_message_kind Enum flag to change how the message will be received and + * delivered */ RCLCPP_PUBLIC SubscriptionBase( @@ -98,7 +111,7 @@ class SubscriptionBase : public std::enable_shared_from_this const rcl_subscription_options_t & subscription_options, const SubscriptionEventCallbacks & event_callbacks, bool use_default_callbacks, - SubscriptionType subscription_type = SubscriptionType::ROS_MESSAGE); + DeliveredMessageKind delivered_message_kind = DeliveredMessageKind::ROS_MESSAGE); /// Destructor. RCLCPP_PUBLIC @@ -249,10 +262,10 @@ class SubscriptionBase : public std::enable_shared_from_this /// Return the type of the subscription. /** - * \return `SubscriptionType`, which adjusts how messages are received and delivered. + * \return `DeliveredMessageKind`, which adjusts how messages are received and delivered. */ RCLCPP_PUBLIC - SubscriptionType + DeliveredMessageKind get_subscription_type() const; /// Get matching publisher count. @@ -650,7 +663,7 @@ class SubscriptionBase : public std::enable_shared_from_this RCLCPP_DISABLE_COPY(SubscriptionBase) rosidl_message_type_support_t type_support_; - SubscriptionType subscription_type_; + DeliveredMessageKind delivered_message_type_; std::atomic subscription_in_use_by_wait_set_{false}; std::atomic intra_process_subscription_waitable_in_use_by_wait_set_{false}; diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 45059bb953..2986cec81e 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -605,8 +605,8 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) message_info.get_rmw_message_info().from_intra_process = false; switch (subscription->get_subscription_type()) { - // Take ROS message - case rclcpp::SubscriptionType::ROS_MESSAGE: + // Deliver ROS message + case rclcpp::DeliveredMessageKind::ROS_MESSAGE: { if (subscription->can_loan_messages()) { // This is the case where a loaned message is taken from the middleware via @@ -659,8 +659,8 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) break; } - // Take serialized message - case rclcpp::SubscriptionType::SERIALIZED_MESSAGE: + // Deliver serialized message + case rclcpp::DeliveredMessageKind::SERIALIZED_MESSAGE: { // This is the case where a copy of the serialized message is taken from // the middleware via inter-process communication. @@ -679,21 +679,15 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) } // DYNAMIC SUBSCRIPTION ======================================================================== - // Take dynamic message directly from the middleware - case rclcpp::SubscriptionType::DYNAMIC_MESSAGE_DIRECT: - { - throw std::runtime_error("Unimplemented"); - } - - // Take serialized and then convert to dynamic message - case rclcpp::SubscriptionType::DYNAMIC_MESSAGE_FROM_SERIALIZED: + // Deliver dynamic message + case rclcpp::DeliveredMessageKind::DYNAMIC_MESSAGE: { throw std::runtime_error("Unimplemented"); } default: { - throw std::runtime_error("Subscription type is not supported"); + throw std::runtime_error("Delivered message kind is not supported"); } } return; diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 38271cde09..18ccab0eb0 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -44,7 +44,7 @@ SubscriptionBase::SubscriptionBase( const rcl_subscription_options_t & subscription_options, const SubscriptionEventCallbacks & event_callbacks, bool use_default_callbacks, - SubscriptionType subscription_type) + DeliveredMessageKind delivered_message_kind) : node_base_(node_base), node_handle_(node_base_->get_shared_rcl_node_handle()), node_logger_(rclcpp::get_node_logger(node_handle_.get())), @@ -52,16 +52,8 @@ SubscriptionBase::SubscriptionBase( intra_process_subscription_id_(0), event_callbacks_(event_callbacks), type_support_(type_support_handle), - subscription_type_(subscription_type) + delivered_message_type_(delivered_message_kind) { - if (!rmw_feature_supported(RMW_MIDDLEWARE_CAN_TAKE_DYNAMIC_MESSAGE) && - subscription_type == rclcpp::SubscriptionType::DYNAMIC_MESSAGE_DIRECT) - { - throw std::runtime_error( - "Cannot set subscription to take dynamic message directly, feature not supported in rmw" - ); - } - auto custom_deletor = [node_handle = this->node_handle_](rcl_subscription_t * rcl_subs) { if (rcl_subscription_fini(rcl_subs, node_handle.get()) != RCL_RET_OK) { @@ -269,13 +261,13 @@ SubscriptionBase::get_message_type_support_handle() const bool SubscriptionBase::is_serialized() const { - return subscription_type_ == rclcpp::SubscriptionType::SERIALIZED_MESSAGE; + return delivered_message_type_ == rclcpp::DeliveredMessageKind::SERIALIZED_MESSAGE; } -rclcpp::SubscriptionType +rclcpp::DeliveredMessageKind SubscriptionBase::get_subscription_type() const { - return subscription_type_; + return delivered_message_type_; } size_t From 6ffd54d61d1ba3f92a32e06d0be810745f359b37 Mon Sep 17 00:00:00 2001 From: Michael Babenko <122300485+michaelbabenko@users.noreply.github.com> Date: Fri, 14 Apr 2023 18:15:03 +0100 Subject: [PATCH 216/455] Support publishing loaned messages in LifecyclePublisher (#2159) * Support loaned messages in LifecyclePublisher Signed-off-by: Michael Babenko --- .../rclcpp_lifecycle/lifecycle_publisher.hpp | 18 ++++++++++++++++++ .../test/test_lifecycle_publisher.cpp | 16 ++++++++++++++++ 2 files changed, 34 insertions(+) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp index fa7c5b0b5e..ccc60c71d4 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -91,6 +91,24 @@ class LifecyclePublisher : public SimpleManagedEntity, rclcpp::Publisher::publish(msg); } + /// LifecyclePublisher publish function + /** + * The publish function checks whether the communication + * was enabled or disabled and forwards the message + * to the actual rclcpp Publisher base class + */ + virtual void + publish( + rclcpp::LoanedMessage::ROSMessageType, Alloc> && loaned_msg) + { + if (!this->is_activated()) { + log_publisher_not_enabled(); + return; + } + rclcpp::Publisher::publish(std::move(loaned_msg)); + } + void on_activate() override { diff --git a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp index 2305276d38..ac93bd86a7 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp @@ -129,6 +129,10 @@ TEST_P(TestLifecyclePublisher, publish_managed_by_node) { auto msg_ptr = std::make_unique(); EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr))); } + { + auto loaned_msg = node_->publisher()->borrow_loaned_message(); + EXPECT_NO_THROW(node_->publisher()->publish(std::move(loaned_msg))); + } node_->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE), ret); ASSERT_EQ(success, ret); @@ -143,6 +147,10 @@ TEST_P(TestLifecyclePublisher, publish_managed_by_node) { auto msg_ptr = std::make_unique(); EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr))); } + { + auto loaned_msg = node_->publisher()->borrow_loaned_message(); + EXPECT_NO_THROW(node_->publisher()->publish(std::move(loaned_msg))); + } } TEST_P(TestLifecyclePublisher, publish) { @@ -157,6 +165,10 @@ TEST_P(TestLifecyclePublisher, publish) { auto msg_ptr = std::make_unique(); EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr))); } + { + auto loaned_msg = node_->publisher()->borrow_loaned_message(); + EXPECT_NO_THROW(node_->publisher()->publish(std::move(loaned_msg))); + } node_->publisher()->on_activate(); EXPECT_TRUE(node_->publisher()->is_activated()); { @@ -167,6 +179,10 @@ TEST_P(TestLifecyclePublisher, publish) { auto msg_ptr = std::make_unique(); EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr))); } + { + auto loaned_msg = node_->publisher()->borrow_loaned_message(); + EXPECT_NO_THROW(node_->publisher()->publish(std::move(loaned_msg))); + } } INSTANTIATE_TEST_SUITE_P( From e3d9d819af09d3aced7f887b5c9add75f5eaa514 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 14 Apr 2023 13:56:41 -0500 Subject: [PATCH 217/455] Create common structures for executors to use (#2143) * Deprecate callback_group call taking context * Add base executor objects that can be used by implementors * Template common operations * Add callback to EntitiesCollector constructor * Make function to check automatically added callback groups take a list * Make executor own the notify waitable * Add pending queue to collector, remove from waitable Also change node's get_guard_condition to return shared_ptr * Change interrupt guard condition to shared_ptr Check if guard condition is valid before adding it to the waitable * Make get_notify_guard_condition follow API tick-tock * Improve callback group tick-tocking * Add thread safety annotations and make locks consistent * Remove the "add_valid_node" API * Only notify if the trigger condition is valid * Only trigger if valid and needed Signed-off-by: Michael Carroll --- rclcpp/CMakeLists.txt | 3 + rclcpp/include/rclcpp/callback_group.hpp | 60 ++- rclcpp/include/rclcpp/executor.hpp | 3 +- .../executor_entities_collection.hpp | 212 +++++++++ .../executors/executor_entities_collector.hpp | 270 ++++++++++++ .../executors/executor_notify_waitable.hpp | 129 ++++++ .../rclcpp/node_interfaces/node_base.hpp | 11 +- .../node_interfaces/node_base_interface.hpp | 22 +- rclcpp/src/rclcpp/callback_group.cpp | 28 +- rclcpp/src/rclcpp/executor.cpp | 28 +- .../executor_entities_collection.cpp | 203 +++++++++ .../executors/executor_entities_collector.cpp | 416 ++++++++++++++++++ .../executors/executor_notify_waitable.cpp | 129 ++++++ .../static_executor_entities_collector.cpp | 4 +- .../static_single_threaded_executor.cpp | 8 +- .../src/rclcpp/node_interfaces/node_base.cpp | 42 +- .../src/rclcpp/node_interfaces/node_graph.cpp | 3 +- .../rclcpp/node_interfaces/node_services.cpp | 6 +- .../rclcpp/node_interfaces/node_timers.cpp | 3 +- .../rclcpp/node_interfaces/node_topics.cpp | 6 +- .../rclcpp/node_interfaces/node_waitables.cpp | 3 +- rclcpp/test/rclcpp/CMakeLists.txt | 18 + .../executors/test_entities_collector.cpp | 320 ++++++++++++++ .../test_executor_notify_waitable.cpp | 97 ++++ 24 files changed, 1981 insertions(+), 43 deletions(-) create mode 100644 rclcpp/include/rclcpp/executors/executor_entities_collection.hpp create mode 100644 rclcpp/include/rclcpp/executors/executor_entities_collector.hpp create mode 100644 rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp create mode 100644 rclcpp/src/rclcpp/executors/executor_entities_collection.cpp create mode 100644 rclcpp/src/rclcpp/executors/executor_entities_collector.cpp create mode 100644 rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp create mode 100644 rclcpp/test/rclcpp/executors/test_entities_collector.cpp create mode 100644 rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 96351f7528..3bd558de6e 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -59,6 +59,9 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/executable_list.cpp src/rclcpp/executor.cpp src/rclcpp/executors.cpp + src/rclcpp/executors/executor_entities_collection.cpp + src/rclcpp/executors/executor_entities_collector.cpp + src/rclcpp/executors/executor_notify_waitable.cpp src/rclcpp/executors/multi_threaded_executor.cpp src/rclcpp/executors/single_threaded_executor.cpp src/rclcpp/executors/static_executor_entities_collector.cpp diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index 7d03edf343..97579fcf8c 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -93,11 +93,54 @@ class CallbackGroup * determines whether a callback group is automatically added to an executor * with the node with which it is associated. */ + [[deprecated("Use CallbackGroup constructor with context function argument")]] RCLCPP_PUBLIC explicit CallbackGroup( CallbackGroupType group_type, bool automatically_add_to_executor_with_node = true); + /// Constructor for CallbackGroup. + /** + * Callback Groups have a type, either 'Mutually Exclusive' or 'Reentrant' + * and when creating one the type must be specified. + * + * Callbacks in Reentrant Callback Groups must be able to: + * - run at the same time as themselves (reentrant) + * - run at the same time as other callbacks in their group + * - run at the same time as other callbacks in other groups + * + * Callbacks in Mutually Exclusive Callback Groups: + * - will not be run multiple times simultaneously (non-reentrant) + * - will not be run at the same time as other callbacks in their group + * - but must run at the same time as callbacks in other groups + * + * Additionally, callback groups have a property which determines whether or + * not they are added to an executor with their associated node automatically. + * When creating a callback group the automatically_add_to_executor_with_node + * argument determines this behavior, and if true it will cause the newly + * created callback group to be added to an executor with the node when the + * Executor::add_node method is used. + * If false, this callback group will not be added automatically and would + * have to be added to an executor manually using the + * Executor::add_callback_group method. + * + * Whether the node was added to the executor before creating the callback + * group, or after, is irrelevant; the callback group will be automatically + * added to the executor in either case. + * + * \param[in] group_type The type of the callback group. + * \param[in] get_node_context Lambda to retrieve the node context when + * checking that the creating node is valid and using the guard condition. + * \param[in] automatically_add_to_executor_with_node A boolean that + * determines whether a callback group is automatically added to an executor + * with the node with which it is associated. + */ + RCLCPP_PUBLIC + explicit CallbackGroup( + CallbackGroupType group_type, + std::function get_node_context, + bool automatically_add_to_executor_with_node = true); + /// Default destructor. RCLCPP_PUBLIC ~CallbackGroup(); @@ -178,11 +221,24 @@ class CallbackGroup bool automatically_add_to_executor_with_node() const; - /// Defer creating the notify guard condition and return it. + /// Retrieve the guard condition used to signal changes to this callback group. + /** + * \param[in] context_ptr context to use when creating the guard condition + * \return guard condition if it is valid, otherwise nullptr. + */ + [[deprecated("Use get_notify_guard_condition() without arguments")]] RCLCPP_PUBLIC rclcpp::GuardCondition::SharedPtr get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr); + /// Retrieve the guard condition used to signal changes to this callback group. + /** + * \return guard condition if it is valid, otherwise nullptr. + */ + RCLCPP_PUBLIC + rclcpp::GuardCondition::SharedPtr + get_notify_guard_condition(); + /// Trigger the notify guard condition. RCLCPP_PUBLIC void @@ -234,6 +290,8 @@ class CallbackGroup std::shared_ptr notify_guard_condition_ = nullptr; std::recursive_mutex notify_guard_condition_mutex_; + std::function get_context_; + private: template typename TypeT::SharedPtr _find_ptrs_if_impl( diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 94a8488557..3e654faa54 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -637,8 +637,9 @@ class Executor std::atomic_bool spinning; /// Guard condition for signaling the rmw layer to wake up for special events. - rclcpp::GuardCondition interrupt_guard_condition_; + std::shared_ptr interrupt_guard_condition_; + /// Guard condition for signaling the rmw layer to wake up for system shutdown. std::shared_ptr shutdown_guard_condition_; /// Wait set for managing entities that the rmw layer waits on. diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp new file mode 100644 index 0000000000..98a92ccdd8 --- /dev/null +++ b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp @@ -0,0 +1,212 @@ +// Copyright 2023 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__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_ +#define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace rclcpp +{ +namespace executors +{ + +/// Structure to represent a single entity's entry in a collection +template +struct CollectionEntry +{ + /// Weak pointer to entity type + using EntityWeakPtr = typename EntityValueType::WeakPtr; + /// Shared pointer to entity type + using EntitySharedPtr = typename EntityValueType::SharedPtr; + + /// The entity + EntityWeakPtr entity; + + /// If relevant, the entity's corresponding callback_group + rclcpp::CallbackGroup::WeakPtr callback_group; +}; + +/// Update a collection based on another collection +/* + * Iterates update_from and update_to to see which entities have been added/removed between + * the two collections. + * + * For each new entry (in update_from, but not in update_to), + * add the entity and fire the on_added callback + * For each removed entry (in update_to, but not in update_from), + * remove the entity and fire the on_removed callback. + * + * \param[in] update_from The collection representing the next iteration's state + * \param[inout] update_to The collection representing the current iteration's state + * \param[in] on_added Callback fired when a new entity is detected + * \param[in] on_removed Callback fired when an entity is removed + */ +template +void update_entities( + const CollectionType & update_from, + CollectionType & update_to, + std::function on_added, + std::function on_removed +) +{ + for (auto it = update_to.begin(); it != update_to.end(); ) { + if (update_from.count(it->first) == 0) { + auto entity = it->second.entity.lock(); + if (entity) { + on_removed(entity); + } + it = update_to.erase(it); + } else { + ++it; + } + } + for (auto it = update_from.begin(); it != update_from.end(); ++it) { + if (update_to.count(it->first) == 0) { + auto entity = it->second.entity.lock(); + if (entity) { + on_added(entity); + } + update_to.insert(*it); + } + } +} + +/// A collection of entities, indexed by their corresponding handles +template +class EntityCollection + : public std::unordered_map> +{ +public: + /// Key type of the map + using Key = const EntityKeyType *; + + /// Weak pointer to entity type + using EntityWeakPtr = typename EntityValueType::WeakPtr; + + /// Shared pointer to entity type + using EntitySharedPtr = typename EntityValueType::SharedPtr; + + /// Update this collection based on the contents of another collection + /** + * Update the internal state of this collection, firing callbacks when entities have been + * added or removed. + * + * \param[in] other Collection to compare to + * \param[in] on_added Callback for when entities have been added + * \param[in] on_removed Callback for when entities have been removed + */ + void update( + const EntityCollection & other, + std::function on_added, + std::function on_removed) + { + update_entities(other, *this, on_added, on_removed); + } +}; + +/// Represent the total set of entities for a single executor +/** + * This allows the entities to be stored from ExecutorEntitiesCollector. + * The structure also makes in convenient to re-evaluate when entities have been added or removed. + */ +struct ExecutorEntitiesCollection +{ + /// Collection type for timer entities + using TimerCollection = EntityCollection; + + /// Collection type for subscription entities + using SubscriptionCollection = EntityCollection; + + /// Collection type for client entities + using ClientCollection = EntityCollection; + + /// Collection type for service entities + using ServiceCollection = EntityCollection; + + /// Collection type for waitable entities + using WaitableCollection = EntityCollection; + + /// Collection type for guard condition entities + using GuardConditionCollection = EntityCollection; + + /// Collection of timers currently in use by the executor. + TimerCollection timers; + + /// Collection of subscriptions currently in use by the executor. + SubscriptionCollection subscriptions; + + /// Collection of clients currently in use by the executor. + ClientCollection clients; + + /// Collection of services currently in use by the executor. + ServiceCollection services; + + /// Collection of guard conditions currently in use by the executor. + GuardConditionCollection guard_conditions; + + /// Collection of waitables currently in use by the executor. + WaitableCollection waitables; + + /// Check if the entities collection is empty + /** + * \return true if all member collections are empty, false otherwise + */ + bool empty() const; + + /// Clear the entities collection + void clear(); +}; + +/// Build an entities collection from callback groups +/** + * Iterates a list of callback groups and adds entities from each valid group + * + * \param[in] callback_groups List of callback groups to check for entities + * \param[inout] colletion Entities collection to populate with found entities + */ +void +build_entities_collection( + const std::vector & callback_groups, + ExecutorEntitiesCollection & collection); + +/// Build a queue of executables ready to be executed +/** + * Iterates a list of entities and adds them to a queue if they are ready. + * + * \param[in] collection Collection of entities corresponding to the current wait set. + * \param[in] wait_result Result of rclcpp::WaitSet::wait corresponding to the collection. + * \return A queue of executables that have been marked ready by the waitset. + */ +std::deque +ready_executables( + const ExecutorEntitiesCollection & collection, + rclcpp::WaitResult & wait_result +); + +} // namespace executors +} // namespace rclcpp + +#endif // RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_ diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp new file mode 100644 index 0000000000..ad9bc84fad --- /dev/null +++ b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp @@ -0,0 +1,270 @@ +// Copyright 2023 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__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_ +#define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_ + +#include +#include +#include +#include +#include + +#include "rcpputils/thread_safety_annotations.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace rclcpp +{ +namespace executors +{ + +/// Class to monitor a set of nodes and callback groups for changes in entity membership +/** + * This is to be used with an executor to track the membership of various nodes, groups, + * and entities (timers, subscriptions, clients, services, etc) and report status to the + * executor. + * + * In general, users will add either nodes or callback groups to an executor. + * Each node may have callback groups that are automatically associated with executors, + * or callback groups that must be manually associated with an executor. + * + * This object tracks both types of callback groups as well as nodes that have been + * previously added to the executor. + * When a new callback group is added/removed or new entities are added/removed, the + * corresponding node or callback group will signal this to the executor so that the + * entity collection may be rebuilt according to that executor's implementation. + * + */ +class ExecutorEntitiesCollector +{ +public: + /// Constructor + /** + * \param[in] notify_waitable Waitable that is used to signal to the executor + * when nodes or callback groups have been added or removed. + */ + RCLCPP_PUBLIC + explicit ExecutorEntitiesCollector( + std::shared_ptr notify_waitable); + + /// Destructor + RCLCPP_PUBLIC + ~ExecutorEntitiesCollector(); + + /// Indicate if the entities collector has pending additions or removals. + /** + * \return true if there are pending additions or removals + */ + bool has_pending() const; + + /// Add a node to the entity collector + /** + * \param[in] node_ptr a shared pointer that points to a node base interface + * \throw std::runtime_error if the node is associated with an executor + */ + RCLCPP_PUBLIC + void + add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); + + /// Remove a node from the entity collector + /** + * \param[in] node_ptr a shared pointer that points to a node base interface + * \throw std::runtime_error if the node is associated with an executor + * \throw std::runtime_error if the node is associated with this executor + */ + RCLCPP_PUBLIC + void + remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); + + /// Add a callback group to the entity collector + /** + * \param[in] group_ptr a shared pointer that points to a callback group + * \throw std::runtime_error if the callback_group is associated with an executor + */ + RCLCPP_PUBLIC + void + add_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr); + + /// Remove a callback group from the entity collector + /** + * \param[in] group_ptr a shared pointer that points to a callback group + * \throw std::runtime_error if the callback_group is not associated with an executor + * \throw std::runtime_error if the callback_group is not associated with this executor + */ + RCLCPP_PUBLIC + void + remove_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr); + + /// Get all callback groups known to this entity collector + /** + * This will include manually added and automatically added (node associated) groups + * \return vector of all callback groups + */ + RCLCPP_PUBLIC + std::vector + get_all_callback_groups() const; + + /// Get manually-added callback groups known to this entity collector + /** + * This will include callback groups that have been added via add_callback_group + * \return vector of manually-added callback groups + */ + RCLCPP_PUBLIC + std::vector + get_manually_added_callback_groups() const; + + /// Get automatically-added callback groups known to this entity collector + /** + * This will include callback groups that are associated with nodes added via add_node + * \return vector of automatically-added callback groups + */ + RCLCPP_PUBLIC + std::vector + get_automatically_added_callback_groups() const; + + /// Update the underlying collections + /** + * This will prune nodes and callback groups that are no longer valid as well + * as add new callback groups from any associated nodes. + */ + RCLCPP_PUBLIC + void + update_collections(); + +protected: + using NodeCollection = std::set< + rclcpp::node_interfaces::NodeBaseInterface::WeakPtr, + std::owner_less>; + + using CallbackGroupCollection = std::set< + rclcpp::CallbackGroup::WeakPtr, + std::owner_less>; + + using WeakNodesToGuardConditionsMap = std::map< + rclcpp::node_interfaces::NodeBaseInterface::WeakPtr, + rclcpp::GuardCondition::WeakPtr, + std::owner_less>; + + using WeakGroupsToGuardConditionsMap = std::map< + rclcpp::CallbackGroup::WeakPtr, + rclcpp::GuardCondition::WeakPtr, + std::owner_less>; + + /// Implementation of removing a node from the collector. + /** + * This will disassociate the node from the collector and remove any + * automatically-added callback groups + * + * This takes and returns an iterator so it may be used as: + * + * it = remove_weak_node(it); + * + * \param[in] weak_node iterator to the weak node to be removed + * \return Valid updated iterator in the same collection + */ + RCLCPP_PUBLIC + NodeCollection::iterator + remove_weak_node(NodeCollection::iterator weak_node) RCPPUTILS_TSA_REQUIRES(mutex_); + + /// Implementation of removing a callback group from the collector. + /** + * This will disassociate the callback group from the collector + * + * This takes and returns an iterator so it may be used as: + * + * it = remove_weak_callback_group(it); + * + * \param[in] weak_group_it iterator to the weak group to be removed + * \param[in] collection the collection to remove the group from + * (manually or automatically added) + * \return Valid updated iterator in the same collection + */ + RCLCPP_PUBLIC + CallbackGroupCollection::iterator + remove_weak_callback_group( + CallbackGroupCollection::iterator weak_group_it, + CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_); + + /// Implementation of adding a callback group + /** + * \param[in] group_ptr the group to add + * \param[in] collection the collection to add the group to + */ + RCLCPP_PUBLIC + void + add_callback_group_to_collection( + rclcpp::CallbackGroup::SharedPtr group_ptr, + CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_); + + /// Iterate over queued added/remove nodes and callback_groups + RCLCPP_PUBLIC + void + process_queues() RCPPUTILS_TSA_REQUIRES(mutex_); + + /// Check a collection of nodes and add any new callback_groups that + /// are set to be automatically associated via the node. + RCLCPP_PUBLIC + void + add_automatically_associated_callback_groups( + const NodeCollection & nodes_to_check) RCPPUTILS_TSA_REQUIRES(mutex_); + + /// Check all nodes and group for expired weak pointers and remove them. + RCLCPP_PUBLIC + void + prune_invalid_nodes_and_groups() RCPPUTILS_TSA_REQUIRES(mutex_); + + /// mutex to protect collections and pending queues + mutable std::mutex mutex_; + + /// Callback groups that were added via `add_callback_group` + CallbackGroupCollection manually_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// Callback groups that were added by their association with added nodes + CallbackGroupCollection automatically_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// nodes that are associated with the executor + NodeCollection weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// Track guard conditions associated with added nodes + WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// Track guard conditions associated with added callback groups + WeakGroupsToGuardConditionsMap weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// nodes that have been added since the last update. + NodeCollection pending_added_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// nodes that have been removed since the last update. + NodeCollection pending_removed_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// callback groups that have been added since the last update. + CallbackGroupCollection pending_manually_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// callback groups that have been removed since the last update. + CallbackGroupCollection pending_manually_removed_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// Waitable to add guard conditions to + std::shared_ptr notify_waitable_; +}; +} // namespace executors +} // namespace rclcpp +// +#endif // RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_ diff --git a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp new file mode 100644 index 0000000000..88158952d9 --- /dev/null +++ b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp @@ -0,0 +1,129 @@ +// Copyright 2023 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__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_ +#define RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace executors +{ + +/// Maintain a collection of guard conditions from associated nodes and callback groups +/// to signal to the executor when associated entities have changed. +class ExecutorNotifyWaitable : public rclcpp::Waitable +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(ExecutorNotifyWaitable) + + // Constructor + /** + * \param[in] on_execute_callback Callback to execute when one of the conditions + * of this waitable has signaled the wait_set. + */ + RCLCPP_PUBLIC + explicit ExecutorNotifyWaitable(std::function on_execute_callback = {}); + + // Destructor + RCLCPP_PUBLIC + ~ExecutorNotifyWaitable() override = default; + + RCLCPP_PUBLIC + ExecutorNotifyWaitable(const ExecutorNotifyWaitable & other); + + + RCLCPP_PUBLIC + ExecutorNotifyWaitable & operator=(const ExecutorNotifyWaitable & other); + + /// Add conditions to the wait set + /** + * \param[inout] wait_set structure that conditions will be added to + */ + RCLCPP_PUBLIC + void + add_to_wait_set(rcl_wait_set_t * wait_set) override; + + /// Check conditions against the wait set + /** + * \param[inout] wait_set structure that internal elements will be checked against. + * \return true if this waitable is ready to be executed, false otherwise. + */ + RCLCPP_PUBLIC + bool + is_ready(rcl_wait_set_t * wait_set) override; + + /// Perform work associated with the waitable. + /** + * This will call the callback provided in the constructor. + * \param[in] data Data to be use for the execute, if available, else nullptr. + */ + RCLCPP_PUBLIC + void + execute(std::shared_ptr & data) override; + + /// Retrieve data to be used in the next execute call. + /** + * \return If available, data to be used, otherwise nullptr + */ + RCLCPP_PUBLIC + std::shared_ptr + take_data() override; + + /// Add a guard condition to be waited on. + /** + * \param[in] guard_condition The guard condition to add. + */ + RCLCPP_PUBLIC + void + add_guard_condition(rclcpp::GuardCondition::WeakPtr guard_condition); + + /// Remove a guard condition from being waited on. + /** + * \param[in] guard_condition The guard condition to remove. + */ + RCLCPP_PUBLIC + void + remove_guard_condition(rclcpp::GuardCondition::WeakPtr guard_condition); + + /// Get the number of ready guard_conditions + /** + * \return The number of guard_conditions associated with the Waitable. + */ + RCLCPP_PUBLIC + size_t + get_number_of_ready_guard_conditions() override; + +private: + /// Callback to run when waitable executes + std::function execute_callback_; + + std::mutex guard_condition_mutex_; + + /// The collection of guard conditions to be waited on. + std::set> notify_guard_conditions_; +}; + +} // namespace executors +} // namespace rclcpp + +#endif // RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index a6c84e4aa0..6173a08d50 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -121,10 +121,19 @@ class NodeBase : public NodeBaseInterface, public std::enable_shared_from_this notify_guard_condition_; bool notify_guard_condition_is_valid_; }; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index fd4f64b22b..e5a3198275 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -148,13 +148,33 @@ class NodeBaseInterface /** * For example, this should be notified when a publisher is added or removed. * - * \return the GuardCondition if it is valid, else thow runtime error + * \return the GuardCondition if it is valid, else throw runtime error */ RCLCPP_PUBLIC virtual rclcpp::GuardCondition & get_notify_guard_condition() = 0; + /// Return a guard condition that should be notified when the internal node state changes. + /** + * For example, this should be notified when a publisher is added or removed. + * + * \return the GuardCondition if it is valid, else nullptr + */ + RCLCPP_PUBLIC + virtual + rclcpp::GuardCondition::SharedPtr + get_shared_notify_guard_condition() = 0; + + /// Trigger the guard condition that notifies of internal node state changes. + /** + * For example, this should be notified when a publisher is added or removed. + */ + RCLCPP_PUBLIC + virtual + void + trigger_notify_guard_condition() = 0; + /// Return the default preference for using intra process communication. RCLCPP_PUBLIC virtual diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index 734c781a69..753a441332 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -31,10 +31,12 @@ using rclcpp::CallbackGroupType; CallbackGroup::CallbackGroup( CallbackGroupType group_type, + std::function get_context, bool automatically_add_to_executor_with_node) : type_(group_type), associated_with_executor_(false), can_be_taken_from_(true), - automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node) + automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node), + get_context_(get_context) {} CallbackGroup::~CallbackGroup() @@ -111,6 +113,7 @@ CallbackGroup::automatically_add_to_executor_with_node() const return automatically_add_to_executor_with_node_; } +// \TODO(mjcarroll) Deprecated, remove on tock rclcpp::GuardCondition::SharedPtr CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr) { @@ -129,6 +132,29 @@ CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr conte return notify_guard_condition_; } +rclcpp::GuardCondition::SharedPtr +CallbackGroup::get_notify_guard_condition() +{ + std::lock_guard lock(notify_guard_condition_mutex_); + if (!this->get_context_) { + throw std::runtime_error("Callback group was created without context and not passed context"); + } + auto context_ptr = this->get_context_(); + if (context_ptr && context_ptr->is_valid()) { + if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) { + if (associated_with_executor_) { + trigger_notify_guard_condition(); + } + notify_guard_condition_ = nullptr; + } + if (!notify_guard_condition_) { + notify_guard_condition_ = std::make_shared(context_ptr); + } + return notify_guard_condition_; + } + return nullptr; +} + void CallbackGroup::trigger_notify_guard_condition() { diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 2986cec81e..ccd1b61016 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -39,14 +39,11 @@ using namespace std::chrono_literals; using rclcpp::exceptions::throw_from_rcl_error; -using rclcpp::AnyExecutable; using rclcpp::Executor; -using rclcpp::ExecutorOptions; -using rclcpp::FutureReturnCode; Executor::Executor(const rclcpp::ExecutorOptions & options) : spinning(false), - interrupt_guard_condition_(options.context), + interrupt_guard_condition_(std::make_shared(options.context)), shutdown_guard_condition_(std::make_shared(options.context)), memory_strategy_(options.memory_strategy) { @@ -66,7 +63,7 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) memory_strategy_->add_guard_condition(*shutdown_guard_condition_.get()); // Put the executor's guard condition in - memory_strategy_->add_guard_condition(interrupt_guard_condition_); + memory_strategy_->add_guard_condition(*interrupt_guard_condition_.get()); rcl_allocator_t allocator = memory_strategy_->get_allocator(); rcl_ret_t ret = rcl_wait_set_init( @@ -128,7 +125,7 @@ Executor::~Executor() } // Remove and release the sigint guard condition memory_strategy_->remove_guard_condition(shutdown_guard_condition_.get()); - memory_strategy_->remove_guard_condition(&interrupt_guard_condition_); + memory_strategy_->remove_guard_condition(interrupt_guard_condition_.get()); // Remove shutdown callback handle registered to Context if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) { @@ -223,8 +220,7 @@ Executor::add_callback_group_to_map( weak_groups_to_nodes_.insert(std::make_pair(weak_group_ptr, node_ptr)); if (node_ptr->get_context()->is_valid()) { - auto callback_group_guard_condition = - group_ptr->get_notify_guard_condition(node_ptr->get_context()); + auto callback_group_guard_condition = group_ptr->get_notify_guard_condition(); weak_groups_to_guard_conditions_[weak_group_ptr] = callback_group_guard_condition.get(); // Add the callback_group's notify condition to the guard condition handles memory_strategy_->add_guard_condition(*callback_group_guard_condition); @@ -233,7 +229,7 @@ Executor::add_callback_group_to_map( if (notify) { // Interrupt waiting to handle new node try { - interrupt_guard_condition_.trigger(); + interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string( @@ -281,10 +277,10 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt } }); - const auto & gc = node_ptr->get_notify_guard_condition(); - weak_nodes_to_guard_conditions_[node_ptr] = &gc; + const auto gc = node_ptr->get_shared_notify_guard_condition(); + weak_nodes_to_guard_conditions_[node_ptr] = gc.get(); // Add the node's notify condition to the guard condition handles - memory_strategy_->add_guard_condition(gc); + memory_strategy_->add_guard_condition(*gc); weak_nodes_.push_back(node_ptr); } @@ -321,7 +317,7 @@ Executor::remove_callback_group_from_map( if (notify) { try { - interrupt_guard_condition_.trigger(); + interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string( @@ -389,7 +385,7 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node } } - memory_strategy_->remove_guard_condition(&node_ptr->get_notify_guard_condition()); + memory_strategy_->remove_guard_condition(node_ptr->get_shared_notify_guard_condition().get()); weak_nodes_to_guard_conditions_.erase(node_ptr); std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); @@ -502,7 +498,7 @@ Executor::cancel() { spinning.store(false); try { - interrupt_guard_condition_.trigger(); + interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string("Failed to trigger guard condition in cancel: ") + ex.what()); @@ -551,7 +547,7 @@ Executor::execute_any_executable(AnyExecutable & any_exec) // Wake the wait, because it may need to be recalculated or work that // was previously blocked is now available. try { - interrupt_guard_condition_.trigger(); + interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string( diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp new file mode 100644 index 0000000000..567b28014e --- /dev/null +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -0,0 +1,203 @@ +// Copyright 2023 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. + +#include "rclcpp/executors/executor_entities_collection.hpp" + +namespace rclcpp +{ +namespace executors +{ +bool ExecutorEntitiesCollection::empty() const +{ + return subscriptions.empty() && + timers.empty() && + guard_conditions.empty() && + clients.empty() && + services.empty() && + waitables.empty(); +} + +void ExecutorEntitiesCollection::clear() +{ + subscriptions.clear(); + timers.clear(); + guard_conditions.clear(); + clients.clear(); + services.clear(); + waitables.clear(); +} + + +void +build_entities_collection( + const std::vector & callback_groups, + ExecutorEntitiesCollection & collection) +{ + collection.clear(); + + for (auto weak_group_ptr : callback_groups) { + auto group_ptr = weak_group_ptr.lock(); + if (!group_ptr) { + continue; + } + + if (group_ptr->can_be_taken_from().load()) { + group_ptr->collect_all_ptrs( + [&collection, weak_group_ptr](const rclcpp::SubscriptionBase::SharedPtr & subscription) { + collection.subscriptions.insert( + { + subscription->get_subscription_handle().get(), + {subscription, weak_group_ptr} + }); + }, + [&collection, weak_group_ptr](const rclcpp::ServiceBase::SharedPtr & service) { + collection.services.insert( + { + service->get_service_handle().get(), + {service, weak_group_ptr} + }); + }, + [&collection, weak_group_ptr](const rclcpp::ClientBase::SharedPtr & client) { + collection.clients.insert( + { + client->get_client_handle().get(), + {client, weak_group_ptr} + }); + }, + [&collection, weak_group_ptr](const rclcpp::TimerBase::SharedPtr & timer) { + collection.timers.insert( + { + timer->get_timer_handle().get(), + {timer, weak_group_ptr} + }); + }, + [&collection, weak_group_ptr](const rclcpp::Waitable::SharedPtr & waitable) { + collection.waitables.insert( + { + waitable.get(), + {waitable, weak_group_ptr} + }); + } + ); + } + } +} + +template +void check_ready( + EntityCollectionType & collection, + std::deque & executables, + size_t size_of_waited_entities, + typename EntityCollectionType::Key * waited_entities, + std::function fill_executable) +{ + for (size_t ii = 0; ii < size_of_waited_entities; ++ii) { + if (!waited_entities[ii]) {continue;} + auto entity_iter = collection.find(waited_entities[ii]); + if (entity_iter != collection.end()) { + auto entity = entity_iter->second.entity.lock(); + if (!entity) { + continue; + } + + auto callback_group = entity_iter->second.callback_group.lock(); + if (callback_group && !callback_group->can_be_taken_from().load()) { + continue; + } + rclcpp::AnyExecutable exec; + + exec.callback_group = callback_group; + if (fill_executable(exec, entity)) { + executables.push_back(exec); + } + } + } +} + +std::deque +ready_executables( + const ExecutorEntitiesCollection & collection, + rclcpp::WaitResult & wait_result +) +{ + std::deque ret; + + if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { + return ret; + } + auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set(); + check_ready( + collection.timers, + ret, + rcl_wait_set.size_of_timers, + rcl_wait_set.timers, + [](rclcpp::AnyExecutable & exec, auto timer) { + if (!timer->call()) { + return false; + } + exec.timer = timer; + return true; + }); + + check_ready( + collection.subscriptions, + ret, + rcl_wait_set.size_of_subscriptions, + rcl_wait_set.subscriptions, + [](rclcpp::AnyExecutable & exec, auto subscription) { + exec.subscription = subscription; + return true; + }); + + + check_ready( + collection.services, + ret, + rcl_wait_set.size_of_services, + rcl_wait_set.services, + [](rclcpp::AnyExecutable & exec, auto service) { + exec.service = service; + return true; + }); + + check_ready( + collection.clients, + ret, + rcl_wait_set.size_of_clients, + rcl_wait_set.clients, + [](rclcpp::AnyExecutable & exec, auto client) { + exec.client = client; + return true; + }); + + for (auto & [handle, entry] : collection.waitables) { + auto waitable = entry.entity.lock(); + if (waitable && waitable->is_ready(&rcl_wait_set)) { + auto group = entry.callback_group.lock(); + if (group && !group->can_be_taken_from().load()) { + continue; + } + + rclcpp::AnyExecutable exec; + exec.waitable = waitable; + exec.callback_group = group; + ret.push_back(exec); + } + } + return ret; +} + +} // namespace executors +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp new file mode 100644 index 0000000000..84ada64925 --- /dev/null +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -0,0 +1,416 @@ +// Copyright 2023 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. + +#include + +#include "rclcpp/executors/executor_entities_collector.hpp" +#include "rclcpp/executors/executor_notify_waitable.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" + +namespace rclcpp +{ +namespace executors +{ + +ExecutorEntitiesCollector::ExecutorEntitiesCollector( + std::shared_ptr notify_waitable) +: notify_waitable_(notify_waitable) +{ +} + +ExecutorEntitiesCollector::~ExecutorEntitiesCollector() +{ + for (auto weak_node_it = weak_nodes_.begin(); weak_node_it != weak_nodes_.end(); ) { + weak_node_it = remove_weak_node(weak_node_it); + } + + for (auto weak_group_it = automatically_added_groups_.begin(); + weak_group_it != automatically_added_groups_.end(); ) + { + weak_group_it = remove_weak_callback_group(weak_group_it, automatically_added_groups_); + } + + for (auto weak_group_it = manually_added_groups_.begin(); + weak_group_it != manually_added_groups_.end(); ) + { + weak_group_it = remove_weak_callback_group(weak_group_it, manually_added_groups_); + } + + for (auto weak_node_ptr : pending_added_nodes_) { + auto node_ptr = weak_node_ptr.lock(); + if (node_ptr) { + node_ptr->get_associated_with_executor_atomic().store(false); + } + } + pending_added_nodes_.clear(); + pending_removed_nodes_.clear(); + + for (auto weak_group_ptr : pending_manually_added_groups_) { + auto group_ptr = weak_group_ptr.lock(); + if (group_ptr) { + group_ptr->get_associated_with_executor_atomic().store(false); + } + } + pending_manually_added_groups_.clear(); + pending_manually_removed_groups_.clear(); +} + +bool +ExecutorEntitiesCollector::has_pending() const +{ + std::lock_guard lock(mutex_); + return pending_manually_added_groups_.size() != 0 || + pending_manually_removed_groups_.size() != 0 || + pending_added_nodes_.size() != 0 || + pending_removed_nodes_.size() != 0; +} + +void +ExecutorEntitiesCollector::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) +{ + // If the node already has an executor + std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); + if (has_executor.exchange(true)) { + throw std::runtime_error( + std::string("Node '") + node_ptr->get_fully_qualified_name() + + "' has already been added to an executor."); + } + + std::lock_guard lock(mutex_); + bool associated = weak_nodes_.count(node_ptr) != 0; + bool add_queued = pending_added_nodes_.count(node_ptr) != 0; + bool remove_queued = pending_removed_nodes_.count(node_ptr) != 0; + + if ((associated || add_queued) && !remove_queued) { + throw std::runtime_error( + std::string("Node '") + node_ptr->get_fully_qualified_name() + + "' has already been added to this executor."); + } + + this->pending_added_nodes_.insert(node_ptr); +} + +void +ExecutorEntitiesCollector::remove_node( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) +{ + if (!node_ptr->get_associated_with_executor_atomic().load()) { + throw std::runtime_error( + std::string("Node '") + node_ptr->get_fully_qualified_name() + + "' needs to be associated with an executor."); + } + + std::lock_guard lock(mutex_); + bool associated = weak_nodes_.count(node_ptr) != 0; + bool add_queued = pending_added_nodes_.count(node_ptr) != 0; + bool remove_queued = pending_removed_nodes_.count(node_ptr) != 0; + + if (!(associated || add_queued) || remove_queued) { + throw std::runtime_error( + std::string("Node '") + node_ptr->get_fully_qualified_name() + + "' needs to be associated with this executor."); + } + + this->pending_removed_nodes_.insert(node_ptr); +} + +void +ExecutorEntitiesCollector::add_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr) +{ + std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); + if (has_executor.exchange(true)) { + throw std::runtime_error("Callback group has already been added to an executor."); + } + + std::lock_guard lock(mutex_); + bool associated = manually_added_groups_.count(group_ptr) != 0; + bool add_queued = pending_manually_added_groups_.count(group_ptr) != 0; + bool remove_queued = pending_manually_removed_groups_.count(group_ptr) != 0; + + if ((associated || add_queued) && !remove_queued) { + throw std::runtime_error("Callback group has already been added to this executor."); + } + + this->pending_manually_added_groups_.insert(group_ptr); +} + +void +ExecutorEntitiesCollector::remove_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr) +{ + if (!group_ptr->get_associated_with_executor_atomic().load()) { + throw std::runtime_error("Callback group needs to be associated with an executor."); + } + /** + * TODO(mjcarroll): The callback groups, being created by a node, should never outlive + * the node. Since we haven't historically enforced this, turning this on may cause + * previously-functional code to fail. + * Consider re-enablng this check (along with corresponding CallbackGroup::has_valid_node), + * when we can guarantee node/group lifetimes. + if (!group_ptr->has_valid_node()) { + throw std::runtime_error("Node must not be deleted before its callback group(s)."); + } + */ + + auto weak_group_ptr = rclcpp::CallbackGroup::WeakPtr(group_ptr); + std::lock_guard lock(mutex_); + bool associated = manually_added_groups_.count(group_ptr) != 0; + bool add_queued = pending_manually_added_groups_.count(group_ptr) != 0; + bool remove_queued = pending_manually_removed_groups_.count(group_ptr) != 0; + + if (!(associated || add_queued) || remove_queued) { + throw std::runtime_error("Callback group needs to be associated with this executor."); + } + + this->pending_manually_removed_groups_.insert(group_ptr); +} + +std::vector +ExecutorEntitiesCollector::get_all_callback_groups() const +{ + std::vector groups; + std::lock_guard lock(mutex_); + for (const auto & group_ptr : manually_added_groups_) { + groups.push_back(group_ptr); + } + for (auto const & group_ptr : automatically_added_groups_) { + groups.push_back(group_ptr); + } + return groups; +} + +std::vector +ExecutorEntitiesCollector::get_manually_added_callback_groups() const +{ + std::vector groups; + std::lock_guard lock(mutex_); + for (const auto & group_ptr : manually_added_groups_) { + groups.push_back(group_ptr); + } + return groups; +} + +std::vector +ExecutorEntitiesCollector::get_automatically_added_callback_groups() const +{ + std::vector groups; + std::lock_guard lock(mutex_); + for (auto const & group_ptr : automatically_added_groups_) { + groups.push_back(group_ptr); + } + return groups; +} + +void +ExecutorEntitiesCollector::update_collections() +{ + std::lock_guard lock(mutex_); + this->process_queues(); + this->add_automatically_associated_callback_groups(this->weak_nodes_); + this->prune_invalid_nodes_and_groups(); +} + +ExecutorEntitiesCollector::NodeCollection::iterator +ExecutorEntitiesCollector::remove_weak_node(NodeCollection::iterator weak_node) +{ + // Disassociate the guard condition from the executor notify waitable + auto guard_condition_it = weak_nodes_to_guard_conditions_.find(*weak_node); + if (guard_condition_it != weak_nodes_to_guard_conditions_.end()) { + this->notify_waitable_->remove_guard_condition(guard_condition_it->second); + weak_nodes_to_guard_conditions_.erase(guard_condition_it); + } + + // Mark the node as disassociated (if the node is still valid) + auto node_ptr = weak_node->lock(); + if (node_ptr) { + std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); + has_executor.store(false); + } + + // Remove the node from tracked nodes + return weak_nodes_.erase(weak_node); +} + +ExecutorEntitiesCollector::CallbackGroupCollection::iterator +ExecutorEntitiesCollector::remove_weak_callback_group( + CallbackGroupCollection::iterator weak_group_it, + CallbackGroupCollection & collection +) +{ + // Disassociate the guard condition from the executor notify waitable + auto guard_condition_it = weak_groups_to_guard_conditions_.find(*weak_group_it); + if (guard_condition_it != weak_groups_to_guard_conditions_.end()) { + this->notify_waitable_->remove_guard_condition(guard_condition_it->second); + weak_groups_to_guard_conditions_.erase(guard_condition_it); + } + + // Mark the node as disassociated (if the group is still valid) + auto group_ptr = weak_group_it->lock(); + if (group_ptr) { + /** + * TODO(mjcarroll): The callback groups, being created by a node, should never outlive + * the node. Since we haven't historically enforced this, turning this on may cause + * previously-functional code to fail. + * Consider re-enablng this check (along with corresponding CallbackGroup::has_valid_node), + * when we can guarantee node/group lifetimes. + if (!group_ptr->has_valid_node()) { + throw std::runtime_error("Node must not be deleted before its callback group(s)."); + } + */ + std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); + has_executor.store(false); + } + + // Remove the node from tracked nodes + return collection.erase(weak_group_it); +} + +void +ExecutorEntitiesCollector::add_callback_group_to_collection( + rclcpp::CallbackGroup::SharedPtr group_ptr, + CallbackGroupCollection & collection) +{ + auto iter = collection.insert(group_ptr); + if (iter.second == false) { + throw std::runtime_error("Callback group has already been added to this executor."); + } + + // Store node guard condition in map and add it to the notify waitable + auto group_guard_condition = group_ptr->get_notify_guard_condition(); + weak_groups_to_guard_conditions_.insert({group_ptr, group_guard_condition}); + this->notify_waitable_->add_guard_condition(group_guard_condition); +} + +void +ExecutorEntitiesCollector::process_queues() +{ + for (auto weak_node_ptr : pending_added_nodes_) { + auto node_ptr = weak_node_ptr.lock(); + if (!node_ptr) { + continue; + } + weak_nodes_.insert(weak_node_ptr); + this->add_automatically_associated_callback_groups({weak_node_ptr}); + + // Store node guard condition in map and add it to the notify waitable + auto node_guard_condition = node_ptr->get_shared_notify_guard_condition(); + weak_nodes_to_guard_conditions_.insert({weak_node_ptr, node_guard_condition}); + this->notify_waitable_->add_guard_condition(node_guard_condition); + } + pending_added_nodes_.clear(); + + for (auto weak_node_ptr : pending_removed_nodes_) { + auto node_it = weak_nodes_.find(weak_node_ptr); + if (node_it != weak_nodes_.end()) { + remove_weak_node(node_it); + } else { + throw std::runtime_error("Node needs to be associated with this executor."); + } + + auto node_ptr = weak_node_ptr.lock(); + if (node_ptr) { + for (auto group_it = automatically_added_groups_.begin(); + group_it != automatically_added_groups_.end(); ) + { + auto group_ptr = group_it->lock(); + if (node_ptr->callback_group_in_node(group_ptr)) { + group_it = remove_weak_callback_group(group_it, automatically_added_groups_); + } else { + ++group_it; + } + } + } + } + pending_removed_nodes_.clear(); + + for (auto weak_group_ptr : pending_manually_added_groups_) { + auto group_ptr = weak_group_ptr.lock(); + if (group_ptr) { + this->add_callback_group_to_collection(group_ptr, manually_added_groups_); + } + } + pending_manually_added_groups_.clear(); + + for (auto weak_group_ptr : pending_manually_removed_groups_) { + auto group_ptr = weak_group_ptr.lock(); + if (group_ptr) { + auto group_it = manually_added_groups_.find(group_ptr); + if (group_it != manually_added_groups_.end()) { + remove_weak_callback_group(group_it, manually_added_groups_); + } else { + throw std::runtime_error( + "Attempting to remove a callback group not added to this executor."); + } + } + } + pending_manually_removed_groups_.clear(); +} + +void +ExecutorEntitiesCollector::add_automatically_associated_callback_groups( + const NodeCollection & nodes_to_check) +{ + for (auto & weak_node : nodes_to_check) { + auto node = weak_node.lock(); + if (node) { + node->for_each_callback_group( + [this, node](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + if (!group_ptr->get_associated_with_executor_atomic().load() && + group_ptr->automatically_add_to_executor_with_node()) + { + std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); + if (has_executor.exchange(true)) { + throw std::runtime_error("Callback group has already been added to an executor."); + } + this->add_callback_group_to_collection(group_ptr, this->automatically_added_groups_); + } + }); + } + } +} + +void +ExecutorEntitiesCollector::prune_invalid_nodes_and_groups() +{ + for (auto node_it = weak_nodes_.begin(); + node_it != weak_nodes_.end(); ) + { + if (node_it->expired()) { + node_it = remove_weak_node(node_it); + } else { + node_it++; + } + } + for (auto group_it = automatically_added_groups_.begin(); + group_it != automatically_added_groups_.end(); ) + { + if (group_it->expired()) { + group_it = remove_weak_callback_group(group_it, automatically_added_groups_); + } else { + group_it++; + } + } + for (auto group_it = manually_added_groups_.begin(); + group_it != manually_added_groups_.end(); ) + { + if (group_it->expired()) { + group_it = remove_weak_callback_group(group_it, manually_added_groups_); + } else { + group_it++; + } + } +} + +} // namespace executors +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp new file mode 100644 index 0000000000..c0ad8a25a4 --- /dev/null +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -0,0 +1,129 @@ +// Copyright 2023 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. + +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/executors/executor_notify_waitable.hpp" + +namespace rclcpp +{ +namespace executors +{ + +ExecutorNotifyWaitable::ExecutorNotifyWaitable(std::function on_execute_callback) +: execute_callback_(on_execute_callback) +{ +} + +ExecutorNotifyWaitable::ExecutorNotifyWaitable(const ExecutorNotifyWaitable & other) +: ExecutorNotifyWaitable(other.execute_callback_) +{ + this->notify_guard_conditions_ = other.notify_guard_conditions_; +} + +ExecutorNotifyWaitable & ExecutorNotifyWaitable::operator=(const ExecutorNotifyWaitable & other) +{ + if (this != &other) { + this->execute_callback_ = other.execute_callback_; + this->notify_guard_conditions_ = other.notify_guard_conditions_; + } + return *this; +} + +void +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_) { + auto guard_condition = weak_guard_condition.lock(); + if (guard_condition) { + auto rcl_guard_condition = &guard_condition->get_rcl_guard_condition(); + + rcl_ret_t ret = rcl_wait_set_add_guard_condition( + wait_set, + rcl_guard_condition, NULL); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to add guard condition to wait set"); + } + } + } +} + +bool +ExecutorNotifyWaitable::is_ready(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) { + auto rcl_guard_condition = wait_set->guard_conditions[ii]; + + if (nullptr == rcl_guard_condition) { + continue; + } + for (auto weak_guard_condition : this->notify_guard_conditions_) { + auto guard_condition = weak_guard_condition.lock(); + if (guard_condition && &guard_condition->get_rcl_guard_condition() == rcl_guard_condition) { + any_ready = true; + } + } + } + return any_ready; +} + +void +ExecutorNotifyWaitable::execute(std::shared_ptr & data) +{ + (void) data; + this->execute_callback_(); +} + +std::shared_ptr +ExecutorNotifyWaitable::take_data() +{ + return nullptr; +} + +void +ExecutorNotifyWaitable::add_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition) +{ + std::lock_guard lock(guard_condition_mutex_); + auto guard_condition = weak_guard_condition.lock(); + if (guard_condition && notify_guard_conditions_.count(weak_guard_condition) == 0) { + notify_guard_conditions_.insert(weak_guard_condition); + } +} + +void +ExecutorNotifyWaitable::remove_guard_condition(rclcpp::GuardCondition::WeakPtr guard_condition) +{ + std::lock_guard lock(guard_condition_mutex_); + if (notify_guard_conditions_.count(guard_condition) != 0) { + notify_guard_conditions_.erase(guard_condition); + } +} + +size_t +ExecutorNotifyWaitable::get_number_of_ready_guard_conditions() +{ + std::lock_guard lock(guard_condition_mutex_); + return notify_guard_conditions_.size(); +} + +} // namespace executors +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp index bf50e062f3..6fd0b56a85 100644 --- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp @@ -109,8 +109,8 @@ StaticExecutorEntitiesCollector::execute(std::shared_ptr & data) std::lock_guard guard{new_nodes_mutex_}; for (const auto & weak_node : new_nodes_) { if (auto node_ptr = weak_node.lock()) { - const auto & gc = node_ptr->get_notify_guard_condition(); - weak_nodes_to_guard_conditions_[node_ptr] = &gc; + weak_nodes_to_guard_conditions_[node_ptr] = + node_ptr->get_shared_notify_guard_condition().get(); } } new_nodes_.clear(); diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 209fcde556..3c14b37b45 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -139,7 +139,7 @@ StaticSingleThreadedExecutor::add_callback_group( bool is_new_node = entities_collector_->add_callback_group(group_ptr, node_ptr); if (is_new_node && notify) { // Interrupt waiting to handle new node - interrupt_guard_condition_.trigger(); + interrupt_guard_condition_->trigger(); } } @@ -150,7 +150,7 @@ StaticSingleThreadedExecutor::add_node( bool is_new_node = entities_collector_->add_node(node_ptr); if (is_new_node && notify) { // Interrupt waiting to handle new node - interrupt_guard_condition_.trigger(); + interrupt_guard_condition_->trigger(); } } @@ -167,7 +167,7 @@ StaticSingleThreadedExecutor::remove_callback_group( bool node_removed = entities_collector_->remove_callback_group(group_ptr); // If the node was matched and removed, interrupt waiting if (node_removed && notify) { - interrupt_guard_condition_.trigger(); + interrupt_guard_condition_->trigger(); } } @@ -181,7 +181,7 @@ StaticSingleThreadedExecutor::remove_node( } // If the node was matched and removed, interrupt waiting if (notify) { - interrupt_guard_condition_.trigger(); + interrupt_guard_condition_->trigger(); } } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 89d8acf6fa..6544d69214 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -45,7 +45,7 @@ NodeBase::NodeBase( node_handle_(nullptr), default_callback_group_(default_callback_group), associated_with_executor_(false), - notify_guard_condition_(context), + notify_guard_condition_(std::make_shared(context)), notify_guard_condition_is_valid_(false) { // Create the rcl node and store it in a shared_ptr with a custom destructor. @@ -132,8 +132,10 @@ NodeBase::NodeBase( // Create the default callback group, if needed. if (nullptr == default_callback_group_) { using rclcpp::CallbackGroupType; + // Default callback group is mutually exclusive and automatically associated with + // any executors that this node is added to. default_callback_group_ = - NodeBase::create_callback_group(CallbackGroupType::MutuallyExclusive); + NodeBase::create_callback_group(CallbackGroupType::MutuallyExclusive, true); } // Indicate the notify_guard_condition is now valid. @@ -202,11 +204,27 @@ NodeBase::create_callback_group( rclcpp::CallbackGroupType group_type, bool automatically_add_to_executor_with_node) { + auto weak_context = this->get_context()->weak_from_this(); + auto get_node_context = [weak_context]() -> rclcpp::Context::SharedPtr { + return weak_context.lock(); + }; + auto group = std::make_shared( group_type, + get_node_context, automatically_add_to_executor_with_node); std::lock_guard lock(callback_groups_mutex_); callback_groups_.push_back(group); + + // This guard condition is generally used to signal to this node's executor that a callback + // group has been added that should be considered for new entities. + // If this is creating the default callback group, then the notify guard condition won't be + // ready or needed yet, as the node is not done being constructed and therefore cannot be added. + // If the callback group is not automatically associated with this node's executors, then + // triggering the guard condition is also unnecessary, it will be manually added to an exector. + if (notify_guard_condition_is_valid_ && automatically_add_to_executor_with_node) { + this->trigger_notify_guard_condition(); + } return group; } @@ -253,9 +271,29 @@ NodeBase::get_notify_guard_condition() if (!notify_guard_condition_is_valid_) { throw std::runtime_error("failed to get notify guard condition because it is invalid"); } + return *notify_guard_condition_; +} + +rclcpp::GuardCondition::SharedPtr +NodeBase::get_shared_notify_guard_condition() +{ + std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); + if (!notify_guard_condition_is_valid_) { + return nullptr; + } return notify_guard_condition_; } +void +NodeBase::trigger_notify_guard_condition() +{ + std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); + if (!notify_guard_condition_is_valid_) { + throw std::runtime_error("failed to trigger notify guard condition because it is invalid"); + } + notify_guard_condition_->trigger(); +} + bool NodeBase::get_use_intra_process_default() const { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index 6f6ed13be7..1228703cb6 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -533,9 +533,8 @@ NodeGraph::notify_graph_change() } } graph_cv_.notify_all(); - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string("failed to notify wait set on graph change: ") + ex.what()); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp index 2f1afd3224..e9c4a5266e 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp @@ -42,9 +42,8 @@ NodeServices::add_service( group->add_service(service_base_ptr); // Notify the executor that a new service was created using the parent Node. - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( @@ -69,9 +68,8 @@ NodeServices::add_client( group->add_client(client_base_ptr); // Notify the executor that a new client was created using the parent Node. - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( diff --git a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp index d2e821a9e6..96097def22 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp @@ -42,9 +42,8 @@ NodeTimers::add_timer( } callback_group->add_timer(timer); - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); callback_group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index 167a35f35d..659129d62c 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -70,9 +70,8 @@ NodeTopics::add_publisher( } // Notify the executor that a new publisher was created using the parent Node. - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); callback_group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( @@ -119,9 +118,8 @@ NodeTopics::add_subscription( } // Notify the executor that a new subscription was created using the parent Node. - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); callback_group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( diff --git a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp index 1d1fe2ce59..02a9de82b0 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp @@ -42,9 +42,8 @@ NodeWaitables::add_waitable( group->add_waitable(waitable_ptr); // Notify the executor that a new waitable was created using the parent Node. - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index d4da759c02..ddb219b558 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -688,6 +688,24 @@ if(TARGET test_static_executor_entities_collector) target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME} mimick) endif() +ament_add_gtest(test_entities_collector executors/test_entities_collector.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) +if(TARGET test_entities_collector) + ament_target_dependencies(test_entities_collector + "rcl" + "test_msgs") + target_link_libraries(test_entities_collector ${PROJECT_NAME} mimick) +endif() + +ament_add_gtest(test_executor_notify_waitable executors/test_executor_notify_waitable.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) +if(TARGET test_executor_notify_waitable) + ament_target_dependencies(test_executor_notify_waitable + "rcl" + "test_msgs") + target_link_libraries(test_executor_notify_waitable ${PROJECT_NAME} mimick) +endif() + ament_add_gtest(test_guard_condition test_guard_condition.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_guard_condition) diff --git a/rclcpp/test/rclcpp/executors/test_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_entities_collector.cpp new file mode 100644 index 0000000000..930dc68aff --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_entities_collector.cpp @@ -0,0 +1,320 @@ +// Copyright 2023 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. + +#include + +#include "rclcpp/executors/executor_notify_waitable.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/executor_entities_collector.hpp" + +#include "../../utils/rclcpp_gtest_macros.hpp" + +class TestExecutorEntitiesCollector : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + + notify_waitable = std::make_shared(); + entities_collector = std::make_shared( + notify_waitable); + } + + void TearDown() + { + rclcpp::shutdown(); + } + + std::shared_ptr notify_waitable; + std::shared_ptr entities_collector; +}; + +TEST_F(TestExecutorEntitiesCollector, add_remove_node) { + auto node1 = std::make_shared("node1", "ns"); + + // Add a node + EXPECT_NO_THROW(entities_collector->add_node(node1->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector->update_collections()); + + // Remove a node + EXPECT_NO_THROW(entities_collector->remove_node(node1->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector->update_collections()); +} + +TEST_F(TestExecutorEntitiesCollector, add_node_twice) { + auto node1 = std::make_shared("node1", "ns"); + + EXPECT_NO_THROW(entities_collector->add_node(node1->get_node_base_interface())); + + RCLCPP_EXPECT_THROW_EQ( + entities_collector->add_node(node1->get_node_base_interface()), + std::runtime_error("Node '/ns/node1' has already been added to an executor.")); + + EXPECT_NO_THROW(entities_collector->update_collections()); +} + +TEST_F(TestExecutorEntitiesCollector, add_associated_node) { + auto node1 = std::make_shared("node1", "ns"); + + // Simulate node being associated somewhere else + auto & has_executor = node1->get_node_base_interface()->get_associated_with_executor_atomic(); + has_executor.store(true); + + // Add an already-associated node + RCLCPP_EXPECT_THROW_EQ( + entities_collector->remove_node(node1->get_node_base_interface()), + std::runtime_error("Node '/ns/node1' needs to be associated with this executor.")); + + has_executor.store(false); +} + +TEST_F(TestExecutorEntitiesCollector, remove_unassociated_node) { + auto node1 = std::make_shared("node1", "ns"); + + // Add an already-associated node + RCLCPP_EXPECT_THROW_EQ( + entities_collector->remove_node(node1->get_node_base_interface()), + std::runtime_error("Node '/ns/node1' needs to be associated with an executor.")); + + // Simulate node being associated somewhere else + auto & has_executor = node1->get_node_base_interface()->get_associated_with_executor_atomic(); + has_executor.store(true); + + // Add an already-associated node + RCLCPP_EXPECT_THROW_EQ( + entities_collector->remove_node(node1->get_node_base_interface()), + std::runtime_error("Node '/ns/node1' needs to be associated with this executor.")); +} + +TEST_F(TestExecutorEntitiesCollector, add_remove_node_before_update) { + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); + + auto node1 = std::make_shared("node1", "ns"); + auto node2 = std::make_shared("node2", "ns"); + + // Add and remove nodes without running updatenode + EXPECT_NO_THROW(entities_collector.add_node(node1->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector.add_node(node2->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector.remove_node(node1->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector.remove_node(node2->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector.update_collections()); +} + +TEST_F(TestExecutorEntitiesCollector, add_callback_group) { + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); + + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + // Add a callback group and update + entities_collector.add_callback_group(cb_group); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + // Remove callback group and update + entities_collector.remove_callback_group(cb_group); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); +} + +TEST_F(TestExecutorEntitiesCollector, add_node_default_callback_group) { + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); + + auto node = std::make_shared("node1", "ns"); + entities_collector.add_node(node->get_node_base_interface()); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 1u); +} + +TEST_F(TestExecutorEntitiesCollector, add_callback_group_after_add_node) { + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); + + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector.add_node(node->get_node_base_interface()); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 2u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 2u); + + RCLCPP_EXPECT_THROW_EQ( + entities_collector.add_callback_group(cb_group), + std::runtime_error("Callback group has already been added to an executor.")); +} + +TEST_F(TestExecutorEntitiesCollector, add_callback_group_twice) { + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); + + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector.add_callback_group(cb_group); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + cb_group->get_associated_with_executor_atomic().exchange(false); + RCLCPP_EXPECT_THROW_EQ( + entities_collector.add_callback_group(cb_group), + std::runtime_error("Callback group has already been added to this executor.")); +} + +TEST_F(TestExecutorEntitiesCollector, remove_callback_group_after_node) { + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); + + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector.add_callback_group(cb_group); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + node.reset(); + + /** + * TODO(mjcarroll): Assert this when we are enforcing that nodes must be destroyed + * after their created callback groups. + RCLCPP_EXPECT_THROW_EQ( + entities_collector.remove_callback_group(cb_group), + std::runtime_error("Node must not be deleted before its callback group(s).")); + */ + EXPECT_NO_THROW(entities_collector.update_collections()); +} + +TEST_F(TestExecutorEntitiesCollector, remove_callback_group_after_node2) { + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); + + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector.add_callback_group(cb_group); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + EXPECT_NO_THROW(entities_collector.remove_callback_group(cb_group)); + + node.reset(); + + /** + * TODO(mjcarroll): Assert this when we are enforcing that nodes must be destroyed + * after their created callback groups. + RCLCPP_EXPECT_THROW_EQ( + entities_collector.remove_callback_group(cb_group), + std::runtime_error("Node must not be deleted before its callback group(s).")); + */ + EXPECT_NO_THROW(entities_collector.update_collections()); +} + +TEST_F(TestExecutorEntitiesCollector, remove_callback_group_twice) { + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); + + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector.add_callback_group(cb_group); + entities_collector.update_collections(); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.remove_callback_group(cb_group); + entities_collector.update_collections(); + + RCLCPP_EXPECT_THROW_EQ( + entities_collector.remove_callback_group(cb_group), + std::runtime_error("Callback group needs to be associated with an executor.")); +} + +TEST_F(TestExecutorEntitiesCollector, remove_node_opposite_order) { + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); + + auto node1 = std::make_shared("node1", "ns"); + EXPECT_NO_THROW(entities_collector.add_node(node1->get_node_base_interface())); + + auto node2 = std::make_shared("node2", "ns"); + EXPECT_NO_THROW(entities_collector.add_node(node2->get_node_base_interface())); + + EXPECT_NO_THROW(entities_collector.remove_node(node2->get_node_base_interface())); +} diff --git a/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp b/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp new file mode 100644 index 0000000000..ab7f730a2e --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp @@ -0,0 +1,97 @@ +// Copyright 2023 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. + +#include + +#include +#include +#include + +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rcpputils/scope_exit.hpp" + +#include "rclcpp/executors/executor_notify_waitable.hpp" + +#include "../../utils/rclcpp_gtest_macros.hpp" + + +class TestExecutorNotifyWaitable : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestExecutorNotifyWaitable, construct_destruct) { + { + auto waitable = std::make_shared(); + waitable.reset(); + } + { + auto on_execute_callback = []() {}; + auto waitable = + std::make_shared(on_execute_callback); + waitable.reset(); + } +} + +TEST_F(TestExecutorNotifyWaitable, add_remove_guard_conditions) { + auto on_execute_callback = []() {}; + auto waitable = + std::make_shared(on_execute_callback); + + auto node = std::make_shared("my_node", "/ns"); + auto notify_guard_condition = + + node->get_node_base_interface()->get_shared_notify_guard_condition(); + EXPECT_NO_THROW(waitable->add_guard_condition(notify_guard_condition)); + EXPECT_NO_THROW(waitable->remove_guard_condition(notify_guard_condition)); +} + +TEST_F(TestExecutorNotifyWaitable, wait) { + int on_execute_calls = 0; + auto on_execute_callback = [&on_execute_calls]() {on_execute_calls++;}; + + auto waitable = + std::make_shared(on_execute_callback); + + auto node = std::make_shared("my_node", "/ns"); + auto notify_guard_condition = + node->get_node_base_interface()->get_shared_notify_guard_condition(); + EXPECT_NO_THROW(waitable->add_guard_condition(notify_guard_condition)); + + auto default_cbg = node->get_node_base_interface()->get_default_callback_group(); + ASSERT_NE(nullptr, default_cbg->get_notify_guard_condition()); + + auto waitables = node->get_node_waitables_interface(); + waitables->add_waitable(std::static_pointer_cast(waitable), default_cbg); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + executor.spin_all(std::chrono::seconds(1)); + EXPECT_EQ(1u, on_execute_calls); + + // on_execute_callback doesn't change if the topology doesn't change + executor.spin_all(std::chrono::seconds(1)); + EXPECT_EQ(1u, on_execute_calls); +} From a7048e115d9316b54161180fa3bddab29bce7cc3 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Mon, 17 Apr 2023 19:33:58 +0100 Subject: [PATCH 218/455] add events-executor and timers-manager in rclcpp (#2155) * add events-executor and timers-manager in rclcpp fix check for execute_timers_separate_thread; ignore on_ready_callback if asked to execute a timer; reduce usage of void pointers * have the executor notify waitable fiddle with guard condition callbacks only if necessary Signed-off-by: Alberto Soragna --- rclcpp/CMakeLists.txt | 2 + rclcpp/include/rclcpp/executors.hpp | 1 + .../executors/executor_notify_waitable.hpp | 33 +- .../events_executor/events_executor.hpp | 286 +++++++++ .../events_executor_event_types.hpp | 46 ++ .../events_executor/events_queue.hpp | 100 ++++ .../events_executor/simple_events_queue.hpp | 134 +++++ .../rclcpp/experimental/timers_manager.hpp | 553 ++++++++++++++++++ .../executors/executor_notify_waitable.cpp | 60 +- .../events_executor/events_executor.cpp | 488 ++++++++++++++++ .../rclcpp/experimental/timers_manager.cpp | 299 ++++++++++ rclcpp/test/rclcpp/CMakeLists.txt | 23 + .../rclcpp/executors/test_events_executor.cpp | 549 +++++++++++++++++ .../rclcpp/executors/test_events_queue.cpp | 82 +++ .../test/rclcpp/executors/test_executors.cpp | 216 ++++++- .../test_add_callback_groups_to_executor.cpp | 125 +++- rclcpp/test/rclcpp/test_timers_manager.cpp | 419 +++++++++++++ 17 files changed, 3377 insertions(+), 39 deletions(-) create mode 100644 rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp create mode 100644 rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp create mode 100644 rclcpp/include/rclcpp/experimental/executors/events_executor/events_queue.hpp create mode 100644 rclcpp/include/rclcpp/experimental/executors/events_executor/simple_events_queue.hpp create mode 100644 rclcpp/include/rclcpp/experimental/timers_manager.hpp create mode 100644 rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp create mode 100644 rclcpp/src/rclcpp/experimental/timers_manager.cpp create mode 100644 rclcpp/test/rclcpp/executors/test_events_executor.cpp create mode 100644 rclcpp/test/rclcpp/executors/test_events_queue.cpp create mode 100644 rclcpp/test/rclcpp/test_timers_manager.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 3bd558de6e..b2d1023064 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -67,6 +67,8 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/executors/static_executor_entities_collector.cpp src/rclcpp/executors/static_single_threaded_executor.cpp src/rclcpp/expand_topic_or_service_name.cpp + src/rclcpp/experimental/executors/events_executor/events_executor.cpp + src/rclcpp/experimental/timers_manager.cpp src/rclcpp/future_return_code.cpp src/rclcpp/generic_publisher.cpp src/rclcpp/generic_subscription.cpp diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 36fb0d63cf..f7ff06055f 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -21,6 +21,7 @@ #include "rclcpp/executors/multi_threaded_executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp/executors/static_single_threaded_executor.hpp" +#include "rclcpp/experimental/executors/events_executor/events_executor.hpp" #include "rclcpp/node.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" diff --git a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp index 88158952d9..eee8e59793 100644 --- a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp @@ -88,6 +88,25 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable std::shared_ptr take_data() override; + /// Take the data from an entity ID so that it can be consumed with `execute`. + /** + * \param[in] id ID of the entity to take data from. + * \return If available, data to be used, otherwise nullptr + * \sa rclcpp::Waitable::take_data_by_entity_id + */ + RCLCPP_PUBLIC + std::shared_ptr + take_data_by_entity_id(size_t id) override; + + /// Set a callback to be called whenever the waitable becomes ready. + /** + * \param[in] callback callback to set + * \sa rclcpp::Waitable::set_on_ready_callback + */ + RCLCPP_PUBLIC + void + set_on_ready_callback(std::function callback) override; + /// Add a guard condition to be waited on. /** * \param[in] guard_condition The guard condition to add. @@ -96,13 +115,21 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable void add_guard_condition(rclcpp::GuardCondition::WeakPtr guard_condition); + /// Unset any callback registered via set_on_ready_callback. + /** + * \sa rclcpp::Waitable::clear_on_ready_callback + */ + RCLCPP_PUBLIC + void + clear_on_ready_callback() override; + /// Remove a guard condition from being waited on. /** - * \param[in] guard_condition The guard condition to remove. + * \param[in] weak_guard_condition The guard condition to remove. */ RCLCPP_PUBLIC void - remove_guard_condition(rclcpp::GuardCondition::WeakPtr guard_condition); + remove_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition); /// Get the number of ready guard_conditions /** @@ -118,6 +145,8 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable std::mutex guard_condition_mutex_; + std::function on_ready_callback_; + /// The collection of guard conditions to be waited on. std::set> notify_guard_conditions_; diff --git a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp new file mode 100644 index 0000000000..148ede66ea --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp @@ -0,0 +1,286 @@ +// Copyright 2023 iRobot Corporation. +// +// 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__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_ +#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/executor_entities_collection.hpp" +#include "rclcpp/executors/executor_entities_collector.hpp" +#include "rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp" +#include "rclcpp/experimental/executors/events_executor/events_queue.hpp" +#include "rclcpp/experimental/executors/events_executor/simple_events_queue.hpp" +#include "rclcpp/experimental/timers_manager.hpp" +#include "rclcpp/node.hpp" + +namespace rclcpp +{ +namespace experimental +{ +namespace executors +{ + +/// Events executor implementation +/** + * This executor uses an events queue and a timers manager to execute entities from its + * associated nodes and callback groups. + * ROS 2 entities allow to set callback functions that are invoked when the entity is triggered + * or has work to do. The events-executor sets these callbacks such that they push an + * event into its queue. + * + * This executor tries to reduce as much as possible the amount of maintenance operations. + * This allows to use customized `EventsQueue` classes to achieve different goals such + * as very low CPU usage, bounded memory requirement, determinism, etc. + * + * The executor uses a weak ownership model and it locks entities only while executing + * their related events. + * + * To run this executor: + * rclcpp::experimental::executors::EventsExecutor executor; + * executor.add_node(node); + * executor.spin(); + * executor.remove_node(node); + */ +class EventsExecutor : public rclcpp::Executor +{ + friend class EventsExecutorEntitiesCollector; + +public: + RCLCPP_SMART_PTR_DEFINITIONS(EventsExecutor) + + /// Default constructor. See the default constructor for Executor. + /** + * \param[in] events_queue The queue used to store events. + * \param[in] execute_timers_separate_thread If true, timers are executed in a separate + * thread. If false, timers are executed in the same thread as all other entities. + * \param[in] options Options used to configure the executor. + */ + RCLCPP_PUBLIC + explicit EventsExecutor( + rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue = std::make_unique< + rclcpp::experimental::executors::SimpleEventsQueue>(), + bool execute_timers_separate_thread = false, + const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions()); + + /// Default destructor. + RCLCPP_PUBLIC + virtual ~EventsExecutor(); + + /// Events executor implementation of spin. + /** + * This function will block until work comes in, execute it, and keep blocking. + * It will only be interrupted by a CTRL-C (managed by the global signal handler). + * \throws std::runtime_error when spin() called while already spinning + */ + RCLCPP_PUBLIC + void + spin() override; + + /// Events executor implementation of spin some + /** + * This non-blocking function will execute the timers and events + * that were ready when this API was called, until timeout or no + * more work available. New ready-timers/events arrived while + * executing work, won't be taken into account here. + * + * Example: + * while(condition) { + * spin_some(); + * sleep(); // User should have some sync work or + * // sleep to avoid a 100% CPU usage + * } + */ + RCLCPP_PUBLIC + void + spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)) override; + + /// Events executor implementation of spin all + /** + * This non-blocking function will execute timers and events + * until timeout or no more work available. If new ready-timers/events + * arrive while executing work available, they will be executed + * as long as the timeout hasn't expired. + * + * Example: + * while(condition) { + * spin_all(); + * sleep(); // User should have some sync work or + * // sleep to avoid a 100% CPU usage + * } + */ + RCLCPP_PUBLIC + void + spin_all(std::chrono::nanoseconds max_duration) override; + + /// Add a node to the executor. + /** + * \sa rclcpp::Executor::add_node + */ + RCLCPP_PUBLIC + void + add_node( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + bool notify = true) override; + + /// Convenience function which takes Node and forwards NodeBaseInterface. + /** + * \sa rclcpp::EventsExecutor::add_node + */ + RCLCPP_PUBLIC + void + add_node(std::shared_ptr node_ptr, bool notify = true) override; + + /// Remove a node from the executor. + /** + * \sa rclcpp::Executor::remove_node + */ + RCLCPP_PUBLIC + void + remove_node( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + bool notify = true) override; + + /// Convenience function which takes Node and forwards NodeBaseInterface. + /** + * \sa rclcpp::Executor::remove_node + */ + RCLCPP_PUBLIC + void + remove_node(std::shared_ptr node_ptr, bool notify = true) override; + + /// Add a callback group to an executor. + /** + * \sa rclcpp::Executor::add_callback_group + */ + RCLCPP_PUBLIC + void + add_callback_group( + rclcpp::CallbackGroup::SharedPtr group_ptr, + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + bool notify = true) override; + + /// Remove callback group from the executor + /** + * \sa rclcpp::Executor::remove_callback_group + */ + RCLCPP_PUBLIC + void + remove_callback_group( + rclcpp::CallbackGroup::SharedPtr group_ptr, + bool notify = true) override; + + /// Get callback groups that belong to executor. + /** + * \sa rclcpp::Executor::get_all_callback_groups() + */ + RCLCPP_PUBLIC + std::vector + get_all_callback_groups() override; + + /// Get callback groups that belong to executor. + /** + * \sa rclcpp::Executor::get_manually_added_callback_groups() + */ + RCLCPP_PUBLIC + std::vector + get_manually_added_callback_groups() override; + + /// Get callback groups that belong to executor. + /** + * \sa rclcpp::Executor::get_automatically_added_callback_groups_from_nodes() + */ + RCLCPP_PUBLIC + std::vector + get_automatically_added_callback_groups_from_nodes() override; + +protected: + /// Internal implementation of spin_once + RCLCPP_PUBLIC + void + spin_once_impl(std::chrono::nanoseconds timeout) override; + + /// Internal implementation of spin_some + RCLCPP_PUBLIC + void + spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive); + +private: + RCLCPP_DISABLE_COPY(EventsExecutor) + + /// Execute a provided executor event if its associated entities are available + void + execute_event(const ExecutorEvent & event); + + /// Collect entities from callback groups and refresh the current collection with them + void + refresh_current_collection_from_callback_groups(); + + /// Refresh the current collection using the provided new_collection + void + refresh_current_collection(const rclcpp::executors::ExecutorEntitiesCollection & new_collection); + + /// Create a listener callback function for the provided entity + std::function + create_entity_callback(void * entity_key, ExecutorEventType type); + + /// Create a listener callback function for the provided waitable entity + std::function + create_waitable_callback(const rclcpp::Waitable * waitable_id); + + /// Searches for the provided entity_id in the collection and returns the entity if valid + template + typename CollectionType::EntitySharedPtr + retrieve_entity(typename CollectionType::Key entity_id, CollectionType & collection) + { + // Check if the entity_id is in the collection + auto it = collection.find(entity_id); + if (it == collection.end()) { + return nullptr; + } + + // Check if the entity associated with the entity_id is valid + // and remove it from the collection if it isn't + auto entity = it->second.entity.lock(); + if (!entity) { + collection.erase(it); + } + + // Return the retrieved entity (this can be a nullptr if the entity was not valid) + return entity; + } + + /// Queue where entities can push events + rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue_; + + std::shared_ptr entities_collector_; + std::shared_ptr current_entities_collection_; + std::shared_ptr notify_waitable_; + + /// Flag used to reduce the number of unnecessary waitable events + std::atomic notify_waitable_event_pushed_ {false}; + + /// Timers manager used to track and/or execute associated timers + std::shared_ptr timers_manager_; +}; + +} // namespace executors +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp new file mode 100644 index 0000000000..79c2c5f905 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp @@ -0,0 +1,46 @@ +// Copyright 2023 iRobot Corporation. +// +// 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__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_ +#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_ + +namespace rclcpp +{ +namespace experimental +{ +namespace executors +{ + +enum ExecutorEventType +{ + CLIENT_EVENT, + SUBSCRIPTION_EVENT, + SERVICE_EVENT, + TIMER_EVENT, + WAITABLE_EVENT +}; + +struct ExecutorEvent +{ + const void * entity_key; + int waitable_data; + ExecutorEventType type; + size_t num_events; +}; + +} // namespace executors +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_queue.hpp b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_queue.hpp new file mode 100644 index 0000000000..24282d6027 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_queue.hpp @@ -0,0 +1,100 @@ +// Copyright 2023 iRobot Corporation. +// +// 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__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_QUEUE_HPP_ +#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_QUEUE_HPP_ + +#include + +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +#include "rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp" + +namespace rclcpp +{ +namespace experimental +{ +namespace executors +{ + +/** + * @brief This abstract class can be used to implement different types of queues + * where `ExecutorEvent` can be stored. + * The derived classes should choose which underlying container to use and + * the strategy for pushing and popping events. + * For example a queue implementation may be bounded or unbounded and have + * different pruning strategies. + * Implementations may or may not check the validity of events and decide how to handle + * the situation where an event is not valid anymore (e.g. a subscription history cache overruns) + */ +class EventsQueue +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(EventsQueue) + + RCLCPP_PUBLIC + EventsQueue() = default; + + /** + * @brief Destruct the object. + */ + RCLCPP_PUBLIC + virtual ~EventsQueue() = default; + + /** + * @brief push event into the queue + * @param event The event to push into the queue + */ + RCLCPP_PUBLIC + virtual + void + enqueue(const rclcpp::experimental::executors::ExecutorEvent & event) = 0; + + /** + * @brief Extracts an event from the queue, eventually waiting until timeout + * if none is available. + * @return true if event has been found, false if timeout + */ + RCLCPP_PUBLIC + virtual + bool + dequeue( + rclcpp::experimental::executors::ExecutorEvent & event, + std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max()) = 0; + + /** + * @brief Test whether queue is empty + * @return true if the queue's size is 0, false otherwise. + */ + RCLCPP_PUBLIC + virtual + bool + empty() const = 0; + + /** + * @brief Returns the number of elements in the queue. + * @return the number of elements in the queue. + */ + RCLCPP_PUBLIC + virtual + size_t + size() const = 0; +}; + +} // namespace executors +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_QUEUE_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/executors/events_executor/simple_events_queue.hpp b/rclcpp/include/rclcpp/experimental/executors/events_executor/simple_events_queue.hpp new file mode 100644 index 0000000000..7b18a95fcf --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/executors/events_executor/simple_events_queue.hpp @@ -0,0 +1,134 @@ +// Copyright 2023 iRobot Corporation. +// +// 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__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_ +#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/experimental/executors/events_executor/events_queue.hpp" + +namespace rclcpp +{ +namespace experimental +{ +namespace executors +{ + +/** + * @brief This class implements an EventsQueue as a simple wrapper around a std::queue. + * It does not perform any checks about the size of queue, which can grow + * unbounded without being pruned. + * The simplicity of this implementation makes it suitable for optimizing CPU usage. + */ +class SimpleEventsQueue : public EventsQueue +{ +public: + RCLCPP_PUBLIC + ~SimpleEventsQueue() override = default; + + /** + * @brief enqueue event into the queue + * Thread safe + * @param event The event to enqueue into the queue + */ + RCLCPP_PUBLIC + void + enqueue(const rclcpp::experimental::executors::ExecutorEvent & event) override + { + rclcpp::experimental::executors::ExecutorEvent single_event = event; + single_event.num_events = 1; + { + std::unique_lock lock(mutex_); + for (size_t ev = 0; ev < event.num_events; ev++) { + event_queue_.push(single_event); + } + } + events_queue_cv_.notify_one(); + } + + /** + * @brief waits for an event until timeout, gets a single event + * Thread safe + * @return true if event, false if timeout + */ + RCLCPP_PUBLIC + bool + dequeue( + rclcpp::experimental::executors::ExecutorEvent & event, + std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max()) override + { + std::unique_lock lock(mutex_); + + // Initialize to true because it's only needed if we have a valid timeout + bool has_data = true; + if (timeout != std::chrono::nanoseconds::max()) { + has_data = + events_queue_cv_.wait_for(lock, timeout, [this]() {return !event_queue_.empty();}); + } else { + events_queue_cv_.wait(lock, [this]() {return !event_queue_.empty();}); + } + + if (has_data) { + event = event_queue_.front(); + event_queue_.pop(); + return true; + } + + return false; + } + + /** + * @brief Test whether queue is empty + * Thread safe + * @return true if the queue's size is 0, false otherwise. + */ + RCLCPP_PUBLIC + bool + empty() const override + { + std::unique_lock lock(mutex_); + return event_queue_.empty(); + } + + /** + * @brief Returns the number of elements in the queue. + * Thread safe + * @return the number of elements in the queue. + */ + RCLCPP_PUBLIC + size_t + size() const override + { + std::unique_lock lock(mutex_); + return event_queue_.size(); + } + +private: + // The underlying queue implementation + std::queue event_queue_; + // Mutex to protect read/write access to the queue + mutable std::mutex mutex_; + // Variable used to notify when an event is added to the queue + std::condition_variable events_queue_cv_; +}; + +} // namespace executors +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/timers_manager.hpp b/rclcpp/include/rclcpp/experimental/timers_manager.hpp new file mode 100644 index 0000000000..e8830c01a0 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/timers_manager.hpp @@ -0,0 +1,553 @@ +// Copyright 2023 iRobot Corporation. +// +// 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__EXPERIMENTAL__TIMERS_MANAGER_HPP_ +#define RCLCPP__EXPERIMENTAL__TIMERS_MANAGER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/context.hpp" +#include "rclcpp/timer.hpp" + +namespace rclcpp +{ +namespace experimental +{ + +/** + * @brief This class provides a way for storing and executing timer objects. + * It provides APIs to suit the needs of different applications and execution models. + * All public APIs provided by this class are thread-safe. + * + * Timers management + * This class provides APIs to add/remove timers to/from an internal storage. + * It keeps a list of weak pointers from added timers, and locks them only when + * they need to be executed or modified. + * Timers are kept ordered in a binary-heap priority queue. + * Calls to add/remove APIs will temporarily block the execution of the timers and + * will require to reorder the internal priority queue. + * Because of this, they have a not-negligible impact on the performance. + * + * Timers execution + * The most efficient use of this class consists in letting a TimersManager object + * to spawn a thread where timers are monitored and optionally executed. + * This can be controlled via the `start` and `stop` methods. + * Ready timers can either be executed or an on_ready_callback can be used to notify + * other entities that they are ready and need to be executed. + * Other APIs allow to directly execute a given timer. + * + * This class assumes that the `execute_callback()` API of the stored timers is never + * called by other entities, but it can only be called from here. + * If this assumption is not respected, the heap property may be invalidated, + * so timers may be executed out of order, without this object noticing it. + * + */ +class TimersManager +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimersManager) + + /** + * @brief Construct a new TimersManager object + * + * @param context custom context to be used. + * Shared ownership of the context is held until destruction. + * @param on_ready_callback The timers on ready callback. The presence of this function + * indicates what to do when the TimersManager is running and a timer becomes ready. + * The TimersManager is considered "running" when the `start` method has been called. + * If it's callable, it will be invoked instead of the timer callback. + * If it's not callable, then the TimersManager will + * directly execute timers when they are ready. + * All the methods that execute a given timer (e.g. `execute_head_timer` + * or `execute_ready_timer`) without the TimersManager being `running`, i.e. + * without actually explicitly waiting for the timer to become ready, will ignore this + * callback. + */ + RCLCPP_PUBLIC + TimersManager( + std::shared_ptr context, + std::function on_ready_callback = nullptr); + + /** + * @brief Destruct the TimersManager object making sure to stop thread and release memory. + */ + RCLCPP_PUBLIC + ~TimersManager(); + + /** + * @brief Adds a new timer to the storage, maintaining weak ownership of it. + * Function is thread safe and it can be called regardless of the state of the timers thread. + * + * @param timer the timer to add. + * @throws std::invalid_argument if timer is a nullptr. + */ + RCLCPP_PUBLIC + void add_timer(rclcpp::TimerBase::SharedPtr timer); + + /** + * @brief Remove a single timer from the object storage. + * Will do nothing if the timer was not being stored here. + * Function is thread safe and it can be called regardless of the state of the timers thread. + * + * @param timer the timer to remove. + */ + RCLCPP_PUBLIC + void remove_timer(rclcpp::TimerBase::SharedPtr timer); + + /** + * @brief Remove all the timers stored in the object. + * Function is thread safe and it can be called regardless of the state of the timers thread. + */ + RCLCPP_PUBLIC + void clear(); + + /** + * @brief Starts a thread that takes care of executing the timers stored in this object. + * Function will throw an error if the timers thread was already running. + */ + RCLCPP_PUBLIC + void start(); + + /** + * @brief Stops the timers thread. + * Will do nothing if the timer thread was not running. + */ + RCLCPP_PUBLIC + void stop(); + + /** + * @brief Get the number of timers that are currently ready. + * This function is thread safe. + * + * @return size_t number of ready timers. + * @throws std::runtime_error if the timers thread was already running. + */ + RCLCPP_PUBLIC + size_t get_number_ready_timers(); + + /** + * @brief Executes head timer if ready. + * This function is thread safe. + * This function will try to execute the timer callback regardless of whether + * the TimersManager on_ready_callback was passed during construction. + * + * @return true if head timer was ready. + * @throws std::runtime_error if the timers thread was already running. + */ + RCLCPP_PUBLIC + bool execute_head_timer(); + + /** + * @brief Executes timer identified by its ID. + * This function is thread safe. + * This function will try to execute the timer callback regardless of whether + * the TimersManager on_ready_callback was passed during construction. + * + * @param timer_id the timer ID of the timer to execute + */ + RCLCPP_PUBLIC + void execute_ready_timer(const rclcpp::TimerBase * timer_id); + + /** + * @brief Get the amount of time before the next timer triggers. + * This function is thread safe. + * + * @return std::chrono::nanoseconds to wait, + * the returned value could be negative if the timer is already expired + * or std::chrono::nanoseconds::max() if there are no timers stored in the object. + * @throws std::runtime_error if the timers thread was already running. + */ + RCLCPP_PUBLIC + std::chrono::nanoseconds get_head_timeout(); + +private: + RCLCPP_DISABLE_COPY(TimersManager) + + using TimerPtr = rclcpp::TimerBase::SharedPtr; + using WeakTimerPtr = rclcpp::TimerBase::WeakPtr; + + // Forward declaration + class TimersHeap; + + /** + * @brief This class allows to store weak pointers to timers in a heap-like data structure. + * The root of the heap is the timer that triggers first. + * Since this class uses weak ownership, it is not guaranteed that it represents a valid heap + * at any point in time as timers could go out of scope, thus invalidating it. + * The "validate_and_lock" API allows to restore the heap property and also returns a locked version + * of the timers heap. + * This class is not thread safe and requires external mutexes to protect its usage. + */ + class WeakTimersHeap + { +public: + /** + * @brief Add a new timer to the heap. After the addition, the heap property is enforced. + * + * @param timer new timer to add. + * @return true if timer has been added, false if it was already there. + */ + bool add_timer(TimerPtr timer) + { + TimersHeap locked_heap = this->validate_and_lock(); + bool added = locked_heap.add_timer(std::move(timer)); + + if (added) { + // Re-create the weak heap with the new timer added + this->store(locked_heap); + } + + return added; + } + + /** + * @brief Remove a timer from the heap. After the removal, the heap property is enforced. + * + * @param timer timer to remove. + * @return true if timer has been removed, false if it was not there. + */ + bool remove_timer(TimerPtr timer) + { + TimersHeap locked_heap = this->validate_and_lock(); + bool removed = locked_heap.remove_timer(std::move(timer)); + + if (removed) { + // Re-create the weak heap with the timer removed + this->store(locked_heap); + } + + return removed; + } + + /** + * @brief Retrieve the timer identified by the key + * @param timer_id The ID of the timer to retrieve. + * @return TimerPtr if there's a timer associated with the ID, nullptr otherwise + */ + TimerPtr get_timer(const rclcpp::TimerBase * timer_id) + { + for (auto & weak_timer : weak_heap_) { + auto timer = weak_timer.lock(); + if (timer.get() == timer_id) { + return timer; + } + } + return nullptr; + } + + /** + * @brief Returns a const reference to the front element. + */ + const WeakTimerPtr & front() const + { + return weak_heap_.front(); + } + + /** + * @brief Returns whether the heap is empty or not. + */ + bool empty() const + { + return weak_heap_.empty(); + } + + /** + * @brief This function restores the current object as a valid heap + * and it returns a locked version of it. + * Timers that went out of scope are removed from the container. + * It is the only public API to access and manipulate the stored timers. + * + * @return TimersHeap owned timers corresponding to the current object + */ + TimersHeap validate_and_lock() + { + TimersHeap locked_heap; + bool any_timer_destroyed = false; + + for (auto weak_timer : weak_heap_) { + auto timer = weak_timer.lock(); + if (timer) { + // This timer is valid, so add it to the locked heap + // Note: we access friend private `owned_heap_` member field. + locked_heap.owned_heap_.push_back(std::move(timer)); + } else { + // This timer went out of scope, so we don't add it to locked heap + // and we mark the corresponding flag. + // It's not needed to erase it from weak heap, as we are going to re-heapify. + // Note: we can't exit from the loop here, as we need to find all valid timers. + any_timer_destroyed = true; + } + } + + // If a timer has gone out of scope, then the remaining elements do not represent + // a valid heap anymore. We need to re-heapify the timers heap. + if (any_timer_destroyed) { + locked_heap.heapify(); + // Re-create the weak heap now that elements have been heapified again + this->store(locked_heap); + } + + return locked_heap; + } + + /** + * @brief This function allows to recreate the heap of weak pointers + * from an heap of owned pointers. + * It is required to be called after a locked TimersHeap generated from this object + * has been modified in any way (e.g. timers triggered, added, removed). + * + * @param heap timers heap to store as weak pointers + */ + void store(const TimersHeap & heap) + { + weak_heap_.clear(); + // Note: we access friend private `owned_heap_` member field. + for (auto t : heap.owned_heap_) { + weak_heap_.push_back(t); + } + } + + /** + * @brief Remove all timers from the heap. + */ + void clear() + { + weak_heap_.clear(); + } + +private: + std::vector weak_heap_; + }; + + /** + * @brief This class is the equivalent of WeakTimersHeap but with ownership of the timers. + * It can be generated by locking the weak version. + * It provides operations to manipulate the heap. + * This class is not thread safe and requires external mutexes to protect its usage. + */ + class TimersHeap + { +public: + /** + * @brief Try to add a new timer to the heap. + * After the addition, the heap property is preserved. + * @param timer new timer to add. + * @return true if timer has been added, false if it was already there. + */ + bool add_timer(TimerPtr timer) + { + // Nothing to do if the timer is already stored here + auto it = std::find(owned_heap_.begin(), owned_heap_.end(), timer); + if (it != owned_heap_.end()) { + return false; + } + + owned_heap_.push_back(std::move(timer)); + std::push_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater); + + return true; + } + + /** + * @brief Try to remove a timer from the heap. + * After the removal, the heap property is preserved. + * @param timer timer to remove. + * @return true if timer has been removed, false if it was not there. + */ + bool remove_timer(TimerPtr timer) + { + // Nothing to do if the timer is not stored here + auto it = std::find(owned_heap_.begin(), owned_heap_.end(), timer); + if (it == owned_heap_.end()) { + return false; + } + + owned_heap_.erase(it); + this->heapify(); + + return true; + } + + /** + * @brief Returns a reference to the front element. + * @return reference to front element. + */ + TimerPtr & front() + { + return owned_heap_.front(); + } + + /** + * @brief Returns a const reference to the front element. + * @return const reference to front element. + */ + const TimerPtr & front() const + { + return owned_heap_.front(); + } + + /** + * @brief Returns whether the heap is empty or not. + * @return true if the heap is empty. + */ + bool empty() const + { + return owned_heap_.empty(); + } + + /** + * @brief Returns the size of the heap. + * @return the number of valid timers in the heap. + */ + size_t size() const + { + return owned_heap_.size(); + } + + /** + * @brief Get the number of timers that are currently ready. + * @return size_t number of ready timers. + */ + size_t get_number_ready_timers() const + { + size_t ready_timers = 0; + + for (TimerPtr t : owned_heap_) { + if (t->is_ready()) { + ready_timers++; + } + } + + return ready_timers; + } + + /** + * @brief Restore a valid heap after the root value has been replaced (e.g. timer triggered). + */ + void heapify_root() + { + // The following code is a more efficient version than doing + // pop_heap, pop_back, push_back, push_heap + // as it removes the need for the last push_heap + + // Push the modified element (i.e. the current root) at the bottom of the heap + owned_heap_.push_back(owned_heap_[0]); + // Exchange first and last-1 elements and reheapify + std::pop_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater); + // Remove last element + owned_heap_.pop_back(); + } + + /** + * @brief Completely restores the structure to a valid heap + */ + void heapify() + { + std::make_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater); + } + + /** + * @brief Helper function to clear the "on_reset_callback" on all associated timers. + */ + void clear_timers_on_reset_callbacks() + { + for (TimerPtr & t : owned_heap_) { + t->clear_on_reset_callback(); + } + } + + /** + * @brief Friend declaration to allow the `validate_and_lock()` function to access the + * underlying heap container + */ + friend TimersHeap WeakTimersHeap::validate_and_lock(); + + /** + * @brief Friend declaration to allow the `store()` function to access the + * underlying heap container + */ + friend void WeakTimersHeap::store(const TimersHeap & heap); + +private: + /** + * @brief Comparison function between timers. + * @return true if `a` triggers after `b`. + */ + static bool timer_greater(TimerPtr a, TimerPtr b) + { + // TODO(alsora): this can cause an error if timers are using different clocks + return a->time_until_trigger() > b->time_until_trigger(); + } + + std::vector owned_heap_; + }; + + /** + * @brief Implements a loop that keeps executing ready timers. + * This function is executed in the timers thread. + */ + void run_timers(); + + /** + * @brief Get the amount of time before the next timer triggers. + * This function is not thread safe, acquire a mutex before calling it. + * + * @return std::chrono::nanoseconds to wait, + * the returned value could be negative if the timer is already expired + * or std::chrono::nanoseconds::max() if the heap is empty. + * This function is not thread safe, acquire the timers_mutex_ before calling it. + */ + std::chrono::nanoseconds get_head_timeout_unsafe(); + + /** + * @brief Executes all the timers currently ready when the function is invoked + * while keeping the heap correctly sorted. + * This function is not thread safe, acquire the timers_mutex_ before calling it. + */ + void execute_ready_timers_unsafe(); + + // Callback to be called when timer is ready + std::function on_ready_callback_ = nullptr; + + // Thread used to run the timers execution task + std::thread timers_thread_; + // Protects access to timers + std::mutex timers_mutex_; + // Protects access to stop() + std::mutex stop_mutex_; + // Notifies the timers thread whenever timers are added/removed + std::condition_variable timers_cv_; + // Flag used as predicate by timers_cv_ that denotes one or more timers being added/removed + bool timers_updated_ {false}; + // Indicates whether the timers thread is currently running or not + std::atomic running_ {false}; + // Parent context used to understand if ROS is still active + std::shared_ptr context_; + // Timers heap storage with weak ownership + WeakTimersHeap weak_timers_heap_; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__TIMERS_MANAGER_HPP_ diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index c0ad8a25a4..15a31cd60d 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -99,6 +99,52 @@ ExecutorNotifyWaitable::take_data() return nullptr; } +std::shared_ptr +ExecutorNotifyWaitable::take_data_by_entity_id(size_t id) +{ + (void) id; + return nullptr; +} + +void +ExecutorNotifyWaitable::set_on_ready_callback(std::function callback) +{ + // The second argument of the callback could be used to identify which guard condition + // triggered the event. + // We could indicate which of the guard conditions was triggered, but the executor + // is already going to check that. + auto gc_callback = [callback](size_t count) { + callback(count, 0); + }; + + std::lock_guard lock(guard_condition_mutex_); + + on_ready_callback_ = gc_callback; + for (auto weak_gc : notify_guard_conditions_) { + auto gc = weak_gc.lock(); + if (!gc) { + continue; + } + gc->set_on_trigger_callback(on_ready_callback_); + } +} + +RCLCPP_PUBLIC +void +ExecutorNotifyWaitable::clear_on_ready_callback() +{ + std::lock_guard lock(guard_condition_mutex_); + + on_ready_callback_ = nullptr; + for (auto weak_gc : notify_guard_conditions_) { + auto gc = weak_gc.lock(); + if (!gc) { + continue; + } + gc->set_on_trigger_callback(nullptr); + } +} + void ExecutorNotifyWaitable::add_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition) { @@ -106,15 +152,23 @@ ExecutorNotifyWaitable::add_guard_condition(rclcpp::GuardCondition::WeakPtr weak auto guard_condition = weak_guard_condition.lock(); if (guard_condition && notify_guard_conditions_.count(weak_guard_condition) == 0) { notify_guard_conditions_.insert(weak_guard_condition); + if (on_ready_callback_) { + guard_condition->set_on_trigger_callback(on_ready_callback_); + } } } void -ExecutorNotifyWaitable::remove_guard_condition(rclcpp::GuardCondition::WeakPtr guard_condition) +ExecutorNotifyWaitable::remove_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition) { std::lock_guard lock(guard_condition_mutex_); - if (notify_guard_conditions_.count(guard_condition) != 0) { - notify_guard_conditions_.erase(guard_condition); + if (notify_guard_conditions_.count(weak_guard_condition) != 0) { + notify_guard_conditions_.erase(weak_guard_condition); + auto guard_condition = weak_guard_condition.lock(); + // If this notify waitable doesn't have an on_ready_callback, then there's nothing to unset + if (guard_condition && on_ready_callback_) { + guard_condition->set_on_trigger_callback(nullptr); + } } } diff --git a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp new file mode 100644 index 0000000000..094ff21282 --- /dev/null +++ b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp @@ -0,0 +1,488 @@ +// Copyright 2023 iRobot Corporation. +// +// 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. + +#include "rclcpp/experimental/executors/events_executor/events_executor.hpp" + +#include +#include +#include + +#include "rcpputils/scope_exit.hpp" + +using namespace std::chrono_literals; + +using rclcpp::experimental::executors::EventsExecutor; + +EventsExecutor::EventsExecutor( + rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue, + bool execute_timers_separate_thread, + const rclcpp::ExecutorOptions & options) +: rclcpp::Executor(options) +{ + // Get ownership of the queue used to store events. + if (!events_queue) { + throw std::invalid_argument("events_queue can't be a null pointer"); + } + events_queue_ = std::move(events_queue); + + // Create timers manager + // The timers manager can be used either to only track timers (in this case an expired + // timer will generate an executor event and then it will be executed by the executor thread) + // or it can also take care of executing expired timers in its dedicated thread. + std::function timer_on_ready_cb = nullptr; + if (!execute_timers_separate_thread) { + timer_on_ready_cb = [this](const rclcpp::TimerBase * timer_id) { + ExecutorEvent event = {timer_id, -1, ExecutorEventType::TIMER_EVENT, 1}; + this->events_queue_->enqueue(event); + }; + } + timers_manager_ = + std::make_shared(context_, timer_on_ready_cb); + + notify_waitable_ = std::make_shared( + [this]() { + // This callback is invoked when: + // - the interrupt or shutdown guard condition is triggered: + // ---> we need to wake up the executor so that it can terminate + // - a node or callback group guard condition is triggered: + // ---> the entities collection is changed, we need to update callbacks + notify_waitable_event_pushed_ = false; + this->refresh_current_collection_from_callback_groups(); + }); + + notify_waitable_->add_guard_condition(interrupt_guard_condition_); + notify_waitable_->add_guard_condition(shutdown_guard_condition_); + + notify_waitable_->set_on_ready_callback( + this->create_waitable_callback(notify_waitable_.get())); + + auto notify_waitable_entity_id = notify_waitable_.get(); + notify_waitable_->set_on_ready_callback( + [this, notify_waitable_entity_id](size_t num_events, int waitable_data) { + // The notify waitable has a special callback. + // We don't care about how many events as when we wake up the executor we are going to + // process everything regardless. + // For the same reason, if an event of this type has already been pushed but it has not been + // processed yet, we avoid pushing additional events. + (void)num_events; + if (notify_waitable_event_pushed_.exchange(true)) { + return; + } + + ExecutorEvent event = + {notify_waitable_entity_id, waitable_data, ExecutorEventType::WAITABLE_EVENT, 1}; + this->events_queue_->enqueue(event); + }); + + this->entities_collector_ = + std::make_shared(notify_waitable_); + + this->current_entities_collection_ = + std::make_shared(); +} + +EventsExecutor::~EventsExecutor() +{ + spinning.store(false); + notify_waitable_->clear_on_ready_callback(); + this->refresh_current_collection({}); +} + +void +EventsExecutor::spin() +{ + if (spinning.exchange(true)) { + throw std::runtime_error("spin() called while already spinning"); + } + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + + timers_manager_->start(); + RCPPUTILS_SCOPE_EXIT(timers_manager_->stop(); ); + + while (rclcpp::ok(context_) && spinning.load()) { + // Wait until we get an event + ExecutorEvent event; + bool has_event = events_queue_->dequeue(event); + if (has_event) { + this->execute_event(event); + } + } +} + +void +EventsExecutor::spin_some(std::chrono::nanoseconds max_duration) +{ + return this->spin_some_impl(max_duration, false); +} + +void +EventsExecutor::spin_all(std::chrono::nanoseconds max_duration) +{ + if (max_duration <= 0ns) { + throw std::invalid_argument("max_duration must be positive"); + } + return this->spin_some_impl(max_duration, true); +} + +void +EventsExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive) +{ + if (spinning.exchange(true)) { + throw std::runtime_error("spin_some() called while already spinning"); + } + + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + + auto start = std::chrono::steady_clock::now(); + + auto max_duration_not_elapsed = [max_duration, start]() { + if (std::chrono::nanoseconds(0) == max_duration) { + // told to spin forever if need be + return true; + } else if (std::chrono::steady_clock::now() - start < max_duration) { + // told to spin only for some maximum amount of time + return true; + } + // spun too long + return false; + }; + + // Get the number of events and timers ready at start + const size_t ready_events_at_start = events_queue_->size(); + size_t executed_events = 0; + const size_t ready_timers_at_start = timers_manager_->get_number_ready_timers(); + size_t executed_timers = 0; + + while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { + // Execute first ready event from queue if exists + if (exhaustive || (executed_events < ready_events_at_start)) { + bool has_event = !events_queue_->empty(); + + if (has_event) { + ExecutorEvent event; + bool ret = events_queue_->dequeue(event, std::chrono::nanoseconds(0)); + if (ret) { + this->execute_event(event); + executed_events++; + continue; + } + } + } + + // Execute first timer if it is ready + if (exhaustive || (executed_timers < ready_timers_at_start)) { + bool timer_executed = timers_manager_->execute_head_timer(); + if (timer_executed) { + executed_timers++; + continue; + } + } + + // If there's no more work available, exit + break; + } +} + +void +EventsExecutor::spin_once_impl(std::chrono::nanoseconds timeout) +{ + // In this context a negative input timeout means no timeout + if (timeout < 0ns) { + timeout = std::chrono::nanoseconds::max(); + } + + // Select the smallest between input timeout and timer timeout + bool is_timer_timeout = false; + auto next_timer_timeout = timers_manager_->get_head_timeout(); + if (next_timer_timeout < timeout) { + timeout = next_timer_timeout; + is_timer_timeout = true; + } + + ExecutorEvent event; + bool has_event = events_queue_->dequeue(event, timeout); + + // If we wake up from the wait with an event, it means that it + // arrived before any of the timers expired. + if (has_event) { + this->execute_event(event); + } else if (is_timer_timeout) { + timers_manager_->execute_head_timer(); + } +} + +void +EventsExecutor::add_node( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) +{ + // This field is unused because we don't have to wake up the executor when a node is added. + (void) notify; + + // Add node to entities collector + this->entities_collector_->add_node(node_ptr); + + this->refresh_current_collection_from_callback_groups(); +} + +void +EventsExecutor::add_node(std::shared_ptr node_ptr, bool notify) +{ + this->add_node(node_ptr->get_node_base_interface(), notify); +} + +void +EventsExecutor::remove_node( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) +{ + // This field is unused because we don't have to wake up the executor when a node is removed. + (void)notify; + + // Remove node from entities collector. + // This will result in un-setting all the event callbacks from its entities. + // After this function returns, this executor will not receive any more events associated + // to these entities. + this->entities_collector_->remove_node(node_ptr); + + this->refresh_current_collection_from_callback_groups(); +} + +void +EventsExecutor::remove_node(std::shared_ptr node_ptr, bool notify) +{ + this->remove_node(node_ptr->get_node_base_interface(), notify); +} + +void +EventsExecutor::execute_event(const ExecutorEvent & event) +{ + switch (event.type) { + case ExecutorEventType::CLIENT_EVENT: + { + auto client = this->retrieve_entity( + static_cast(event.entity_key), + current_entities_collection_->clients); + + if (client) { + for (size_t i = 0; i < event.num_events; i++) { + execute_client(client); + } + } + + break; + } + case ExecutorEventType::SUBSCRIPTION_EVENT: + { + auto subscription = this->retrieve_entity( + static_cast(event.entity_key), + current_entities_collection_->subscriptions); + if (subscription) { + for (size_t i = 0; i < event.num_events; i++) { + execute_subscription(subscription); + } + } + break; + } + case ExecutorEventType::SERVICE_EVENT: + { + auto service = this->retrieve_entity( + static_cast(event.entity_key), + current_entities_collection_->services); + + if (service) { + for (size_t i = 0; i < event.num_events; i++) { + execute_service(service); + } + } + + break; + } + case ExecutorEventType::TIMER_EVENT: + { + timers_manager_->execute_ready_timer( + static_cast(event.entity_key)); + break; + } + case ExecutorEventType::WAITABLE_EVENT: + { + auto waitable = this->retrieve_entity( + static_cast(event.entity_key), + current_entities_collection_->waitables); + if (waitable) { + for (size_t i = 0; i < event.num_events; i++) { + auto data = waitable->take_data_by_entity_id(event.waitable_data); + waitable->execute(data); + } + } + break; + } + } +} + +void +EventsExecutor::add_callback_group( + rclcpp::CallbackGroup::SharedPtr group_ptr, + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + bool notify) +{ + // This field is unused because we don't have to wake up + // the executor when a callback group is added. + (void)notify; + (void)node_ptr; + + this->entities_collector_->add_callback_group(group_ptr); + + this->refresh_current_collection_from_callback_groups(); +} + +void +EventsExecutor::remove_callback_group( + rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify) +{ + // This field is unused because we don't have to wake up + // the executor when a callback group is removed. + (void)notify; + + this->entities_collector_->remove_callback_group(group_ptr); + + this->refresh_current_collection_from_callback_groups(); +} + +std::vector +EventsExecutor::get_all_callback_groups() +{ + this->entities_collector_->update_collections(); + return this->entities_collector_->get_all_callback_groups(); +} + +std::vector +EventsExecutor::get_manually_added_callback_groups() +{ + this->entities_collector_->update_collections(); + return this->entities_collector_->get_manually_added_callback_groups(); +} + +std::vector +EventsExecutor::get_automatically_added_callback_groups_from_nodes() +{ + this->entities_collector_->update_collections(); + return this->entities_collector_->get_automatically_added_callback_groups(); +} + +void +EventsExecutor::refresh_current_collection_from_callback_groups() +{ + this->entities_collector_->update_collections(); + auto callback_groups = this->entities_collector_->get_all_callback_groups(); + rclcpp::executors::ExecutorEntitiesCollection new_collection; + rclcpp::executors::build_entities_collection(callback_groups, new_collection); + + // TODO(alsora): this may be implemented in a better way. + // We need the notify waitable to be included in the executor "current_collection" + // because we need to be able to retrieve events for it. + // We could explicitly check for the notify waitable ID when we receive a waitable event + // but I think that it's better if the waitable was in the collection and it could be + // retrieved in the "standard" way. + // To do it, we need to add the notify waitable as an entry in both the new and + // current collections such that it's neither added or removed. + rclcpp::CallbackGroup::WeakPtr weak_group_ptr; + new_collection.waitables.insert( + { + this->notify_waitable_.get(), + {this->notify_waitable_, weak_group_ptr} + }); + + this->current_entities_collection_->waitables.insert( + { + this->notify_waitable_.get(), + {this->notify_waitable_, weak_group_ptr} + }); + + this->refresh_current_collection(new_collection); +} + +void +EventsExecutor::refresh_current_collection( + const rclcpp::executors::ExecutorEntitiesCollection & new_collection) +{ + current_entities_collection_->timers.update( + new_collection.timers, + [this](rclcpp::TimerBase::SharedPtr timer) {timers_manager_->add_timer(timer);}, + [this](rclcpp::TimerBase::SharedPtr timer) {timers_manager_->remove_timer(timer);}); + + current_entities_collection_->subscriptions.update( + new_collection.subscriptions, + [this](auto subscription) { + subscription->set_on_new_message_callback( + this->create_entity_callback( + subscription->get_subscription_handle().get(), ExecutorEventType::SUBSCRIPTION_EVENT)); + }, + [](auto subscription) {subscription->clear_on_new_message_callback();}); + + current_entities_collection_->clients.update( + new_collection.clients, + [this](auto client) { + client->set_on_new_response_callback( + this->create_entity_callback( + client->get_client_handle().get(), ExecutorEventType::CLIENT_EVENT)); + }, + [](auto client) {client->clear_on_new_response_callback();}); + + current_entities_collection_->services.update( + new_collection.services, + [this](auto service) { + service->set_on_new_request_callback( + this->create_entity_callback( + service->get_service_handle().get(), ExecutorEventType::SERVICE_EVENT)); + }, + [](auto service) {service->clear_on_new_request_callback();}); + + // DO WE NEED THIS? WE ARE NOT DOING ANYTHING WITH GUARD CONDITIONS + /* + current_entities_collection_->guard_conditions.update(new_collection.guard_conditions, + [](auto guard_condition) {(void)guard_condition;}, + [](auto guard_condition) {guard_condition->set_on_trigger_callback(nullptr);}); + */ + + current_entities_collection_->waitables.update( + new_collection.waitables, + [this](auto waitable) { + waitable->set_on_ready_callback( + this->create_waitable_callback(waitable.get())); + }, + [](auto waitable) {waitable->clear_on_ready_callback();}); +} + +std::function +EventsExecutor::create_entity_callback( + void * entity_key, ExecutorEventType event_type) +{ + std::function + callback = [this, entity_key, event_type](size_t num_events) { + ExecutorEvent event = {entity_key, -1, event_type, num_events}; + this->events_queue_->enqueue(event); + }; + return callback; +} + +std::function +EventsExecutor::create_waitable_callback(const rclcpp::Waitable * entity_key) +{ + std::function + callback = [this, entity_key](size_t num_events, int waitable_data) { + ExecutorEvent event = + {entity_key, waitable_data, ExecutorEventType::WAITABLE_EVENT, num_events}; + this->events_queue_->enqueue(event); + }; + return callback; +} diff --git a/rclcpp/src/rclcpp/experimental/timers_manager.cpp b/rclcpp/src/rclcpp/experimental/timers_manager.cpp new file mode 100644 index 0000000000..f4ecd16b76 --- /dev/null +++ b/rclcpp/src/rclcpp/experimental/timers_manager.cpp @@ -0,0 +1,299 @@ +// Copyright 2023 iRobot Corporation. +// +// 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. + +#include "rclcpp/experimental/timers_manager.hpp" + +#include + +#include +#include +#include +#include + +#include "rcpputils/scope_exit.hpp" + +using rclcpp::experimental::TimersManager; + +TimersManager::TimersManager( + std::shared_ptr context, + std::function on_ready_callback) +{ + context_ = context; + on_ready_callback_ = on_ready_callback; +} + +TimersManager::~TimersManager() +{ + // Remove all timers + this->clear(); + + // Make sure timers thread is stopped before destroying this object + this->stop(); +} + +void TimersManager::add_timer(rclcpp::TimerBase::SharedPtr timer) +{ + if (!timer) { + throw std::invalid_argument("TimersManager::add_timer() trying to add nullptr timer"); + } + + bool added = false; + { + std::unique_lock lock(timers_mutex_); + added = weak_timers_heap_.add_timer(timer); + timers_updated_ = timers_updated_ || added; + } + + timer->set_on_reset_callback( + [this](size_t arg) { + { + (void)arg; + std::unique_lock lock(timers_mutex_); + timers_updated_ = true; + } + timers_cv_.notify_one(); + }); + + if (added) { + // Notify that a timer has been added + timers_cv_.notify_one(); + } +} + +void TimersManager::start() +{ + // Make sure that the thread is not already running + if (running_.exchange(true)) { + throw std::runtime_error("TimersManager::start() can't start timers thread as already running"); + } + + timers_thread_ = std::thread(&TimersManager::run_timers, this); +} + +void TimersManager::stop() +{ + // Lock stop() function to prevent race condition in destructor + std::unique_lock lock(stop_mutex_); + running_ = false; + + // Notify the timers manager thread to wake up + { + std::unique_lock lock(timers_mutex_); + timers_updated_ = true; + } + timers_cv_.notify_one(); + + // Join timers thread if it's running + if (timers_thread_.joinable()) { + timers_thread_.join(); + } +} + +std::chrono::nanoseconds TimersManager::get_head_timeout() +{ + // Do not allow to interfere with the thread running + if (running_) { + throw std::runtime_error( + "get_head_timeout() can't be used while timers thread is running"); + } + + std::unique_lock lock(timers_mutex_); + return this->get_head_timeout_unsafe(); +} + +size_t TimersManager::get_number_ready_timers() +{ + // Do not allow to interfere with the thread running + if (running_) { + throw std::runtime_error( + "get_number_ready_timers() can't be used while timers thread is running"); + } + + std::unique_lock lock(timers_mutex_); + TimersHeap locked_heap = weak_timers_heap_.validate_and_lock(); + return locked_heap.get_number_ready_timers(); +} + +bool TimersManager::execute_head_timer() +{ + // Do not allow to interfere with the thread running + if (running_) { + throw std::runtime_error( + "execute_head_timer() can't be used while timers thread is running"); + } + + std::unique_lock lock(timers_mutex_); + + TimersHeap timers_heap = weak_timers_heap_.validate_and_lock(); + + // Nothing to do if we don't have any timer + if (timers_heap.empty()) { + return false; + } + + TimerPtr head_timer = timers_heap.front(); + + const bool timer_ready = head_timer->is_ready(); + if (timer_ready) { + // NOTE: here we always execute the timer, regardless of whether the + // on_ready_callback is set or not. + head_timer->call(); + head_timer->execute_callback(); + timers_heap.heapify_root(); + weak_timers_heap_.store(timers_heap); + } + + return timer_ready; +} + +void TimersManager::execute_ready_timer(const rclcpp::TimerBase * timer_id) +{ + TimerPtr ready_timer; + { + std::unique_lock lock(timers_mutex_); + ready_timer = weak_timers_heap_.get_timer(timer_id); + } + if (ready_timer) { + ready_timer->execute_callback(); + } +} + +std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe() +{ + // If we don't have any weak pointer, then we just return maximum timeout + if (weak_timers_heap_.empty()) { + return std::chrono::nanoseconds::max(); + } + // Weak heap is not empty, so try to lock the first element. + // If it is still a valid pointer, it is guaranteed to be the correct head + TimerPtr head_timer = weak_timers_heap_.front().lock(); + + if (!head_timer) { + // The first element has expired, we can't make other assumptions on the heap + // and we need to entirely validate it. + TimersHeap locked_heap = weak_timers_heap_.validate_and_lock(); + // NOTE: the following operations will not modify any element in the heap, so we + // don't have to call `weak_timers_heap_.store(locked_heap)` at the end. + + if (locked_heap.empty()) { + return std::chrono::nanoseconds::max(); + } + head_timer = locked_heap.front(); + } + + return head_timer->time_until_trigger(); +} + +void TimersManager::execute_ready_timers_unsafe() +{ + // We start by locking the timers + TimersHeap locked_heap = weak_timers_heap_.validate_and_lock(); + + // Nothing to do if we don't have any timer + if (locked_heap.empty()) { + return; + } + + // Keep executing timers until they are ready and they were already ready when we started. + // The two checks prevent this function from blocking indefinitely if the + // time required for executing the timers is longer than their period. + + TimerPtr head_timer = locked_heap.front(); + const size_t number_ready_timers = locked_heap.get_number_ready_timers(); + size_t executed_timers = 0; + while (executed_timers < number_ready_timers && head_timer->is_ready()) { + head_timer->call(); + if (on_ready_callback_) { + on_ready_callback_(head_timer.get()); + } else { + head_timer->execute_callback(); + } + + executed_timers++; + // Executing a timer will result in updating its time_until_trigger, so re-heapify + locked_heap.heapify_root(); + // Get new head timer + head_timer = locked_heap.front(); + } + + // After having performed work on the locked heap we reflect the changes to weak one. + // Timers will be already sorted the next time we need them if none went out of scope. + weak_timers_heap_.store(locked_heap); +} + +void TimersManager::run_timers() +{ + // Make sure the running flag is set to false when we exit from this function + // to allow restarting the timers thread. + RCPPUTILS_SCOPE_EXIT(this->running_.store(false); ); + + while (rclcpp::ok(context_) && running_) { + // Lock mutex + std::unique_lock lock(timers_mutex_); + + std::chrono::nanoseconds time_to_sleep = get_head_timeout_unsafe(); + + // No need to wait if a timer is already available + if (time_to_sleep > std::chrono::nanoseconds::zero()) { + if (time_to_sleep != std::chrono::nanoseconds::max()) { + // Wait until timeout or notification that timers have been updated + timers_cv_.wait_for(lock, time_to_sleep, [this]() {return timers_updated_;}); + } else { + // Wait until notification that timers have been updated + timers_cv_.wait(lock, [this]() {return timers_updated_;}); + } + } + + // Reset timers updated flag + timers_updated_ = false; + + // Execute timers + this->execute_ready_timers_unsafe(); + } +} + +void TimersManager::clear() +{ + { + // Lock mutex and then clear all data structures + std::unique_lock lock(timers_mutex_); + + TimersHeap locked_heap = weak_timers_heap_.validate_and_lock(); + locked_heap.clear_timers_on_reset_callbacks(); + + weak_timers_heap_.clear(); + + timers_updated_ = true; + } + + // Notify timers thread such that it can re-compute its timeout + timers_cv_.notify_one(); +} + +void TimersManager::remove_timer(TimerPtr timer) +{ + bool removed = false; + { + std::unique_lock lock(timers_mutex_); + removed = weak_timers_heap_.remove_timer(timer); + + timers_updated_ = timers_updated_ || removed; + } + + if (removed) { + // Notify timers thread such that it can re-compute its timeout + timers_cv_.notify_one(); + timer->clear_on_reset_callback(); + } +} diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index ddb219b558..449250ed7a 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -618,6 +618,14 @@ if(TARGET test_timer) target_link_libraries(test_timer ${PROJECT_NAME} mimick) endif() +ament_add_gtest(test_timers_manager test_timers_manager.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_timers_manager) + ament_target_dependencies(test_timers_manager + "rcl") + target_link_libraries(test_timers_manager ${PROJECT_NAME} mimick) +endif() + ament_add_gtest(test_time_source test_time_source.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_time_source) @@ -706,6 +714,21 @@ if(TARGET test_executor_notify_waitable) target_link_libraries(test_executor_notify_waitable ${PROJECT_NAME} mimick) endif() +ament_add_gtest(test_events_executor executors/test_events_executor.cpp TIMEOUT 5) +if(TARGET test_events_executor) + ament_target_dependencies(test_events_executor + "test_msgs") + target_link_libraries(test_events_executor ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_events_queue executors/test_events_queue.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_events_queue) + ament_target_dependencies(test_events_queue + "test_msgs") + target_link_libraries(test_events_queue ${PROJECT_NAME}) +endif() + ament_add_gtest(test_guard_condition test_guard_condition.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_guard_condition) diff --git a/rclcpp/test/rclcpp/executors/test_events_executor.cpp b/rclcpp/test/rclcpp/executors/test_events_executor.cpp new file mode 100644 index 0000000000..fbe83fcddc --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_events_executor.cpp @@ -0,0 +1,549 @@ +// Copyright 2023 iRobot Corporation. +// +// 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. + +#include + +#include +#include +#include + +#include "rclcpp/experimental/executors/events_executor/events_executor.hpp" + +#include "test_msgs/srv/empty.hpp" +#include "test_msgs/msg/empty.hpp" + +using namespace std::chrono_literals; + +using rclcpp::experimental::executors::EventsExecutor; + +class TestEventsExecutor : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestEventsExecutor, run_pub_sub) +{ + // rmw_connextdds doesn't support events-executor + if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { + GTEST_SKIP(); + } + + auto node = std::make_shared("node"); + + bool msg_received = false; + auto subscription = node->create_subscription( + "topic", rclcpp::SensorDataQoS(), + [&msg_received](test_msgs::msg::Empty::ConstSharedPtr msg) + { + (void)msg; + msg_received = true; + }); + + auto publisher = node->create_publisher("topic", rclcpp::SensorDataQoS()); + + EventsExecutor executor; + executor.add_node(node); + + bool spin_exited = false; + std::thread spinner([&spin_exited, &executor, this]() { + executor.spin(); + spin_exited = true; + }); + + auto msg = std::make_unique(); + publisher->publish(std::move(msg)); + + // Wait some time for the subscription to receive the message + auto start = std::chrono::high_resolution_clock::now(); + while ( + !msg_received && + !spin_exited && + (std::chrono::high_resolution_clock::now() - start < 1s)) + { + auto time = std::chrono::high_resolution_clock::now() - start; + auto time_msec = std::chrono::duration_cast(time); + std::this_thread::sleep_for(25ms); + } + + executor.cancel(); + spinner.join(); + executor.remove_node(node); + + EXPECT_TRUE(msg_received); + EXPECT_TRUE(spin_exited); +} + +TEST_F(TestEventsExecutor, run_clients_servers) +{ + // rmw_connextdds doesn't support events-executor + if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { + GTEST_SKIP(); + } + + auto node = std::make_shared("node"); + + bool request_received = false; + bool response_received = false; + auto service = + node->create_service( + "service", + [&request_received]( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) + { + request_received = true; + }); + auto client = node->create_client("service"); + + EventsExecutor executor; + executor.add_node(node); + + bool spin_exited = false; + std::thread spinner([&spin_exited, &executor, this]() { + executor.spin(); + spin_exited = true; + }); + + auto request = std::make_shared(); + client->async_send_request( + request, + [&response_received](rclcpp::Client::SharedFuture result_future) { + (void)result_future; + response_received = true; + }); + + // Wait some time for the client-server to be invoked + auto start = std::chrono::steady_clock::now(); + while ( + !response_received && + !spin_exited && + (std::chrono::steady_clock::now() - start < 1s)) + { + std::this_thread::sleep_for(5ms); + } + + executor.cancel(); + spinner.join(); + executor.remove_node(node); + + EXPECT_TRUE(request_received); + EXPECT_TRUE(response_received); + EXPECT_TRUE(spin_exited); +} + +TEST_F(TestEventsExecutor, spin_once_max_duration_timeout) +{ + // rmw_connextdds doesn't support events-executor + if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { + GTEST_SKIP(); + } + + auto node = std::make_shared("node"); + + EventsExecutor executor; + executor.add_node(node); + + // Consume previous events so we have a fresh start + executor.spin_all(1s); + + size_t t_runs = 0; + auto t = node->create_wall_timer( + 10s, + [&]() { + t_runs++; + }); + + // This first spin_once takes care of the waitable event + // generated by the addition of the timer to the node + executor.spin_once(1s); + EXPECT_EQ(0u, t_runs); + + auto start = std::chrono::steady_clock::now(); + + // This second spin_once should take care of the timer, + executor.spin_once(10ms); + + // but doesn't spin the time enough to call the timer callback. + EXPECT_EQ(0u, t_runs); + EXPECT_TRUE(std::chrono::steady_clock::now() - start < 200ms); +} + +TEST_F(TestEventsExecutor, spin_once_max_duration_timer) +{ + // rmw_connextdds doesn't support events-executor + if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { + GTEST_SKIP(); + } + + auto node = std::make_shared("node"); + + EventsExecutor executor; + executor.add_node(node); + + // Consume previous events so we have a fresh start + executor.spin_all(1s); + + size_t t_runs = 0; + auto t = node->create_wall_timer( + 10ms, + [&]() { + t_runs++; + }); + + // This first spin_once takes care of the waitable event + // generated by the addition of the timer to the node + executor.spin_once(1s); + EXPECT_EQ(0u, t_runs); + + auto start = std::chrono::steady_clock::now(); + + // This second spin_once should take care of the timer + executor.spin_once(11ms); + + EXPECT_EQ(1u, t_runs); + EXPECT_TRUE(std::chrono::steady_clock::now() - start < 200ms); +} + +TEST_F(TestEventsExecutor, spin_some_max_duration) +{ + // rmw_connextdds doesn't support events-executor + if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { + GTEST_SKIP(); + } + + { + auto node = std::make_shared("node"); + + size_t t_runs = 0; + auto t = node->create_wall_timer( + 10s, + [&]() { + t_runs++; + }); + + EventsExecutor executor; + executor.add_node(node); + + auto start = std::chrono::steady_clock::now(); + executor.spin_some(10ms); + + EXPECT_EQ(0u, t_runs); + EXPECT_TRUE(std::chrono::steady_clock::now() - start < 200ms); + } + + { + auto node = std::make_shared("node"); + + size_t t_runs = 0; + auto t = node->create_wall_timer( + 10ms, + [&]() { + t_runs++; + }); + + // Sleep some time for the timer to be ready when spin + std::this_thread::sleep_for(10ms); + + EventsExecutor executor; + executor.add_node(node); + + auto start = std::chrono::steady_clock::now(); + executor.spin_some(10s); + + EXPECT_EQ(1u, t_runs); + EXPECT_TRUE(std::chrono::steady_clock::now() - start < 200ms); + } +} + +TEST_F(TestEventsExecutor, spin_some_zero_duration) +{ + // rmw_connextdds doesn't support events-executor + if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { + GTEST_SKIP(); + } + + auto node = std::make_shared("node"); + + size_t t_runs = 0; + auto t = node->create_wall_timer( + 20ms, + [&]() { + t_runs++; + }); + + // Sleep some time for the timer to be ready when spin + std::this_thread::sleep_for(20ms); + + EventsExecutor executor; + executor.add_node(node); + executor.spin_some(0ms); + + EXPECT_EQ(1u, t_runs); +} + +TEST_F(TestEventsExecutor, spin_all_max_duration) +{ + // rmw_connextdds doesn't support events-executor + if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { + GTEST_SKIP(); + } + + { + auto node = std::make_shared("node"); + + size_t t_runs = 0; + auto t = node->create_wall_timer( + 10s, + [&]() { + t_runs++; + }); + + EventsExecutor executor; + executor.add_node(node); + + auto start = std::chrono::steady_clock::now(); + executor.spin_all(10ms); + + EXPECT_EQ(0u, t_runs); + EXPECT_TRUE(std::chrono::steady_clock::now() - start < 200ms); + } + + { + auto node = std::make_shared("node"); + + size_t t_runs = 0; + auto t = node->create_wall_timer( + 10ms, + [&]() { + t_runs++; + }); + + // Sleep some time for the timer to be ready when spin + std::this_thread::sleep_for(10ms); + + EventsExecutor executor; + executor.add_node(node); + + auto start = std::chrono::steady_clock::now(); + executor.spin_all(10s); + + EXPECT_EQ(1u, t_runs); + EXPECT_TRUE(std::chrono::steady_clock::now() - start < 200ms); + } + + EventsExecutor executor; + EXPECT_THROW(executor.spin_all(0ms), std::invalid_argument); + EXPECT_THROW(executor.spin_all(-5ms), std::invalid_argument); +} + +TEST_F(TestEventsExecutor, cancel_while_timers_running) +{ + // rmw_connextdds doesn't support events-executor + if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { + GTEST_SKIP(); + } + + auto node = std::make_shared("node"); + + EventsExecutor executor; + executor.add_node(node); + + // Take care of previous events for a fresh start + executor.spin_all(1s); + + size_t t1_runs = 0; + auto t1 = node->create_wall_timer( + 1ms, + [&]() { + t1_runs++; + std::this_thread::sleep_for(50ms); + }); + + size_t t2_runs = 0; + auto t2 = node->create_wall_timer( + 1ms, + [&]() { + t2_runs++; + std::this_thread::sleep_for(50ms); + }); + + + std::thread spinner([&executor, this]() {executor.spin();}); + + std::this_thread::sleep_for(10ms); + // Call cancel while t1 callback is still being executed + executor.cancel(); + spinner.join(); + + // Depending on the latency on the system, t2 may start to execute before cancel is signaled + EXPECT_GE(1u, t1_runs); + EXPECT_GE(1u, t2_runs); +} + +TEST_F(TestEventsExecutor, cancel_while_timers_waiting) +{ + // rmw_connextdds doesn't support events-executor + if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { + GTEST_SKIP(); + } + + auto node = std::make_shared("node"); + + size_t t1_runs = 0; + auto t1 = node->create_wall_timer( + 100s, + [&]() { + t1_runs++; + }); + + EventsExecutor executor; + executor.add_node(node); + + auto start = std::chrono::steady_clock::now(); + std::thread spinner([&executor, this]() {executor.spin();}); + + std::this_thread::sleep_for(10ms); + executor.cancel(); + spinner.join(); + + EXPECT_EQ(0u, t1_runs); + EXPECT_TRUE(std::chrono::steady_clock::now() - start < 1s); +} + +TEST_F(TestEventsExecutor, destroy_entities) +{ + // This test fails on Windows! We skip it for now + GTEST_SKIP(); + + // rmw_connextdds doesn't support events-executor + if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { + GTEST_SKIP(); + } + + // Create a publisher node and start publishing messages + auto node_pub = std::make_shared("node_pub"); + auto publisher = node_pub->create_publisher("topic", rclcpp::QoS(10)); + auto timer = node_pub->create_wall_timer( + 2ms, [&]() {publisher->publish(std::make_unique());}); + EventsExecutor executor_pub; + executor_pub.add_node(node_pub); + std::thread spinner([&executor_pub, this]() {executor_pub.spin();}); + + // Create a node with two different subscriptions to the topic + auto node_sub = std::make_shared("node_sub"); + size_t callback_count_1 = 0; + auto subscription_1 = + node_sub->create_subscription( + "topic", rclcpp::QoS(10), [&](test_msgs::msg::Empty::ConstSharedPtr) {callback_count_1++;}); + size_t callback_count_2 = 0; + auto subscription_2 = + node_sub->create_subscription( + "topic", rclcpp::QoS(10), [&](test_msgs::msg::Empty::ConstSharedPtr) {callback_count_2++;}); + EventsExecutor executor_sub; + executor_sub.add_node(node_sub); + + // Wait some time while messages are published + std::this_thread::sleep_for(10ms); + + // Destroy one of the two subscriptions + subscription_1.reset(); + + // Let subscriptions executor spin + executor_sub.spin_some(10ms); + + // The callback count of the destroyed subscription remained at 0 + EXPECT_EQ(0u, callback_count_1); + EXPECT_LT(0u, callback_count_2); + + executor_pub.cancel(); + spinner.join(); +} + +// Testing construction of a subscriptions with QoS event callback functions. +std::string * g_pub_log_msg; +std::string * g_sub_log_msg; +std::promise * g_log_msgs_promise; +TEST_F(TestEventsExecutor, test_default_incompatible_qos_callbacks) +{ + // rmw_connextdds doesn't support events-executor + if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { + GTEST_SKIP(); + } + + auto node = std::make_shared("node"); + rcutils_logging_output_handler_t original_output_handler = rcutils_logging_get_output_handler(); + + std::string pub_log_msg; + std::string sub_log_msg; + std::promise log_msgs_promise; + g_pub_log_msg = &pub_log_msg; + g_sub_log_msg = &sub_log_msg; + g_log_msgs_promise = &log_msgs_promise; + auto logger_callback = []( + const rcutils_log_location_t * /*location*/, + int /*level*/, const char * /*name*/, rcutils_time_point_value_t /*timestamp*/, + const char * format, va_list * args) -> void { + char buffer[1024]; + vsnprintf(buffer, sizeof(buffer), format, *args); + const std::string msg = buffer; + if (msg.rfind("New subscription discovered on topic '/test_topic'", 0) == 0) { + *g_pub_log_msg = buffer; + } else if (msg.rfind("New publisher discovered on topic '/test_topic'", 0) == 0) { + *g_sub_log_msg = buffer; + } + + if (!g_pub_log_msg->empty() && !g_sub_log_msg->empty()) { + g_log_msgs_promise->set_value(); + } + }; + rcutils_logging_set_output_handler(logger_callback); + + std::shared_future log_msgs_future = log_msgs_promise.get_future(); + + rclcpp::QoS qos_profile_publisher(10); + qos_profile_publisher.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + auto publisher = node->create_publisher( + "test_topic", qos_profile_publisher); + + rclcpp::QoS qos_profile_subscription(10); + qos_profile_subscription.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + auto subscription = node->create_subscription( + "test_topic", qos_profile_subscription, [&](test_msgs::msg::Empty::ConstSharedPtr) {}); + + EventsExecutor ex; + ex.add_node(node->get_node_base_interface()); + + const auto timeout = std::chrono::seconds(10); + ex.spin_until_future_complete(log_msgs_future, timeout); + + EXPECT_EQ( + "New subscription discovered on topic '/test_topic', requesting incompatible QoS. " + "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", + pub_log_msg); + EXPECT_EQ( + "New publisher discovered on topic '/test_topic', offering incompatible QoS. " + "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", + sub_log_msg); + + rcutils_logging_set_output_handler(original_output_handler); +} diff --git a/rclcpp/test/rclcpp/executors/test_events_queue.cpp b/rclcpp/test/rclcpp/executors/test_events_queue.cpp new file mode 100644 index 0000000000..de8242b55b --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_events_queue.cpp @@ -0,0 +1,82 @@ +// Copyright 2023 iRobot Corporation. +// +// 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. + +#include + +#include + +#include "rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp" +#include "rclcpp/experimental/executors/events_executor/simple_events_queue.hpp" + +using namespace std::chrono_literals; + +TEST(TestEventsQueue, SimpleQueueTest) +{ + // Create a SimpleEventsQueue and a local queue + auto simple_queue = std::make_unique(); + rclcpp::experimental::executors::ExecutorEvent event {}; + bool ret = false; + + // Make sure the queue is empty at startup + EXPECT_TRUE(simple_queue->empty()); + EXPECT_EQ(simple_queue->size(), 0u); + + // Push 11 messages + for (uint32_t i = 1; i < 11; i++) { + rclcpp::experimental::executors::ExecutorEvent stub_event {}; + stub_event.num_events = 1; + simple_queue->enqueue(stub_event); + + EXPECT_FALSE(simple_queue->empty()); + EXPECT_EQ(simple_queue->size(), i); + } + + // Pop one message + ret = simple_queue->dequeue(event); + EXPECT_TRUE(ret); + EXPECT_FALSE(simple_queue->empty()); + EXPECT_EQ(simple_queue->size(), 9u); + + // Pop one message + ret = simple_queue->dequeue(event, std::chrono::nanoseconds(0)); + EXPECT_TRUE(ret); + EXPECT_FALSE(simple_queue->empty()); + EXPECT_EQ(simple_queue->size(), 8u); + + while (!simple_queue->empty()) { + ret = simple_queue->dequeue(event); + EXPECT_TRUE(ret); + } + + EXPECT_TRUE(simple_queue->empty()); + EXPECT_EQ(simple_queue->size(), 0u); + + ret = simple_queue->dequeue(event, std::chrono::nanoseconds(0)); + EXPECT_FALSE(ret); + + // Lets push an event into the queue and get it back + rclcpp::experimental::executors::ExecutorEvent push_event = { + simple_queue.get(), + 99, + rclcpp::experimental::executors::ExecutorEventType::SUBSCRIPTION_EVENT, + 1}; + + simple_queue->enqueue(push_event); + ret = simple_queue->dequeue(event); + EXPECT_TRUE(ret); + EXPECT_EQ(push_event.entity_key, event.entity_key); + EXPECT_EQ(push_event.waitable_data, event.waitable_data); + EXPECT_EQ(push_event.type, event.type); + EXPECT_EQ(push_event.num_events, event.num_events); +} diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index eb6652f19b..38b6ddf870 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -92,7 +92,8 @@ using ExecutorTypes = ::testing::Types< rclcpp::executors::SingleThreadedExecutor, rclcpp::executors::MultiThreadedExecutor, - rclcpp::executors::StaticSingleThreadedExecutor>; + rclcpp::executors::StaticSingleThreadedExecutor, + rclcpp::experimental::executors::EventsExecutor>; class ExecutorTypeNames { @@ -113,6 +114,10 @@ class ExecutorTypeNames return "StaticSingleThreadedExecutor"; } + if (std::is_same()) { + return "EventsExecutor"; + } + return ""; } }; @@ -126,12 +131,22 @@ TYPED_TEST_SUITE(TestExecutors, ExecutorTypes, ExecutorTypeNames); using StandardExecutors = ::testing::Types< rclcpp::executors::SingleThreadedExecutor, - rclcpp::executors::MultiThreadedExecutor>; + rclcpp::executors::MultiThreadedExecutor, + rclcpp::experimental::executors::EventsExecutor>; TYPED_TEST_SUITE(TestExecutorsStable, StandardExecutors, ExecutorTypeNames); // Make sure that executors detach from nodes when destructing -TYPED_TEST(TestExecutors, detachOnDestruction) { +TYPED_TEST(TestExecutors, detachOnDestruction) +{ using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + { ExecutorType executor; executor.add_node(this->node); @@ -145,8 +160,17 @@ TYPED_TEST(TestExecutors, detachOnDestruction) { // Make sure that the executor can automatically remove expired nodes correctly // Currently fails for StaticSingleThreadedExecutor so it is being skipped, see: // https://github.com/ros2/rclcpp/issues/1231 -TYPED_TEST(TestExecutorsStable, addTemporaryNode) { +TYPED_TEST(TestExecutorsStable, addTemporaryNode) +{ using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + ExecutorType executor; { @@ -163,9 +187,37 @@ TYPED_TEST(TestExecutorsStable, addTemporaryNode) { spinner.join(); } +// Make sure that a spinning empty executor can be cancelled +TYPED_TEST(TestExecutors, emptyExecutor) +{ + using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + + ExecutorType executor; + std::thread spinner([&]() {EXPECT_NO_THROW(executor.spin());}); + std::this_thread::sleep_for(50ms); + executor.cancel(); + spinner.join(); +} + // Check executor throws properly if the same node is added a second time -TYPED_TEST(TestExecutors, addNodeTwoExecutors) { +TYPED_TEST(TestExecutors, addNodeTwoExecutors) +{ using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + ExecutorType executor1; ExecutorType executor2; EXPECT_NO_THROW(executor1.add_node(this->node)); @@ -174,8 +226,17 @@ TYPED_TEST(TestExecutors, addNodeTwoExecutors) { } // Check simple spin example -TYPED_TEST(TestExecutors, spinWithTimer) { +TYPED_TEST(TestExecutors, spinWithTimer) +{ using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + ExecutorType executor; bool timer_completed = false; @@ -196,8 +257,17 @@ TYPED_TEST(TestExecutors, spinWithTimer) { executor.remove_node(this->node, true); } -TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { +TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) +{ using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + ExecutorType executor; executor.add_node(this->node); @@ -222,8 +292,17 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { } // Check executor exits immediately if future is complete. -TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) { +TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) +{ using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + ExecutorType executor; executor.add_node(this->node); @@ -244,8 +323,17 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) { } // Same test, but uses a shared future. -TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) { +TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) +{ using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + ExecutorType executor; executor.add_node(this->node); @@ -267,8 +355,17 @@ TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) { } // For a longer running future that should require several iterations of spin_once -TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) { +TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) +{ using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + ExecutorType executor; executor.add_node(this->node); @@ -313,8 +410,17 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) { } // Check spin_until_future_complete timeout works as expected -TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) { +TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) +{ using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + ExecutorType executor; executor.add_node(this->node); @@ -380,6 +486,13 @@ class TestWaitable : public rclcpp::Waitable return nullptr; } + std::shared_ptr + take_data_by_entity_id(size_t id) override + { + (void) id; + return nullptr; + } + void execute(std::shared_ptr & data) override { @@ -388,6 +501,21 @@ class TestWaitable : public rclcpp::Waitable std::this_thread::sleep_for(3ms); } + void + set_on_ready_callback(std::function callback) override + { + auto gc_callback = [callback](size_t count) { + callback(count, 0); + }; + gc_.set_on_trigger_callback(gc_callback); + } + + void + clear_on_ready_callback() override + { + gc_.set_on_trigger_callback(nullptr); + } + size_t get_number_of_ready_guard_conditions() override {return 1;} @@ -402,8 +530,17 @@ class TestWaitable : public rclcpp::Waitable rclcpp::GuardCondition gc_; }; -TYPED_TEST(TestExecutors, spinAll) { +TYPED_TEST(TestExecutors, spinAll) +{ using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + ExecutorType executor; auto waitable_interfaces = this->node->get_node_waitables_interface(); auto my_waitable = std::make_shared(); @@ -443,8 +580,17 @@ TYPED_TEST(TestExecutors, spinAll) { spinner.join(); } -TYPED_TEST(TestExecutors, spinSome) { +TYPED_TEST(TestExecutors, spinSome) +{ using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + ExecutorType executor; auto waitable_interfaces = this->node->get_node_waitables_interface(); auto my_waitable = std::make_shared(); @@ -472,8 +618,9 @@ TYPED_TEST(TestExecutors, spinSome) { this->publisher->publish(test_msgs::msg::Empty()); std::this_thread::sleep_for(1ms); } - - EXPECT_EQ(1u, my_waitable->get_count()); + // The count of "execute" depends on whether the executor starts spinning before (1) or after (0) + // the first iteration of the while loop + EXPECT_LE(1u, my_waitable->get_count()); waitable_interfaces->remove_waitable(my_waitable, nullptr); EXPECT_TRUE(spin_exited); // Cancel if it hasn't exited already. @@ -483,8 +630,17 @@ TYPED_TEST(TestExecutors, spinSome) { } // Check spin_node_until_future_complete with node base pointer -TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) { +TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) +{ using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + ExecutorType executor; std::promise promise; @@ -498,8 +654,17 @@ TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) { } // Check spin_node_until_future_complete with node pointer -TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodePtr) { +TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodePtr) +{ using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + ExecutorType executor; std::promise promise; @@ -513,8 +678,17 @@ TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodePtr) { } // Check spin_until_future_complete can be properly interrupted. -TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) { +TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) +{ using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + ExecutorType executor; executor.add_node(this->node); @@ -556,7 +730,8 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) { } // Check spin_until_future_complete with node base pointer (instantiates its own executor) -TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) { +TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) +{ rclcpp::init(0, nullptr); { @@ -576,7 +751,8 @@ TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) { } // Check spin_until_future_complete with node pointer (instantiates its own executor) -TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) { +TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) +{ rclcpp::init(0, nullptr); { diff --git a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp index 02fa0b7a94..49131638db 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -46,11 +46,15 @@ class TestAddCallbackGroupsToExecutor : public ::testing::Test } }; +template +class TestAddCallbackGroupsToExecutorStable : public TestAddCallbackGroupsToExecutor {}; + using ExecutorTypes = ::testing::Types< rclcpp::executors::SingleThreadedExecutor, rclcpp::executors::MultiThreadedExecutor, - rclcpp::executors::StaticSingleThreadedExecutor>; + rclcpp::executors::StaticSingleThreadedExecutor, + rclcpp::experimental::executors::EventsExecutor>; class ExecutorTypeNames { @@ -71,17 +75,38 @@ class ExecutorTypeNames return "StaticSingleThreadedExecutor"; } + if (std::is_same()) { + return "EventsExecutor"; + } + return ""; } }; TYPED_TEST_SUITE(TestAddCallbackGroupsToExecutor, ExecutorTypes, ExecutorTypeNames); +// StaticSingleThreadedExecutor is not included in these tests for now +using StandardExecutors = + ::testing::Types< + rclcpp::executors::SingleThreadedExecutor, + rclcpp::executors::MultiThreadedExecutor, + rclcpp::experimental::executors::EventsExecutor>; +TYPED_TEST_SUITE(TestAddCallbackGroupsToExecutorStable, StandardExecutors, ExecutorTypeNames); + /* * Test adding callback groups. */ -TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups) { +TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups) +{ using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + auto node = std::make_shared("my_node", "/ns"); auto timer_callback = []() {}; rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( @@ -127,8 +152,17 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups) { /* * Test removing callback groups. */ -TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups) { +TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups) +{ using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + auto node = std::make_shared("my_node", "/ns"); auto timer_callback = []() {}; rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( @@ -158,7 +192,16 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups) { */ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_duplicate_callback_groups) { - rclcpp::executors::MultiThreadedExecutor executor; + using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + + ExecutorType executor; auto node = std::make_shared("my_node", "/ns"); auto timer_callback = []() {}; rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( @@ -176,7 +219,16 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_duplicate_callback_groups) */ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_to_executor) { - rclcpp::executors::MultiThreadedExecutor executor; + using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + + ExecutorType executor; auto node = std::make_shared("my_node", "/ns"); executor.add_node(node->get_node_base_interface()); ASSERT_EQ(executor.get_all_callback_groups().size(), 1u); @@ -210,13 +262,22 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t */ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_unallowable_callback_groups) { + using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + + ExecutorType executor; auto node = std::make_shared("my_node", "/ns"); auto timer_callback = []() {}; rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer( 2s, timer_callback, cb_grp); - rclcpp::executors::MultiThreadedExecutor executor; executor.add_callback_group(cb_grp, node->get_node_base_interface()); ASSERT_EQ(executor.get_all_callback_groups().size(), 1u); @@ -245,14 +306,23 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_unallowable_callback_groups) */ TYPED_TEST(TestAddCallbackGroupsToExecutor, one_node_many_callback_groups_many_executors) { + using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + + ExecutorType timer_executor; + ExecutorType sub_executor; auto node = std::make_shared("my_node", "/ns"); auto timer_callback = []() {}; rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer( 2s, timer_callback, cb_grp); - rclcpp::executors::MultiThreadedExecutor timer_executor; - rclcpp::executors::MultiThreadedExecutor sub_executor; timer_executor.add_callback_group(cb_grp, node->get_node_base_interface()); const rclcpp::QoS qos(10); auto options = rclcpp::SubscriptionOptions(); @@ -282,14 +352,23 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, one_node_many_callback_groups_many_e * because the executor can't be triggered while a subscriber created, see * https://github.com/ros2/rclcpp/issues/1611 */ -TYPED_TEST(TestAddCallbackGroupsToExecutor, subscriber_triggered_to_receive_message) +TYPED_TEST(TestAddCallbackGroupsToExecutorStable, subscriber_triggered_to_receive_message) { + using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + auto node = std::make_shared("my_node", "/ns"); // create a thread running an executor with a new callback group for a coming subscriber rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); - rclcpp::executors::SingleThreadedExecutor cb_grp_executor; + ExecutorType cb_grp_executor; std::promise received_message_promise; auto received_message_future = received_message_promise.get_future(); @@ -329,7 +408,7 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, subscriber_triggered_to_receive_mess timer_promise.set_value(); }; - rclcpp::executors::SingleThreadedExecutor timer_executor; + ExecutorType timer_executor; timer = node->create_wall_timer(100ms, timer_callback); timer_executor.add_node(node); auto future = timer_promise.get_future(); @@ -346,8 +425,17 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, subscriber_triggered_to_receive_mess * because the executor can't be triggered while a subscriber created, see * https://github.com/ros2/rclcpp/issues/2067 */ -TYPED_TEST(TestAddCallbackGroupsToExecutor, callback_group_create_after_spin) +TYPED_TEST(TestAddCallbackGroupsToExecutorStable, callback_group_create_after_spin) { + using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + auto node = std::make_shared("my_node", "/ns"); // create a publisher to send data @@ -357,7 +445,7 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, callback_group_create_after_spin) publisher->publish(test_msgs::msg::Empty()); // create a thread running an executor - rclcpp::executors::SingleThreadedExecutor executor; + ExecutorType executor; executor.add_node(node); std::promise received_message_promise; auto received_message_future = received_message_promise.get_future(); @@ -392,7 +480,16 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, callback_group_create_after_spin) */ TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_group) { - rclcpp::executors::MultiThreadedExecutor executor; + using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + + ExecutorType executor; auto node = std::make_shared("my_node", "/ns"); auto timer_callback = []() {}; rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( diff --git a/rclcpp/test/rclcpp/test_timers_manager.cpp b/rclcpp/test/rclcpp/test_timers_manager.cpp new file mode 100644 index 0000000000..635ec322c0 --- /dev/null +++ b/rclcpp/test/rclcpp/test_timers_manager.cpp @@ -0,0 +1,419 @@ +// Copyright 2023 iRobot Corporation. +// +// 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. + +#include + +#include +#include +#include + +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/experimental/timers_manager.hpp" + +using namespace std::chrono_literals; + +using rclcpp::experimental::TimersManager; + +using CallbackT = std::function; +using TimerT = rclcpp::WallTimer; + +class TestTimersManager : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +static void execute_all_ready_timers(std::shared_ptr timers_manager) +{ + bool head_was_ready = false; + do { + head_was_ready = timers_manager->execute_head_timer(); + } while (head_was_ready); +} + +TEST_F(TestTimersManager, empty_manager) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + EXPECT_EQ(std::chrono::nanoseconds::max(), timers_manager->get_head_timeout()); + EXPECT_FALSE(timers_manager->execute_head_timer()); + EXPECT_NO_THROW(timers_manager->clear()); + EXPECT_NO_THROW(timers_manager->start()); + EXPECT_NO_THROW(timers_manager->stop()); +} + +TEST_F(TestTimersManager, add_run_remove_timer) +{ + size_t t_runs = 0; + auto t = TimerT::make_shared( + 10ms, + [&t_runs]() { + t_runs++; + }, + rclcpp::contexts::get_global_default_context()); + std::weak_ptr t_weak = t; + + // Add the timer to the timers manager + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + timers_manager->add_timer(t); + + // Sleep for more 3 times the timer period + std::this_thread::sleep_for(30ms); + + // The timer is executed only once, even if we slept 3 times the period + execute_all_ready_timers(timers_manager); + EXPECT_EQ(1u, t_runs); + + // Remove the timer from the manager + timers_manager->remove_timer(t); + + t.reset(); + // The timer is now not valid anymore + EXPECT_FALSE(t_weak.lock() != nullptr); +} + +TEST_F(TestTimersManager, clear) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + auto t1 = TimerT::make_shared(1ms, CallbackT(), rclcpp::contexts::get_global_default_context()); + std::weak_ptr t1_weak = t1; + auto t2 = TimerT::make_shared(1ms, CallbackT(), rclcpp::contexts::get_global_default_context()); + std::weak_ptr t2_weak = t2; + + timers_manager->add_timer(t1); + timers_manager->add_timer(t2); + + EXPECT_TRUE(t1_weak.lock() != nullptr); + EXPECT_TRUE(t2_weak.lock() != nullptr); + + timers_manager->clear(); + + t1.reset(); + t2.reset(); + + EXPECT_FALSE(t1_weak.lock() != nullptr); + EXPECT_FALSE(t2_weak.lock() != nullptr); +} + +TEST_F(TestTimersManager, remove_not_existing_timer) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + // Try to remove a nullptr timer + EXPECT_NO_THROW(timers_manager->remove_timer(nullptr)); + + auto t = TimerT::make_shared(1ms, CallbackT(), rclcpp::contexts::get_global_default_context()); + timers_manager->add_timer(t); + + // Remove twice the same timer + timers_manager->remove_timer(t); + EXPECT_NO_THROW(timers_manager->remove_timer(t)); +} + +TEST_F(TestTimersManager, timers_thread_exclusive_usage) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + timers_manager->start(); + + EXPECT_THROW(timers_manager->start(), std::exception); + EXPECT_THROW(timers_manager->get_head_timeout(), std::exception); + EXPECT_THROW(timers_manager->execute_head_timer(), std::exception); + + timers_manager->stop(); + + EXPECT_NO_THROW(timers_manager->get_head_timeout()); + EXPECT_NO_THROW(timers_manager->execute_head_timer()); +} + +TEST_F(TestTimersManager, add_timer_twice) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + auto t = TimerT::make_shared(1ms, CallbackT(), rclcpp::contexts::get_global_default_context()); + + timers_manager->add_timer(t); + EXPECT_NO_THROW(timers_manager->add_timer(t)); +} + +TEST_F(TestTimersManager, add_nullptr) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + EXPECT_THROW(timers_manager->add_timer(nullptr), std::exception); +} + +TEST_F(TestTimersManager, head_not_ready) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + size_t t_runs = 0; + auto t = TimerT::make_shared( + 10s, + [&t_runs]() { + t_runs++; + }, + rclcpp::contexts::get_global_default_context()); + + timers_manager->add_timer(t); + + // Timer will take 10s to get ready, so nothing to execute here + bool ret = timers_manager->execute_head_timer(); + EXPECT_FALSE(ret); + EXPECT_EQ(0u, t_runs); +} + +TEST_F(TestTimersManager, timers_order) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + size_t t1_runs = 0; + auto t1 = TimerT::make_shared( + 10ms, + [&t1_runs]() { + t1_runs++; + }, + rclcpp::contexts::get_global_default_context()); + + size_t t2_runs = 0; + auto t2 = TimerT::make_shared( + 30ms, + [&t2_runs]() { + t2_runs++; + }, + rclcpp::contexts::get_global_default_context()); + + size_t t3_runs = 0; + auto t3 = TimerT::make_shared( + 100ms, + [&t3_runs]() { + t3_runs++; + }, + rclcpp::contexts::get_global_default_context()); + + // Add timers in a random order + timers_manager->add_timer(t2); + timers_manager->add_timer(t3); + timers_manager->add_timer(t1); + + std::this_thread::sleep_for(10ms); + execute_all_ready_timers(timers_manager); + EXPECT_EQ(1u, t1_runs); + EXPECT_EQ(0u, t2_runs); + EXPECT_EQ(0u, t3_runs); + + std::this_thread::sleep_for(30ms); + execute_all_ready_timers(timers_manager); + EXPECT_EQ(2u, t1_runs); + EXPECT_EQ(1u, t2_runs); + EXPECT_EQ(0u, t3_runs); + + std::this_thread::sleep_for(100ms); + execute_all_ready_timers(timers_manager); + EXPECT_EQ(3u, t1_runs); + EXPECT_EQ(2u, t2_runs); + EXPECT_EQ(1u, t3_runs); + + timers_manager->remove_timer(t1); + + std::this_thread::sleep_for(30ms); + execute_all_ready_timers(timers_manager); + EXPECT_EQ(3u, t1_runs); + EXPECT_EQ(3u, t2_runs); + EXPECT_EQ(1u, t3_runs); +} + +TEST_F(TestTimersManager, start_stop_timers_thread) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + auto t = TimerT::make_shared(1ms, []() {}, rclcpp::contexts::get_global_default_context()); + timers_manager->add_timer(t); + + // Calling start multiple times will throw an error + EXPECT_NO_THROW(timers_manager->start()); + EXPECT_THROW(timers_manager->start(), std::exception); + + // Calling stop multiple times does not throw an error + EXPECT_NO_THROW(timers_manager->stop()); + EXPECT_NO_THROW(timers_manager->stop()); +} + +TEST_F(TestTimersManager, timers_thread) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + size_t t1_runs = 0; + auto t1 = TimerT::make_shared( + 1ms, + [&t1_runs]() { + t1_runs++; + }, + rclcpp::contexts::get_global_default_context()); + + size_t t2_runs = 0; + auto t2 = TimerT::make_shared( + 1ms, + [&t2_runs]() { + t2_runs++; + }, + rclcpp::contexts::get_global_default_context()); + + // Add timers + timers_manager->add_timer(t1); + timers_manager->add_timer(t2); + + // Run timers thread for a while + timers_manager->start(); + std::this_thread::sleep_for(5ms); + timers_manager->stop(); + + EXPECT_LT(1u, t1_runs); + EXPECT_LT(1u, t2_runs); + EXPECT_EQ(t1_runs, t2_runs); +} + +TEST_F(TestTimersManager, destructor) +{ + size_t t_runs = 0; + auto t = TimerT::make_shared( + 1ms, + [&t_runs]() { + t_runs++; + }, + rclcpp::contexts::get_global_default_context()); + std::weak_ptr t_weak = t; + + // When the timers manager is destroyed, it will stop the thread + // and clear the timers + { + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + timers_manager->add_timer(t); + + timers_manager->start(); + std::this_thread::sleep_for(100ms); + + EXPECT_LT(1u, t_runs); + } + + // The thread is not running anymore, so this value does not increase + size_t runs = t_runs; + std::this_thread::sleep_for(100ms); + EXPECT_EQ(runs, t_runs); + t.reset(); + EXPECT_FALSE(t_weak.lock() != nullptr); +} + +TEST_F(TestTimersManager, add_remove_while_thread_running) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + size_t t1_runs = 0; + auto t1 = TimerT::make_shared( + 1ms, + [&t1_runs]() { + t1_runs++; + }, + rclcpp::contexts::get_global_default_context()); + + size_t t2_runs = 0; + auto t2 = TimerT::make_shared( + 1ms, + [&t2_runs]() { + t2_runs++; + }, + rclcpp::contexts::get_global_default_context()); + + // Add timers + timers_manager->add_timer(t1); + + // Start timers thread + timers_manager->start(); + + // After a while remove t1 and add t2 + std::this_thread::sleep_for(5ms); + timers_manager->remove_timer(t1); + size_t tmp_t1 = t1_runs; + timers_manager->add_timer(t2); + + // Wait some more time and then stop + std::this_thread::sleep_for(5ms); + timers_manager->stop(); + + // t1 has stopped running + EXPECT_EQ(tmp_t1, t1_runs); + // t2 is correctly running + EXPECT_LT(1u, t2_runs); +} + +TEST_F(TestTimersManager, infinite_loop) +{ + // This test makes sure that even if timers have a period shorter than the duration + // of their callback the functions never block indefinitely. + + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + size_t t1_runs = 0; + auto t1 = TimerT::make_shared( + 1ms, + [&t1_runs]() { + t1_runs++; + std::this_thread::sleep_for(5ms); + }, + rclcpp::contexts::get_global_default_context()); + + size_t t2_runs = 0; + auto t2 = TimerT::make_shared( + 1ms, + [&t2_runs]() { + t2_runs++; + std::this_thread::sleep_for(5ms); + }, + rclcpp::contexts::get_global_default_context()); + + timers_manager->add_timer(t1); + timers_manager->add_timer(t2); + + // Start a timers thread and make sure that we can stop it later + timers_manager->start(); + std::this_thread::sleep_for(50ms); + timers_manager->stop(); + + EXPECT_LT(0u, t1_runs); + EXPECT_LT(0u, t2_runs); +} From 9c03a463c15a3c1e741db0413f08609f8f904857 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 18 Apr 2023 10:29:30 -0500 Subject: [PATCH 219/455] Picking ABI-incompatible executor changes (#2170) * Picking ABI-incompatible executor changes * Add PIMPL Signed-off-by: Michael Carroll --- rclcpp/include/rclcpp/callback_group.hpp | 7 + rclcpp/include/rclcpp/executor.hpp | 4 + .../executor_entities_collection.hpp | 9 +- rclcpp/src/rclcpp/callback_group.cpp | 10 + rclcpp/src/rclcpp/executor.cpp | 5 +- .../executor_entities_collection.cpp | 207 ++++++++++-------- 6 files changed, 147 insertions(+), 95 deletions(-) diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index 97579fcf8c..43c7daa888 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -180,6 +180,13 @@ class CallbackGroup return _find_ptrs_if_impl(func, waitable_ptrs_); } + /// Get the total number of entities in this callback group. + /** + * \return the number of entities in the callback group. + */ + RCLCPP_PUBLIC + size_t size() const; + RCLCPP_PUBLIC std::atomic_bool & can_be_taken_from(); diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 3e654faa54..2d5ca2149a 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -51,6 +51,7 @@ typedef std::map impl_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp index 98a92ccdd8..166bb99119 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp @@ -198,14 +198,15 @@ build_entities_collection( * * \param[in] collection Collection of entities corresponding to the current wait set. * \param[in] wait_result Result of rclcpp::WaitSet::wait corresponding to the collection. - * \return A queue of executables that have been marked ready by the waitset. + * \param[inout] queue of executables to append new ready executables to + * \return number of new ready executables */ -std::deque +size_t ready_executables( const ExecutorEntitiesCollection & collection, - rclcpp::WaitResult & wait_result + rclcpp::WaitResult & wait_result, + std::deque & executables ); - } // namespace executors } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index 753a441332..77f6c87bd9 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -56,6 +56,16 @@ CallbackGroup::type() const return type_; } +size_t +CallbackGroup::size() const +{ + return + subscription_ptrs_.size() + + service_ptrs_.size() + + client_ptrs_.size() + + timer_ptrs_.size() + + waitable_ptrs_.size(); +} void CallbackGroup::collect_all_ptrs( std::function sub_func, std::function service_func, diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index ccd1b61016..a7a2b8278b 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -41,11 +41,14 @@ using namespace std::chrono_literals; using rclcpp::exceptions::throw_from_rcl_error; using rclcpp::Executor; +class rclcpp::ExecutorImplementation {}; + Executor::Executor(const rclcpp::ExecutorOptions & options) : spinning(false), interrupt_guard_condition_(std::make_shared(options.context)), shutdown_guard_condition_(std::make_shared(options.context)), - memory_strategy_(options.memory_strategy) + memory_strategy_(options.memory_strategy), + impl_(std::make_unique()) { // Store the context for later use. context_ = options.context; diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index 567b28014e..88a878824a 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -20,12 +20,13 @@ namespace executors { bool ExecutorEntitiesCollection::empty() const { - return subscriptions.empty() && - timers.empty() && - guard_conditions.empty() && - clients.empty() && - services.empty() && - waitables.empty(); + return + subscriptions.empty() && + timers.empty() && + guard_conditions.empty() && + clients.empty() && + services.empty() && + waitables.empty(); } void ExecutorEntitiesCollection::clear() @@ -38,7 +39,6 @@ void ExecutorEntitiesCollection::clear() waitables.clear(); } - void build_entities_collection( const std::vector & callback_groups, @@ -94,109 +94,136 @@ build_entities_collection( } } -template -void check_ready( - EntityCollectionType & collection, - std::deque & executables, - size_t size_of_waited_entities, - typename EntityCollectionType::Key * waited_entities, - std::function fill_executable) +size_t +ready_executables( + const ExecutorEntitiesCollection & collection, + rclcpp::WaitResult & wait_result, + std::deque & executables +) { - for (size_t ii = 0; ii < size_of_waited_entities; ++ii) { - if (!waited_entities[ii]) {continue;} - auto entity_iter = collection.find(waited_entities[ii]); - if (entity_iter != collection.end()) { + size_t added = 0; + if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { + return added; + } + auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set(); + + // Cache shared pointers to groups to avoid extra work re-locking them + std::map> group_map; + + auto group_cache = [&group_map](const rclcpp::CallbackGroup::WeakPtr & weak_cbg_ptr) + { + if (group_map.count(weak_cbg_ptr) == 0) { + group_map.insert({weak_cbg_ptr, weak_cbg_ptr.lock()}); + } + return group_map.find(weak_cbg_ptr)->second; + }; + + for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) { + if (nullptr == rcl_wait_set.timers[ii]) {continue;} + auto entity_iter = collection.timers.find(rcl_wait_set.timers[ii]); + if (entity_iter != collection.timers.end()) { auto entity = entity_iter->second.entity.lock(); if (!entity) { continue; } - - auto callback_group = entity_iter->second.callback_group.lock(); - if (callback_group && !callback_group->can_be_taken_from().load()) { + auto group_info = group_cache(entity_iter->second.callback_group); + if (group_info && !group_info->can_be_taken_from().load()) { + continue; + } + if (!entity->call()) { continue; } rclcpp::AnyExecutable exec; + exec.timer = entity; + exec.callback_group = group_info; + executables.push_back(exec); + added++; + } + } - exec.callback_group = callback_group; - if (fill_executable(exec, entity)) { - executables.push_back(exec); + for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) { + if (nullptr == rcl_wait_set.subscriptions[ii]) {continue;} + auto entity_iter = collection.subscriptions.find(rcl_wait_set.subscriptions[ii]); + if (entity_iter != collection.subscriptions.end()) { + auto entity = entity_iter->second.entity.lock(); + if (!entity) { + continue; } + auto group_info = group_cache(entity_iter->second.callback_group); + if (group_info && !group_info->can_be_taken_from().load()) { + continue; + } + rclcpp::AnyExecutable exec; + exec.subscription = entity; + exec.callback_group = group_info; + executables.push_back(exec); + added++; } } -} -std::deque -ready_executables( - const ExecutorEntitiesCollection & collection, - rclcpp::WaitResult & wait_result -) -{ - std::deque ret; - - if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { - return ret; - } - auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set(); - check_ready( - collection.timers, - ret, - rcl_wait_set.size_of_timers, - rcl_wait_set.timers, - [](rclcpp::AnyExecutable & exec, auto timer) { - if (!timer->call()) { - return false; + for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) { + if (nullptr == rcl_wait_set.services[ii]) {continue;} + auto entity_iter = collection.services.find(rcl_wait_set.services[ii]); + if (entity_iter != collection.services.end()) { + auto entity = entity_iter->second.entity.lock(); + if (!entity) { + continue; } - exec.timer = timer; - return true; - }); - - check_ready( - collection.subscriptions, - ret, - rcl_wait_set.size_of_subscriptions, - rcl_wait_set.subscriptions, - [](rclcpp::AnyExecutable & exec, auto subscription) { - exec.subscription = subscription; - return true; - }); - - - check_ready( - collection.services, - ret, - rcl_wait_set.size_of_services, - rcl_wait_set.services, - [](rclcpp::AnyExecutable & exec, auto service) { - exec.service = service; - return true; - }); - - check_ready( - collection.clients, - ret, - rcl_wait_set.size_of_clients, - rcl_wait_set.clients, - [](rclcpp::AnyExecutable & exec, auto client) { - exec.client = client; - return true; - }); - - for (auto & [handle, entry] : collection.waitables) { - auto waitable = entry.entity.lock(); - if (waitable && waitable->is_ready(&rcl_wait_set)) { - auto group = entry.callback_group.lock(); - if (group && !group->can_be_taken_from().load()) { + auto group_info = group_cache(entity_iter->second.callback_group); + if (group_info && !group_info->can_be_taken_from().load()) { continue; } + rclcpp::AnyExecutable exec; + exec.service = entity; + exec.callback_group = group_info; + executables.push_back(exec); + added++; + } + } + for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) { + if (nullptr == rcl_wait_set.clients[ii]) {continue;} + auto entity_iter = collection.clients.find(rcl_wait_set.clients[ii]); + if (entity_iter != collection.clients.end()) { + auto entity = entity_iter->second.entity.lock(); + if (!entity) { + continue; + } + auto group_info = group_cache(entity_iter->second.callback_group); + if (group_info && !group_info->can_be_taken_from().load()) { + continue; + } rclcpp::AnyExecutable exec; - exec.waitable = waitable; - exec.callback_group = group; - ret.push_back(exec); + exec.client = entity; + exec.callback_group = group_info; + executables.push_back(exec); + added++; + } + } + + for (auto & [handle, entry] : collection.waitables) { + auto waitable = entry.entity.lock(); + if (!waitable) { + continue; + } + if (!waitable->is_ready(&rcl_wait_set)) { + continue; } + auto group_info = group_cache(entry.callback_group); + if (group_info && !group_info->can_be_taken_from().load()) { + continue; + } + rclcpp::AnyExecutable exec; + exec.waitable = waitable; + exec.callback_group = group_info; + exec.data = waitable->take_data(); + executables.push_back(exec); + added++; } - return ret; + + return added; } } // namespace executors From 3610b68348adbc441f130e058f8a8e38032e855a Mon Sep 17 00:00:00 2001 From: Lei Liu <64953129+llapx@users.noreply.github.com> Date: Tue, 18 Apr 2023 23:30:00 +0800 Subject: [PATCH 220/455] Add support for logging service. (#2122) * Add support for logging service. * Update to not modify interfaces and not change time_source * Use unique_ptr for NodeBuiltinExecutorImpl * Use user thread to run logger service * Update code for lifecycle_node Signed-off-by: Barry Xu Signed-off-by: Lei Liu --- .../rclcpp/node_interfaces/node_logging.hpp | 15 +- .../node_logging_interface.hpp | 8 + rclcpp/include/rclcpp/node_options.hpp | 21 ++ rclcpp/src/rclcpp/node.cpp | 6 +- .../rclcpp/node_interfaces/node_logging.cpp | 56 ++++- rclcpp/src/rclcpp/node_options.cpp | 13 ++ rclcpp/test/rclcpp/CMakeLists.txt | 7 + rclcpp/test/rclcpp/test_logger_service.cpp | 214 ++++++++++++++++++ rclcpp/test/rclcpp/test_node_options.cpp | 5 + rclcpp_lifecycle/src/lifecycle_node.cpp | 2 +- 10 files changed, 342 insertions(+), 5 deletions(-) create mode 100644 rclcpp/test/rclcpp/test_logger_service.cpp diff --git a/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp b/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp index ea91064be7..30e7495368 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp @@ -23,6 +23,9 @@ #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/visibility_control.hpp" +#include "rcl_interfaces/srv/get_logger_levels.hpp" +#include "rcl_interfaces/srv/set_logger_levels.hpp" + namespace rclcpp { namespace node_interfaces @@ -35,7 +38,7 @@ class NodeLogging : public NodeLoggingInterface RCLCPP_SMART_PTR_ALIASES_ONLY(NodeLoggingInterface) RCLCPP_PUBLIC - explicit NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base); + explicit NodeLogging(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base); RCLCPP_PUBLIC virtual @@ -49,13 +52,21 @@ class NodeLogging : public NodeLoggingInterface const char * get_logger_name() const override; + RCLCPP_PUBLIC + void + create_logger_services( + node_interfaces::NodeServicesInterface::SharedPtr node_services) override; + private: RCLCPP_DISABLE_COPY(NodeLogging) /// Handle to the NodeBaseInterface given in the constructor. - rclcpp::node_interfaces::NodeBaseInterface * node_base_; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; rclcpp::Logger logger_; + + rclcpp::Service::SharedPtr get_loggers_service_; + rclcpp::Service::SharedPtr set_loggers_service_; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp index 870a81a53b..7f9776cf36 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp @@ -19,6 +19,7 @@ #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" #include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/visibility_control.hpp" @@ -54,6 +55,13 @@ class NodeLoggingInterface virtual const char * get_logger_name() const = 0; + + /// create logger services + RCLCPP_PUBLIC + virtual + void + create_logger_services( + node_interfaces::NodeServicesInterface::SharedPtr node_services) = 0; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_options.hpp b/rclcpp/include/rclcpp/node_options.hpp index 47a97fd3c4..a30ce4cf11 100644 --- a/rclcpp/include/rclcpp/node_options.hpp +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -50,6 +50,7 @@ class NodeOptions * - clock_type = RCL_ROS_TIME * - clock_qos = rclcpp::ClockQoS() * - use_clock_thread = true + * - enable_logger_service = false * - rosout_qos = rclcpp::RosoutQoS() * - parameter_event_qos = rclcpp::ParameterEventQoS * - with history setting and depth from rmw_qos_profile_parameter_events @@ -232,6 +233,24 @@ class NodeOptions NodeOptions & start_parameter_services(bool start_parameter_services); + /// Return the enable_logger_service flag. + RCLCPP_PUBLIC + bool + enable_logger_service() const; + + /// Set the enable_logger_service flag, return this for logger idiom. + /** + * If true, ROS services are created to allow external nodes to get + * and set logger levels of this node. + * + * If false, loggers will still be configured and set logger levels locally, + * but logger levels cannot be changed remotely . + * + */ + RCLCPP_PUBLIC + NodeOptions & + enable_logger_service(bool enable_log_service); + /// Return the start_parameter_event_publisher flag. RCLCPP_PUBLIC bool @@ -421,6 +440,8 @@ class NodeOptions bool use_clock_thread_ {true}; + bool enable_logger_service_ {false}; + rclcpp::QoS parameter_event_qos_ = rclcpp::ParameterEventsQoS( rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events) ); diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index c1fbdb1f98..53e77dd796 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -167,7 +167,7 @@ Node::Node( options.use_intra_process_comms(), options.enable_topic_statistics())), node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), - node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())), + node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_)), node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())), node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get(), node_timers_.get())), node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())), @@ -225,6 +225,10 @@ Node::Node( node_topics_->resolve_topic_name("/parameter_events"), options.parameter_event_qos(), rclcpp::detail::PublisherQosParametersTraits{}); + + if (options.enable_logger_service()) { + node_logging_->create_logger_services(node_services_); + } } Node::Node( diff --git a/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp b/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp index 3c2266d403..4f7476a09e 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "rclcpp/node_impl.hpp" #include "rclcpp/node_interfaces/node_logging.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" using rclcpp::node_interfaces::NodeLogging; -NodeLogging::NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base) +NodeLogging::NodeLogging(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base) : node_base_(node_base) { logger_ = rclcpp::get_logger(NodeLogging::get_logger_name()); @@ -37,3 +39,55 @@ NodeLogging::get_logger_name() const { return rcl_node_get_logger_name(node_base_->get_rcl_node_handle()); } + +void NodeLogging::create_logger_services( + node_interfaces::NodeServicesInterface::SharedPtr node_services) +{ + rclcpp::ServicesQoS qos_profile; + const std::string node_name = node_base_->get_name(); + auto callback_group = node_base_->get_default_callback_group(); + + get_loggers_service_ = rclcpp::create_service( + node_base_, node_services, + node_name + "/get_logger_levels", + []( + const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + for (auto & name : request->names) { + rcl_interfaces::msg::LoggerLevel logger_level; + logger_level.name = name; + auto ret = rcutils_logging_get_logger_level(name.c_str()); + if (ret < 0) { + logger_level.level = 0; + } else { + logger_level.level = static_cast(ret); + } + response->levels.push_back(std::move(logger_level)); + } + }, + qos_profile, callback_group); + + set_loggers_service_ = rclcpp::create_service( + node_base_, node_services, + node_name + "/set_logger_levels", + []( + const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + rcl_interfaces::msg::SetLoggerLevelsResult result; + for (auto & level : request->levels) { + auto ret = rcutils_logging_set_logger_level(level.name.c_str(), level.level); + if (ret != RCUTILS_RET_OK) { + result.successful = false; + result.reason = rcutils_get_error_string().str; + } else { + result.successful = true; + } + response->results.push_back(std::move(result)); + } + }, + qos_profile, callback_group); +} diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index b90a4b27e7..ada87ce5fe 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -248,6 +248,19 @@ NodeOptions::start_parameter_services(bool start_parameter_services) return *this; } +bool +NodeOptions::enable_logger_service() const +{ + return this->enable_logger_service_; +} + +NodeOptions & +NodeOptions::enable_logger_service(bool enable_logger_service) +{ + this->enable_logger_service_ = enable_logger_service; + return *this; +} + bool NodeOptions::start_parameter_event_publisher() const { diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 449250ed7a..dcb1d36d8e 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -649,6 +649,13 @@ if(TARGET test_wait_for_message) target_link_libraries(test_wait_for_message ${PROJECT_NAME}) endif() +ament_add_gtest(test_logger_service test_logger_service.cpp) +if(TARGET test_logger_service) + ament_target_dependencies(test_logger_service + "rcl_interfaces") + target_link_libraries(test_logger_service ${PROJECT_NAME}) +endif() + ament_add_gtest(test_interface_traits test_interface_traits.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_interface_traits) diff --git a/rclcpp/test/rclcpp/test_logger_service.cpp b/rclcpp/test/rclcpp/test_logger_service.cpp new file mode 100644 index 0000000000..92392f82aa --- /dev/null +++ b/rclcpp/test/rclcpp/test_logger_service.cpp @@ -0,0 +1,214 @@ +// Copyright 2023 Sony Group Corporation. +// +// 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. + +#include + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/node.hpp" +#include "rcl_interfaces/srv/get_logger_levels.hpp" +#include "rcl_interfaces/srv/set_logger_levels.hpp" + +using namespace std::chrono_literals; + +class TestLoggerService : public ::testing::Test +{ +protected: + void SetUp() + { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options = rclcpp::NodeOptions(); + options.enable_logger_service(true); + node_ = std::make_shared("test_logger_service", "/test", options); + } + + void TearDown() + { + rclcpp::shutdown(); + } + + rclcpp::Node::SharedPtr node_; + std::thread thread_; +}; + +TEST_F(TestLoggerService, check_connect_get_logger_service) { + auto client = node_->create_client( + "/test/test_logger_service/get_logger_levels"); + ASSERT_TRUE(client->wait_for_service(2s)); +} + +TEST_F(TestLoggerService, check_connect_set_logger_service) { + auto client = node_->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_TRUE(client->wait_for_service(2s)); +} + +TEST_F(TestLoggerService, test_set_and_get_one_logging_level) { + std::string test_logger_name = "rcl"; + uint8_t test_logger_level = 20; + { + auto client = node_->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_TRUE(client->wait_for_service(1s)); + auto request = std::make_shared(); + auto level = rcl_interfaces::msg::LoggerLevel(); + level.name = test_logger_name; + level.level = test_logger_level; + request->levels.push_back(level); + auto result = client->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result), + rclcpp::FutureReturnCode::SUCCESS); + auto result_get = result.get(); + ASSERT_EQ(result_get->results.size(), 1u); + ASSERT_TRUE(result_get->results[0].successful); + ASSERT_STREQ(result_get->results[0].reason.c_str(), ""); + } + + { + auto client = node_->create_client( + "/test/test_logger_service/get_logger_levels"); + ASSERT_TRUE(client->wait_for_service(1s)); + auto request = std::make_shared(); + request->names.emplace_back(test_logger_name); + auto result = client->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result), + rclcpp::FutureReturnCode::SUCCESS); + auto result_get = result.get(); + ASSERT_EQ(result_get->levels.size(), 1u); + ASSERT_STREQ(result_get->levels[0].name.c_str(), test_logger_name.c_str()); + ASSERT_EQ(result_get->levels[0].level, test_logger_level); + } +} + +TEST_F(TestLoggerService, test_set_and_get_multi_logging_level) { + std::vector> test_data { + {"rcl", 30}, + {"rclcpp", 40}, + {"/test/test_logger_service", 50} + }; + + // Set multi log levels + { + auto client = node_->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_TRUE(client->wait_for_service(1s)); + auto request = std::make_shared(); + for (auto & set_level : test_data) { + auto level = rcl_interfaces::msg::LoggerLevel(); + level.name = std::get<0>(set_level); + level.level = std::get<1>(set_level); + request->levels.push_back(level); + } + auto result = client->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result), + rclcpp::FutureReturnCode::SUCCESS); + auto result_get = result.get(); + ASSERT_EQ(result_get->results.size(), test_data.size()); + for (uint32_t i = 0; i < test_data.size(); i++) { + ASSERT_TRUE(result_get->results[0].successful); + } + } + + // Get multi log levels + { + auto client = node_->create_client( + "/test/test_logger_service/get_logger_levels"); + ASSERT_TRUE(client->wait_for_service(1s)); + auto request = std::make_shared(); + for (auto & set_level : test_data) { + request->names.emplace_back(std::get<0>(set_level)); + } + auto result = client->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result), + rclcpp::FutureReturnCode::SUCCESS); + auto result_get = result.get(); + ASSERT_EQ(result_get->levels.size(), test_data.size()); + for (uint32_t i = 0; i < test_data.size(); i++) { + ASSERT_STREQ(result_get->levels[i].name.c_str(), std::get<0>(test_data[i]).c_str()); + ASSERT_EQ(result_get->levels[i].level, std::get<1>(test_data[i])); + } + } +} + +TEST_F(TestLoggerService, test_set_logging_level_with_invalid_param) { + std::vector> test_data { + {"rcl", 12}, + {"/test/test_logger_service", 22} + }; + + // Set multi log levels + { + auto client = node_->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_TRUE(client->wait_for_service(1s)); + auto request = std::make_shared(); + for (auto & set_level : test_data) { + auto level = rcl_interfaces::msg::LoggerLevel(); + level.name = std::get<0>(set_level); + level.level = std::get<1>(set_level); + request->levels.push_back(level); + } + auto result = client->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result), + rclcpp::FutureReturnCode::SUCCESS); + auto result_get = result.get(); + ASSERT_EQ(result_get->results.size(), test_data.size()); + for (uint32_t i = 0; i < test_data.size(); i++) { + ASSERT_FALSE(result_get->results[i].successful); + // Check string starts with prefix + ASSERT_EQ( + result_get->results[i].reason.rfind("Unable to determine severity_string for severity", 0), + 0); + } + } +} + +TEST_F(TestLoggerService, test_set_logging_level_with_partial_invalid_param) { + std::vector> test_data { + {"rcl", 20}, + {"rclcpp", 22}, + {"/test/test_logger_service", 30} + }; + + // Set multi log levels + { + auto client = node_->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_TRUE(client->wait_for_service(1s)); + auto request = std::make_shared(); + for (auto & set_level : test_data) { + auto level = rcl_interfaces::msg::LoggerLevel(); + level.name = std::get<0>(set_level); + level.level = std::get<1>(set_level); + request->levels.push_back(level); + } + auto result = client->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result), + rclcpp::FutureReturnCode::SUCCESS); + auto result_get = result.get(); + ASSERT_EQ(result_get->results.size(), test_data.size()); + ASSERT_TRUE(result_get->results[0].successful); + ASSERT_FALSE(result_get->results[1].successful); + ASSERT_TRUE(result_get->results[2].successful); + } +} diff --git a/rclcpp/test/rclcpp/test_node_options.cpp b/rclcpp/test/rclcpp/test_node_options.cpp index 2468923229..49af93aa8a 100644 --- a/rclcpp/test/rclcpp/test_node_options.cpp +++ b/rclcpp/test/rclcpp/test_node_options.cpp @@ -266,6 +266,11 @@ TEST(TestNodeOptions, bool_setters_and_getters) { EXPECT_FALSE(options.automatically_declare_parameters_from_overrides()); options.automatically_declare_parameters_from_overrides(true); EXPECT_TRUE(options.automatically_declare_parameters_from_overrides()); + + options.enable_logger_service(false); + EXPECT_FALSE(options.enable_logger_service()); + options.enable_logger_service(true); + EXPECT_TRUE(options.enable_logger_service()); } TEST(TestNodeOptions, parameter_event_qos) { diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index a57a95e6be..4c0b94cb42 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -76,7 +76,7 @@ LifecycleNode::LifecycleNode( options.use_intra_process_comms(), options.enable_topic_statistics())), node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), - node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())), + node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_)), node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())), node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get(), node_timers_.get())), node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())), From 9d2849cb0ada510073dc90d7497aa2380a97b51a Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 18 Apr 2023 15:35:41 +0000 Subject: [PATCH 221/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 9 +++++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 6 ++++++ 4 files changed, 21 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index cb6be1fd3f..c875005ef5 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add support for logging service. (`#2122 `_) +* Picking ABI-incompatible executor changes (`#2170 `_) +* add events-executor and timers-manager in rclcpp (`#2155 `_) +* Create common structures for executors to use (`#2143 `_) +* Implement deliver message kind (`#2168 `_) +* Contributors: Alberto Soragna, Lei Liu, Michael Carroll, methylDragon + 20.0.0 (2023-04-13) ------------------- * applied tracepoints for ring_buffer (`#2091 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index f4e1e540da..cb7008445c 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 20.0.0 (2023-04-13) ------------------- * extract the result response before the callback is issued. (`#2132 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index aaa389eef0..57e53cdb92 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 20.0.0 (2023-04-13) ------------------- * Update all rclcpp packages to C++17. (`#2121 `_) diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index e5a66b031c..21266a206a 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,12 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add support for logging service. (`#2122 `_) +* Support publishing loaned messages in LifecyclePublisher (`#2159 `_) +* Contributors: Lei Liu, Michael Babenko + 20.0.0 (2023-04-13) ------------------- * Fixes to silence some clang warnings. (`#2127 `_) From a431256383743551f8d58565bfb2dc31eb18d8e8 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 18 Apr 2023 15:35:47 +0000 Subject: [PATCH 222/455] 21.0.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index c875005ef5..f1316f5173 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +21.0.0 (2023-04-18) +------------------- * Add support for logging service. (`#2122 `_) * Picking ABI-incompatible executor changes (`#2170 `_) * add events-executor and timers-manager in rclcpp (`#2155 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index f940e43472..433775a6bc 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 20.0.0 + 21.0.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index cb7008445c..7cd3078e46 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +21.0.0 (2023-04-18) +------------------- 20.0.0 (2023-04-13) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 35d22fed39..2c133d11d9 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 20.0.0 + 21.0.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 57e53cdb92..f4ce7e5fa5 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +21.0.0 (2023-04-18) +------------------- 20.0.0 (2023-04-13) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 8308ce7630..75571b6c9d 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 20.0.0 + 21.0.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 21266a206a..7bb01e2b48 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +21.0.0 (2023-04-18) +------------------- * Add support for logging service. (`#2122 `_) * Support publishing loaned messages in LifecyclePublisher (`#2159 `_) * Contributors: Lei Liu, Michael Babenko diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 1df53fdfdd..26dba11d29 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 20.0.0 + 21.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 77ede02251733c605280fdcf51516c201b0bc471 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 27 Apr 2023 16:53:54 +0800 Subject: [PATCH 223/455] 21.1.0 --- rclcpp/CHANGELOG.rst | 3 +++ rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 16 insertions(+), 4 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index f1316f5173..32830939c8 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +21.1.0 (2023-04-27) +------------------- + 21.0.0 (2023-04-18) ------------------- * Add support for logging service. (`#2122 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 433775a6bc..1823f4d463 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 21.0.0 + 21.1.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 7cd3078e46..26f00ce559 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +21.1.0 (2023-04-27) +------------------- + 21.0.0 (2023-04-18) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 2c133d11d9..bfda7c1343 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 21.0.0 + 21.1.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index f4ce7e5fa5..144faaf2a5 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +21.1.0 (2023-04-27) +------------------- + 21.0.0 (2023-04-18) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 75571b6c9d..5a63923544 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 21.0.0 + 21.1.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 7bb01e2b48..9ab0142005 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +21.1.0 (2023-04-27) +------------------- + 21.0.0 (2023-04-18) ------------------- * Add support for logging service. (`#2122 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 26dba11d29..ac710d1431 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 21.0.0 + 21.1.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 6ca1023ef77f19897d74b5864300125d92ac47b3 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 1 May 2023 13:56:29 -0700 Subject: [PATCH 224/455] Fix delivered message kind (#2175) Signed-off-by: methylDragon --- rclcpp/include/rclcpp/subscription_base.hpp | 6 +++--- rclcpp/src/rclcpp/executor.cpp | 2 +- rclcpp/src/rclcpp/subscription_base.cpp | 8 ++++---- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index f4957824a5..b122e96575 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -260,13 +260,13 @@ class SubscriptionBase : public std::enable_shared_from_this bool is_serialized() const; - /// Return the type of the subscription. + /// Return the delivered message kind. /** * \return `DeliveredMessageKind`, which adjusts how messages are received and delivered. */ RCLCPP_PUBLIC DeliveredMessageKind - get_subscription_type() const; + get_delivered_message_kind() const; /// Get matching publisher count. /** \return The number of publishers on this topic. */ @@ -663,7 +663,7 @@ class SubscriptionBase : public std::enable_shared_from_this RCLCPP_DISABLE_COPY(SubscriptionBase) rosidl_message_type_support_t type_support_; - DeliveredMessageKind delivered_message_type_; + DeliveredMessageKind delivered_message_kind_; std::atomic subscription_in_use_by_wait_set_{false}; std::atomic intra_process_subscription_waitable_in_use_by_wait_set_{false}; diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index a7a2b8278b..50529a102a 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -603,7 +603,7 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) rclcpp::MessageInfo message_info; message_info.get_rmw_message_info().from_intra_process = false; - switch (subscription->get_subscription_type()) { + switch (subscription->get_delivered_message_kind()) { // Deliver ROS message case rclcpp::DeliveredMessageKind::ROS_MESSAGE: { diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 18ccab0eb0..6db3bf5ceb 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -52,7 +52,7 @@ SubscriptionBase::SubscriptionBase( intra_process_subscription_id_(0), event_callbacks_(event_callbacks), type_support_(type_support_handle), - delivered_message_type_(delivered_message_kind) + delivered_message_kind_(delivered_message_kind) { auto custom_deletor = [node_handle = this->node_handle_](rcl_subscription_t * rcl_subs) { @@ -261,13 +261,13 @@ SubscriptionBase::get_message_type_support_handle() const bool SubscriptionBase::is_serialized() const { - return delivered_message_type_ == rclcpp::DeliveredMessageKind::SERIALIZED_MESSAGE; + return delivered_message_kind_ == rclcpp::DeliveredMessageKind::SERIALIZED_MESSAGE; } rclcpp::DeliveredMessageKind -SubscriptionBase::get_subscription_type() const +SubscriptionBase::get_delivered_message_kind() const { - return delivered_message_type_; + return delivered_message_kind_; } size_t From b812790ee301d9aa536c79b9636736471701d1c3 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 1 May 2023 17:06:55 -0400 Subject: [PATCH 225/455] Fix a format-security warning when building with clang (#2171) In particular, you should never have a "bare" string in a printf-like call; that could potentially access uninitialized memory. Instead, make sure to format the string with %s. Signed-off-by: Chris Lalancette --- .../test/rclcpp/test_rosout_subscription.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/rclcpp/test/rclcpp/test_rosout_subscription.cpp b/rclcpp/test/rclcpp/test_rosout_subscription.cpp index ea761fdc0c..c5f00bb26d 100644 --- a/rclcpp/test/rclcpp/test_rosout_subscription.cpp +++ b/rclcpp/test/rclcpp/test_rosout_subscription.cpp @@ -70,7 +70,7 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild) { // before calling get_child of Logger { RCLCPP_INFO( - rclcpp::get_logger(logger_name), this->rosout_msg_data.c_str()); + rclcpp::get_logger(logger_name), "%s", this->rosout_msg_data.c_str()); auto future = received_msg_promise.get_future(); auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); ASSERT_EQ(rclcpp::FutureReturnCode::TIMEOUT, return_code); @@ -83,7 +83,7 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild) { // after calling get_child of Logger // 1. use child_logger directly { - RCLCPP_INFO(child_logger, this->rosout_msg_data.c_str()); + RCLCPP_INFO(child_logger, "%s", this->rosout_msg_data.c_str()); auto future = received_msg_promise.get_future(); auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); @@ -93,7 +93,7 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild) { // 2. use rclcpp::get_logger { - RCLCPP_INFO(rclcpp::get_logger(logger_name), this->rosout_msg_data.c_str()); + RCLCPP_INFO(rclcpp::get_logger(logger_name), "%s", this->rosout_msg_data.c_str()); auto future = received_msg_promise.get_future(); auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); @@ -104,7 +104,7 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild) { // `child_logger` is end of life, there is no sublogger { - RCLCPP_INFO(rclcpp::get_logger(logger_name), this->rosout_msg_data.c_str()); + RCLCPP_INFO(rclcpp::get_logger(logger_name), "%s", this->rosout_msg_data.c_str()); auto future = received_msg_promise.get_future(); auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); ASSERT_EQ(rclcpp::FutureReturnCode::TIMEOUT, return_code); @@ -119,7 +119,7 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_parent_log) { rclcpp::Logger logger = this->node->get_logger(); ASSERT_EQ(logger.get_name(), logger_name); - RCLCPP_INFO(logger, this->rosout_msg_data.c_str()); + RCLCPP_INFO(logger, "%s", this->rosout_msg_data.c_str()); auto future = received_msg_promise.get_future(); auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); @@ -133,14 +133,14 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_child_log) { this->rosout_msg_name = logger_name; rclcpp::Logger logger = this->node->get_logger(); - RCLCPP_INFO(logger, this->rosout_msg_data.c_str()); + RCLCPP_INFO(logger, "%s", this->rosout_msg_data.c_str()); auto future = received_msg_promise.get_future(); auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); ASSERT_EQ(rclcpp::FutureReturnCode::TIMEOUT, return_code); received_msg_promise = {}; logger = this->node->get_logger().get_child("child1"); - RCLCPP_INFO(logger, this->rosout_msg_data.c_str()); + RCLCPP_INFO(logger, "%s", this->rosout_msg_data.c_str()); future = received_msg_promise.get_future(); return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); @@ -148,14 +148,14 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_child_log) { received_msg_promise = {}; logger = this->node->get_logger().get_child("child2"); - RCLCPP_INFO(logger, this->rosout_msg_data.c_str()); + RCLCPP_INFO(logger, "%s", this->rosout_msg_data.c_str()); future = received_msg_promise.get_future(); return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); ASSERT_EQ(rclcpp::FutureReturnCode::TIMEOUT, return_code); received_msg_promise = {}; this->rosout_msg_name = "ns.test_rosout_subscription.child2"; - RCLCPP_INFO(logger, this->rosout_msg_data.c_str()); + RCLCPP_INFO(logger, "%s", this->rosout_msg_data.c_str()); future = received_msg_promise.get_future(); return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); @@ -171,7 +171,7 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild_hierarchy) { rclcpp::Logger grandchild_logger = this->node->get_logger().get_child("child").get_child("grandchild"); ASSERT_EQ(grandchild_logger.get_name(), logger_name); - RCLCPP_INFO(grandchild_logger, this->rosout_msg_data.c_str()); + RCLCPP_INFO(grandchild_logger, "%s", this->rosout_msg_data.c_str()); auto future = received_msg_promise.get_future(); auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); From 86c77143c96d85711a87f2a5adcc4d7f0fb0dbeb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=98ystein=20Sture?= Date: Thu, 4 May 2023 21:48:20 +0200 Subject: [PATCH 226/455] Add missing stdexcept include (#2186) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Øystein Sture --- rclcpp/include/rclcpp/context.hpp | 1 + rclcpp/src/rclcpp/logging_mutex.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 45c70b9af2..4417f4d675 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #include "rcl/context.h" #include "rcl/guard_condition.h" diff --git a/rclcpp/src/rclcpp/logging_mutex.cpp b/rclcpp/src/rclcpp/logging_mutex.cpp index 308a21fe73..bbbe9bbeed 100644 --- a/rclcpp/src/rclcpp/logging_mutex.cpp +++ b/rclcpp/src/rclcpp/logging_mutex.cpp @@ -14,6 +14,7 @@ #include #include +#include #include "rcutils/macros.h" From 6e1fea14e1fdab856f31bba6c4030c321afa86f7 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Fri, 5 May 2023 15:04:35 +0100 Subject: [PATCH 227/455] Fix race condition in events-executor (#2177) The initial implementation of the events-executor contained a bug where the executor would end up in an inconsistent state and stop processing interrupt/shutdown notifications. Manually adding a node to the executor results in a) producing a notify waitable event and b) refreshing the executor collections. The inconsistent state would happen if the event was processed before the collections were finished to be refreshed: the executor would pick up the event but be unable to process it. This would leave the `notify_waitable_event_pushed_` flag to true, preventing additional notify waitable events to be pushed. The behavior is observable only under heavy load. Signed-off-by: Alberto Soragna --- .../events_executor/events_executor.hpp | 5 ++ .../events_executor/events_executor.cpp | 37 ++++---- .../test/rclcpp/executors/test_executors.cpp | 85 +++++++++++++++++-- 3 files changed, 103 insertions(+), 24 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp index 148ede66ea..2365b67697 100644 --- a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp +++ b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp @@ -243,6 +243,11 @@ class EventsExecutor : public rclcpp::Executor std::function create_waitable_callback(const rclcpp::Waitable * waitable_id); + /// Utility to add the notify waitable to an entities collection + void + add_notify_waitable_to_collection( + rclcpp::executors::ExecutorEntitiesCollection::WaitableCollection & collection); + /// Searches for the provided entity_id in the collection and returns the entity if valid template typename CollectionType::EntitySharedPtr 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 094ff21282..33e9cb67bd 100644 --- a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp +++ b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp @@ -50,6 +50,9 @@ EventsExecutor::EventsExecutor( timers_manager_ = std::make_shared(context_, timer_on_ready_cb); + this->current_entities_collection_ = + std::make_shared(); + notify_waitable_ = std::make_shared( [this]() { // This callback is invoked when: @@ -61,6 +64,10 @@ EventsExecutor::EventsExecutor( this->refresh_current_collection_from_callback_groups(); }); + // Make sure that the notify waitable is immediately added to the collection + // to avoid missing events + this->add_notify_waitable_to_collection(current_entities_collection_->waitables); + notify_waitable_->add_guard_condition(interrupt_guard_condition_); notify_waitable_->add_guard_condition(shutdown_guard_condition_); @@ -87,9 +94,6 @@ EventsExecutor::EventsExecutor( this->entities_collector_ = std::make_shared(notify_waitable_); - - this->current_entities_collection_ = - std::make_shared(); } EventsExecutor::~EventsExecutor() @@ -395,18 +399,8 @@ EventsExecutor::refresh_current_collection_from_callback_groups() // retrieved in the "standard" way. // To do it, we need to add the notify waitable as an entry in both the new and // current collections such that it's neither added or removed. - rclcpp::CallbackGroup::WeakPtr weak_group_ptr; - new_collection.waitables.insert( - { - this->notify_waitable_.get(), - {this->notify_waitable_, weak_group_ptr} - }); - - this->current_entities_collection_->waitables.insert( - { - this->notify_waitable_.get(), - {this->notify_waitable_, weak_group_ptr} - }); + this->add_notify_waitable_to_collection(new_collection.waitables); + this->add_notify_waitable_to_collection(current_entities_collection_->waitables); this->refresh_current_collection(new_collection); } @@ -486,3 +480,16 @@ EventsExecutor::create_waitable_callback(const rclcpp::Waitable * entity_key) }; return callback; } + +void +EventsExecutor::add_notify_waitable_to_collection( + rclcpp::executors::ExecutorEntitiesCollection::WaitableCollection & collection) +{ + // The notify waitable is not associated to any group, so use an invalid one + rclcpp::CallbackGroup::WeakPtr weak_group_ptr; + collection.insert( + { + this->notify_waitable_.get(), + {this->notify_waitable_, weak_group_ptr} + }); +} diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 38b6ddf870..232baaace3 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -20,12 +20,14 @@ #include #include +#include #include #include #include #include #include #include +#include #include "rcl/error_handling.h" #include "rcl/time.h" @@ -43,18 +45,10 @@ template class TestExecutors : public ::testing::Test { public: - static void SetUpTestCase() + void SetUp() { rclcpp::init(0, nullptr); - } - - static void TearDownTestCase() - { - rclcpp::shutdown(); - } - void SetUp() - { const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); std::stringstream test_name; test_name << test_info->test_case_name() << "_" << test_info->name(); @@ -75,6 +69,8 @@ class TestExecutors : public ::testing::Test publisher.reset(); subscription.reset(); node.reset(); + + rclcpp::shutdown(); } rclcpp::Node::SharedPtr node; @@ -729,6 +725,77 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) spinner.join(); } +// This test verifies that the add_node operation is robust wrt race conditions. +// It's mostly meant to prevent regressions in the events-executor, but the operation should be +// thread-safe in all executor implementations. +// The initial implementation of the events-executor contained a bug where the executor +// would end up in an inconsistent state and stop processing interrupt/shutdown notifications. +// Manually adding a node to the executor results in a) producing a notify waitable event +// and b) refreshing the executor collections. +// The inconsistent state would happen if the event was processed before the collections were +// finished to be refreshed: the executor would pick up the event but be unable to process it. +// This would leave the `notify_waitable_event_pushed_` flag to true, preventing additional +// notify waitable events to be pushed. +// The behavior is observable only under heavy load, so this test spawns several worker +// threads. Due to the nature of the bug, this test may still succeed even if the +// bug is present. However repeated runs will show its flakiness nature and indicate +// an eventual regression. +TYPED_TEST(TestExecutors, testRaceConditionAddNode) +{ + using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + + // Spawn some threads to do some heavy work + std::atomic should_cancel = false; + std::vector stress_threads; + for (size_t i = 0; i < 5 * std::thread::hardware_concurrency(); i++) { + stress_threads.emplace_back( + [&should_cancel, i]() { + // This is just some arbitrary heavy work + volatile size_t total = 0; + for (size_t k = 0; k < 549528914167; k++) { + if (should_cancel) { + break; + } + total += k * (i + 42); + } + }); + } + + // Create an executor + auto executor = std::make_shared(); + // Start spinning + auto executor_thread = std::thread( + [executor]() { + executor->spin(); + }); + // Add a node to the executor + executor->add_node(this->node); + + // Cancel the executor (make sure that it's already spinning first) + while (!executor->is_spinning() && rclcpp::ok()) { + continue; + } + executor->cancel(); + + // Try to join the thread after cancelling the executor + // This is the "test". We want to make sure that we can still cancel the executor + // regardless of the presence of race conditions + executor_thread.join(); + + // The test is now completed: we can join the stress threads + should_cancel = true; + for (auto & t : stress_threads) { + t.join(); + } +} + // Check spin_until_future_complete with node base pointer (instantiates its own executor) TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) { From 26426adda9ad605ab1b0281dd6b4877367f92afd Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 11 May 2023 18:58:48 +0800 Subject: [PATCH 228/455] Changelog Signed-off-by: Yadunund --- rclcpp/CHANGELOG.rst | 8 ++++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 3 +++ 4 files changed, 17 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 32830939c8..fd9a518bd4 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix race condition in events-executor (`#2177 `_) +* Add missing stdexcept include (`#2186 `_) +* Fix a format-security warning when building with clang (`#2171 `_) +* Fix delivered message kind (`#2175 `_) +* Contributors: Alberto Soragna, Chris Lalancette, methylDragon, Øystein Sture + 21.1.0 (2023-04-27) ------------------- diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 26f00ce559..31a4140264 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 21.1.0 (2023-04-27) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 144faaf2a5..a77c066342 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 21.1.0 (2023-04-27) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 9ab0142005..3fd108d08f 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 21.1.0 (2023-04-27) ------------------- From 58bcd3b8228413aff34a35210f4549ddb06a8d10 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 11 May 2023 18:59:17 +0800 Subject: [PATCH 229/455] 21.1.1 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index fd9a518bd4..4930b9d41d 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +21.1.1 (2023-05-11) +------------------- * Fix race condition in events-executor (`#2177 `_) * Add missing stdexcept include (`#2186 `_) * Fix a format-security warning when building with clang (`#2171 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 1823f4d463..bdd5dd5b22 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 21.1.0 + 21.1.1 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 31a4140264..2338c2f9b7 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +21.1.1 (2023-05-11) +------------------- 21.1.0 (2023-04-27) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index bfda7c1343..7800a95571 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 21.1.0 + 21.1.1 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index a77c066342..9b7487879f 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +21.1.1 (2023-05-11) +------------------- 21.1.0 (2023-04-27) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 5a63923544..5d852ddd0e 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 21.1.0 + 21.1.1 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 3fd108d08f..57380c9362 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +21.1.1 (2023-05-11) +------------------- 21.1.0 (2023-04-27) ------------------- diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index ac710d1431..c73990e112 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 21.1.0 + 21.1.1 Package containing a prototype for lifecycle implementation Ivan Paunovic From d7fdb6184c7f5c2f3a1e7c2a0ddcfd8ba5044452 Mon Sep 17 00:00:00 2001 From: mauropasse Date: Mon, 15 May 2023 20:55:08 +0100 Subject: [PATCH 230/455] Declare rclcpp callbacks before the rcl entities (#2024) This is to ensure callbacks are destroyed last on entities destruction, avoiding the gap in time in which rmw entities hold a reference to a destroyed function. Signed-off-by: Mauro Passerino Co-authored-by: Mauro Passerino --- rclcpp/include/rclcpp/client.hpp | 10 +++++++--- rclcpp/include/rclcpp/service.hpp | 10 +++++++--- rclcpp/include/rclcpp/subscription_base.hpp | 11 ++++++++--- 3 files changed, 22 insertions(+), 9 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index f3346a067c..92f2f4af8d 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -363,12 +363,16 @@ class ClientBase std::shared_ptr context_; rclcpp::Logger node_logger_; + std::recursive_mutex callback_mutex_; + // It is important to declare on_new_response_callback_ before + // client_handle_, so on destruction the client is + // destroyed first. Otherwise, the rmw client callback + // would point briefly to a destroyed function. + std::function on_new_response_callback_{nullptr}; + // Declare client_handle_ after callback std::shared_ptr client_handle_; std::atomic in_use_by_wait_set_{false}; - - std::recursive_mutex callback_mutex_; - std::function on_new_response_callback_{nullptr}; }; template diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 3cfc11e9ca..d73cb76444 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -265,15 +265,19 @@ class ServiceBase std::shared_ptr node_handle_; + std::recursive_mutex callback_mutex_; + // It is important to declare on_new_request_callback_ before + // service_handle_, so on destruction the service is + // destroyed first. Otherwise, the rmw service callback + // would point briefly to a destroyed function. + std::function on_new_request_callback_{nullptr}; + // Declare service_handle_ after callback std::shared_ptr service_handle_; bool owns_rcl_handle_ = true; rclcpp::Logger node_logger_; std::atomic in_use_by_wait_set_{false}; - - std::recursive_mutex callback_mutex_; - 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 b122e96575..615f3852b6 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -645,6 +645,14 @@ class SubscriptionBase : public std::enable_shared_from_this rclcpp::node_interfaces::NodeBaseInterface * const node_base_; std::shared_ptr node_handle_; + + std::recursive_mutex callback_mutex_; + // It is important to declare on_new_message_callback_ before + // subscription_handle_, so on destruction the subscription is + // destroyed first. Otherwise, the rmw subscription callback + // would point briefly to a destroyed function. + std::function on_new_message_callback_{nullptr}; + // Declare subscription_handle_ after callback std::shared_ptr subscription_handle_; std::shared_ptr intra_process_subscription_handle_; rclcpp::Logger node_logger_; @@ -669,9 +677,6 @@ 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::recursive_mutex callback_mutex_; - std::function on_new_message_callback_{nullptr}; }; } // namespace rclcpp From c4f57a79984acfacec8759d60ba968138845c187 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Thu, 18 May 2023 14:43:26 +0100 Subject: [PATCH 231/455] add mutex to protect events_executor current entity collection (#2187) * add mutex to protect events_executor current entity collection and unit-test Signed-off-by: Alberto Soragna * be more precise with mutex locks; make stress test less stressfull Signed-off-by: Alberto Soragna * fix uncrustify error Signed-off-by: Alberto Soragna --------- Signed-off-by: Alberto Soragna --- .../events_executor/events_executor.hpp | 5 +- .../events_executor/events_executor.cpp | 49 ++++++++++++----- .../test/rclcpp/executors/test_executors.cpp | 54 +++++++++++++++++++ 3 files changed, 93 insertions(+), 15 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp index 2365b67697..dd5b1ebe63 100644 --- a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp +++ b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp @@ -274,9 +274,12 @@ class EventsExecutor : public rclcpp::Executor rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue_; std::shared_ptr entities_collector_; - std::shared_ptr current_entities_collection_; std::shared_ptr notify_waitable_; + /// Mutex to protect the current_entities_collection_ + std::recursive_mutex collection_mutex_; + std::shared_ptr current_entities_collection_; + /// Flag used to reduce the number of unnecessary waitable events std::atomic notify_waitable_event_pushed_ {false}; 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 33e9cb67bd..64b07c0814 100644 --- a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp +++ b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp @@ -273,10 +273,13 @@ EventsExecutor::execute_event(const ExecutorEvent & event) switch (event.type) { case ExecutorEventType::CLIENT_EVENT: { - auto client = this->retrieve_entity( - static_cast(event.entity_key), - current_entities_collection_->clients); - + rclcpp::ClientBase::SharedPtr client; + { + std::lock_guard lock(collection_mutex_); + client = this->retrieve_entity( + static_cast(event.entity_key), + current_entities_collection_->clients); + } if (client) { for (size_t i = 0; i < event.num_events; i++) { execute_client(client); @@ -287,9 +290,13 @@ EventsExecutor::execute_event(const ExecutorEvent & event) } case ExecutorEventType::SUBSCRIPTION_EVENT: { - auto subscription = this->retrieve_entity( - static_cast(event.entity_key), - current_entities_collection_->subscriptions); + rclcpp::SubscriptionBase::SharedPtr subscription; + { + std::lock_guard lock(collection_mutex_); + subscription = this->retrieve_entity( + static_cast(event.entity_key), + current_entities_collection_->subscriptions); + } if (subscription) { for (size_t i = 0; i < event.num_events; i++) { execute_subscription(subscription); @@ -299,10 +306,13 @@ EventsExecutor::execute_event(const ExecutorEvent & event) } case ExecutorEventType::SERVICE_EVENT: { - auto service = this->retrieve_entity( - static_cast(event.entity_key), - current_entities_collection_->services); - + rclcpp::ServiceBase::SharedPtr service; + { + std::lock_guard lock(collection_mutex_); + service = this->retrieve_entity( + static_cast(event.entity_key), + current_entities_collection_->services); + } if (service) { for (size_t i = 0; i < event.num_events; i++) { execute_service(service); @@ -319,9 +329,13 @@ EventsExecutor::execute_event(const ExecutorEvent & event) } case ExecutorEventType::WAITABLE_EVENT: { - auto waitable = this->retrieve_entity( - static_cast(event.entity_key), - current_entities_collection_->waitables); + rclcpp::Waitable::SharedPtr waitable; + { + std::lock_guard lock(collection_mutex_); + waitable = this->retrieve_entity( + static_cast(event.entity_key), + current_entities_collection_->waitables); + } if (waitable) { for (size_t i = 0; i < event.num_events; i++) { auto data = waitable->take_data_by_entity_id(event.waitable_data); @@ -386,6 +400,7 @@ EventsExecutor::get_automatically_added_callback_groups_from_nodes() void EventsExecutor::refresh_current_collection_from_callback_groups() { + // Build the new collection this->entities_collector_->update_collections(); auto callback_groups = this->entities_collector_->get_all_callback_groups(); rclcpp::executors::ExecutorEntitiesCollection new_collection; @@ -400,6 +415,9 @@ EventsExecutor::refresh_current_collection_from_callback_groups() // To do it, we need to add the notify waitable as an entry in both the new and // current collections such that it's neither added or removed. this->add_notify_waitable_to_collection(new_collection.waitables); + + // Acquire lock before modifying the current collection + std::lock_guard lock(collection_mutex_); this->add_notify_waitable_to_collection(current_entities_collection_->waitables); this->refresh_current_collection(new_collection); @@ -409,6 +427,9 @@ void EventsExecutor::refresh_current_collection( const rclcpp::executors::ExecutorEntitiesCollection & new_collection) { + // Acquire lock before modifying the current collection + std::lock_guard lock(collection_mutex_); + current_entities_collection_->timers.update( new_collection.timers, [this](rclcpp::TimerBase::SharedPtr timer) {timers_manager_->add_timer(timer);}, diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 232baaace3..e9aad61685 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -796,6 +796,60 @@ TYPED_TEST(TestExecutors, testRaceConditionAddNode) } } +// This test verifies the thread-safety of adding and removing a node +// while the executor is spinning and events are ready. +// This test does not contain expectations, but rather it verifies that +// we can run a "stressful routine" without crashing. +TYPED_TEST(TestExecutors, stressAddRemoveNode) +{ + using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + + ExecutorType executor; + + // A timer that is "always" ready (the timer callback doesn't do anything) + auto timer = this->node->create_wall_timer(std::chrono::nanoseconds(1), []() {}); + + // This thread spins the executor until it's cancelled + std::thread spinner_thread([&]() { + executor.spin(); + }); + + // This thread publishes data in a busy loop (the node has a subscription) + std::thread publisher_thread1([&]() { + for (size_t i = 0; i < 100000; i++) { + this->publisher->publish(test_msgs::msg::Empty()); + } + }); + std::thread publisher_thread2([&]() { + for (size_t i = 0; i < 100000; i++) { + this->publisher->publish(test_msgs::msg::Empty()); + } + }); + + // This thread adds/remove the node that contains the entities in a busy loop + std::thread add_remove_thread([&]() { + for (size_t i = 0; i < 100000; i++) { + executor.add_node(this->node); + executor.remove_node(this->node); + } + }); + + // Wait for the threads that do real work to finish + publisher_thread1.join(); + publisher_thread2.join(); + add_remove_thread.join(); + + executor.cancel(); + spinner_thread.join(); +} + // Check spin_until_future_complete with node base pointer (instantiates its own executor) TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) { From fce021b14967f691340a6e90b15bdaa37a2008bd Mon Sep 17 00:00:00 2001 From: DensoADAS <46967124+DensoADAS@users.noreply.github.com> Date: Fri, 19 May 2023 20:31:59 +0200 Subject: [PATCH 232/455] Feature/available capacity of ipm (#2173) * added available_capacity to get the lowest number of free capacity for intra-process communication for a publisher Signed-off-by: Joshua Hampp * added unit tests for available_capacity Signed-off-by: Joshua Hampp Signed-off-by: Joshua Hampp * fixed typos in comments Signed-off-by: Joshua Hampp * Updated warning Co-authored-by: Alberto Soragna Signed-off-by: Joshua Hampp * returning 0 if ipm is disabled in lowest_available_ipm_capacity Signed-off-by: Joshua Hampp * return 0 if no subscribers are present in lowest_available_capacity Signed-off-by: Joshua Hampp * updated unit test Signed-off-by: Joshua Hampp * update unit test Signed-off-by: Joshua Hampp * moved available_capacity to a lambda function to be able to handle subscriptions which went out of scope Signed-off-by: Joshua Hampp * updated unit test to check subscriptions which went out of scope Signed-off-by: Joshua Hampp --------- Signed-off-by: Joshua Hampp Signed-off-by: Joshua Hampp Co-authored-by: Joshua Hampp Co-authored-by: Joshua Hampp Co-authored-by: Alberto Soragna --- .../buffers/buffer_implementation_base.hpp | 1 + .../buffers/intra_process_buffer.hpp | 6 + .../buffers/ring_buffer_implementation.hpp | 23 ++++ .../experimental/intra_process_manager.hpp | 5 + .../subscription_intra_process_base.hpp | 5 + .../subscription_intra_process_buffer.hpp | 5 + rclcpp/include/rclcpp/publisher_base.hpp | 11 ++ rclcpp/src/rclcpp/intra_process_manager.cpp | 47 ++++++++ rclcpp/src/rclcpp/publisher_base.cpp | 19 +++ .../test/rclcpp/test_intra_process_buffer.cpp | 72 ++++++++++++ .../rclcpp/test_intra_process_manager.cpp | 108 ++++++++++++++++++ rclcpp/test/rclcpp/test_publisher.cpp | 35 ++++++ 12 files changed, 337 insertions(+) diff --git a/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp b/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp index 1e5346116a..83a6c7462c 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp @@ -33,6 +33,7 @@ class BufferImplementationBase virtual void clear() = 0; virtual bool has_data() const = 0; + virtual size_t available_capacity() const = 0; }; } // namespace buffers diff --git a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp index 05092bb23b..af936cb342 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp @@ -44,6 +44,7 @@ class IntraProcessBufferBase virtual bool has_data() const = 0; virtual bool use_take_shared_method() const = 0; + virtual size_t available_capacity() const = 0; }; template< @@ -143,6 +144,11 @@ class TypedIntraProcessBuffer : public IntraProcessBuffer::value; } + size_t available_capacity() const override + { + return buffer_->available_capacity(); + } + private: std::unique_ptr> buffer_; diff --git a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp index 2c06ea6cbe..5c6ba59291 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp @@ -148,6 +148,18 @@ class RingBufferImplementation : public BufferImplementationBase return is_full_(); } + /// Get the remaining capacity to store messages + /** + * This member function is thread-safe. + * + * \return the number of free capacity for new messages + */ + size_t available_capacity() const + { + std::lock_guard lock(mutex_); + return available_capacity_(); + } + void clear() { TRACEPOINT(rclcpp_ring_buffer_clear, static_cast(this)); @@ -189,6 +201,17 @@ class RingBufferImplementation : public BufferImplementationBase return size_ == capacity_; } + /// Get the remaining capacity to store messages + /** + * This member function is not thread-safe. + * + * \return the number of free capacity for new messages + */ + inline size_t available_capacity_() const + { + return capacity_ - size_; + } + size_t capacity_; std::vector ring_buffer_; diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 11f2dda6a4..a152632a53 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -306,6 +306,11 @@ class IntraProcessManager rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr get_subscription_intra_process(uint64_t intra_process_subscription_id); + /// Return the lowest available capacity for all subscription buffers for a publisher id. + RCLCPP_PUBLIC + size_t + lowest_available_capacity(const uint64_t intra_process_publisher_id) const; + private: struct SplittedSubscriptions { diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 6583e74ae7..c590207b90 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -62,6 +62,11 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable void add_to_wait_set(rcl_wait_set_t * wait_set) override; + RCLCPP_PUBLIC + virtual + size_t + available_capacity() const = 0; + bool is_ready(rcl_wait_set_t * wait_set) override = 0; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 30debed83a..16c4b6a8db 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -169,6 +169,11 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff return buffer_->use_take_shared_method(); } + size_t available_capacity() const override + { + return buffer_->available_capacity(); + } + protected: void trigger_guard_condition() override diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index d9181bea43..3253595361 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -215,6 +215,17 @@ class PublisherBase : public std::enable_shared_from_this std::vector get_network_flow_endpoints() const; + /// Return the lowest available capacity for all subscription buffers. + /** + * For intraprocess communication return the lowest buffer capacity for all subscriptions. + * If intraprocess is disabled or no intraprocess subscriptions present, return maximum of size_t. + * On failure return 0. + * \return lowest buffer capacity for all subscriptions + */ + RCLCPP_PUBLIC + size_t + lowest_available_ipm_capacity() const; + /// Wait until all published messages are acknowledged or until the specified timeout elapses. /** * This method waits until all published messages are acknowledged by all matching diff --git a/rclcpp/src/rclcpp/intra_process_manager.cpp b/rclcpp/src/rclcpp/intra_process_manager.cpp index efce4afeaf..ea068f03ae 100644 --- a/rclcpp/src/rclcpp/intra_process_manager.cpp +++ b/rclcpp/src/rclcpp/intra_process_manager.cpp @@ -225,5 +225,52 @@ IntraProcessManager::can_communicate( return true; } +size_t +IntraProcessManager::lowest_available_capacity(const uint64_t intra_process_publisher_id) const +{ + size_t capacity = std::numeric_limits::max(); + + auto publisher_it = pub_to_subs_.find(intra_process_publisher_id); + if (publisher_it == pub_to_subs_.end()) { + // Publisher is either invalid or no longer exists. + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling lowest_available_capacity for invalid or no longer existing publisher id"); + return 0u; + } + + if (publisher_it->second.take_shared_subscriptions.empty() && + publisher_it->second.take_ownership_subscriptions.empty()) + { + // no subscriptions available + return 0u; + } + + auto available_capacity = [this, &capacity](const uint64_t intra_process_subscription_id) + { + auto subscription_it = subscriptions_.find(intra_process_subscription_id); + if (subscription_it != subscriptions_.end()) { + auto subscription = subscription_it->second.lock(); + if (subscription) { + capacity = std::min(capacity, subscription->available_capacity()); + } + } else { + // Subscription is either invalid or no longer exists. + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling available_capacity for invalid or no longer existing subscription id"); + } + }; + + for (const auto sub_id : publisher_it->second.take_shared_subscriptions) { + available_capacity(sub_id); + } + + for (const auto sub_id : publisher_it->second.take_ownership_subscriptions) { + available_capacity(sub_id); + } + + return capacity; +} } // namespace experimental } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index 698db2d559..0e37dcf3fa 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -384,3 +384,22 @@ std::vector PublisherBase::get_network_flow_endpoin return network_flow_endpoint_vector; } + +size_t PublisherBase::lowest_available_ipm_capacity() const +{ + if (!intra_process_is_enabled_) { + return 0u; + } + + auto ipm = weak_ipm_.lock(); + + if (!ipm) { + // TODO(ivanpauno): should this raise an error? + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Intra process manager died for a publisher."); + return 0u; + } + + return ipm->lowest_available_capacity(intra_process_publisher_id_); +} diff --git a/rclcpp/test/rclcpp/test_intra_process_buffer.cpp b/rclcpp/test/rclcpp/test_intra_process_buffer.cpp index 16c457c96f..eace6445a9 100644 --- a/rclcpp/test/rclcpp/test_intra_process_buffer.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_buffer.cpp @@ -238,3 +238,75 @@ TEST(TestIntraProcessBuffer, unique_buffer_consume) { EXPECT_EQ(original_value, *popped_unique_msg); EXPECT_EQ(original_message_pointer, popped_message_pointer); } + +/* + Check the available buffer capacity while storing and consuming data from an intra-process + buffer. + The initial available buffer capacity should equal the buffer size. + Inserting a message should decrease the available buffer capacity by 1. + Consuming a message should increase the available buffer capacity by 1. + */ +TEST(TestIntraProcessBuffer, available_capacity) { + using MessageT = char; + using Alloc = std::allocator; + using Deleter = std::default_delete; + using SharedMessageT = std::shared_ptr; + using UniqueMessageT = std::unique_ptr; + using UniqueIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, UniqueMessageT>; + + constexpr auto history_depth = 5u; + + auto buffer_impl = + std::make_unique>( + history_depth); + + UniqueIntraProcessBufferT intra_process_buffer(std::move(buffer_impl)); + + EXPECT_EQ(history_depth, intra_process_buffer.available_capacity()); + + auto original_unique_msg = std::make_unique('a'); + auto original_message_pointer = reinterpret_cast(original_unique_msg.get()); + auto original_value = *original_unique_msg; + + intra_process_buffer.add_unique(std::move(original_unique_msg)); + + EXPECT_EQ(history_depth - 1u, intra_process_buffer.available_capacity()); + + SharedMessageT popped_shared_msg; + popped_shared_msg = intra_process_buffer.consume_shared(); + auto popped_message_pointer = reinterpret_cast(popped_shared_msg.get()); + + EXPECT_EQ(history_depth, intra_process_buffer.available_capacity()); + EXPECT_EQ(original_value, *popped_shared_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); + + original_unique_msg = std::make_unique('b'); + original_message_pointer = reinterpret_cast(original_unique_msg.get()); + original_value = *original_unique_msg; + + intra_process_buffer.add_unique(std::move(original_unique_msg)); + + auto second_unique_msg = std::make_unique('c'); + auto second_message_pointer = reinterpret_cast(second_unique_msg.get()); + auto second_value = *second_unique_msg; + + intra_process_buffer.add_unique(std::move(second_unique_msg)); + + EXPECT_EQ(history_depth - 2u, intra_process_buffer.available_capacity()); + + UniqueMessageT popped_unique_msg; + popped_unique_msg = intra_process_buffer.consume_unique(); + popped_message_pointer = reinterpret_cast(popped_unique_msg.get()); + + EXPECT_EQ(history_depth - 1u, intra_process_buffer.available_capacity()); + EXPECT_EQ(original_value, *popped_unique_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); + + popped_unique_msg = intra_process_buffer.consume_unique(); + popped_message_pointer = reinterpret_cast(popped_unique_msg.get()); + + EXPECT_EQ(history_depth, intra_process_buffer.available_capacity()); + EXPECT_EQ(second_value, *popped_unique_msg); + EXPECT_EQ(second_message_pointer, popped_message_pointer); +} diff --git a/rclcpp/test/rclcpp/test_intra_process_manager.cpp b/rclcpp/test/rclcpp/test_intra_process_manager.cpp index 45d916b004..83cf586e99 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager.cpp @@ -156,18 +156,26 @@ class IntraProcessBuffer { message_ptr = reinterpret_cast(msg.get()); shared_msg = msg; + ++num_msgs; } void add(MessageUniquePtr msg) { message_ptr = reinterpret_cast(msg.get()); unique_msg = std::move(msg); + ++num_msgs; } void pop(std::uintptr_t & msg_ptr) { msg_ptr = message_ptr; message_ptr = 0; + --num_msgs; + } + + size_t size() const + { + return num_msgs; } // need to store the messages somewhere otherwise the memory address will be reused @@ -175,6 +183,8 @@ class IntraProcessBuffer MessageUniquePtr unique_msg; std::uintptr_t message_ptr; + // count add and pop + size_t num_msgs = 0u; }; } // namespace mock @@ -221,6 +231,10 @@ class SubscriptionIntraProcessBase return topic_name.c_str(); } + virtual + size_t + available_capacity() const = 0; + rclcpp::QoS qos_profile; std::string topic_name; }; @@ -280,6 +294,12 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase return take_shared_method; } + size_t + available_capacity() const override + { + return qos_profile.depth() - buffer->size(); + } + bool take_shared_method; typename rclcpp::experimental::buffers::mock::IntraProcessBuffer::UniquePtr buffer; @@ -712,3 +732,91 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { EXPECT_EQ(original_message_pointer, received_message_pointer_10); EXPECT_NE(original_message_pointer, received_message_pointer_11); } + +/* + This tests the method "lowest_available_capacity": + - Creates 1 publisher. + - The available buffer capacity should be at least history size. + - Add 2 subscribers. + - Add everything to the intra-process manager. + - All the entities are expected to have different ids. + - Check the subscriptions count for the publisher. + - The available buffer capacity should be the history size. + - Publish one message (without receiving it). + - The available buffer capacity should decrease by 1. + - Publish another message (without receiving it). + - The available buffer capacity should decrease by 1. + - One subscriber receives one message. + - The available buffer capacity should stay the same, + as the other subscriber still has not freed its buffer. + - The other subscriber receives one message. + - The available buffer capacity should increase by 1. + - One subscription goes out of scope. + - The available buffer capacity should not change. + */ +TEST(TestIntraProcessManager, lowest_available_capacity) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; + + constexpr auto history_depth = 10u; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared(rclcpp::QoS(history_depth).best_effort()); + + auto s1 = std::make_shared(rclcpp::QoS(history_depth).best_effort()); + auto s2 = std::make_shared(rclcpp::QoS(history_depth).best_effort()); + + auto p1_id = ipm->add_publisher(p1); + p1->set_intra_process_manager(p1_id, ipm); + + auto c1 = ipm->lowest_available_capacity(p1_id); + + ASSERT_LE(0u, c1); + + auto s1_id = ipm->add_subscription(s1); + auto s2_id = ipm->add_subscription(s2); + + bool unique_ids = s1_id != s2_id && p1_id != s1_id; + ASSERT_TRUE(unique_ids); + + size_t p1_subs = ipm->get_subscription_count(p1_id); + size_t non_existing_pub_subs = ipm->get_subscription_count(42); + ASSERT_EQ(2u, p1_subs); + ASSERT_EQ(0u, non_existing_pub_subs); + + c1 = ipm->lowest_available_capacity(p1_id); + auto non_existing_pub_c = ipm->lowest_available_capacity(42); + + ASSERT_EQ(history_depth, c1); + ASSERT_EQ(0u, non_existing_pub_c); + + auto unique_msg = std::make_unique(); + p1->publish(std::move(unique_msg)); + + c1 = ipm->lowest_available_capacity(p1_id); + ASSERT_EQ(history_depth - 1u, c1); + + unique_msg = std::make_unique(); + p1->publish(std::move(unique_msg)); + + c1 = ipm->lowest_available_capacity(p1_id); + ASSERT_EQ(history_depth - 2u, c1); + + s1->pop(); + + c1 = ipm->lowest_available_capacity(p1_id); + ASSERT_EQ(history_depth - 2u, c1); + + s2->pop(); + + c1 = ipm->lowest_available_capacity(p1_id); + ASSERT_EQ(history_depth - 1u, c1); + + ipm->get_subscription_intra_process(s1_id).reset(); + + c1 = ipm->lowest_available_capacity(p1_id); + ASSERT_EQ(history_depth - 1u, c1); +} diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 20a46194fc..a0c3ec8b75 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -629,6 +629,41 @@ TEST_P(TestPublisherWaitForAllAcked, check_wait_for_all_acked_with_QosPolicy) { EXPECT_TRUE(pub->wait_for_all_acked(std::chrono::milliseconds(6000))); } +TEST_F(TestPublisher, lowest_available_ipm_capacity) { + constexpr auto history_depth = 10u; + + initialize(rclcpp::NodeOptions().use_intra_process_comms(true)); + + rclcpp::PublisherOptionsWithAllocator> options_ipm_disabled; + options_ipm_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + + rclcpp::PublisherOptionsWithAllocator> options_ipm_enabled; + options_ipm_enabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + + auto do_nothing = [](std::shared_ptr) {}; + auto pub_ipm_disabled = node->create_publisher( + "topic", history_depth, + options_ipm_disabled); + auto pub_ipm_enabled = node->create_publisher( + "topic", history_depth, + options_ipm_enabled); + auto sub = node->create_subscription( + "topic", + history_depth, + do_nothing); + + ASSERT_EQ(1, pub_ipm_enabled->get_intra_process_subscription_count()); + ASSERT_EQ(0, pub_ipm_disabled->lowest_available_ipm_capacity()); + ASSERT_EQ(history_depth, pub_ipm_enabled->lowest_available_ipm_capacity()); + + auto msg = std::make_shared(); + ASSERT_NO_THROW(pub_ipm_disabled->publish(*msg)); + ASSERT_NO_THROW(pub_ipm_enabled->publish(*msg)); + + ASSERT_EQ(0, pub_ipm_disabled->lowest_available_ipm_capacity()); + ASSERT_EQ(history_depth - 1u, pub_ipm_enabled->lowest_available_ipm_capacity()); +} + INSTANTIATE_TEST_SUITE_P( TestWaitForAllAckedWithParm, TestPublisherWaitForAllAcked, From f8072f2fa265000a2a1be8f3dbe7e04d9dfb1c91 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 23 May 2023 12:22:13 +0800 Subject: [PATCH 233/455] remove nolint since ament_cpplint updated for the c++17 header (#2198) Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/any_subscription_callback.hpp | 2 +- rclcpp/include/rclcpp/client.hpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 65b29d8535..5b662536ed 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -20,7 +20,7 @@ #include #include #include -#include // NOLINT[build/include_order] +#include #include "rosidl_runtime_cpp/traits.hpp" #include "tracetools/tracetools.h" diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 92f2f4af8d..85b0a2d5f9 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -20,13 +20,13 @@ #include #include #include -#include // NOLINT, cpplint doesn't think this is a cpp std header +#include #include #include #include #include #include -#include // NOLINT +#include #include #include "rcl/client.h" From 867ad62da26e0bbd4f3cad02a5b64e04d1958294 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 7 Jun 2023 13:28:05 +0000 Subject: [PATCH 234/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 8 ++++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 3 +++ 4 files changed, 17 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 4930b9d41d..bd252f29c9 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* remove nolint since ament_cpplint updated for the c++17 header (`#2198 `_) +* Feature/available capacity of ipm (`#2173 `_) +* add mutex to protect events_executor current entity collection (`#2187 `_) +* Declare rclcpp callbacks before the rcl entities (`#2024 `_) +* Contributors: Alberto Soragna, Chen Lihui, DensoADAS, mauropasse + 21.1.1 (2023-05-11) ------------------- * Fix race condition in events-executor (`#2177 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 2338c2f9b7..d6b797ca73 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 21.1.1 (2023-05-11) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 9b7487879f..1dedbac6ff 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 21.1.1 (2023-05-11) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 57380c9362..ee47d810c6 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 21.1.1 (2023-05-11) ------------------- From dab9c8acdc8bc79808255d27d36036c5f0f78e6b Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 7 Jun 2023 13:28:18 +0000 Subject: [PATCH 235/455] 21.2.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index bd252f29c9..8db2b9f3ad 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +21.2.0 (2023-06-07) +------------------- * remove nolint since ament_cpplint updated for the c++17 header (`#2198 `_) * Feature/available capacity of ipm (`#2173 `_) * add mutex to protect events_executor current entity collection (`#2187 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index bdd5dd5b22..5e3ad24704 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 21.1.1 + 21.2.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index d6b797ca73..8a85833911 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +21.2.0 (2023-06-07) +------------------- 21.1.1 (2023-05-11) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 7800a95571..3d4fad3fad 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 21.1.1 + 21.2.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 1dedbac6ff..72112af1b2 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +21.2.0 (2023-06-07) +------------------- 21.1.1 (2023-05-11) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 5d852ddd0e..12721f888b 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 21.1.1 + 21.2.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index ee47d810c6..63dae81da6 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +21.2.0 (2023-06-07) +------------------- 21.1.1 (2023-05-11) ------------------- diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index c73990e112..c84c440d85 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 21.1.1 + 21.2.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 4efc05266b4fb19569e712648c092b32d3e8005e Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Thu, 8 Jun 2023 10:38:49 -0700 Subject: [PATCH 236/455] Use TRACETOOLS_ prefix for tracepoint-related macros (#2162) Signed-off-by: Christophe Bedard --- .../include/rclcpp/any_service_callback.hpp | 8 ++++---- .../rclcpp/any_subscription_callback.hpp | 20 +++++++++---------- .../buffers/intra_process_buffer.hpp | 2 +- .../buffers/ring_buffer_implementation.hpp | 11 ++++++---- .../subscription_intra_process.hpp | 2 +- .../subscription_intra_process_buffer.hpp | 2 +- rclcpp/include/rclcpp/publisher.hpp | 8 ++++---- rclcpp/include/rclcpp/service.hpp | 6 +++--- rclcpp/include/rclcpp/subscription.hpp | 6 +++--- rclcpp/include/rclcpp/timer.hpp | 10 +++++----- rclcpp/src/rclcpp/executor.cpp | 8 ++++---- .../rclcpp/node_interfaces/node_timers.cpp | 2 +- rclcpp/src/rclcpp/subscription_base.cpp | 2 +- 13 files changed, 45 insertions(+), 42 deletions(-) diff --git a/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp/include/rclcpp/any_service_callback.hpp index f8c2592fc5..918d8e5a29 100644 --- a/rclcpp/include/rclcpp/any_service_callback.hpp +++ b/rclcpp/include/rclcpp/any_service_callback.hpp @@ -156,7 +156,7 @@ class AnyServiceCallback const std::shared_ptr & request_header, std::shared_ptr request) { - TRACEPOINT(callback_start, static_cast(this), false); + TRACETOOLS_TRACEPOINT(callback_start, static_cast(this), false); if (std::holds_alternative(callback_)) { // TODO(ivanpauno): Remove the set method, and force the users of this class // to pass a callback at construnciton. @@ -182,7 +182,7 @@ class AnyServiceCallback const auto & cb = std::get(callback_); cb(request_header, std::move(request), response); } - TRACEPOINT(callback_end, static_cast(this)); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); return response; } @@ -191,9 +191,9 @@ class AnyServiceCallback #ifndef TRACETOOLS_DISABLED std::visit( [this](auto && arg) { - if (TRACEPOINT_ENABLED(rclcpp_callback_register)) { + if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) { char * symbol = tracetools::get_symbol(arg); - DO_TRACEPOINT( + TRACETOOLS_DO_TRACEPOINT( rclcpp_callback_register, static_cast(this), symbol); diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 5b662536ed..2d8795934e 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -492,7 +492,7 @@ class AnySubscriptionCallback std::shared_ptr message, const rclcpp::MessageInfo & message_info) { - TRACEPOINT(callback_start, static_cast(this), false); + TRACETOOLS_TRACEPOINT(callback_start, static_cast(this), false); // Check if the variant is "unset", throw if it is. if (callback_variant_.index() == 0) { if (std::get<0>(callback_variant_) == nullptr) { @@ -583,7 +583,7 @@ class AnySubscriptionCallback static_assert(always_false_v, "unhandled callback type"); } }, callback_variant_); - TRACEPOINT(callback_end, static_cast(this)); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); } // Dispatch when input is a serialized message and the output could be anything. @@ -592,7 +592,7 @@ class AnySubscriptionCallback std::shared_ptr serialized_message, const rclcpp::MessageInfo & message_info) { - TRACEPOINT(callback_start, static_cast(this), false); + TRACETOOLS_TRACEPOINT(callback_start, static_cast(this), false); // Check if the variant is "unset", throw if it is. if (callback_variant_.index() == 0) { if (std::get<0>(callback_variant_) == nullptr) { @@ -663,7 +663,7 @@ class AnySubscriptionCallback static_assert(always_false_v, "unhandled callback type"); } }, callback_variant_); - TRACEPOINT(callback_end, static_cast(this)); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); } void @@ -671,7 +671,7 @@ class AnySubscriptionCallback std::shared_ptr message, const rclcpp::MessageInfo & message_info) { - TRACEPOINT(callback_start, static_cast(this), true); + TRACETOOLS_TRACEPOINT(callback_start, static_cast(this), true); // Check if the variant is "unset", throw if it is. if (callback_variant_.index() == 0) { if (std::get<0>(callback_variant_) == nullptr) { @@ -793,7 +793,7 @@ class AnySubscriptionCallback static_assert(always_false_v, "unhandled callback type"); } }, callback_variant_); - TRACEPOINT(callback_end, static_cast(this)); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); } void @@ -801,7 +801,7 @@ class AnySubscriptionCallback std::unique_ptr message, const rclcpp::MessageInfo & message_info) { - TRACEPOINT(callback_start, static_cast(this), true); + TRACETOOLS_TRACEPOINT(callback_start, static_cast(this), true); // Check if the variant is "unset", throw if it is. if (callback_variant_.index() == 0) { if (std::get<0>(callback_variant_) == nullptr) { @@ -927,7 +927,7 @@ class AnySubscriptionCallback static_assert(always_false_v, "unhandled callback type"); } }, callback_variant_); - TRACEPOINT(callback_end, static_cast(this)); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); } constexpr @@ -965,9 +965,9 @@ class AnySubscriptionCallback #ifndef TRACETOOLS_DISABLED std::visit( [this](auto && callback) { - if (TRACEPOINT_ENABLED(rclcpp_callback_register)) { + if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) { char * symbol = tracetools::get_symbol(callback); - DO_TRACEPOINT( + TRACETOOLS_DO_TRACEPOINT( rclcpp_callback_register, static_cast(this), symbol); diff --git a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp index af936cb342..114e9196d2 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp @@ -96,7 +96,7 @@ class TypedIntraProcessBuffer : public IntraProcessBuffer(buffer_.get()), static_cast(this)); diff --git a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp index 5c6ba59291..e67aae973a 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp @@ -52,7 +52,10 @@ class RingBufferImplementation : public BufferImplementationBase if (capacity == 0) { throw std::invalid_argument("capacity must be a positive, non-zero value"); } - TRACEPOINT(rclcpp_construct_ring_buffer, static_cast(this), capacity_); + TRACETOOLS_TRACEPOINT( + rclcpp_construct_ring_buffer, + static_cast(this), + capacity_); } virtual ~RingBufferImplementation() {} @@ -69,7 +72,7 @@ class RingBufferImplementation : public BufferImplementationBase write_index_ = next_(write_index_); ring_buffer_[write_index_] = std::move(request); - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_ring_buffer_enqueue, static_cast(this), write_index_, @@ -98,7 +101,7 @@ class RingBufferImplementation : public BufferImplementationBase } auto request = std::move(ring_buffer_[read_index_]); - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_ring_buffer_dequeue, static_cast(this), read_index_, @@ -162,7 +165,7 @@ class RingBufferImplementation : public BufferImplementationBase void clear() { - TRACEPOINT(rclcpp_ring_buffer_clear, static_cast(this)); + TRACETOOLS_TRACEPOINT(rclcpp_ring_buffer_clear, static_cast(this)); } private: diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index ec89ebc5ef..39c391a027 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -87,7 +87,7 @@ class SubscriptionIntraProcess buffer_type), any_callback_(callback) { - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_subscription_callback_added, static_cast(this), static_cast(&any_callback_)); diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 16c4b6a8db..f564cbb047 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -93,7 +93,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff buffer_type, qos_profile, std::make_shared(subscribed_type_allocator_)); - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_ipb_to_subscription, static_cast(buffer_.get()), static_cast(this)); diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index f474a67192..6025125db1 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -421,7 +421,7 @@ class Publisher : public PublisherBase void do_inter_process_publish(const ROSMessageType & msg) { - TRACEPOINT(rclcpp_publish, nullptr, static_cast(&msg)); + TRACETOOLS_TRACEPOINT(rclcpp_publish, nullptr, static_cast(&msg)); auto status = rcl_publish(publisher_handle_.get(), &msg, nullptr); if (RCL_RET_PUBLISHER_INVALID == status) { @@ -484,7 +484,7 @@ class Publisher : public PublisherBase if (!msg) { throw std::runtime_error("cannot publish msg which is a null pointer"); } - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_intra_publish, static_cast(publisher_handle_.get()), msg.get()); @@ -506,7 +506,7 @@ class Publisher : public PublisherBase if (!msg) { throw std::runtime_error("cannot publish msg which is a null pointer"); } - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_intra_publish, static_cast(publisher_handle_.get()), msg.get()); @@ -529,7 +529,7 @@ class Publisher : public PublisherBase if (!msg) { throw std::runtime_error("cannot publish msg which is a null pointer"); } - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_intra_publish, static_cast(publisher_handle_.get()), msg.get()); diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index d73cb76444..2ae79a6637 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -352,7 +352,7 @@ class Service rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service"); } - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_service_callback_added, static_cast(get_service_handle().get()), static_cast(&any_callback_)); @@ -387,7 +387,7 @@ class Service } service_handle_ = service_handle; - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_service_callback_added, static_cast(get_service_handle().get()), static_cast(&any_callback_)); @@ -424,7 +424,7 @@ class Service // In this case, rcl owns the service handle memory service_handle_ = std::shared_ptr(new rcl_service_t); service_handle_->impl = service_handle->impl; - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_service_callback_added, static_cast(get_service_handle().get()), static_cast(&any_callback_)); diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index eb1a980dd3..b0857fb102 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -185,7 +185,7 @@ class Subscription : public SubscriptionBase this->get_topic_name(), // important to get like this, as it has the fully-qualified name qos_profile, resolve_intra_process_buffer_type(options_.intra_process_buffer_type, callback)); - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_subscription_init, static_cast(get_subscription_handle().get()), static_cast(subscription_intra_process_.get())); @@ -201,11 +201,11 @@ class Subscription : public SubscriptionBase this->subscription_topic_statistics_ = std::move(subscription_topic_statistics); } - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_subscription_init, static_cast(get_subscription_handle().get()), static_cast(this)); - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_subscription_callback_added, static_cast(this), static_cast(&any_callback_)); diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 91b1705985..cc40736ee9 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -223,14 +223,14 @@ class GenericTimer : public TimerBase ) : TimerBase(clock, period, context), callback_(std::forward(callback)) { - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_timer_callback_added, static_cast(get_timer_handle().get()), reinterpret_cast(&callback_)); #ifndef TRACETOOLS_DISABLED - if (TRACEPOINT_ENABLED(rclcpp_callback_register)) { + if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) { char * symbol = tracetools::get_symbol(callback_); - DO_TRACEPOINT( + TRACETOOLS_DO_TRACEPOINT( rclcpp_callback_register, reinterpret_cast(&callback_), symbol); @@ -269,9 +269,9 @@ class GenericTimer : public TimerBase void execute_callback() override { - TRACEPOINT(callback_start, reinterpret_cast(&callback_), false); + TRACETOOLS_TRACEPOINT(callback_start, reinterpret_cast(&callback_), false); execute_callback_delegate<>(); - TRACEPOINT(callback_end, reinterpret_cast(&callback_)); + TRACETOOLS_TRACEPOINT(callback_end, reinterpret_cast(&callback_)); } // void specialization diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 50529a102a..3deb2d1706 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -525,13 +525,13 @@ Executor::execute_any_executable(AnyExecutable & any_exec) return; } if (any_exec.timer) { - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_executor_execute, static_cast(any_exec.timer->get_timer_handle().get())); execute_timer(any_exec.timer); } if (any_exec.subscription) { - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_executor_execute, static_cast(any_exec.subscription->get_subscription_handle().get())); execute_subscription(any_exec.subscription); @@ -726,7 +726,7 @@ Executor::execute_client( void Executor::wait_for_work(std::chrono::nanoseconds timeout) { - TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count()); + TRACETOOLS_TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count()); { std::lock_guard guard(mutex_); @@ -882,7 +882,7 @@ Executor::get_next_ready_executable_from_map( const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { - TRACEPOINT(rclcpp_executor_get_next_ready); + TRACETOOLS_TRACEPOINT(rclcpp_executor_get_next_ready); bool success = false; std::lock_guard guard{mutex_}; // Check the timers to see if there are any that are ready diff --git a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp index 96097def22..e72ddf91d3 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp @@ -50,7 +50,7 @@ NodeTimers::add_timer( std::string("failed to notify wait set on timer creation: ") + ex.what()); } - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_timer_link_node, static_cast(timer->get_timer_handle().get()), static_cast(node_base_->get_rcl_node_handle())); diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 6db3bf5ceb..67a9a6c56f 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -218,7 +218,7 @@ SubscriptionBase::take_type_erased(void * message_out, rclcpp::MessageInfo & mes &message_info_out.get_rmw_message_info(), nullptr // rmw_subscription_allocation_t is unused here ); - TRACEPOINT(rclcpp_take, static_cast(message_out)); + TRACETOOLS_TRACEPOINT(rclcpp_take, static_cast(message_out)); if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { return false; } else if (RCL_RET_OK != ret) { From b3518d12ca2a123174384a740a6430f3c1660448 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 9 Jun 2023 11:18:04 -0500 Subject: [PATCH 237/455] Remove flaky stressAddRemoveNode test (#2206) Signed-off-by: Michael Carroll --- .../test/rclcpp/executors/test_executors.cpp | 54 ------------------- 1 file changed, 54 deletions(-) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index e9aad61685..232baaace3 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -796,60 +796,6 @@ TYPED_TEST(TestExecutors, testRaceConditionAddNode) } } -// This test verifies the thread-safety of adding and removing a node -// while the executor is spinning and events are ready. -// This test does not contain expectations, but rather it verifies that -// we can run a "stressful routine" without crashing. -TYPED_TEST(TestExecutors, stressAddRemoveNode) -{ - using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - - ExecutorType executor; - - // A timer that is "always" ready (the timer callback doesn't do anything) - auto timer = this->node->create_wall_timer(std::chrono::nanoseconds(1), []() {}); - - // This thread spins the executor until it's cancelled - std::thread spinner_thread([&]() { - executor.spin(); - }); - - // This thread publishes data in a busy loop (the node has a subscription) - std::thread publisher_thread1([&]() { - for (size_t i = 0; i < 100000; i++) { - this->publisher->publish(test_msgs::msg::Empty()); - } - }); - std::thread publisher_thread2([&]() { - for (size_t i = 0; i < 100000; i++) { - this->publisher->publish(test_msgs::msg::Empty()); - } - }); - - // This thread adds/remove the node that contains the entities in a busy loop - std::thread add_remove_thread([&]() { - for (size_t i = 0; i < 100000; i++) { - executor.add_node(this->node); - executor.remove_node(this->node); - } - }); - - // Wait for the threads that do real work to finish - publisher_thread1.join(); - publisher_thread2.join(); - add_remove_thread.join(); - - executor.cancel(); - spinner_thread.join(); -} - // Check spin_until_future_complete with node base pointer (instantiates its own executor) TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) { From 1fff79089a23c33b2d92872ffd99147506b3fb9e Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 12 Jun 2023 01:48:56 -0400 Subject: [PATCH 238/455] Fix up misspellings of "receive". (#2208) Signed-off-by: Chris Lalancette --- .../include/rclcpp/parameter_events_filter.hpp | 2 +- rclcpp/test/rclcpp/test_subscription.cpp | 16 ++++++++-------- rclcpp/test/rclcpp/test_time_source.cpp | 2 +- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter_events_filter.hpp b/rclcpp/include/rclcpp/parameter_events_filter.hpp index 3aa70d8a85..6960a1bccf 100644 --- a/rclcpp/include/rclcpp/parameter_events_filter.hpp +++ b/rclcpp/include/rclcpp/parameter_events_filter.hpp @@ -48,7 +48,7 @@ class ParameterEventsFilter * * Example Usage: * - * If you have recieved a parameter event and are only interested in parameters foo and + * If you have received a parameter event and are only interested in parameters foo and * bar being added or changed but don't care about deletion. * * ```cpp diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index 6facada4dd..9f9a7ff62d 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -313,13 +313,13 @@ TEST_F(TestSubscription, take) { } test_msgs::msg::Empty msg; rclcpp::MessageInfo msg_info; - bool message_recieved = false; + bool message_received = false; auto start = std::chrono::steady_clock::now(); do { - message_recieved = sub->take(msg, msg_info); + message_received = sub->take(msg, msg_info); std::this_thread::sleep_for(100ms); - } while (!message_recieved && std::chrono::steady_clock::now() - start < 10s); - EXPECT_TRUE(message_recieved); + } while (!message_received && std::chrono::steady_clock::now() - start < 10s); + EXPECT_TRUE(message_received); } // TODO(wjwwood): figure out a good way to test the intra-process exclusion behavior. } @@ -350,13 +350,13 @@ TEST_F(TestSubscription, take_serialized) { } std::shared_ptr msg = sub->create_serialized_message(); rclcpp::MessageInfo msg_info; - bool message_recieved = false; + bool message_received = false; auto start = std::chrono::steady_clock::now(); do { - message_recieved = sub->take_serialized(*msg, msg_info); + message_received = sub->take_serialized(*msg, msg_info); std::this_thread::sleep_for(100ms); - } while (!message_recieved && std::chrono::steady_clock::now() - start < 10s); - EXPECT_TRUE(message_recieved); + } while (!message_received && std::chrono::steady_clock::now() - start < 10s); + EXPECT_TRUE(message_received); } } diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index 26d7cb2192..1437e47126 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -305,7 +305,7 @@ TEST_F(TestTimeSource, clock) { trigger_clock_changes(node, ros_clock, false); - // Even now that we've recieved a message, ROS time should still not be active since the + // Even now that we've received a message, ROS time should still not be active since the // parameter has not been explicitly set. EXPECT_FALSE(ros_clock->ros_time_is_active()); From 4d12bcbca083b7380f7edc6d14008f73c709aaf2 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 12 Jun 2023 12:45:00 +0000 Subject: [PATCH 239/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 7 +++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 3 +++ 4 files changed, 16 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 8db2b9f3ad..2c0aed98fb 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix up misspellings of "receive". (`#2208 `_) +* Remove flaky stressAddRemoveNode test (`#2206 `_) +* Use TRACETOOLS\_ prefix for tracepoint-related macros (`#2162 `_) +* Contributors: Chris Lalancette, Christophe Bedard, Michael Carroll + 21.2.0 (2023-06-07) ------------------- * remove nolint since ament_cpplint updated for the c++17 header (`#2198 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 8a85833911..e92727e88d 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 21.2.0 (2023-06-07) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 72112af1b2..d0b66e2950 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 21.2.0 (2023-06-07) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 63dae81da6..4ef21d77a8 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 21.2.0 (2023-06-07) ------------------- From 3530b0959c51fd0417e92472d427e817a327a4c9 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 12 Jun 2023 12:45:11 +0000 Subject: [PATCH 240/455] 21.3.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 2c0aed98fb..4e3ada2fdb 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +21.3.0 (2023-06-12) +------------------- * Fix up misspellings of "receive". (`#2208 `_) * Remove flaky stressAddRemoveNode test (`#2206 `_) * Use TRACETOOLS\_ prefix for tracepoint-related macros (`#2162 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 5e3ad24704..619f792aa2 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 21.2.0 + 21.3.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index e92727e88d..7efd063835 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +21.3.0 (2023-06-12) +------------------- 21.2.0 (2023-06-07) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 3d4fad3fad..803b356c35 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 21.2.0 + 21.3.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index d0b66e2950..690970a9ad 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +21.3.0 (2023-06-12) +------------------- 21.2.0 (2023-06-07) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 12721f888b..39c8894d34 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 21.2.0 + 21.3.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 4ef21d77a8..059ddfbb75 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +21.3.0 (2023-06-12) +------------------- 21.2.0 (2023-06-07) ------------------- diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index c84c440d85..02962b7007 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 21.2.0 + 21.3.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 3a64349aec183637435eee245e5a0ac0f7297ed0 Mon Sep 17 00:00:00 2001 From: Christopher Wecht Date: Wed, 14 Jun 2023 14:33:33 +0200 Subject: [PATCH 241/455] Enable callback group tests for connextdds (#2182) * Enable callback group tests for connextdds * Enable executors and event executor tests for connextdds * Enable qos events tests for connextdds * Less flaky qos_event tests Signed-off-by: Christopher Wecht --- .../rclcpp/executors/test_events_executor.cpp | 55 -------- .../test/rclcpp/executors/test_executors.cpp | 120 ------------------ .../test_add_callback_groups_to_executor.cpp | 63 --------- rclcpp/test/rclcpp/test_qos_event.cpp | 76 ++++++----- 4 files changed, 36 insertions(+), 278 deletions(-) diff --git a/rclcpp/test/rclcpp/executors/test_events_executor.cpp b/rclcpp/test/rclcpp/executors/test_events_executor.cpp index fbe83fcddc..0d678438ed 100644 --- a/rclcpp/test/rclcpp/executors/test_events_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_events_executor.cpp @@ -43,11 +43,6 @@ class TestEventsExecutor : public ::testing::Test TEST_F(TestEventsExecutor, run_pub_sub) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); bool msg_received = false; @@ -95,11 +90,6 @@ TEST_F(TestEventsExecutor, run_pub_sub) TEST_F(TestEventsExecutor, run_clients_servers) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); bool request_received = false; @@ -153,11 +143,6 @@ TEST_F(TestEventsExecutor, run_clients_servers) TEST_F(TestEventsExecutor, spin_once_max_duration_timeout) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); EventsExecutor executor; @@ -190,11 +175,6 @@ TEST_F(TestEventsExecutor, spin_once_max_duration_timeout) TEST_F(TestEventsExecutor, spin_once_max_duration_timer) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); EventsExecutor executor; @@ -226,11 +206,6 @@ TEST_F(TestEventsExecutor, spin_once_max_duration_timer) TEST_F(TestEventsExecutor, spin_some_max_duration) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - { auto node = std::make_shared("node"); @@ -277,11 +252,6 @@ TEST_F(TestEventsExecutor, spin_some_max_duration) TEST_F(TestEventsExecutor, spin_some_zero_duration) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); size_t t_runs = 0; @@ -303,11 +273,6 @@ TEST_F(TestEventsExecutor, spin_some_zero_duration) TEST_F(TestEventsExecutor, spin_all_max_duration) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - { auto node = std::make_shared("node"); @@ -358,11 +323,6 @@ TEST_F(TestEventsExecutor, spin_all_max_duration) TEST_F(TestEventsExecutor, cancel_while_timers_running) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); EventsExecutor executor; @@ -402,11 +362,6 @@ TEST_F(TestEventsExecutor, cancel_while_timers_running) TEST_F(TestEventsExecutor, cancel_while_timers_waiting) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); size_t t1_runs = 0; @@ -435,11 +390,6 @@ TEST_F(TestEventsExecutor, destroy_entities) // This test fails on Windows! We skip it for now GTEST_SKIP(); - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - // Create a publisher node and start publishing messages auto node_pub = std::make_shared("node_pub"); auto publisher = node_pub->create_publisher("topic", rclcpp::QoS(10)); @@ -485,11 +435,6 @@ std::string * g_sub_log_msg; std::promise * g_log_msgs_promise; TEST_F(TestEventsExecutor, test_default_incompatible_qos_callbacks) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); rcutils_logging_output_handler_t original_output_handler = rcutils_logging_get_output_handler(); diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 232baaace3..fe509511e8 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -135,14 +135,6 @@ TYPED_TEST_SUITE(TestExecutorsStable, StandardExecutors, ExecutorTypeNames); TYPED_TEST(TestExecutors, detachOnDestruction) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - { ExecutorType executor; executor.add_node(this->node); @@ -159,14 +151,6 @@ TYPED_TEST(TestExecutors, detachOnDestruction) TYPED_TEST(TestExecutorsStable, addTemporaryNode) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; { @@ -187,14 +171,6 @@ TYPED_TEST(TestExecutorsStable, addTemporaryNode) TYPED_TEST(TestExecutors, emptyExecutor) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; std::thread spinner([&]() {EXPECT_NO_THROW(executor.spin());}); std::this_thread::sleep_for(50ms); @@ -206,14 +182,6 @@ TYPED_TEST(TestExecutors, emptyExecutor) TYPED_TEST(TestExecutors, addNodeTwoExecutors) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor1; ExecutorType executor2; EXPECT_NO_THROW(executor1.add_node(this->node)); @@ -225,14 +193,6 @@ TYPED_TEST(TestExecutors, addNodeTwoExecutors) TYPED_TEST(TestExecutors, spinWithTimer) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; bool timer_completed = false; @@ -256,14 +216,6 @@ TYPED_TEST(TestExecutors, spinWithTimer) TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; executor.add_node(this->node); @@ -291,14 +243,6 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; executor.add_node(this->node); @@ -322,14 +266,6 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; executor.add_node(this->node); @@ -354,14 +290,6 @@ TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; executor.add_node(this->node); @@ -409,14 +337,6 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; executor.add_node(this->node); @@ -529,14 +449,6 @@ class TestWaitable : public rclcpp::Waitable TYPED_TEST(TestExecutors, spinAll) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; auto waitable_interfaces = this->node->get_node_waitables_interface(); auto my_waitable = std::make_shared(); @@ -579,14 +491,6 @@ TYPED_TEST(TestExecutors, spinAll) TYPED_TEST(TestExecutors, spinSome) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; auto waitable_interfaces = this->node->get_node_waitables_interface(); auto my_waitable = std::make_shared(); @@ -629,14 +533,6 @@ TYPED_TEST(TestExecutors, spinSome) TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; std::promise promise; @@ -653,14 +549,6 @@ TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodePtr) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; std::promise promise; @@ -677,14 +565,6 @@ TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodePtr) TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; executor.add_node(this->node); diff --git a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp index 49131638db..9d52d14035 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -99,13 +99,6 @@ TYPED_TEST_SUITE(TestAddCallbackGroupsToExecutorStable, StandardExecutors, Execu TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } auto node = std::make_shared("my_node", "/ns"); auto timer_callback = []() {}; @@ -155,13 +148,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups) TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } auto node = std::make_shared("my_node", "/ns"); auto timer_callback = []() {}; @@ -193,13 +179,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups) TYPED_TEST(TestAddCallbackGroupsToExecutor, add_duplicate_callback_groups) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } ExecutorType executor; auto node = std::make_shared("my_node", "/ns"); @@ -220,13 +199,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_duplicate_callback_groups) TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_to_executor) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } ExecutorType executor; auto node = std::make_shared("my_node", "/ns"); @@ -263,13 +235,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t TYPED_TEST(TestAddCallbackGroupsToExecutor, add_unallowable_callback_groups) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } ExecutorType executor; auto node = std::make_shared("my_node", "/ns"); @@ -307,13 +272,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_unallowable_callback_groups) TYPED_TEST(TestAddCallbackGroupsToExecutor, one_node_many_callback_groups_many_executors) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } ExecutorType timer_executor; ExecutorType sub_executor; @@ -355,13 +313,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, one_node_many_callback_groups_many_e TYPED_TEST(TestAddCallbackGroupsToExecutorStable, subscriber_triggered_to_receive_message) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } auto node = std::make_shared("my_node", "/ns"); @@ -428,13 +379,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutorStable, subscriber_triggered_to_receiv TYPED_TEST(TestAddCallbackGroupsToExecutorStable, callback_group_create_after_spin) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } auto node = std::make_shared("my_node", "/ns"); @@ -481,13 +425,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutorStable, callback_group_create_after_sp TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_group) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } ExecutorType executor; auto node = std::make_shared("my_node", "/ns"); diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index 6b522d7ea2..3803a07d65 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -314,11 +314,6 @@ TEST_F(TestQosEvent, add_to_wait_set) { TEST_F(TestQosEvent, test_on_new_event_callback) { - // rmw_connextdds doesn't support rmw_event_set_callback() interface - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto offered_deadline = rclcpp::Duration(std::chrono::milliseconds(1)); auto requested_deadline = rclcpp::Duration(std::chrono::milliseconds(2)); @@ -360,11 +355,6 @@ TEST_F(TestQosEvent, test_on_new_event_callback) TEST_F(TestQosEvent, test_invalid_on_new_event_callback) { - // rmw_connextdds doesn't support rmw_event_set_callback() interface - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - 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;}; @@ -439,11 +429,6 @@ TEST_F(TestQosEvent, test_invalid_on_new_event_callback) TEST_F(TestQosEvent, test_pub_matched_event_by_set_event_callback) { - // rmw_connextdds doesn't support rmw_event_set_callback() interface - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - std::atomic_size_t matched_count = 0; rclcpp::PublisherOptions pub_options; @@ -451,8 +436,10 @@ TEST_F(TestQosEvent, test_pub_matched_event_by_set_event_callback) auto pub = node->create_publisher( topic_name, 10, pub_options); - auto matched_event_callback = [&matched_count](size_t count) { + std::promise prom; + auto matched_event_callback = [&matched_count, &prom](size_t count) { matched_count += count; + prom.set_value(); }; pub->set_on_new_qos_event_callback(matched_event_callback, RCL_PUBLISHER_MATCHED); @@ -460,34 +447,32 @@ TEST_F(TestQosEvent, test_pub_matched_event_by_set_event_callback) rclcpp::executors::SingleThreadedExecutor ex; ex.add_node(node->get_node_base_interface()); - const auto timeout = std::chrono::milliseconds(200); + const auto timeout = std::chrono::seconds(10); { auto sub1 = node->create_subscription(topic_name, 10, message_callback); - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; EXPECT_EQ(matched_count, static_cast(1)); { auto sub2 = node->create_subscription( topic_name, 10, message_callback); - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; EXPECT_EQ(matched_count, static_cast(2)); } - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; EXPECT_EQ(matched_count, static_cast(3)); } - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); EXPECT_EQ(matched_count, static_cast(4)); } TEST_F(TestQosEvent, test_sub_matched_event_by_set_event_callback) { - // rmw_connextdds doesn't support rmw_event_set_callback() interface - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - std::atomic_size_t matched_count = 0; rclcpp::SubscriptionOptions sub_options; @@ -495,8 +480,10 @@ TEST_F(TestQosEvent, test_sub_matched_event_by_set_event_callback) auto sub = node->create_subscription( topic_name, 10, message_callback, sub_options); - auto matched_event_callback = [&matched_count](size_t count) { + std::promise prom; + auto matched_event_callback = [&matched_count, &prom](size_t count) { matched_count += count; + prom.set_value(); }; sub->set_on_new_qos_event_callback(matched_event_callback, RCL_SUBSCRIPTION_MATCHED); @@ -504,39 +491,44 @@ TEST_F(TestQosEvent, test_sub_matched_event_by_set_event_callback) rclcpp::executors::SingleThreadedExecutor ex; ex.add_node(node->get_node_base_interface()); - const auto timeout = std::chrono::milliseconds(200); + const auto timeout = std::chrono::seconds(10000); { auto pub1 = node->create_publisher(topic_name, 10); - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; EXPECT_EQ(matched_count, static_cast(1)); { auto pub2 = node->create_publisher(topic_name, 10); - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; EXPECT_EQ(matched_count, static_cast(2)); } - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; EXPECT_EQ(matched_count, static_cast(3)); } - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); EXPECT_EQ(matched_count, static_cast(4)); } TEST_F(TestQosEvent, test_pub_matched_event_by_option_event_callback) { rmw_matched_status_t matched_expected_result; + std::promise prom; rclcpp::PublisherOptions pub_options; pub_options.event_callbacks.matched_callback = - [&matched_expected_result](rmw_matched_status_t & s) { + [&matched_expected_result, &prom](rmw_matched_status_t & s) { EXPECT_EQ(s.total_count, matched_expected_result.total_count); EXPECT_EQ(s.total_count_change, matched_expected_result.total_count_change); EXPECT_EQ(s.current_count, matched_expected_result.current_count); EXPECT_EQ(s.current_count_change, matched_expected_result.current_count_change); + prom.set_value(); }; auto pub = node->create_publisher( @@ -551,11 +543,12 @@ TEST_F(TestQosEvent, test_pub_matched_event_by_option_event_callback) matched_expected_result.current_count = 1; matched_expected_result.current_count_change = 1; - const auto timeout = std::chrono::milliseconds(200); + const auto timeout = std::chrono::seconds(10); { auto sub = node->create_subscription(topic_name, 10, message_callback); - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; // destroy a connected subscription matched_expected_result.total_count = 1; @@ -563,20 +556,22 @@ TEST_F(TestQosEvent, test_pub_matched_event_by_option_event_callback) matched_expected_result.current_count = 0; matched_expected_result.current_count_change = -1; } - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); } TEST_F(TestQosEvent, test_sub_matched_event_by_option_event_callback) { rmw_matched_status_t matched_expected_result; + std::promise prom; rclcpp::SubscriptionOptions sub_options; sub_options.event_callbacks.matched_callback = - [&matched_expected_result](rmw_matched_status_t & s) { + [&matched_expected_result, &prom](rmw_matched_status_t & s) { EXPECT_EQ(s.total_count, matched_expected_result.total_count); EXPECT_EQ(s.total_count_change, matched_expected_result.total_count_change); EXPECT_EQ(s.current_count, matched_expected_result.current_count); EXPECT_EQ(s.current_count_change, matched_expected_result.current_count_change); + prom.set_value(); }; auto sub = node->create_subscription( topic_name, 10, message_callback, sub_options); @@ -590,10 +585,11 @@ TEST_F(TestQosEvent, test_sub_matched_event_by_option_event_callback) matched_expected_result.current_count = 1; matched_expected_result.current_count_change = 1; - const auto timeout = std::chrono::milliseconds(200); + const auto timeout = std::chrono::seconds(10); { auto pub1 = node->create_publisher(topic_name, 10); - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; // destroy a connected publisher matched_expected_result.total_count = 1; @@ -601,5 +597,5 @@ TEST_F(TestQosEvent, test_sub_matched_event_by_option_event_callback) matched_expected_result.current_count = 0; matched_expected_result.current_count_change = -1; } - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); } From 005f6aefe96b7857fd08c99c04a751c5e2f93528 Mon Sep 17 00:00:00 2001 From: Eloy Briceno <51831786+Voldivh@users.noreply.github.com> Date: Wed, 21 Jun 2023 09:47:14 -0500 Subject: [PATCH 242/455] Modifies timers API to select autostart state (#2005) * Modifies timers API to select autostart state * Removes unnecessary variables * Adds autostart documentation and expands some timer test Signed-off-by: Voldivh --- rclcpp/include/rclcpp/create_timer.hpp | 23 +++++++++++------- rclcpp/include/rclcpp/node.hpp | 4 +++- rclcpp/include/rclcpp/node_impl.hpp | 6 +++-- rclcpp/include/rclcpp/timer.hpp | 18 ++++++++++---- rclcpp/src/rclcpp/timer.cpp | 9 +++---- rclcpp/test/rclcpp/test_create_timer.cpp | 26 ++++++++++++++++++++ rclcpp/test/rclcpp/test_timer.cpp | 30 ++++++++++++++++++++++++ 7 files changed, 96 insertions(+), 20 deletions(-) diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index d371466f0d..64d5b8e322 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -90,7 +90,8 @@ create_timer( rclcpp::Clock::SharedPtr clock, rclcpp::Duration period, CallbackT && callback, - rclcpp::CallbackGroup::SharedPtr group = nullptr) + rclcpp::CallbackGroup::SharedPtr group = nullptr, + bool autostart = true) { return create_timer( clock, @@ -98,7 +99,8 @@ create_timer( std::forward(callback), group, node_base.get(), - node_timers.get()); + node_timers.get(), + autostart); } /// Create a timer with a given clock @@ -109,7 +111,8 @@ create_timer( rclcpp::Clock::SharedPtr clock, rclcpp::Duration period, CallbackT && callback, - rclcpp::CallbackGroup::SharedPtr group = nullptr) + rclcpp::CallbackGroup::SharedPtr group = nullptr, + bool autostart = true) { return create_timer( clock, @@ -117,7 +120,8 @@ create_timer( std::forward(callback), group, rclcpp::node_interfaces::get_node_base_interface(node).get(), - rclcpp::node_interfaces::get_node_timers_interface(node).get()); + rclcpp::node_interfaces::get_node_timers_interface(node).get(), + autostart); } /// Convenience method to create a general timer with node resources. @@ -132,6 +136,7 @@ create_timer( * \param group callback group * \param node_base node base interface * \param node_timers node timer interface + * \param autostart defines if the timer should start it's countdown on initialization or not. * \return shared pointer to a generic timer * \throws std::invalid_argument if either clock, node_base or node_timers * are nullptr, or period is negative or too large @@ -144,7 +149,8 @@ create_timer( CallbackT callback, rclcpp::CallbackGroup::SharedPtr group, node_interfaces::NodeBaseInterface * node_base, - node_interfaces::NodeTimersInterface * node_timers) + node_interfaces::NodeTimersInterface * node_timers, + bool autostart = true) { if (clock == nullptr) { throw std::invalid_argument{"clock cannot be null"}; @@ -160,7 +166,7 @@ create_timer( // Add a new generic timer. auto timer = rclcpp::GenericTimer::make_shared( - std::move(clock), period_ns, std::move(callback), node_base->get_context()); + std::move(clock), period_ns, std::move(callback), node_base->get_context(), autostart); node_timers->add_timer(timer, group); return timer; } @@ -187,7 +193,8 @@ create_wall_timer( CallbackT callback, rclcpp::CallbackGroup::SharedPtr group, node_interfaces::NodeBaseInterface * node_base, - node_interfaces::NodeTimersInterface * node_timers) + node_interfaces::NodeTimersInterface * node_timers, + bool autostart = true) { if (node_base == nullptr) { throw std::invalid_argument{"input node_base cannot be null"}; @@ -201,7 +208,7 @@ create_wall_timer( // Add a new wall timer. auto timer = rclcpp::WallTimer::make_shared( - period_ns, std::move(callback), node_base->get_context()); + period_ns, std::move(callback), node_base->get_context(), autostart); node_timers->add_timer(timer, group); return timer; } diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 7ecb67e9b1..556a6d0318 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -232,13 +232,15 @@ class Node : public std::enable_shared_from_this * \param[in] period Time interval between triggers of the callback. * \param[in] callback User-defined callback function. * \param[in] group Callback group to execute this timer's callback in. + * \param[in] autostart The state of the clock on initialization. */ template typename rclcpp::WallTimer::SharedPtr create_wall_timer( std::chrono::duration period, CallbackT callback, - rclcpp::CallbackGroup::SharedPtr group = nullptr); + rclcpp::CallbackGroup::SharedPtr group = nullptr, + bool autostart = true); /// Create a timer that uses the node clock to drive the callback. /** diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index d6b090d4d1..5f1fb7b3df 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -110,14 +110,16 @@ typename rclcpp::WallTimer::SharedPtr Node::create_wall_timer( std::chrono::duration period, CallbackT callback, - rclcpp::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group, + bool autostart) { return rclcpp::create_wall_timer( period, std::move(callback), group, this->node_base_.get(), - this->node_timers_.get()); + this->node_timers_.get(), + autostart); } template diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index cc40736ee9..6060d8bd78 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -53,12 +53,17 @@ class TimerBase * \param clock A clock to use for time and sleeping * \param period The interval at which the timer fires * \param context node context + * \param autostart timer state on initialization + * + * In order to activate a timer that is not started on initialization, + * user should call the reset() method. */ RCLCPP_PUBLIC explicit TimerBase( Clock::SharedPtr clock, std::chrono::nanoseconds period, - rclcpp::Context::SharedPtr context); + rclcpp::Context::SharedPtr context, + bool autostart = true); /// TimerBase destructor RCLCPP_PUBLIC @@ -216,12 +221,13 @@ class GenericTimer : public TimerBase * \param[in] period The interval at which the timer fires. * \param[in] callback User-specified callback function. * \param[in] context custom context to be used. + * \param autostart timer state on initialization */ explicit GenericTimer( Clock::SharedPtr clock, std::chrono::nanoseconds period, FunctorT && callback, - rclcpp::Context::SharedPtr context + rclcpp::Context::SharedPtr context, bool autostart = true ) - : TimerBase(clock, period, context), callback_(std::forward(callback)) + : TimerBase(clock, period, context, autostart), callback_(std::forward(callback)) { TRACETOOLS_TRACEPOINT( rclcpp_timer_callback_added, @@ -330,13 +336,15 @@ class WallTimer : public GenericTimer * \param period The interval at which the timer fires * \param callback The callback function to execute every interval * \param context node context + * \param autostart timer state on initialization */ WallTimer( std::chrono::nanoseconds period, FunctorT && callback, - rclcpp::Context::SharedPtr context) + rclcpp::Context::SharedPtr context, + bool autostart = true) : GenericTimer( - std::make_shared(RCL_STEADY_TIME), period, std::move(callback), context) + std::make_shared(RCL_STEADY_TIME), period, std::move(callback), context, autostart) {} protected: diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 59ddd5e8dd..0dceb6b8d7 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -32,7 +32,8 @@ using rclcpp::TimerBase; TimerBase::TimerBase( rclcpp::Clock::SharedPtr clock, std::chrono::nanoseconds period, - rclcpp::Context::SharedPtr context) + rclcpp::Context::SharedPtr context, + bool autostart) : clock_(clock), timer_handle_(nullptr) { if (nullptr == context) { @@ -64,9 +65,9 @@ TimerBase::TimerBase( rcl_clock_t * clock_handle = clock_->get_clock_handle(); { std::lock_guard clock_guard(clock_->get_clock_mutex()); - rcl_ret_t ret = rcl_timer_init( - timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), nullptr, - rcl_get_default_allocator()); + rcl_ret_t ret = rcl_timer_init2( + timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), + nullptr, rcl_get_default_allocator(), autostart); if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't initialize rcl timer handle"); } diff --git a/rclcpp/test/rclcpp/test_create_timer.cpp b/rclcpp/test/rclcpp/test_create_timer.cpp index f253af4838..c4dc1f7e61 100644 --- a/rclcpp/test/rclcpp/test_create_timer.cpp +++ b/rclcpp/test/rclcpp/test_create_timer.cpp @@ -193,3 +193,29 @@ TEST(TestCreateTimer, timer_function_pointer) rclcpp::shutdown(); } + +TEST(TestCreateTimer, timer_without_autostart) +{ + rclcpp::init(0, nullptr); + auto node = std::make_shared("test_create_timer_node"); + + rclcpp::TimerBase::SharedPtr timer; + timer = rclcpp::create_timer( + node, + node->get_clock(), + rclcpp::Duration(0ms), + []() {}, + nullptr, + false); + + EXPECT_TRUE(timer->is_canceled()); + EXPECT_EQ(timer->time_until_trigger().count(), std::chrono::nanoseconds::max().count()); + + timer->reset(); + EXPECT_LE(timer->time_until_trigger().count(), std::chrono::nanoseconds::max().count()); + EXPECT_FALSE(timer->is_canceled()); + + timer->cancel(); + + rclcpp::shutdown(); +} diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index 59a1218519..33ded455c3 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -73,6 +73,20 @@ class TestTimer : public ::testing::TestWithParam EXPECT_FALSE(timer->is_steady()); break; } + timer_without_autostart = test_node->create_wall_timer( + 100ms, + [this]() -> void + { + this->has_timer_run.store(true); + + if (this->cancel_timer.load()) { + this->timer->cancel(); + } + // prevent any tests running timer from blocking + this->executor->cancel(); + }, nullptr, false); + EXPECT_TRUE(timer_without_autostart->is_steady()); + executor->add_node(test_node); // don't start spinning, let the test dictate when } @@ -93,6 +107,7 @@ class TestTimer : public ::testing::TestWithParam std::atomic cancel_timer; rclcpp::Node::SharedPtr test_node; std::shared_ptr timer; + std::shared_ptr timer_without_autostart; std::shared_ptr executor; }; @@ -334,3 +349,18 @@ INSTANTIATE_TEST_SUITE_P( return std::string("unknown"); } ); + +/// Simple test of a timer without autostart +TEST_P(TestTimer, test_timer_without_autostart) +{ + EXPECT_TRUE(timer_without_autostart->is_canceled()); + EXPECT_EQ( + timer_without_autostart->time_until_trigger().count(), + std::chrono::nanoseconds::max().count()); + // Reset to change start timer + timer_without_autostart->reset(); + EXPECT_LE( + timer_without_autostart->time_until_trigger().count(), + std::chrono::nanoseconds::max().count()); + EXPECT_FALSE(timer_without_autostart->is_canceled()); +} From fe2e0e4c646545625ad9f82e929be651b3a5fd95 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Thu, 22 Jun 2023 08:02:27 -0700 Subject: [PATCH 243/455] warning: comparison of integer expressions of different signedness (#2219) https://github.com/ros2/rclcpp/pull/2167#issuecomment-1597197552 Signed-off-by: Tomoya Fujita --- rclcpp/test/rclcpp/executors/test_executors.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index fe509511e8..0bd0bfcd49 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -738,7 +738,7 @@ class TestIntraprocessExecutors : public ::testing::Test test_name << test_info->test_case_name() << "_" << test_info->name(); node = std::make_shared("node", test_name.str()); - callback_count = 0; + callback_count = 0u; const std::string topic_name = std::string("topic_") + test_name.str(); @@ -747,7 +747,7 @@ class TestIntraprocessExecutors : public ::testing::Test publisher = node->create_publisher(topic_name, rclcpp::QoS(1), po); auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) { - this->callback_count.fetch_add(1); + this->callback_count.fetch_add(1u); }; rclcpp::SubscriptionOptions so; @@ -769,7 +769,7 @@ class TestIntraprocessExecutors : public ::testing::Test rclcpp::Node::SharedPtr node; rclcpp::Publisher::SharedPtr publisher; rclcpp::Subscription::SharedPtr subscription; - std::atomic_int callback_count; + std::atomic_size_t callback_count; }; TYPED_TEST_SUITE(TestIntraprocessExecutors, ExecutorTypes, ExecutorTypeNames); @@ -785,7 +785,7 @@ TYPED_TEST(TestIntraprocessExecutors, testIntraprocessRetrigger) { ExecutorType executor; executor.add_node(this->node); - EXPECT_EQ(0, this->callback_count.load()); + EXPECT_EQ(0u, this->callback_count.load()); this->publisher->publish(test_msgs::msg::Empty()); // Wait for up to 5 seconds for the first message to come available. @@ -799,7 +799,7 @@ TYPED_TEST(TestIntraprocessExecutors, testIntraprocessRetrigger) { EXPECT_EQ(1u, this->callback_count.load()); // reset counter - this->callback_count.store(0); + this->callback_count.store(0u); for (size_t ii = 0; ii < kNumMessages; ++ii) { this->publisher->publish(test_msgs::msg::Empty()); From 2e355e48498d7610534e590992c4dcff55a57f22 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 26 Jun 2023 12:59:56 -0400 Subject: [PATCH 244/455] Revamp the test_subscription.cpp tests. (#2227) The original motiviation to do this was a crash during teardown when using a newer version of gtest. But while I was in here, I did a small overall cleanup, including: 1. Moving code closer to where it is actually used. 2. Getting rid of unused 'using' statements. 3. Adding in missing includes. 4. Properly tearing down and recreating the rclcpp context during test teardown (this fixed the actual bug). 5. Making class members private where possible. 6. Renaming class methods to our usual conventions. Signed-off-by: Chris Lalancette --- rclcpp/test/rclcpp/test_subscription.cpp | 434 +++++++++++------------ 1 file changed, 213 insertions(+), 221 deletions(-) diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index 9f9a7ff62d..867171848c 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -17,6 +17,8 @@ #include #include #include +#include +#include #include #include #include @@ -34,53 +36,7 @@ using namespace std::chrono_literals; class TestSubscription : public ::testing::Test { public: - void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) - { - (void)msg; - } - - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - -protected: - void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) - { - node = std::make_shared("test_subscription", "/ns", node_options); - } - - void TearDown() - { - node.reset(); - } - - rclcpp::Node::SharedPtr node; -}; - -struct TestParameters -{ - TestParameters(rclcpp::QoS qos, std::string description) - : qos(qos), description(description) {} - rclcpp::QoS qos; - std::string description; -}; - -std::ostream & operator<<(std::ostream & out, const TestParameters & params) -{ - out << params.description; - return out; -} - -class TestSubscriptionInvalidIntraprocessQos - : public TestSubscription, - public ::testing::WithParamInterface -{}; - -class TestSubscriptionSub : public ::testing::Test -{ -public: - void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) + void on_message(test_msgs::msg::Empty::ConstSharedPtr msg) { (void)msg; } @@ -88,60 +44,20 @@ class TestSubscriptionSub : public ::testing::Test protected: static void SetUpTestCase() { + rclcpp::init(0, nullptr); } - void SetUp() - { - node = std::make_shared("test_subscription", "/ns"); - subnode = node->create_sub_node("sub_ns"); - } - - void TearDown() - { - node.reset(); - } - - rclcpp::Node::SharedPtr node; - rclcpp::Node::SharedPtr subnode; -}; - -class SubscriptionClassNodeInheritance : public rclcpp::Node -{ -public: - SubscriptionClassNodeInheritance() - : Node("subscription_class_node_inheritance") - { - } - - void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) - { - (void)msg; - } - - void CreateSubscription() + static void TearDownTestCase() { - auto callback = std::bind( - &SubscriptionClassNodeInheritance::OnMessage, this, std::placeholders::_1); - using test_msgs::msg::Empty; - auto sub = this->create_subscription("topic", 10, callback); + rclcpp::shutdown(); } -}; -class SubscriptionClass -{ -public: - void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) + void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) { - (void)msg; + node_ = std::make_shared("test_subscription", "/ns", node_options); } - void CreateSubscription() - { - auto node = std::make_shared("test_subscription_member_callback", "/ns"); - auto callback = std::bind(&SubscriptionClass::OnMessage, this, std::placeholders::_1); - using test_msgs::msg::Empty; - auto sub = node->create_subscription("topic", 10, callback); - } + rclcpp::Node::SharedPtr node_; }; /* @@ -155,7 +71,7 @@ TEST_F(TestSubscription, construction_and_destruction) { }; { constexpr size_t depth = 10u; - auto sub = node->create_subscription("topic", depth, callback); + auto sub = node_->create_subscription("topic", depth, callback); EXPECT_NE(nullptr, sub->get_subscription_handle()); // Converting to base class was necessary for the compiler to choose the const version of @@ -172,40 +88,7 @@ TEST_F(TestSubscription, construction_and_destruction) { { ASSERT_THROW( { - auto sub = node->create_subscription("invalid_topic?", 10, callback); - }, rclcpp::exceptions::InvalidTopicNameError); - } -} - -/* - Testing subscription construction and destruction for subnodes. - */ -TEST_F(TestSubscriptionSub, construction_and_destruction) { - using test_msgs::msg::Empty; - auto callback = [](Empty::ConstSharedPtr msg) { - (void)msg; - }; - { - auto sub = subnode->create_subscription("topic", 1, callback); - EXPECT_STREQ(sub->get_topic_name(), "/ns/sub_ns/topic"); - } - - { - auto sub = subnode->create_subscription("/topic", 1, callback); - EXPECT_STREQ(sub->get_topic_name(), "/topic"); - } - - { - auto sub = subnode->create_subscription("~/topic", 1, callback); - std::string expected_topic_name = - std::string(node->get_namespace()) + "/" + node->get_name() + "/topic"; - EXPECT_STREQ(sub->get_topic_name(), expected_topic_name.c_str()); - } - - { - ASSERT_THROW( - { - auto sub = node->create_subscription("invalid_topic?", 1, callback); + auto sub = node_->create_subscription("invalid_topic?", 10, callback); }, rclcpp::exceptions::InvalidTopicNameError); } } @@ -218,31 +101,31 @@ TEST_F(TestSubscription, various_creation_signatures) { using test_msgs::msg::Empty; auto cb = [](test_msgs::msg::Empty::ConstSharedPtr) {}; { - auto sub = node->create_subscription("topic", 1, cb); + auto sub = node_->create_subscription("topic", 1, cb); (void)sub; } { - auto sub = node->create_subscription("topic", rclcpp::QoS(1), cb); + auto sub = node_->create_subscription("topic", rclcpp::QoS(1), cb); (void)sub; } { auto sub = - node->create_subscription("topic", rclcpp::QoS(rclcpp::KeepLast(1)), cb); + node_->create_subscription("topic", rclcpp::QoS(rclcpp::KeepLast(1)), cb); (void)sub; } { auto sub = - node->create_subscription("topic", rclcpp::QoS(rclcpp::KeepAll()), cb); + node_->create_subscription("topic", rclcpp::QoS(rclcpp::KeepAll()), cb); (void)sub; } { - auto sub = node->create_subscription( + auto sub = node_->create_subscription( "topic", 42, cb, rclcpp::SubscriptionOptions()); (void)sub; } { auto sub = rclcpp::create_subscription( - node, "topic", 42, cb, rclcpp::SubscriptionOptions()); + node_, "topic", 42, cb, rclcpp::SubscriptionOptions()); (void)sub; } { @@ -250,40 +133,78 @@ TEST_F(TestSubscription, various_creation_signatures) { options.allocator = std::make_shared>(); EXPECT_NE(nullptr, options.get_allocator()); auto sub = rclcpp::create_subscription( - node, "topic", 42, cb, options); + node_, "topic", 42, cb, options); (void)sub; } { rclcpp::SubscriptionOptionsBase options_base; rclcpp::SubscriptionOptionsWithAllocator> options(options_base); auto sub = rclcpp::create_subscription( - node, "topic", 42, cb, options); + node_, "topic", 42, cb, options); (void)sub; } } +class SubscriptionClass final +{ +public: + void custom_create_subscription() + { + auto node = std::make_shared("test_subscription_member_callback", "/ns"); + auto callback = std::bind(&SubscriptionClass::on_message, this, std::placeholders::_1); + auto sub = node->create_subscription("topic", 10, callback); + } + +private: + void on_message(test_msgs::msg::Empty::ConstSharedPtr msg) + { + (void)msg; + } +}; + +class SubscriptionClassNodeInheritance final : public rclcpp::Node +{ +public: + SubscriptionClassNodeInheritance() + : Node("subscription_class_node_inheritance") + { + } + + void custom_create_subscription() + { + auto callback = std::bind( + &SubscriptionClassNodeInheritance::on_message, this, std::placeholders::_1); + auto sub = this->create_subscription("topic", 10, callback); + } + +private: + void on_message(test_msgs::msg::Empty::ConstSharedPtr msg) + { + (void)msg; + } +}; + /* Testing subscriptions using std::bind. */ TEST_F(TestSubscription, callback_bind) { initialize(); - using test_msgs::msg::Empty; { // Member callback for plain class SubscriptionClass subscription_object; - subscription_object.CreateSubscription(); + subscription_object.custom_create_subscription(); } { // Member callback for class inheriting from rclcpp::Node SubscriptionClassNodeInheritance subscription_object; - subscription_object.CreateSubscription(); + subscription_object.custom_create_subscription(); } { // Member callback for class inheriting from testing::Test // Regression test for https://github.com/ros2/rclcpp/issues/479 where the TEST_F GTest macro // was interfering with rclcpp's `function_traits`. - auto callback = std::bind(&TestSubscription::OnMessage, this, std::placeholders::_1); - auto sub = node->create_subscription("topic", 1, callback); + auto callback = std::bind(&TestSubscription::on_message, this, std::placeholders::_1); + auto sub = node_->create_subscription("topic", 1, callback); } } @@ -292,10 +213,9 @@ TEST_F(TestSubscription, callback_bind) { */ TEST_F(TestSubscription, take) { initialize(); - 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", 1, do_nothing); test_msgs::msg::Empty msg; rclcpp::MessageInfo msg_info; EXPECT_FALSE(sub->take(msg, msg_info)); @@ -303,10 +223,10 @@ TEST_F(TestSubscription, take) { { rclcpp::SubscriptionOptions so; so.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; - auto sub = node->create_subscription("~/test_take", 1, do_nothing, so); + auto sub = node_->create_subscription("~/test_take", 1, do_nothing, so); rclcpp::PublisherOptions po; po.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; - auto pub = node->create_publisher("~/test_take", 1, po); + auto pub = node_->create_publisher("~/test_take", 1, po); { test_msgs::msg::Empty msg; pub->publish(msg); @@ -329,10 +249,9 @@ TEST_F(TestSubscription, take) { */ TEST_F(TestSubscription, take_serialized) { initialize(); - 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", 1, do_nothing); std::shared_ptr msg = sub->create_serialized_message(); rclcpp::MessageInfo msg_info; EXPECT_FALSE(sub->take_serialized(*msg, msg_info)); @@ -340,10 +259,10 @@ TEST_F(TestSubscription, take_serialized) { { rclcpp::SubscriptionOptions so; so.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; - auto sub = node->create_subscription("~/test_take", 1, do_nothing, so); + auto sub = node_->create_subscription("~/test_take", 1, do_nothing, so); rclcpp::PublisherOptions po; po.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; - auto pub = node->create_publisher("~/test_take", 1, po); + auto pub = node_->create_publisher("~/test_take", 1, po); { test_msgs::msg::Empty msg; pub->publish(msg); @@ -368,7 +287,7 @@ TEST_F(TestSubscription, rcl_subscription_init_error) { // reset() is not needed for triggering exception, just to avoid an unused return value warning EXPECT_THROW( - node->create_subscription("topic", 10, callback).reset(), + node_->create_subscription("topic", 10, callback).reset(), rclcpp::exceptions::RCLError); } @@ -380,7 +299,7 @@ TEST_F(TestSubscription, rcl_subscription_fini_error) { // Cleanup just fails, no exception expected EXPECT_NO_THROW( - node->create_subscription("topic", 10, callback).reset()); + node_->create_subscription("topic", 10, callback).reset()); } TEST_F(TestSubscription, rcl_subscription_get_actual_qos_error) { @@ -389,7 +308,7 @@ TEST_F(TestSubscription, rcl_subscription_get_actual_qos_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_subscription_get_actual_qos, nullptr); - auto sub = node->create_subscription("topic", 10, callback); + auto sub = node_->create_subscription("topic", 10, callback); RCLCPP_EXPECT_THROW_EQ( sub->get_actual_qos(), std::runtime_error("failed to get qos settings: error not set")); } @@ -400,7 +319,7 @@ TEST_F(TestSubscription, rcl_take_type_erased_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_take, RCL_RET_ERROR); - auto sub = node->create_subscription("topic", 10, callback); + auto sub = node_->create_subscription("topic", 10, callback); test_msgs::msg::Empty msg; rclcpp::MessageInfo message_info; @@ -413,7 +332,7 @@ TEST_F(TestSubscription, rcl_take_serialized_message_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_take_serialized_message, RCL_RET_ERROR); - auto sub = node->create_subscription("topic", 10, callback); + auto sub = node_->create_subscription("topic", 10, callback); rclcpp::SerializedMessage msg; rclcpp::MessageInfo message_info; @@ -426,14 +345,14 @@ TEST_F(TestSubscription, rcl_subscription_get_publisher_count_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_subscription_get_publisher_count, RCL_RET_ERROR); - auto sub = node->create_subscription("topic", 10, callback); + auto sub = node_->create_subscription("topic", 10, callback); EXPECT_THROW(sub->get_publisher_count(), rclcpp::exceptions::RCLError); } TEST_F(TestSubscription, handle_loaned_message) { initialize(); auto callback = [](std::shared_ptr) {}; - auto sub = node->create_subscription("topic", 10, callback); + auto sub = node_->create_subscription("topic", 10, callback); test_msgs::msg::Empty msg; rclcpp::MessageInfo message_info; @@ -448,13 +367,13 @@ 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", 10, 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;}; sub->set_on_new_message_callback(increase_c1_cb); - auto pub = node->create_publisher("~/test_take", 3); + auto pub = node_->create_publisher("~/test_take", 3); { test_msgs::msg::Empty msg; pub->publish(msg); @@ -518,13 +437,13 @@ 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", 10, 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;}; sub->set_on_new_intra_process_message_callback(increase_c1_cb); - auto pub = node->create_publisher("~/test_take", 1); + auto pub = node_->create_publisher("~/test_take", 1); { test_msgs::msg::Empty msg; pub->publish(msg); @@ -580,21 +499,150 @@ TEST_F(TestSubscription, on_new_intra_process_message_callback) { EXPECT_THROW(sub->set_on_new_intra_process_message_callback(invalid_cb), std::invalid_argument); } +TEST_F(TestSubscription, get_network_flow_endpoints_errors) { + initialize(); + const rclcpp::QoS subscription_qos(1); + auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr msg) { + (void)msg; + }; + auto subscription = node_->create_subscription( + "topic", subscription_qos, subscription_callback); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_get_network_flow_endpoints, RCL_RET_ERROR); + auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_ERROR); + EXPECT_THROW( + subscription->get_network_flow_endpoints(), + rclcpp::exceptions::RCLError); + } + { + auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_ERROR); + EXPECT_THROW( + subscription->get_network_flow_endpoints(), + rclcpp::exceptions::RCLError); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_get_network_flow_endpoints, RCL_RET_OK); + auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_OK); + EXPECT_NO_THROW(subscription->get_network_flow_endpoints()); + } +} + +class TestSubscriptionSub : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node_ = std::make_shared("test_subscription", "/ns"); + subnode_ = node_->create_sub_node("sub_ns"); + } + + rclcpp::Node::SharedPtr node_; + rclcpp::Node::SharedPtr subnode_; +}; + +/* + Testing subscription construction and destruction for subnodes. + */ +TEST_F(TestSubscriptionSub, construction_and_destruction) { + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr msg) { + (void)msg; + }; + { + auto sub = subnode_->create_subscription("topic", 1, callback); + EXPECT_STREQ(sub->get_topic_name(), "/ns/sub_ns/topic"); + } + + { + auto sub = subnode_->create_subscription("/topic", 1, callback); + EXPECT_STREQ(sub->get_topic_name(), "/topic"); + } + + { + auto sub = subnode_->create_subscription("~/topic", 1, callback); + std::string expected_topic_name = + std::string(node_->get_namespace()) + "/" + node_->get_name() + "/topic"; + EXPECT_STREQ(sub->get_topic_name(), expected_topic_name.c_str()); + } + + { + ASSERT_THROW( + { + auto sub = node_->create_subscription("invalid_topic?", 1, callback); + }, rclcpp::exceptions::InvalidTopicNameError); + } +} + +struct TestParameters final +{ + TestParameters(rclcpp::QoS qos, std::string description) + : qos(qos), description(description) {} + rclcpp::QoS qos; + std::string description; +}; + +std::ostream & operator<<(std::ostream & out, const TestParameters & params) +{ + out << params.description; + return out; +} + +class TestSubscriptionInvalidIntraprocessQos + : public TestSubscription, + public ::testing::WithParamInterface +{}; + +static std::vector invalid_qos_profiles() +{ + std::vector parameters; + + parameters.reserve(3); + parameters.push_back( + TestParameters( + rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(), + "transient_local_qos")); + parameters.push_back( + TestParameters( + rclcpp::QoS(rclcpp::KeepAll()), + "keep_all_qos")); + + return parameters; +} + +INSTANTIATE_TEST_SUITE_P( + TestSubscriptionThrows, TestSubscriptionInvalidIntraprocessQos, + ::testing::ValuesIn(invalid_qos_profiles()), + ::testing::PrintToStringParamName()); + /* Testing subscription with intraprocess enabled and invalid QoS */ TEST_P(TestSubscriptionInvalidIntraprocessQos, test_subscription_throws) { initialize(rclcpp::NodeOptions().use_intra_process_comms(true)); rclcpp::QoS qos = GetParam().qos; - using test_msgs::msg::Empty; { auto callback = std::bind( - &TestSubscriptionInvalidIntraprocessQos::OnMessage, + &TestSubscriptionInvalidIntraprocessQos::on_message, this, std::placeholders::_1); ASSERT_THROW( - {auto subscription = node->create_subscription( + {auto subscription = node_->create_subscription( "topic", qos, callback);}, @@ -612,71 +660,15 @@ TEST_P(TestSubscriptionInvalidIntraprocessQos, test_subscription_throws_intrapro initialize(); rclcpp::QoS qos = GetParam().qos; auto callback = std::bind( - &TestSubscriptionInvalidIntraprocessQos::OnMessage, + &TestSubscriptionInvalidIntraprocessQos::on_message, this, std::placeholders::_1); RCLCPP_EXPECT_THROW_EQ( - {auto subscription = node->create_subscription( + {auto subscription = node_->create_subscription( "topic", qos, callback, options);}, std::runtime_error("Unrecognized IntraProcessSetting value")); } - -static std::vector invalid_qos_profiles() -{ - std::vector parameters; - - parameters.reserve(3); - parameters.push_back( - TestParameters( - rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(), - "transient_local_qos")); - parameters.push_back( - TestParameters( - rclcpp::QoS(rclcpp::KeepAll()), - "keep_all_qos")); - - return parameters; -} - -INSTANTIATE_TEST_SUITE_P( - TestSubscriptionThrows, TestSubscriptionInvalidIntraprocessQos, - ::testing::ValuesIn(invalid_qos_profiles()), - ::testing::PrintToStringParamName()); - -TEST_F(TestSubscription, get_network_flow_endpoints_errors) { - initialize(); - const rclcpp::QoS subscription_qos(1); - auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr msg) { - (void)msg; - }; - auto subscription = node->create_subscription( - "topic", subscription_qos, subscription_callback); - - { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_subscription_get_network_flow_endpoints, RCL_RET_ERROR); - auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_ERROR); - EXPECT_THROW( - subscription->get_network_flow_endpoints(), - rclcpp::exceptions::RCLError); - } - { - auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_ERROR); - EXPECT_THROW( - subscription->get_network_flow_endpoints(), - rclcpp::exceptions::RCLError); - } - { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_subscription_get_network_flow_endpoints, RCL_RET_OK); - auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_OK); - EXPECT_NO_THROW(subscription->get_network_flow_endpoints()); - } -} From 588dba7a70d5953b4133894cbb1316b0e1c21b65 Mon Sep 17 00:00:00 2001 From: Nathan Wiebe Neufeldt Date: Wed, 5 Jul 2023 16:55:11 -0400 Subject: [PATCH 245/455] Move always_false_v to detail namespace (#2232) Since this is a common idiom, especially under this name, we should define the `always_false_v` template within a namespace to avoid conflict with other libraries and user code. This could either be `rclcpp::detail` if it's intended only for internal use or just `rclcpp` if it's intended as a public helper. In this PR, I've initially chosen the former. Signed-off-by: Nathan Wiebe Neufeldt --- .../include/rclcpp/any_subscription_callback.hpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 2d8795934e..4fd912fd10 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -34,15 +34,15 @@ #include "rclcpp/type_adapter.hpp" -template -inline constexpr bool always_false_v = false; - namespace rclcpp { namespace detail { +template +inline constexpr bool always_false_v = false; + template struct MessageDeleterHelper { @@ -580,7 +580,7 @@ class AnySubscriptionCallback } // condition to catch unhandled callback types else { // NOLINT[readability/braces] - static_assert(always_false_v, "unhandled callback type"); + static_assert(detail::always_false_v, "unhandled callback type"); } }, callback_variant_); TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); @@ -660,7 +660,7 @@ class AnySubscriptionCallback } // condition to catch unhandled callback types else { // NOLINT[readability/braces] - static_assert(always_false_v, "unhandled callback type"); + static_assert(detail::always_false_v, "unhandled callback type"); } }, callback_variant_); TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); @@ -790,7 +790,7 @@ class AnySubscriptionCallback } // condition to catch unhandled callback types else { // NOLINT[readability/braces] - static_assert(always_false_v, "unhandled callback type"); + static_assert(detail::always_false_v, "unhandled callback type"); } }, callback_variant_); TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); @@ -924,7 +924,7 @@ class AnySubscriptionCallback } // condition to catch unhandled callback types else { // NOLINT[readability/braces] - static_assert(always_false_v, "unhandled callback type"); + static_assert(detail::always_false_v, "unhandled callback type"); } }, callback_variant_); TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); From deebbc3ad693bb66a77da8bf4c3c8e9c7784c13c Mon Sep 17 00:00:00 2001 From: Emerson Knapp <537409+emersonknapp@users.noreply.github.com> Date: Fri, 7 Jul 2023 10:10:27 -0700 Subject: [PATCH 246/455] Add new node interface TypeDescriptionsInterface to provide GetTypeDescription service (#2224) * TypeDescriptions interface with readonly param configuration * Add parameter descriptor, to make read only * example of spinning in thread for get_type_description service * Add a basic test for the new interface * Fix tests with new parameter * Add comments about builtin parameters Signed-off-by: Emerson Knapp Signed-off-by: William Woodall --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/node.hpp | 7 + .../node_interfaces/node_interfaces.hpp | 2 + .../node_type_descriptions.hpp | 63 +++++++ .../node_type_descriptions_interface.hpp | 44 +++++ rclcpp/src/rclcpp/node.cpp | 13 ++ .../src/rclcpp/node_interfaces/node_base.cpp | 1 + .../node_type_descriptions.cpp | 157 ++++++++++++++++++ rclcpp/test/rclcpp/CMakeLists.txt | 5 + .../node_interfaces/test_node_parameters.cpp | 4 +- .../test_node_type_descriptions.cpp | 63 +++++++ rclcpp/test/rclcpp/test_parameter_client.cpp | 14 +- .../rclcpp_lifecycle/lifecycle_node.hpp | 6 + rclcpp_lifecycle/src/lifecycle_node.cpp | 7 + rclcpp_lifecycle/test/test_lifecycle_node.cpp | 30 ++-- 15 files changed, 400 insertions(+), 17 deletions(-) create mode 100644 rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp create mode 100644 rclcpp/include/rclcpp/node_interfaces/node_type_descriptions_interface.hpp create mode 100644 rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_type_descriptions.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index b2d1023064..bd705be06d 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -92,6 +92,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/node_interfaces/node_time_source.cpp src/rclcpp/node_interfaces/node_timers.cpp src/rclcpp/node_interfaces/node_topics.cpp + src/rclcpp/node_interfaces/node_type_descriptions.cpp src/rclcpp/node_interfaces/node_waitables.cpp src/rclcpp/node_options.cpp src/rclcpp/parameter.cpp diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 556a6d0318..50bd9da6a0 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -56,6 +56,7 @@ #include "rclcpp/node_interfaces/node_time_source_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions_interface.hpp" #include "rclcpp/node_interfaces/node_waitables_interface.hpp" #include "rclcpp/node_options.hpp" #include "rclcpp/parameter.hpp" @@ -1456,6 +1457,11 @@ class Node : public std::enable_shared_from_this rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr get_node_time_source_interface(); + /// Return the Node's internal NodeTypeDescriptionsInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr + get_node_type_descriptions_interface(); + /// Return the sub-namespace, if this is a sub-node, otherwise an empty string. /** * The returned sub-namespace is either the accumulated sub-namespaces which @@ -1588,6 +1594,7 @@ class Node : public std::enable_shared_from_this rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_; + rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr node_type_descriptions_; rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_; const rclcpp::NodeOptions node_options_; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp index bb4886d592..a7c7b7af96 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp @@ -30,6 +30,7 @@ rclcpp::node_interfaces::NodeTimeSourceInterface, \ rclcpp::node_interfaces::NodeTimersInterface, \ rclcpp::node_interfaces::NodeTopicsInterface, \ + rclcpp::node_interfaces::NodeTypeDescriptionsInterface, \ rclcpp::node_interfaces::NodeWaitablesInterface @@ -118,6 +119,7 @@ class NodeInterfaces * - rclcpp::node_interfaces::NodeTimeSourceInterface * - rclcpp::node_interfaces::NodeTimersInterface * - rclcpp::node_interfaces::NodeTopicsInterface + * - rclcpp::node_interfaces::NodeTypeDescriptionsInterface * - rclcpp::node_interfaces::NodeWaitablesInterface * * Or you use custom interfaces as long as you make a template specialization diff --git a/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp new file mode 100644 index 0000000000..8aa563bba2 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp @@ -0,0 +1,63 @@ +// Copyright 2023 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__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_ + +#include + +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions_interface.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Implementation of the NodeTypeDescriptions part of the Node API. +class NodeTypeDescriptions : public NodeTypeDescriptionsInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTypeDescriptions) + + RCLCPP_PUBLIC + explicit NodeTypeDescriptions( + NodeBaseInterface::SharedPtr node_base, + NodeLoggingInterface::SharedPtr node_logging, + NodeParametersInterface::SharedPtr node_parameters, + NodeServicesInterface::SharedPtr node_services); + + RCLCPP_PUBLIC + virtual + ~NodeTypeDescriptions(); + +private: + RCLCPP_DISABLE_COPY(NodeTypeDescriptions) + + // Pimpl hides helper types and functions used for wrapping a C service, which would be + // awkward to expose in this header. + class NodeTypeDescriptionsImpl; + std::unique_ptr impl_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions_interface.hpp new file mode 100644 index 0000000000..e7e0b0af2e --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions_interface.hpp @@ -0,0 +1,44 @@ +// Copyright 2023 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__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_ + +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeTypeDescriptions part of the Node API. +class NodeTypeDescriptionsInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTypeDescriptionsInterface) + + RCLCPP_PUBLIC + virtual + ~NodeTypeDescriptionsInterface() = default; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT( + rclcpp::node_interfaces::NodeTypeDescriptionsInterface, type_descriptions) + +#endif // RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_ diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 53e77dd796..52012439e8 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -36,6 +36,7 @@ #include "rclcpp/node_interfaces/node_time_source.hpp" #include "rclcpp/node_interfaces/node_timers.hpp" #include "rclcpp/node_interfaces/node_topics.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions.hpp" #include "rclcpp/node_interfaces/node_waitables.hpp" #include "rclcpp/qos_overriding_options.hpp" @@ -206,6 +207,12 @@ Node::Node( options.clock_qos(), options.use_clock_thread() )), + node_type_descriptions_(new rclcpp::node_interfaces::NodeTypeDescriptions( + node_base_, + node_logging_, + node_parameters_, + node_services_ + )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), node_options_(options), sub_namespace_(""), @@ -591,6 +598,12 @@ Node::get_node_topics_interface() return node_topics_; } +rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr +Node::get_node_type_descriptions_interface() +{ + return node_type_descriptions_; +} + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr Node::get_node_services_interface() { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 6544d69214..36e1afb932 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -20,6 +20,7 @@ #include "rclcpp/node_interfaces/node_base.hpp" #include "rcl/arguments.h" +#include "rcl/node_type_cache.h" #include "rclcpp/exceptions.hpp" #include "rcutils/logging_macros.h" #include "rmw/validate_namespace.h" diff --git a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp new file mode 100644 index 0000000000..f4b5e20d30 --- /dev/null +++ b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp @@ -0,0 +1,157 @@ +// Copyright 2023 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. + +#include +#include +#include + +#include "rclcpp/node_interfaces/node_type_descriptions.hpp" +#include "rclcpp/parameter_client.hpp" + +#include "type_description_interfaces/srv/get_type_description.h" + +namespace +{ +// Helper wrapper for rclcpp::Service to access ::Request and ::Response types for allocation. +struct GetTypeDescription__C +{ + using Request = type_description_interfaces__srv__GetTypeDescription_Request; + using Response = type_description_interfaces__srv__GetTypeDescription_Response; + using Event = type_description_interfaces__srv__GetTypeDescription_Event; +}; +} // namespace + +// Helper function for C typesupport. +namespace rosidl_typesupport_cpp +{ +template<> +rosidl_service_type_support_t const * +get_service_type_support_handle() +{ + return ROSIDL_GET_SRV_TYPE_SUPPORT(type_description_interfaces, srv, GetTypeDescription); +} +} // namespace rosidl_typesupport_cpp + +namespace rclcpp +{ +namespace node_interfaces +{ + +class NodeTypeDescriptions::NodeTypeDescriptionsImpl +{ +public: + using ServiceT = GetTypeDescription__C; + + rclcpp::Logger logger_; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + rclcpp::Service::SharedPtr type_description_srv_; + + NodeTypeDescriptionsImpl( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services) + : logger_(node_logging->get_logger()), + node_base_(node_base) + { + const std::string enable_param_name = "start_type_description_service"; + + bool enabled = false; + try { + auto enable_param = node_parameters->declare_parameter( + enable_param_name, + rclcpp::ParameterValue(true), + rcl_interfaces::msg::ParameterDescriptor() + .set__name(enable_param_name) + .set__type(rclcpp::PARAMETER_BOOL) + .set__description("Start the ~/get_type_description service for this node.") + .set__read_only(true)); + enabled = enable_param.get(); + } catch (const rclcpp::exceptions::InvalidParameterTypeException & exc) { + RCLCPP_ERROR(logger_, "%s", exc.what()); + throw; + } + + if (enabled) { + auto rcl_node = node_base->get_rcl_node_handle(); + rcl_ret_t rcl_ret = rcl_node_type_description_service_init(rcl_node); + if (rcl_ret != RCL_RET_OK) { + RCLCPP_ERROR( + logger_, "Failed to initialize ~/get_type_description_service: %s", + rcl_get_error_string().str); + throw std::runtime_error( + "Failed to initialize ~/get_type_description service."); + } + + rcl_service_t * rcl_srv = nullptr; + rcl_ret = rcl_node_get_type_description_service(rcl_node, &rcl_srv); + if (rcl_ret != RCL_RET_OK) { + throw std::runtime_error( + "Failed to get initialized ~/get_type_description service from rcl."); + } + + rclcpp::AnyServiceCallback cb; + cb.set( + [this]( + std::shared_ptr header, + std::shared_ptr request, + std::shared_ptr response + ) { + rcl_node_type_description_service_handle_request( + node_base_->get_rcl_node_handle(), + header.get(), + request.get(), + response.get()); + }); + + type_description_srv_ = std::make_shared>( + node_base_->get_shared_rcl_node_handle(), + rcl_srv, + cb); + node_services->add_service( + std::dynamic_pointer_cast(type_description_srv_), + nullptr); + } + } + + ~NodeTypeDescriptionsImpl() + { + if ( + type_description_srv_ && + RCL_RET_OK != rcl_node_type_description_service_fini(node_base_->get_rcl_node_handle())) + { + RCLCPP_ERROR( + logger_, + "Error in shutdown of get_type_description service: %s", rcl_get_error_string().str); + } + } +}; + +NodeTypeDescriptions::NodeTypeDescriptions( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services) +: impl_(new NodeTypeDescriptionsImpl( + node_base, + node_logging, + node_parameters, + node_services)) +{} + +NodeTypeDescriptions::~NodeTypeDescriptions() +{} + +} // namespace node_interfaces +} // namespace rclcpp diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index dcb1d36d8e..8c31a95415 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -262,6 +262,11 @@ if(TARGET test_node_interfaces__node_topics) "test_msgs") target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME} mimick) endif() +ament_add_gtest(test_node_interfaces__node_type_descriptions + node_interfaces/test_node_type_descriptions.cpp) +if(TARGET test_node_interfaces__node_type_descriptions) + target_link_libraries(test_node_interfaces__node_type_descriptions ${PROJECT_NAME} mimick) +endif() ament_add_gtest(test_node_interfaces__node_waitables node_interfaces/test_node_waitables.cpp) if(TARGET test_node_interfaces__node_waitables) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp index 58bf010767..9113c96ca5 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp @@ -77,9 +77,9 @@ TEST_F(TestNodeParameters, list_parameters) std::vector prefixes; const auto list_result = node_parameters->list_parameters(prefixes, 1u); - // Currently the only default parameter is 'use_sim_time', but that may change. + // Currently the default parameters are 'use_sim_time' and 'start_type_description_service' size_t number_of_parameters = list_result.names.size(); - EXPECT_GE(1u, number_of_parameters); + EXPECT_GE(2u, number_of_parameters); const std::string parameter_name = "new_parameter"; const rclcpp::ParameterValue value(true); diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_type_descriptions.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_type_descriptions.cpp new file mode 100644 index 0000000000..79ef6c3bcf --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_type_descriptions.cpp @@ -0,0 +1,63 @@ +// Copyright 2023 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. + +#include + +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions.hpp" + +class TestNodeTypeDescriptions : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeTypeDescriptions, interface_created) +{ + rclcpp::Node node{"node", "ns"}; + ASSERT_NE(nullptr, node.get_node_type_descriptions_interface()); +} + +TEST_F(TestNodeTypeDescriptions, disabled_no_service) +{ + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("start_type_description_service", false); + rclcpp::Node node{"node", "ns", node_options}; + + rcl_node_t * rcl_node = node.get_node_base_interface()->get_rcl_node_handle(); + rcl_service_t * srv = nullptr; + rcl_ret_t ret = rcl_node_get_type_description_service(rcl_node, &srv); + ASSERT_EQ(RCL_RET_NOT_INIT, ret); +} + +TEST_F(TestNodeTypeDescriptions, enabled_creates_service) +{ + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("start_type_description_service", true); + rclcpp::Node node{"node", "ns", node_options}; + + rcl_node_t * rcl_node = node.get_node_base_interface()->get_rcl_node_handle(); + rcl_service_t * srv = nullptr; + rcl_ret_t ret = rcl_node_get_type_description_service(rcl_node, &srv); + ASSERT_EQ(RCL_RET_OK, ret); + ASSERT_NE(nullptr, srv); +} diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index 2ce414d327..afc48a652b 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -59,6 +59,8 @@ class TestParameterClient : public ::testing::Test node_with_option.reset(); } + // "start_type_description_service" and "use_sim_time" + const uint64_t builtin_param_count = 2; rclcpp::Node::SharedPtr node; rclcpp::Node::SharedPtr node_with_option; }; @@ -925,6 +927,7 @@ TEST_F(TestParameterClient, sync_parameter_delete_parameters) { Coverage for async load_parameters */ TEST_F(TestParameterClient, async_parameter_load_parameters) { + const uint64_t expected_param_count = 4 + builtin_param_count; auto load_node = std::make_shared( "load_node", "namespace", @@ -944,12 +947,13 @@ TEST_F(TestParameterClient, async_parameter_load_parameters) { auto list_parameters = asynchronous_client->list_parameters({}, 3); rclcpp::spin_until_future_complete( load_node, list_parameters, std::chrono::milliseconds(100)); - ASSERT_EQ(list_parameters.get().names.size(), static_cast(5)); + ASSERT_EQ(list_parameters.get().names.size(), expected_param_count); } /* Coverage for sync load_parameters */ TEST_F(TestParameterClient, sync_parameter_load_parameters) { + const uint64_t expected_param_count = 4 + builtin_param_count; auto load_node = std::make_shared( "load_node", "namespace", @@ -964,13 +968,14 @@ TEST_F(TestParameterClient, sync_parameter_load_parameters) { ASSERT_EQ(load_future[0].successful, true); // list parameters auto list_parameters = synchronous_client->list_parameters({}, 3); - ASSERT_EQ(list_parameters.names.size(), static_cast(5)); + ASSERT_EQ(list_parameters.names.size(), static_cast(expected_param_count)); } /* Coverage for async load_parameters with complicated regex expression */ TEST_F(TestParameterClient, async_parameter_load_parameters_complicated_regex) { + const uint64_t expected_param_count = 5 + builtin_param_count; auto load_node = std::make_shared( "load_node", "namespace", @@ -990,7 +995,7 @@ TEST_F(TestParameterClient, async_parameter_load_parameters_complicated_regex) { auto list_parameters = asynchronous_client->list_parameters({}, 3); rclcpp::spin_until_future_complete( load_node, list_parameters, std::chrono::milliseconds(100)); - ASSERT_EQ(list_parameters.get().names.size(), static_cast(6)); + ASSERT_EQ(list_parameters.get().names.size(), expected_param_count); // to check the parameter "a_value" std::string param_name = "a_value"; auto param = load_node->get_parameter(param_name); @@ -1020,6 +1025,7 @@ TEST_F(TestParameterClient, async_parameter_load_no_valid_parameter) { Coverage for async load_parameters from maps with complicated regex expression */ TEST_F(TestParameterClient, async_parameter_load_parameters_from_map) { + const uint64_t expected_param_count = 5 + builtin_param_count; auto load_node = std::make_shared( "load_node", "namespace", @@ -1068,7 +1074,7 @@ TEST_F(TestParameterClient, async_parameter_load_parameters_from_map) { auto list_parameters = asynchronous_client->list_parameters({}, 3); rclcpp::spin_until_future_complete( load_node, list_parameters, std::chrono::milliseconds(100)); - ASSERT_EQ(list_parameters.get().names.size(), static_cast(6)); + ASSERT_EQ(list_parameters.get().names.size(), expected_param_count); // to check the parameter "a_value" std::string param_name = "a_value"; auto param = load_node->get_parameter(param_name); diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 197ecf5c19..d3654ab2b2 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -72,6 +72,7 @@ #include "rclcpp/node_interfaces/node_time_source_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions_interface.hpp" #include "rclcpp/node_interfaces/node_waitables_interface.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/publisher.hpp" @@ -823,6 +824,10 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr get_node_time_source_interface(); + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr + get_node_type_descriptions_interface(); + /// Return the Node's internal NodeWaitablesInterface implementation. /** * \sa rclcpp::Node::get_node_waitables_interface @@ -1085,6 +1090,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_; + rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr node_type_descriptions_; rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_; const rclcpp::NodeOptions node_options_; diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 4c0b94cb42..94629911a3 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -43,6 +43,7 @@ #include "rclcpp/node_interfaces/node_time_source.hpp" #include "rclcpp/node_interfaces/node_timers.hpp" #include "rclcpp/node_interfaces/node_topics.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions.hpp" #include "rclcpp/node_interfaces/node_waitables.hpp" #include "rclcpp/parameter_service.hpp" #include "rclcpp/qos.hpp" @@ -113,6 +114,12 @@ LifecycleNode::LifecycleNode( options.clock_qos(), options.use_clock_thread() )), + node_type_descriptions_(new rclcpp::node_interfaces::NodeTypeDescriptions( + node_base_, + node_logging_, + node_parameters_, + node_services_ + )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), node_options_(options), impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_)) diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 31850c589b..db24bd0ee7 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -427,11 +427,15 @@ TEST_F(TestDefaultStateMachine, lifecycle_subscriber) { // Parameters are tested more thoroughly in rclcpp's test_node.cpp // These are provided for coverage of lifecycle node's API TEST_F(TestDefaultStateMachine, declare_parameters) { + // "start_type_description_service" and "use_sim_time" + const uint64_t builtin_param_count = 2; + const uint64_t expected_param_count = 6 + builtin_param_count; auto test_node = std::make_shared("testnode"); auto list_result = test_node->list_parameters({}, 0u); - EXPECT_EQ(list_result.names.size(), 1u); - EXPECT_STREQ(list_result.names[0].c_str(), "use_sim_time"); + EXPECT_EQ(list_result.names.size(), builtin_param_count); + EXPECT_STREQ(list_result.names[0].c_str(), "start_type_description_service"); + EXPECT_STREQ(list_result.names[1].c_str(), "use_sim_time"); const std::string bool_name = "test_boolean"; const std::string int_name = "test_int"; @@ -469,10 +473,11 @@ TEST_F(TestDefaultStateMachine, declare_parameters) { test_node->declare_parameters("test_double", double_parameters); list_result = test_node->list_parameters({}, 0u); - EXPECT_EQ(list_result.names.size(), 7u); + EXPECT_EQ(list_result.names.size(), expected_param_count); // The order of these names is not controlled by lifecycle_node, doing set equality std::set expected_names = { + "start_type_description_service", "test_boolean", "test_double.double_one", "test_double.double_two", @@ -487,11 +492,13 @@ TEST_F(TestDefaultStateMachine, declare_parameters) { } TEST_F(TestDefaultStateMachine, check_parameters) { + const uint64_t builtin_param_count = 2; auto test_node = std::make_shared("testnode"); auto list_result = test_node->list_parameters({}, 0u); - EXPECT_EQ(list_result.names.size(), 1u); - EXPECT_STREQ(list_result.names[0].c_str(), "use_sim_time"); + EXPECT_EQ(list_result.names.size(), builtin_param_count); + EXPECT_STREQ(list_result.names[0].c_str(), "start_type_description_service"); + EXPECT_STREQ(list_result.names[1].c_str(), "use_sim_time"); const std::string bool_name = "test_boolean"; const std::string int_name = "test_int"; @@ -549,8 +556,7 @@ TEST_F(TestDefaultStateMachine, check_parameters) { std::map parameter_map; EXPECT_TRUE(test_node->get_parameters({}, parameter_map)); - // int param, bool param, and use_sim_time - EXPECT_EQ(parameter_map.size(), 3u); + EXPECT_EQ(parameter_map.size(), parameter_names.size() + builtin_param_count); // Check parameter types auto parameter_types = test_node->get_parameter_types(parameter_names); @@ -585,10 +591,12 @@ TEST_F(TestDefaultStateMachine, check_parameters) { // List parameters list_result = test_node->list_parameters({}, 0u); - EXPECT_EQ(list_result.names.size(), 3u); - EXPECT_STREQ(list_result.names[0].c_str(), parameter_names[0].c_str()); - EXPECT_STREQ(list_result.names[1].c_str(), parameter_names[1].c_str()); - EXPECT_STREQ(list_result.names[2].c_str(), "use_sim_time"); + EXPECT_EQ(list_result.names.size(), parameter_names.size() + builtin_param_count); + size_t index = 0; + EXPECT_STREQ(list_result.names[index++].c_str(), "start_type_description_service"); + EXPECT_STREQ(list_result.names[index++].c_str(), parameter_names[0].c_str()); + EXPECT_STREQ(list_result.names[index++].c_str(), parameter_names[1].c_str()); + EXPECT_STREQ(list_result.names[index++].c_str(), "use_sim_time"); // Undeclare parameter test_node->undeclare_parameter(bool_name); From 945d254e321381ecb618c680015ba2b38ce898eb Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 10 Jul 2023 15:59:35 -0400 Subject: [PATCH 247/455] Switch lifecycle to use the RCLCPP macros. (#2233) This ensures that they'll go out to /rosout and the disk. Signed-off-by: Chris Lalancette --- rclcpp_lifecycle/src/lifecycle_node.cpp | 2 +- .../src/lifecycle_node_interface_impl.cpp | 36 +++++++++++++------ .../src/lifecycle_node_interface_impl.hpp | 6 +++- 3 files changed, 31 insertions(+), 13 deletions(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 94629911a3..6e29456d2a 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -122,7 +122,7 @@ LifecycleNode::LifecycleNode( )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), node_options_(options), - impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_)) + impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_, node_logging_)) { impl_->init(enable_communication_interface); diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp index 5c5f7797e1..9074c9cc6e 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp @@ -29,6 +29,7 @@ #include "lifecycle_msgs/srv/get_available_transitions.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -50,9 +51,11 @@ namespace rclcpp_lifecycle LifecycleNode::LifecycleNodeInterfaceImpl::LifecycleNodeInterfaceImpl( std::shared_ptr node_base_interface, - std::shared_ptr node_services_interface) + std::shared_ptr node_services_interface, + std::shared_ptr node_logging_interface) : node_base_interface_(node_base_interface), - node_services_interface_(node_services_interface) + node_services_interface_(node_services_interface), + node_logging_interface_(node_logging_interface) { } @@ -65,8 +68,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::~LifecycleNodeInterfaceImpl() ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle); } if (ret != RCL_RET_OK) { - RCUTILS_LOG_FATAL_NAMED( - "rclcpp_lifecycle", + RCLCPP_FATAL( + node_logging_interface_->get_logger(), "failed to destroy rcl_state_machine"); } } @@ -398,7 +401,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( { std::lock_guard lock(state_machine_mutex_); if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( + RCLCPP_ERROR( + node_logging_interface_->get_logger(), "Unable to change state for state machine for %s: %s", node_base_interface_->get_name(), rcl_get_error_string().str); return RCL_RET_ERROR; @@ -411,7 +415,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( rcl_lifecycle_trigger_transition_by_id( &state_machine_, transition_id, publish_update) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( + RCLCPP_ERROR( + node_logging_interface_->get_logger(), "Unable to start transition %u from current state %s: %s", transition_id, state_machine_.current_state->label, rcl_get_error_string().str); rcutils_reset_error(); @@ -443,7 +448,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( rcl_lifecycle_trigger_transition_by_label( &state_machine_, transition_label, publish_update) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( + RCLCPP_ERROR( + node_logging_interface_->get_logger(), "Failed to finish transition %u. Current state is now: %s (%s)", transition_id, state_machine_.current_state->label, rcl_get_error_string().str); rcutils_reset_error(); @@ -458,7 +464,9 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( // error handling ?! // TODO(karsten1987): iterate over possible ret value if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) { - RCUTILS_LOG_WARN("Error occurred while doing error handling."); + RCLCPP_WARN( + node_logging_interface_->get_logger(), + "Error occurred while doing error handling."); auto error_cb_code = execute_callback(current_state_id, initial_state); auto error_cb_label = get_label_for_return_code(error_cb_code); @@ -467,7 +475,9 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( rcl_lifecycle_trigger_transition_by_label( &state_machine_, error_cb_label, publish_update) != RCL_RET_OK) { - RCUTILS_LOG_ERROR("Failed to call cleanup on error state: %s", rcl_get_error_string().str); + RCLCPP_ERROR( + node_logging_interface_->get_logger(), + "Failed to call cleanup on error state: %s", rcl_get_error_string().str); rcutils_reset_error(); return RCL_RET_ERROR; } @@ -495,8 +505,12 @@ LifecycleNode::LifecycleNodeInterfaceImpl::execute_callback( try { cb_success = callback(State(previous_state)); } catch (const std::exception & e) { - RCUTILS_LOG_ERROR("Caught exception in callback for transition %d", it->first); - RCUTILS_LOG_ERROR("Original error: %s", e.what()); + RCLCPP_ERROR( + node_logging_interface_->get_logger(), + "Caught exception in callback for transition %d", it->first); + RCLCPP_ERROR( + node_logging_interface_->get_logger(), + "Original error: %s", e.what()); cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } } diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index d09f44831c..5cf5bdaacf 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -32,6 +32,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -52,7 +53,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final public: LifecycleNodeInterfaceImpl( std::shared_ptr node_base_interface, - std::shared_ptr node_services_interface); + std::shared_ptr node_services_interface, + std::shared_ptr node_logging_interface); ~LifecycleNodeInterfaceImpl(); @@ -152,6 +154,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final using NodeBasePtr = std::shared_ptr; using NodeServicesPtr = std::shared_ptr; + using NodeLoggingPtr = std::shared_ptr; using ChangeStateSrvPtr = std::shared_ptr>; using GetStateSrvPtr = std::shared_ptr>; using GetAvailableStatesSrvPtr = @@ -163,6 +166,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final NodeBasePtr node_base_interface_; NodeServicesPtr node_services_interface_; + NodeLoggingPtr node_logging_interface_; ChangeStateSrvPtr srv_change_state_; GetStateSrvPtr srv_get_state_; GetAvailableStatesSrvPtr srv_get_available_states_; From a7a9b78feebff603e5c7c5506ce31b89916bde36 Mon Sep 17 00:00:00 2001 From: Emerson Knapp <537409+emersonknapp@users.noreply.github.com> Date: Tue, 11 Jul 2023 05:41:53 -0700 Subject: [PATCH 248/455] Implement get_node_type_descriptions_interface for lifecyclenode and add smoke test for it (#2237) Signed-off-by: Emerson Knapp --- rclcpp/test/rclcpp/test_node.cpp | 1 + .../include/rclcpp_lifecycle/lifecycle_node.hpp | 4 ++++ rclcpp_lifecycle/src/lifecycle_node.cpp | 6 ++++++ rclcpp_lifecycle/test/test_lifecycle_node.cpp | 1 + 4 files changed, 12 insertions(+) diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index 512c2c1384..2f74a998c3 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -78,6 +78,7 @@ TEST_F(TestNode, construction_and_destruction) { EXPECT_NE(nullptr, node->get_node_options().get_rcl_node_options()); EXPECT_NE(nullptr, node->get_graph_event()); EXPECT_NE(nullptr, node->get_clock()); + EXPECT_NE(nullptr, node->get_node_type_descriptions_interface()); } { diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index d3654ab2b2..097537b53a 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -824,6 +824,10 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr get_node_time_source_interface(); + /// Return the Node's internal NodeTypeDescriptionsInterface implementation. + /** + * \sa rclcpp::Node::get_node_type_descriptions_interface + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr get_node_type_descriptions_interface(); diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 6e29456d2a..9e58c4fa70 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -475,6 +475,12 @@ LifecycleNode::get_node_topics_interface() return node_topics_; } +rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr +LifecycleNode::get_node_type_descriptions_interface() +{ + return node_type_descriptions_; +} + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr LifecycleNode::get_node_services_interface() { diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index db24bd0ee7..8a09884e70 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -641,6 +641,7 @@ TEST_F(TestDefaultStateMachine, test_getters) { EXPECT_LT(0u, test_node->now().nanoseconds()); EXPECT_STREQ("testnode", test_node->get_logger().get_name()); EXPECT_NE(nullptr, const_cast(test_node.get())->get_clock()); + EXPECT_NE(nullptr, test_node->get_node_type_descriptions_interface()); } TEST_F(TestDefaultStateMachine, test_graph_topics) { From 4ebc5f61d89e5ee088a64e47c2a7f985284149c6 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 11 Jul 2023 19:48:30 +0000 Subject: [PATCH 249/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 11 +++++++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 7 +++++++ 4 files changed, 24 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 4e3ada2fdb..087976bdf6 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Implement get_node_type_descriptions_interface for lifecyclenode and add smoke test for it (`#2237 `_) +* Add new node interface TypeDescriptionsInterface to provide GetTypeDescription service (`#2224 `_) +* Move always_false_v to detail namespace (`#2232 `_) +* Revamp the test_subscription.cpp tests. (`#2227 `_) +* warning: comparison of integer expressions of different signedness (`#2219 `_) +* Modifies timers API to select autostart state (`#2005 `_) +* Enable callback group tests for connextdds (`#2182 `_) +* Contributors: Chris Lalancette, Christopher Wecht, Eloy Briceno, Emerson Knapp, Nathan Wiebe Neufeldt, Tomoya Fujita + 21.3.0 (2023-06-12) ------------------- * Fix up misspellings of "receive". (`#2208 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 7efd063835..c8a892777b 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 21.3.0 (2023-06-12) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 690970a9ad..b055417985 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 21.3.0 (2023-06-12) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 059ddfbb75..5f8987c354 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,13 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Implement get_node_type_descriptions_interface for lifecyclenode and add smoke test for it (`#2237 `_) +* Switch lifecycle to use the RCLCPP macros. (`#2233 `_) +* Add new node interface TypeDescriptionsInterface to provide GetTypeDescription service (`#2224 `_) +* Contributors: Chris Lalancette, Emerson Knapp + 21.3.0 (2023-06-12) ------------------- From 6e97990a32afa17ab98ef4c800783c5cce801786 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 11 Jul 2023 19:48:37 +0000 Subject: [PATCH 250/455] 22.0.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 087976bdf6..6b534b3db6 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +22.0.0 (2023-07-11) +------------------- * Implement get_node_type_descriptions_interface for lifecyclenode and add smoke test for it (`#2237 `_) * Add new node interface TypeDescriptionsInterface to provide GetTypeDescription service (`#2224 `_) * Move always_false_v to detail namespace (`#2232 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 619f792aa2..781213e0f3 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 21.3.0 + 22.0.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index c8a892777b..0384d51260 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +22.0.0 (2023-07-11) +------------------- 21.3.0 (2023-06-12) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 803b356c35..d376dbf4d5 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 21.3.0 + 22.0.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index b055417985..2264327a43 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +22.0.0 (2023-07-11) +------------------- 21.3.0 (2023-06-12) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 39c8894d34..bc76471f95 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 21.3.0 + 22.0.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 5f8987c354..4b15946372 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +22.0.0 (2023-07-11) +------------------- * Implement get_node_type_descriptions_interface for lifecyclenode and add smoke test for it (`#2237 `_) * Switch lifecycle to use the RCLCPP macros. (`#2233 `_) * Add new node interface TypeDescriptionsInterface to provide GetTypeDescription service (`#2224 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 02962b7007..1633c9ead9 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 21.3.0 + 22.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From c0d72c3ee0ac806379b49ed10cf17364a4bd443a Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 18 Jul 2023 10:18:22 -0400 Subject: [PATCH 251/455] Stop using constref signature of benchmark DoNotOptimize. (#2238) * Stop using constref signature of benchmark DoNotOptimize. Newer versions of google benchmark (1.8.2 in my case) warn that the compiler may optimize away the DoNotOptimize calls when using the constref version. Get away from that here by explicitly *not* calling the constref version, casting where necessary. Signed-off-by: Chris Lalancette --- .../benchmark/benchmark_lifecycle_client.cpp | 11 ++++++---- .../benchmark/benchmark_lifecycle_node.cpp | 22 +++++++++++-------- 2 files changed, 20 insertions(+), 13 deletions(-) diff --git a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp index c8d166ef66..59ed24489e 100644 --- a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp +++ b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp @@ -228,7 +228,8 @@ class BenchmarkLifecycleClient : public performance_test_fixture::PerformanceTes BENCHMARK_F(BenchmarkLifecycleClient, get_state)(benchmark::State & state) { for (auto _ : state) { (void)_; - const auto lifecycle_state = lifecycle_client->get_state(); + + lifecycle_msgs::msg::State lifecycle_state = lifecycle_client->get_state(); if (lifecycle_state.id != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { const std::string msg = std::string("Expected state did not match actual: ") + @@ -268,7 +269,7 @@ BENCHMARK_F(BenchmarkLifecycleClient, get_available_states)(benchmark::State & s for (auto _ : state) { (void)_; constexpr size_t expected_states = 11u; - const auto states = lifecycle_client->get_available_states(); + std::vector states = lifecycle_client->get_available_states(); if (states.size() != expected_states) { const std::string msg = std::string("Expected number of states did not match actual: ") + @@ -284,7 +285,8 @@ BENCHMARK_F(BenchmarkLifecycleClient, get_available_transitions)(benchmark::Stat for (auto _ : state) { (void)_; constexpr size_t expected_transitions = 2u; - const auto transitions = lifecycle_client->get_available_transitions(); + std::vector transitions = + lifecycle_client->get_available_transitions(); if (transitions.size() != expected_transitions) { const std::string msg = std::string("Expected number of transitions did not match actual: ") + @@ -300,7 +302,8 @@ BENCHMARK_F(BenchmarkLifecycleClient, get_transition_graph)(benchmark::State & s for (auto _ : state) { (void)_; constexpr size_t expected_transitions = 25u; - const auto transitions = lifecycle_client->get_transition_graph(); + std::vector transitions = + lifecycle_client->get_transition_graph(); if (transitions.size() != expected_transitions) { const std::string msg = std::string("Expected number of transitions did not match actual: ") + diff --git a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp index 0bf3c48ab9..4be9ad13af 100644 --- a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp @@ -97,13 +97,15 @@ class BenchmarkLifecycleNode : public performance_test_fixture::PerformanceTest BENCHMARK_F(BenchmarkLifecycleNode, get_current_state)(benchmark::State & state) { for (auto _ : state) { (void)_; - const auto & lifecycle_state = node->get_current_state(); + const rclcpp_lifecycle::State & lifecycle_state = node->get_current_state(); if (lifecycle_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { const std::string message = std::string("Node's current state is: ") + std::to_string(lifecycle_state.id()); state.SkipWithError(message.c_str()); } - benchmark::DoNotOptimize(lifecycle_state); + // Google benchmark 1.8.2 warns that the constref DoNotOptimize signature may be optimized away + // by the compiler. Cast const away to ensure we don't get that problem (and warning). + benchmark::DoNotOptimize(const_cast(lifecycle_state)); benchmark::ClobberMemory(); } } @@ -112,7 +114,7 @@ BENCHMARK_F(BenchmarkLifecycleNode, get_available_states)(benchmark::State & sta for (auto _ : state) { (void)_; constexpr size_t expected_states = 11u; - const auto lifecycle_states = node->get_available_states(); + std::vector lifecycle_states = node->get_available_states(); if (lifecycle_states.size() != expected_states) { const std::string msg = std::to_string(lifecycle_states.size()); state.SkipWithError(msg.c_str()); @@ -126,7 +128,7 @@ BENCHMARK_F(BenchmarkLifecycleNode, get_available_transitions)(benchmark::State for (auto _ : state) { (void)_; constexpr size_t expected_transitions = 2u; - const auto & transitions = node->get_available_transitions(); + std::vector transitions = node->get_available_transitions(); if (transitions.size() != expected_transitions) { const std::string msg = std::to_string(transitions.size()); state.SkipWithError(msg.c_str()); @@ -140,7 +142,7 @@ BENCHMARK_F(BenchmarkLifecycleNode, get_transition_graph)(benchmark::State & sta for (auto _ : state) { (void)_; constexpr size_t expected_transitions = 25u; - const auto & transitions = node->get_transition_graph(); + std::vector transitions = node->get_transition_graph(); if (transitions.size() != expected_transitions) { const std::string msg = std::string("Expected number of transitions did not match actual: ") + @@ -162,18 +164,20 @@ BENCHMARK_F(BenchmarkLifecycleNode, transition_valid_state)(benchmark::State & s reset_heap_counters(); for (auto _ : state) { (void)_; - const auto & active = + const rclcpp_lifecycle::State & active = node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); if (active.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { state.SkipWithError("Transition to active state failed"); } - const auto & inactive = + const rclcpp_lifecycle::State & inactive = node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); if (inactive.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { state.SkipWithError("Transition to inactive state failed"); } - benchmark::DoNotOptimize(active); - benchmark::DoNotOptimize(inactive); + // Google benchmark 1.8.2 warns that the constref DoNotOptimize signature may be optimized away + // by the compiler. Cast const away to ensure we don't get that problem (and warning). + benchmark::DoNotOptimize(const_cast(active)); + benchmark::DoNotOptimize(const_cast(inactive)); benchmark::ClobberMemory(); } } From 22a954e1b00ea2ba810666baa92a3c66fe1a8654 Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Fri, 21 Jul 2023 11:26:42 -0700 Subject: [PATCH 252/455] Instrument loaned message publication code path (#2240) Signed-off-by: Christophe Bedard --- rclcpp/include/rclcpp/publisher.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 6025125db1..18229c7a4e 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -456,6 +456,7 @@ class Publisher : public PublisherBase do_loaned_message_publish( std::unique_ptr> msg) { + TRACETOOLS_TRACEPOINT(rclcpp_publish, nullptr, static_cast(msg.get())); auto status = rcl_publish_loaned_message(publisher_handle_.get(), msg.get(), nullptr); if (RCL_RET_PUBLISHER_INVALID == status) { From 5d6e5fa766b9ec1237a698b1eefa2733bcdeb506 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Tue, 1 Aug 2023 22:43:02 -0700 Subject: [PATCH 253/455] associated clocks should be protected by mutex. (#2255) Signed-off-by: Tomoya Fujita --- rclcpp/src/rclcpp/time_source.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 7f3b3b9536..e5c9b994f2 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -54,9 +54,7 @@ class ClocksState final ros_time_active_ = true; // Update all attached clocks to zero or last recorded time - for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { - set_clock(last_time_msg_, true, *it); - } + set_all_clocks(last_time_msg_, true); } // An internal method to use in the clock callback that iterates and disables all clocks @@ -71,11 +69,8 @@ class ClocksState final ros_time_active_ = false; // Update all attached clocks - std::lock_guard guard(clock_list_lock_); - for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { - auto msg = std::make_shared(); - set_clock(msg, false, *it); - } + auto msg = std::make_shared(); + set_all_clocks(msg, false); } // Check if ROS time is active From ea29c142af320bff06f957ec9172caf3a4aff41a Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 3 Aug 2023 20:43:04 +0800 Subject: [PATCH 254/455] Change associated clocks storage to unordered_set (#2257) Signed-off-by: Luca Della Vedova --- rclcpp/src/rclcpp/time_source.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index e5c9b994f2..a0e1d00853 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -90,7 +91,7 @@ class ClocksState final } } std::lock_guard guard(clock_list_lock_); - associated_clocks_.push_back(clock); + associated_clocks_.insert(clock); // Set the clock to zero unless there's a recently received message set_clock(last_time_msg_, ros_time_active_, clock); } @@ -99,10 +100,8 @@ class ClocksState final void detachClock(rclcpp::Clock::SharedPtr clock) { std::lock_guard guard(clock_list_lock_); - auto result = std::find(associated_clocks_.begin(), associated_clocks_.end(), clock); - if (result != associated_clocks_.end()) { - associated_clocks_.erase(result); - } else { + auto removed = associated_clocks_.erase(clock); + if (removed == 0) { RCLCPP_ERROR(logger_, "failed to remove clock"); } } @@ -179,8 +178,8 @@ class ClocksState final // A lock to protect iterating the associated_clocks_ field. std::mutex clock_list_lock_; - // A vector to store references to associated clocks. - std::vector associated_clocks_; + // An unordered_set to store references to associated clocks. + std::unordered_set associated_clocks_; // Local storage of validity of ROS time // This is needed when new clocks are added. From e2965831d51e9be03470cb07f8721012afcade9b Mon Sep 17 00:00:00 2001 From: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Date: Fri, 4 Aug 2023 16:25:26 -0400 Subject: [PATCH 255/455] Adding Missing Group Exceptions (#2256) * Adding Missing Group Exceptions Signed-off-by: CursedRock17 Signed-off-by: Chris Lalancette Co-authored-by: Tomoya Fujita --- rclcpp/include/rclcpp/exceptions/exceptions.hpp | 8 ++++++++ rclcpp/src/rclcpp/node_interfaces/node_services.cpp | 6 ++---- rclcpp/src/rclcpp/node_interfaces/node_timers.cpp | 3 +-- rclcpp/src/rclcpp/node_interfaces/node_topics.cpp | 5 ++--- rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp | 3 +-- rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp | 4 ++-- rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp | 2 +- .../test/rclcpp/node_interfaces/test_node_waitables.cpp | 2 +- 8 files changed, 18 insertions(+), 15 deletions(-) diff --git a/rclcpp/include/rclcpp/exceptions/exceptions.hpp b/rclcpp/include/rclcpp/exceptions/exceptions.hpp index ecb2a09905..36ffc06c49 100644 --- a/rclcpp/include/rclcpp/exceptions/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions/exceptions.hpp @@ -222,6 +222,14 @@ class EventNotRegisteredError : public std::runtime_error : std::runtime_error("event already registered") {} }; +/// Thrown when a callback group is missing from the node, when it wants to utilize the group. +class MissingGroupNodeException : public std::runtime_error +{ +public: + explicit MissingGroupNodeException(const std::string & obj_type) + : std::runtime_error("cannot create: " + obj_type + " , callback group not in node") {} +}; + /// Thrown if passed parameters are inconsistent or invalid class InvalidParametersException : public std::runtime_error { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp index e9c4a5266e..fdd4e83780 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp @@ -32,8 +32,7 @@ NodeServices::add_service( { if (group) { if (!node_base_->callback_group_in_node(group)) { - // TODO(jacquelinekay): use custom exception - throw std::runtime_error("Cannot create service, group not in node."); + throw rclcpp::exceptions::MissingGroupNodeException("service"); } } else { group = node_base_->get_default_callback_group(); @@ -58,8 +57,7 @@ NodeServices::add_client( { if (group) { if (!node_base_->callback_group_in_node(group)) { - // TODO(jacquelinekay): use custom exception - throw std::runtime_error("Cannot create client, group not in node."); + throw rclcpp::exceptions::MissingGroupNodeException("client"); } } else { group = node_base_->get_default_callback_group(); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp index e72ddf91d3..29d3125e1f 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp @@ -34,8 +34,7 @@ NodeTimers::add_timer( { if (callback_group) { if (!node_base_->callback_group_in_node(callback_group)) { - // TODO(jacquelinekay): use custom exception - throw std::runtime_error("Cannot create timer, group not in node."); + throw rclcpp::exceptions::MissingGroupNodeException("timer"); } } else { callback_group = node_base_->get_default_callback_group(); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index 659129d62c..ce71036b93 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -58,7 +58,7 @@ NodeTopics::add_publisher( // Assign to a group. if (callback_group) { if (!node_base_->callback_group_in_node(callback_group)) { - throw std::runtime_error("Cannot create publisher, callback group not in node."); + throw rclcpp::exceptions::MissingGroupNodeException("publisher"); } } else { callback_group = node_base_->get_default_callback_group(); @@ -97,8 +97,7 @@ NodeTopics::add_subscription( // Assign to a group. if (callback_group) { if (!node_base_->callback_group_in_node(callback_group)) { - // TODO(jacquelinekay): use custom exception - throw std::runtime_error("Cannot create subscription, callback group not in node."); + throw rclcpp::exceptions::MissingGroupNodeException("subscription"); } } else { callback_group = node_base_->get_default_callback_group(); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp index 02a9de82b0..96eb8df9cf 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp @@ -32,8 +32,7 @@ NodeWaitables::add_waitable( { if (group) { if (!node_base_->callback_group_in_node(group)) { - // TODO(jacobperron): use custom exception - throw std::runtime_error("Cannot create waitable, group not in node."); + throw rclcpp::exceptions::MissingGroupNodeException("waitable"); } } else { group = node_base_->get_default_callback_group(); diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp index ea55a1aac2..7b00fea972 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp @@ -92,7 +92,7 @@ TEST_F(TestNodeService, add_service) different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); RCLCPP_EXPECT_THROW_EQ( node_services->add_service(service, callback_group_in_different_node), - std::runtime_error("Cannot create service, group not in node.")); + rclcpp::exceptions::MissingGroupNodeException("service")); } TEST_F(TestNodeService, add_service_rcl_trigger_guard_condition_error) @@ -119,7 +119,7 @@ TEST_F(TestNodeService, add_client) different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); RCLCPP_EXPECT_THROW_EQ( node_services->add_client(client, callback_group_in_different_node), - std::runtime_error("Cannot create client, group not in node.")); + rclcpp::exceptions::MissingGroupNodeException("client")); } TEST_F(TestNodeService, add_client_rcl_trigger_guard_condition_error) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp index d038a4b44d..dc92dee610 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp @@ -75,7 +75,7 @@ TEST_F(TestNodeTimers, add_timer) different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); RCLCPP_EXPECT_THROW_EQ( node_timers->add_timer(timer, callback_group_in_different_node), - std::runtime_error("Cannot create timer, group not in node.")); + rclcpp::exceptions::MissingGroupNodeException("timer")); } TEST_F(TestNodeTimers, add_timer_rcl_trigger_guard_condition_error) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp index a1f35c0cdc..e559e57cd1 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp @@ -78,7 +78,7 @@ TEST_F(TestNodeWaitables, add_remove_waitable) node_waitables->add_waitable(waitable, callback_group1)); RCLCPP_EXPECT_THROW_EQ( node_waitables->add_waitable(waitable, callback_group2), - std::runtime_error("Cannot create waitable, group not in node.")); + rclcpp::exceptions::MissingGroupNodeException("waitable")); EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group1)); EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group2)); From a17d26b20ac41cc9d5bf3583de8475bb7c18bb1e Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 8 Aug 2023 23:38:13 +0200 Subject: [PATCH 256/455] Add spin_all shortcut (#2246) Signed-off-by: Tony Najjar --- rclcpp/include/rclcpp/executor.hpp | 15 ++++++++++++ rclcpp/include/rclcpp/executors.hpp | 12 ++++++++++ rclcpp/src/rclcpp/executor.cpp | 16 +++++++++++++ rclcpp/src/rclcpp/executors.cpp | 15 ++++++++++++ rclcpp/test/rclcpp/test_executor.cpp | 34 ++++++++++++++++++++++++++++ 5 files changed, 92 insertions(+) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 2d5ca2149a..b4f1b9d8f9 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -297,6 +297,21 @@ class Executor virtual void spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)); + /// Add a node, complete all immediately available work exhaustively, and remove the node. + /** + * \param[in] node Shared pointer to the node to add. + */ + RCLCPP_PUBLIC + void + spin_node_all( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, + std::chrono::nanoseconds max_duration); + + /// Convenience function which takes Node and forwards NodeBaseInterface. + RCLCPP_PUBLIC + void + spin_node_all(std::shared_ptr node, std::chrono::nanoseconds max_duration); + /// Collect and execute work repeatedly within a duration or until no more work is available. /** * This function can be overridden. The default implementation is suitable for a diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index f7ff06055f..55a38709a7 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -29,6 +29,18 @@ namespace rclcpp { +/// Create a default single-threaded executor and execute all available work exhaustively. +/** \param[in] node_ptr Shared pointer to the node to spin. */ +RCLCPP_PUBLIC +void +spin_all( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + std::chrono::nanoseconds max_duration); + +RCLCPP_PUBLIC +void +spin_all(rclcpp::Node::SharedPtr node_ptr, std::chrono::nanoseconds max_duration); + /// Create a default single-threaded executor and execute any immediately available work. /** \param[in] node_ptr Shared pointer to the node to spin. */ RCLCPP_PUBLIC diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 3deb2d1706..9de5883a34 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -431,6 +431,22 @@ void Executor::spin_some(std::chrono::nanoseconds max_duration) return this->spin_some_impl(max_duration, false); } +void +Executor::spin_node_all( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, + std::chrono::nanoseconds max_duration) +{ + this->add_node(node, false); + spin_all(max_duration); + this->remove_node(node, false); +} + +void +Executor::spin_node_all(std::shared_ptr node, std::chrono::nanoseconds max_duration) +{ + this->spin_node_all(node->get_node_base_interface(), max_duration); +} + void Executor::spin_all(std::chrono::nanoseconds max_duration) { if (max_duration < 0ns) { diff --git a/rclcpp/src/rclcpp/executors.cpp b/rclcpp/src/rclcpp/executors.cpp index 0a900c07da..3ea8d658ad 100644 --- a/rclcpp/src/rclcpp/executors.cpp +++ b/rclcpp/src/rclcpp/executors.cpp @@ -14,6 +14,21 @@ #include "rclcpp/executors.hpp" +void +rclcpp::spin_all( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + std::chrono::nanoseconds max_duration) +{ + rclcpp::executors::SingleThreadedExecutor exec; + exec.spin_node_all(node_ptr, max_duration); +} + +void +rclcpp::spin_all(rclcpp::Node::SharedPtr node_ptr, std::chrono::nanoseconds max_duration) +{ + rclcpp::spin_all(node_ptr->get_node_base_interface(), max_duration); +} + void rclcpp::spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) { diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index bdbb0a1079..706b80aef1 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -546,6 +546,40 @@ TEST_F(TestExecutor, spin_node_once_node) { EXPECT_TRUE(spin_called); } +TEST_F(TestExecutor, spin_node_all_base_interface) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool spin_called = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + spin_called = true; + }); + + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(spin_called); + dummy.spin_node_all(node->get_node_base_interface(), std::chrono::milliseconds(50)); + EXPECT_TRUE(spin_called); +} + +TEST_F(TestExecutor, spin_node_all_node) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool spin_called = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + spin_called = true; + }); + + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(spin_called); + dummy.spin_node_all(node, std::chrono::milliseconds(50)); + EXPECT_TRUE(spin_called); +} + TEST_F(TestExecutor, spin_until_future_complete_future_already_complete) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); From cd0440f1a5ebab5698aeb2065b4aa279bdd9b69e Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 9 Aug 2023 16:56:01 -0400 Subject: [PATCH 257/455] Remove an unused variable from the events executor tests. (#2270) Signed-off-by: Chris Lalancette --- rclcpp/test/rclcpp/executors/test_events_executor.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/rclcpp/test/rclcpp/executors/test_events_executor.cpp b/rclcpp/test/rclcpp/executors/test_events_executor.cpp index 0d678438ed..fb5c9a5166 100644 --- a/rclcpp/test/rclcpp/executors/test_events_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_events_executor.cpp @@ -75,8 +75,6 @@ TEST_F(TestEventsExecutor, run_pub_sub) !spin_exited && (std::chrono::high_resolution_clock::now() - start < 1s)) { - auto time = std::chrono::high_resolution_clock::now() - start; - auto time_msec = std::chrono::duration_cast(time); std::this_thread::sleep_for(25ms); } From 9b4b3da3d4226ec3d716288f961ed288bd030049 Mon Sep 17 00:00:00 2001 From: Emerson Knapp <537409+emersonknapp@users.noreply.github.com> Date: Thu, 10 Aug 2023 05:31:05 -0700 Subject: [PATCH 258/455] Add a pimpl inside rclcpp::Node for future distro backports (#2228) * Add a pimpl inside rclcpp::Node for future distro backports Signed-off-by: Emerson Knapp Co-authored-by: Chris Lalancette --- rclcpp/include/rclcpp/node.hpp | 6 ++++++ rclcpp/src/rclcpp/node.cpp | 19 ++++++++++++++++++- 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 50bd9da6a0..19abf4a602 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -1600,6 +1600,12 @@ class Node : public std::enable_shared_from_this const rclcpp::NodeOptions node_options_; const std::string sub_namespace_; const std::string effective_namespace_; + + class NodeImpl; + // This member is meant to be a place to backport features into stable distributions, + // and new features targeting Rolling should not use this. + // See the comment in node.cpp for more information. + std::shared_ptr hidden_impl_{nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 52012439e8..c31f4ee1f0 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -110,6 +110,22 @@ create_effective_namespace(const std::string & node_namespace, const std::string } // namespace +/// Internal implementation to provide hidden and API/ABI stable changes to the Node. +/** + * This class is intended to be an "escape hatch" within a stable distribution, so that certain + * smaller features and bugfixes can be backported, having a place to put new members, while + * maintaining the ABI. + * + * This is not intended to be a parking place for new features, it should be used for backports + * only, left empty and unallocated in Rolling. + */ +class Node::NodeImpl +{ +public: + NodeImpl() = default; + ~NodeImpl() = default; +}; + Node::Node( const std::string & node_name, const NodeOptions & options) @@ -253,7 +269,8 @@ Node::Node( node_waitables_(other.node_waitables_), node_options_(other.node_options_), sub_namespace_(extend_sub_namespace(other.get_sub_namespace(), sub_namespace)), - effective_namespace_(create_effective_namespace(other.get_namespace(), sub_namespace_)) + effective_namespace_(create_effective_namespace(other.get_namespace(), sub_namespace_)), + hidden_impl_(other.hidden_impl_) { // Validate new effective namespace. int validation_result; From 65f0b70d4aa4d1a3b8dc4c08aae26151ca00ca55 Mon Sep 17 00:00:00 2001 From: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Date: Tue, 15 Aug 2023 18:21:33 -0400 Subject: [PATCH 259/455] Adding Custom Unknown Type Error (#2272) Signed-off-by: CursedRock17 Co-authored-by: Christophe Bedard --- rclcpp/include/rclcpp/exceptions/exceptions.hpp | 8 ++++++++ rclcpp/include/rclcpp/parameter_value.hpp | 1 + rclcpp/src/rclcpp/parameter_value.cpp | 3 +-- 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/exceptions/exceptions.hpp b/rclcpp/include/rclcpp/exceptions/exceptions.hpp index 36ffc06c49..b3a53373ed 100644 --- a/rclcpp/include/rclcpp/exceptions/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions/exceptions.hpp @@ -206,6 +206,14 @@ class UnknownROSArgsError : public std::runtime_error const std::vector unknown_ros_args; }; +/// Thrown when an unknown type is passed +class UnknownTypeError : public std::runtime_error +{ +public: + explicit UnknownTypeError(const std::string & type) + : std::runtime_error("Unknown type: " + type) {} +}; + /// Thrown when an invalid rclcpp::Event object or SharedPtr is encountered. class InvalidEventError : public std::runtime_error { diff --git a/rclcpp/include/rclcpp/parameter_value.hpp b/rclcpp/include/rclcpp/parameter_value.hpp index f74c36a866..fac896ea46 100644 --- a/rclcpp/include/rclcpp/parameter_value.hpp +++ b/rclcpp/include/rclcpp/parameter_value.hpp @@ -24,6 +24,7 @@ #include "rcl_interfaces/msg/parameter_type.hpp" #include "rcl_interfaces/msg/parameter_value.hpp" +#include "rclcpp/exceptions/exceptions.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp diff --git a/rclcpp/src/rclcpp/parameter_value.cpp b/rclcpp/src/rclcpp/parameter_value.cpp index e88b225479..ee46e77673 100644 --- a/rclcpp/src/rclcpp/parameter_value.cpp +++ b/rclcpp/src/rclcpp/parameter_value.cpp @@ -129,8 +129,7 @@ ParameterValue::ParameterValue(const rcl_interfaces::msg::ParameterValue & value case PARAMETER_NOT_SET: break; default: - // TODO(wjwwood): use custom exception - throw std::runtime_error("Unknown type: " + std::to_string(value.type)); + throw rclcpp::exceptions::UnknownTypeError(std::to_string(value.type)); } } From fbe8f28cd13710c5c643a4e7149e509f3a952677 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Fri, 18 Aug 2023 09:15:04 -0700 Subject: [PATCH 260/455] Do not crash Executor when send_response fails due to client failure. (#2276) * Do not crash Executor when send_response fails due to client failure. Related to https://github.com/ros2/ros2/issues/1253 It is not sane that a faulty client can crash our service Executor, as discussed in the referred issue, if the client is not setup properly, send_response may return RCL_RET_TIMEOUT, we should not throw an error in this case. Signed-off-by: Zang MingJie * Update rclcpp/include/rclcpp/service.hpp Co-authored-by: Tomoya Fujita Signed-off-by: Zang MingJie * address review comments. Signed-off-by: Tomoya Fujita --------- Signed-off-by: Zang MingJie Signed-off-by: Tomoya Fujita Co-authored-by: Zang MingJie --- rclcpp/include/rclcpp/service.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 2ae79a6637..9e08dc235d 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -486,6 +486,14 @@ class Service { rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, &response); + if (ret == RCL_RET_TIMEOUT) { + RCLCPP_WARN( + node_logger_.get_child("rclcpp"), + "failed to send response to %s (timeout): %s", + this->get_service_name(), rcl_get_error_string().str); + rcl_reset_error(); + return; + } if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response"); } From a4db4c57a6f3452b3fd3488717b86b15bb162713 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 21 Aug 2023 14:51:57 +0000 Subject: [PATCH 261/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 13 +++++++++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 5 +++++ 4 files changed, 24 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 6b534b3db6..4636d685a6 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Do not crash Executor when send_response fails due to client failure. (`#2276 `_) +* Adding Custom Unknown Type Error (`#2272 `_) +* Add a pimpl inside rclcpp::Node for future distro backports (`#2228 `_) +* Remove an unused variable from the events executor tests. (`#2270 `_) +* Add spin_all shortcut (`#2246 `_) +* Adding Missing Group Exceptions (`#2256 `_) +* Change associated clocks storage to unordered_set (`#2257 `_) +* associated clocks should be protected by mutex. (`#2255 `_) +* Instrument loaned message publication code path (`#2240 `_) +* Contributors: Chris Lalancette, Christophe Bedard, Emerson Knapp, Luca Della Vedova, Lucas Wendland, Tomoya Fujita, Tony Najjar + 22.0.0 (2023-07-11) ------------------- * Implement get_node_type_descriptions_interface for lifecyclenode and add smoke test for it (`#2237 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 0384d51260..22a2c48588 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 22.0.0 (2023-07-11) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 2264327a43..ac74cab772 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 22.0.0 (2023-07-11) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 4b15946372..07eebacb74 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Stop using constref signature of benchmark DoNotOptimize. (`#2238 `_) +* Contributors: Chris Lalancette + 22.0.0 (2023-07-11) ------------------- * Implement get_node_type_descriptions_interface for lifecyclenode and add smoke test for it (`#2237 `_) From 89f0afe9bcc34eb5ef23725869aa095f42004b71 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 21 Aug 2023 14:52:05 +0000 Subject: [PATCH 262/455] 22.1.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 4636d685a6..f2a84b1463 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +22.1.0 (2023-08-21) +------------------- * Do not crash Executor when send_response fails due to client failure. (`#2276 `_) * Adding Custom Unknown Type Error (`#2272 `_) * Add a pimpl inside rclcpp::Node for future distro backports (`#2228 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 781213e0f3..b3384fa629 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 22.0.0 + 22.1.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 22a2c48588..3e80fa6c92 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +22.1.0 (2023-08-21) +------------------- 22.0.0 (2023-07-11) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index d376dbf4d5..b3539ee95f 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 22.0.0 + 22.1.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index ac74cab772..336d432ba9 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +22.1.0 (2023-08-21) +------------------- 22.0.0 (2023-07-11) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index bc76471f95..0e00db2b56 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 22.0.0 + 22.1.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 07eebacb74..5472a87a93 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +22.1.0 (2023-08-21) +------------------- * Stop using constref signature of benchmark DoNotOptimize. (`#2238 `_) * Contributors: Chris Lalancette diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 1633c9ead9..aece0e1916 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 22.0.0 + 22.1.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From fd229d72ff498faf6583c1cc1f0f2f131575b2cf Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Mon, 21 Aug 2023 13:04:49 -0700 Subject: [PATCH 263/455] doc fix: call `canceled` only after goal state is in canceling. (#2266) Signed-off-by: Tomoya Fujita --- rclcpp_action/include/rclcpp_action/server_goal_handle.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index ac9dd49492..b77eaf2787 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -196,7 +196,7 @@ class ServerGoalHandle : public ServerGoalHandleBase /// Indicate that a goal has been canceled. /** - * Only call this if the goal is executing or pending, but has been canceled. + * Only call this if the goal is canceling. * This is a terminal state, no more methods should be called on a goal handle after this is * called. * From 403f305b15b734508859aae3c8dc15f6aeed242c Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 22 Aug 2023 18:22:01 -0400 Subject: [PATCH 264/455] Fix a typo in a comment. (#2283) Signed-off-by: Chris Lalancette --- rclcpp_action/include/rclcpp_action/server_goal_handle.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index b77eaf2787..7d17819189 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -128,7 +128,7 @@ class Server; * accepted. * A `Server` will create an instance and give it to the user in their `handle_accepted` callback. * - * Internally, this class is responsible for coverting between the C++ action type and generic + * Internally, this class is responsible for converting between the C++ action type and generic * types for `rclcpp_action::ServerGoalHandleBase`. */ template From 77db1ed25b4c26ab722a13c268521b923606c04d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 23 Aug 2023 08:15:44 -0400 Subject: [PATCH 265/455] Revamp list_parameters to be more efficient and easier to read. (#2282) 1. Use constref for the loop variable. 2. Do more work outside of the loop. 3. Skip doing unnecessary work where we can inside the loop. With this in place, I measured about a 7% performance improvement over the previous implementation. Signed-off-by: Chris Lalancette --- .../node_interfaces/node_parameters.cpp | 69 +++++++++++-------- 1 file changed, 41 insertions(+), 28 deletions(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index c8bb62e238..b4c899ff9b 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -1038,37 +1038,50 @@ NodeParameters::list_parameters(const std::vector & prefixes, uint6 // TODO(mikaelarguedas) define parameter separator different from "/" to avoid ambiguity // using "." for now const char * separator = "."; - for (auto & kv : parameters_) { - bool get_all = (prefixes.size() == 0) && - ((depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) || - (static_cast(std::count(kv.first.begin(), kv.first.end(), *separator)) < depth)); - bool prefix_matches = std::any_of( - prefixes.cbegin(), prefixes.cend(), - [&kv, &depth, &separator](const std::string & prefix) { - if (kv.first == prefix) { - return true; - } else if (kv.first.find(prefix + separator) == 0) { - size_t length = prefix.length(); - std::string substr = kv.first.substr(length); - // Cast as unsigned integer to avoid warning - return (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) || - (static_cast(std::count(substr.begin(), substr.end(), *separator)) < depth); - } - return false; - }); - if (get_all || prefix_matches) { - result.names.push_back(kv.first); - size_t last_separator = kv.first.find_last_of(separator); - if (std::string::npos != last_separator) { - std::string prefix = kv.first.substr(0, last_separator); - if ( - std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) == - result.prefixes.cend()) - { - result.prefixes.push_back(prefix); + + auto separators_less_than_depth = [&depth, &separator](const std::string & str) -> bool { + return static_cast(std::count(str.begin(), str.end(), *separator)) < depth; + }; + + bool recursive = (prefixes.size() == 0) && + (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE); + + for (const std::pair & kv : parameters_) { + if (!recursive) { + bool get_all = (prefixes.size() == 0) && separators_less_than_depth(kv.first); + if (!get_all) { + bool prefix_matches = std::any_of( + prefixes.cbegin(), prefixes.cend(), + [&kv, &depth, &separator, &separators_less_than_depth](const std::string & prefix) { + if (kv.first == prefix) { + return true; + } else if (kv.first.find(prefix + separator) == 0) { + if (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) { + return true; + } + std::string substr = kv.first.substr(prefix.length()); + return separators_less_than_depth(substr); + } + return false; + }); + + if (!prefix_matches) { + continue; } } } + + result.names.push_back(kv.first); + size_t last_separator = kv.first.find_last_of(separator); + if (std::string::npos != last_separator) { + std::string prefix = kv.first.substr(0, last_separator); + if ( + std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) == + result.prefixes.cend()) + { + result.prefixes.push_back(prefix); + } + } } return result; } From ba175922d3008704c67b10fe82a2f7c0cbee2be4 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 23 Aug 2023 19:43:54 -0400 Subject: [PATCH 266/455] Add rcl_logging_interface as an explicit dependency. (#2284) It is depended on by rclcpp/src/rclcpp/logger.cpp, but the dependency was not explicitly declared (it was being inherited from rcl, I believe). Fix that here. Signed-off-by: Chris Lalancette --- rclcpp/CMakeLists.txt | 2 ++ rclcpp/package.xml | 1 + 2 files changed, 3 insertions(+) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index bd705be06d..7eefe80ea1 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(builtin_interfaces REQUIRED) find_package(libstatistics_collector REQUIRED) find_package(rcl REQUIRED) find_package(rcl_interfaces REQUIRED) +find_package(rcl_logging_interface REQUIRED) find_package(rcl_yaml_param_parser REQUIRED) find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) @@ -207,6 +208,7 @@ ament_target_dependencies(${PROJECT_NAME} "libstatistics_collector" "rcl" "rcl_interfaces" + "rcl_logging_interface" "rcl_yaml_param_parser" "rcpputils" "rcutils" diff --git a/rclcpp/package.xml b/rclcpp/package.xml index b3384fa629..e1c272d949 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -35,6 +35,7 @@ libstatistics_collector rcl + rcl_logging_interface rcl_yaml_param_parser rcpputils rcutils From e7f06398dbcc5ef59362f416535f9e7acb0899ff Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Sat, 26 Aug 2023 11:38:52 -0700 Subject: [PATCH 267/455] add logger level service to lifecycle node. (#2277) Signed-off-by: Tomoya Fujita --- rclcpp_lifecycle/src/lifecycle_node.cpp | 4 +++ rclcpp_lifecycle/test/test_lifecycle_node.cpp | 33 +++++++++++++++++++ 2 files changed, 37 insertions(+) diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 9e58c4fa70..a85d661e63 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -144,6 +144,10 @@ LifecycleNode::LifecycleNode( &LifecycleNodeInterface::on_deactivate, this, std::placeholders::_1)); register_on_error(std::bind(&LifecycleNodeInterface::on_error, this, std::placeholders::_1)); + + if (options.enable_logger_service()) { + node_logging_->create_logger_services(node_services_); + } } LifecycleNode::~LifecycleNode() diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 8a09884e70..ddcc926527 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -25,6 +25,8 @@ #include "lifecycle_msgs/msg/transition.hpp" #include "rcl_lifecycle/rcl_lifecycle.h" +#include "rcl_interfaces/srv/get_logger_levels.hpp" +#include "rcl_interfaces/srv/set_logger_levels.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -34,6 +36,8 @@ using lifecycle_msgs::msg::State; using lifecycle_msgs::msg::Transition; +using namespace std::chrono_literals; + static const std::chrono::nanoseconds DEFAULT_EVENT_TIMEOUT = std::chrono::seconds(3); static const std::chrono::nanoseconds DEFAULT_EVENT_SLEEP_PERIOD = std::chrono::milliseconds(100); @@ -249,6 +253,35 @@ TEST_F(TestDefaultStateMachine, empty_initializer_rcl_errors) { } } +TEST_F(TestDefaultStateMachine, check_logger_services_exist) { + // Logger level services are disabled + { + rclcpp::NodeOptions options = rclcpp::NodeOptions(); + options.enable_logger_service(false); + auto node = std::make_shared( + "test_logger_service", "/test", options); + auto get_client = node->create_client( + "/test/test_logger_service/get_logger_levels"); + ASSERT_FALSE(get_client->wait_for_service(2s)); + auto set_client = node->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_FALSE(set_client->wait_for_service(2s)); + } + // Logger level services are enabled + { + rclcpp::NodeOptions options = rclcpp::NodeOptions(); + options.enable_logger_service(true); + auto node = std::make_shared( + "test_logger_service", "/test", options); + auto get_client = node->create_client( + "/test/test_logger_service/get_logger_levels"); + ASSERT_TRUE(get_client->wait_for_service(2s)); + auto set_client = node->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_TRUE(set_client->wait_for_service(2s)); + } +} + TEST_F(TestDefaultStateMachine, trigger_transition) { auto test_node = std::make_shared("testnode"); From fd58bddb05ee430f7124f5feff0add683db6f86f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 28 Aug 2023 14:17:27 -0400 Subject: [PATCH 268/455] Remove unnecessary lambda captures in the tests. (#2289) * Remove unnecessary lambda captures in the tests. This was pointed out by compiling with clang. Signed-off-by: Chris Lalancette --- rclcpp/test/rclcpp/executors/test_events_executor.cpp | 10 +++++----- rclcpp/test/rclcpp/executors/test_executors.cpp | 8 +++----- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/rclcpp/test/rclcpp/executors/test_events_executor.cpp b/rclcpp/test/rclcpp/executors/test_events_executor.cpp index fb5c9a5166..13092b7067 100644 --- a/rclcpp/test/rclcpp/executors/test_events_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_events_executor.cpp @@ -60,7 +60,7 @@ TEST_F(TestEventsExecutor, run_pub_sub) executor.add_node(node); bool spin_exited = false; - std::thread spinner([&spin_exited, &executor, this]() { + std::thread spinner([&spin_exited, &executor]() { executor.spin(); spin_exited = true; }); @@ -107,7 +107,7 @@ TEST_F(TestEventsExecutor, run_clients_servers) executor.add_node(node); bool spin_exited = false; - std::thread spinner([&spin_exited, &executor, this]() { + std::thread spinner([&spin_exited, &executor]() { executor.spin(); spin_exited = true; }); @@ -346,7 +346,7 @@ TEST_F(TestEventsExecutor, cancel_while_timers_running) }); - std::thread spinner([&executor, this]() {executor.spin();}); + std::thread spinner([&executor]() {executor.spin();}); std::this_thread::sleep_for(10ms); // Call cancel while t1 callback is still being executed @@ -373,7 +373,7 @@ TEST_F(TestEventsExecutor, cancel_while_timers_waiting) executor.add_node(node); auto start = std::chrono::steady_clock::now(); - std::thread spinner([&executor, this]() {executor.spin();}); + std::thread spinner([&executor]() {executor.spin();}); std::this_thread::sleep_for(10ms); executor.cancel(); @@ -395,7 +395,7 @@ TEST_F(TestEventsExecutor, destroy_entities) 2ms, [&]() {publisher->publish(std::make_unique());}); EventsExecutor executor_pub; executor_pub.add_node(node_pub); - std::thread spinner([&executor_pub, this]() {executor_pub.spin();}); + std::thread spinner([&executor_pub]() {executor_pub.spin();}); // Create a node with two different subscriptions to the topic auto node_sub = std::make_shared("node_sub"); diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 0bd0bfcd49..fbd410bcab 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -779,7 +779,7 @@ TYPED_TEST(TestIntraprocessExecutors, testIntraprocessRetrigger) { // that publishers aren't continuing to publish. // This was previously broken in that intraprocess guard conditions were only triggered on // publish and the test was added to prevent future regressions. - const size_t kNumMessages = 100; + static constexpr size_t kNumMessages = 100; using ExecutorType = TypeParam; ExecutorType executor; @@ -808,11 +808,9 @@ TYPED_TEST(TestIntraprocessExecutors, testIntraprocessRetrigger) { // Fire a timer every 10ms up to 5 seconds waiting for subscriptions to be read. loops = 0; auto timer = this->node->create_wall_timer( - std::chrono::milliseconds(10), [this, &executor, &loops, &kNumMessages]() { + std::chrono::milliseconds(10), [this, &executor, &loops]() { loops++; - if (kNumMessages == this->callback_count.load() || - loops == 500) - { + if (kNumMessages == this->callback_count.load() || loops == 500) { executor.cancel(); } }); From 43cf0be15c9c284fc0b434a111e8237523b3d397 Mon Sep 17 00:00:00 2001 From: Jiaqi Li Date: Wed, 30 Aug 2023 01:22:51 +0800 Subject: [PATCH 269/455] Correct the position of a comment. (#2290) Signed-off-by: Jiaqi Li --- rclcpp/src/rclcpp/client.cpp | 2 +- rclcpp_action/src/client.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index e33db71ce2..8388ee1888 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -125,7 +125,6 @@ bool ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout) { auto start = std::chrono::steady_clock::now(); - // make an event to reuse, rather than create a new one each time auto node_ptr = node_graph_.lock(); if (!node_ptr) { throw InvalidNodeError(); @@ -138,6 +137,7 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout) // check was non-blocking, return immediately return false; } + // make an event to reuse, rather than create a new one each time auto event = node_ptr->get_graph_event(); // update the time even on the first loop to account for time spent in the first call // to this->server_is_ready() diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 2d5018d5af..febd2fd905 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -163,7 +163,6 @@ bool ClientBase::wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout) { auto start = std::chrono::steady_clock::now(); - // make an event to reuse, rather than create a new one each time auto node_ptr = pimpl_->node_graph_.lock(); if (!node_ptr) { throw rclcpp::exceptions::InvalidNodeError(); @@ -172,6 +171,7 @@ ClientBase::wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout) if (this->action_server_is_ready()) { return true; } + // make an event to reuse, rather than create a new one each time auto event = node_ptr->get_graph_event(); if (timeout == std::chrono::nanoseconds(0)) { // check was non-blocking, return immediately From bc435776a257fcf76c5b0124bec26f6824342e34 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Thu, 31 Aug 2023 22:50:40 +0300 Subject: [PATCH 270/455] Make Rate to select the clock to work with (#2123) * Make Rate to select the clock to work with Add ROSRate respective with ROS time * Make GenericRate class to be deprecated * Adjust test cases for new rates is_steady() to be deprecated Signed-off-by: Alexey Merzlyakov Co-authored-by: Chris Lalancette --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/rate.hpp | 89 ++++++++- rclcpp/src/rclcpp/rate.cpp | 112 +++++++++++ rclcpp/test/rclcpp/test_rate.cpp | 324 ++++++++++++++++++++++++++++++- 4 files changed, 512 insertions(+), 14 deletions(-) create mode 100644 rclcpp/src/rclcpp/rate.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 7eefe80ea1..c30ad4d818 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -107,6 +107,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/qos.cpp src/rclcpp/event_handler.cpp src/rclcpp/qos_overriding_options.cpp + src/rclcpp/rate.cpp src/rclcpp/serialization.cpp src/rclcpp/serialized_message.cpp src/rclcpp/service.cpp diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 55d3bbcb85..884e462a76 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -19,6 +19,8 @@ #include #include +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" @@ -31,9 +33,20 @@ class RateBase public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(RateBase) + RCLCPP_PUBLIC virtual ~RateBase() {} + + RCLCPP_PUBLIC virtual bool sleep() = 0; + + [[deprecated("use get_type() instead")]] + RCLCPP_PUBLIC virtual bool is_steady() const = 0; + + RCLCPP_PUBLIC + virtual rcl_clock_type_t get_type() const = 0; + + RCLCPP_PUBLIC virtual void reset() = 0; }; @@ -42,14 +55,13 @@ using std::chrono::duration_cast; using std::chrono::nanoseconds; template -class GenericRate : public RateBase +class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRate : public RateBase { public: RCLCPP_SMART_PTR_DEFINITIONS(GenericRate) explicit GenericRate(double rate) - : GenericRate( - duration_cast(duration(1.0 / rate))) + : period_(duration_cast(duration(1.0 / rate))), last_interval_(Clock::now()) {} explicit GenericRate(std::chrono::nanoseconds period) : period_(period), last_interval_(Clock::now()) @@ -87,12 +99,18 @@ class GenericRate : public RateBase return true; } + [[deprecated("use get_type() instead")]] virtual bool is_steady() const { return Clock::is_steady; } + virtual rcl_clock_type_t get_type() const + { + return Clock::is_steady ? RCL_STEADY_TIME : RCL_SYSTEM_TIME; + } + virtual void reset() { @@ -112,8 +130,69 @@ class GenericRate : public RateBase std::chrono::time_point last_interval_; }; -using Rate = GenericRate; -using WallRate = GenericRate; +class Rate : public RateBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(Rate) + + RCLCPP_PUBLIC + explicit Rate( + const double rate, + Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME)); + + RCLCPP_PUBLIC + explicit Rate( + const Duration & period, + Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME)); + + RCLCPP_PUBLIC + virtual bool + sleep(); + + [[deprecated("use get_type() instead")]] + RCLCPP_PUBLIC + virtual bool + is_steady() const; + + RCLCPP_PUBLIC + virtual rcl_clock_type_t + get_type() const; + + RCLCPP_PUBLIC + virtual void + reset(); + + RCLCPP_PUBLIC + Duration + period() const; + +private: + RCLCPP_DISABLE_COPY(Rate) + + Clock::SharedPtr clock_; + Duration period_; + Time last_interval_; +}; + +class WallRate : public Rate +{ +public: + RCLCPP_PUBLIC + explicit WallRate(const double rate); + + RCLCPP_PUBLIC + explicit WallRate(const Duration & period); +}; + +class ROSRate : public Rate +{ +public: + RCLCPP_PUBLIC + explicit ROSRate(const double rate); + + RCLCPP_PUBLIC + explicit ROSRate(const Duration & period); +}; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/rate.cpp b/rclcpp/src/rclcpp/rate.cpp new file mode 100644 index 0000000000..04a1f57185 --- /dev/null +++ b/rclcpp/src/rclcpp/rate.cpp @@ -0,0 +1,112 @@ +// Copyright 2023 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. + +#include "rclcpp/rate.hpp" + +#include + +namespace rclcpp +{ + +Rate::Rate( + const double rate, Clock::SharedPtr clock) +: clock_(clock), period_(0, 0), last_interval_(clock_->now()) +{ + if (rate <= 0.0) { + throw std::invalid_argument{"rate must be greater than 0"}; + } + period_ = Duration::from_seconds(1.0 / rate); +} + +Rate::Rate( + const Duration & period, Clock::SharedPtr clock) +: clock_(clock), period_(period), last_interval_(clock_->now()) +{ + if (period <= Duration(0, 0)) { + throw std::invalid_argument{"period must be greater than 0"}; + } +} + +bool +Rate::sleep() +{ + // Time coming into sleep + auto now = clock_->now(); + // Time of next interval + auto next_interval = last_interval_ + period_; + // Detect backwards time flow + if (now < last_interval_) { + // Best thing to do is to set the next_interval to now + period + next_interval = now + period_; + } + // Update the interval + last_interval_ += period_; + // If the time_to_sleep is negative or zero, don't sleep + if (next_interval <= now) { + // If an entire cycle was missed then reset next interval. + // This might happen if the loop took more than a cycle. + // Or if time jumps forward. + if (now > next_interval + period_) { + last_interval_ = now + period_; + } + // Either way do not sleep and return false + return false; + } + // Calculate the time to sleep + auto time_to_sleep = next_interval - now; + // Sleep (will get interrupted by ctrl-c, may not sleep full time) + return clock_->sleep_for(time_to_sleep); +} + +bool +Rate::is_steady() const +{ + return clock_->get_clock_type() == RCL_STEADY_TIME; +} + +rcl_clock_type_t +Rate::get_type() const +{ + return clock_->get_clock_type(); +} + +void +Rate::reset() +{ + last_interval_ = clock_->now(); +} + +Duration +Rate::period() const +{ + return period_; +} + +WallRate::WallRate(const double rate) +: Rate(rate, std::make_shared(RCL_STEADY_TIME)) +{} + +WallRate::WallRate(const Duration & period) +: Rate(period, std::make_shared(RCL_STEADY_TIME)) +{} + +ROSRate::ROSRate(const double rate) +: Rate(rate, std::make_shared(RCL_ROS_TIME)) +{} + +ROSRate::ROSRate(const Duration & period) +: Rate(period, std::make_shared(RCL_ROS_TIME)) +{} + +} // namespace rclcpp diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp index d6608d59f6..5ab64ee608 100644 --- a/rclcpp/test/rclcpp/test_rate.cpp +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -14,14 +14,19 @@ #include +#include #include #include "rclcpp/rate.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + /* - Basic tests for the Rate and WallRate classes. + Basic tests for the Rate, WallRate and ROSRate classes. */ TEST(TestRate, rate_basics) { + rclcpp::init(0, nullptr); + auto period = std::chrono::milliseconds(1000); auto offset = std::chrono::milliseconds(500); auto epsilon = std::chrono::milliseconds(100); @@ -29,8 +34,23 @@ TEST(TestRate, rate_basics) { auto start = std::chrono::system_clock::now(); rclcpp::Rate r(period); - EXPECT_EQ(period, r.period()); + EXPECT_EQ(rclcpp::Duration(period), r.period()); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif ASSERT_FALSE(r.is_steady()); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + ASSERT_EQ(RCL_SYSTEM_TIME, r.get_type()); ASSERT_TRUE(r.sleep()); auto one = std::chrono::system_clock::now(); auto delta = one - start; @@ -59,9 +79,13 @@ TEST(TestRate, rate_basics) { auto five = std::chrono::system_clock::now(); delta = five - four; ASSERT_TRUE(epsilon > delta); + + rclcpp::shutdown(); } TEST(TestRate, wall_rate_basics) { + rclcpp::init(0, nullptr); + auto period = std::chrono::milliseconds(100); auto offset = std::chrono::milliseconds(50); auto epsilon = std::chrono::milliseconds(1); @@ -69,8 +93,25 @@ TEST(TestRate, wall_rate_basics) { auto start = std::chrono::steady_clock::now(); rclcpp::WallRate r(period); - EXPECT_EQ(period, r.period()); + EXPECT_EQ(rclcpp::Duration(period), r.period()); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif ASSERT_TRUE(r.is_steady()); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + ASSERT_EQ(RCL_STEADY_TIME, r.get_type()); ASSERT_TRUE(r.sleep()); auto one = std::chrono::steady_clock::now(); auto delta = one - start; @@ -99,23 +140,288 @@ TEST(TestRate, wall_rate_basics) { auto five = std::chrono::steady_clock::now(); delta = five - four; EXPECT_GT(epsilon, delta); + + rclcpp::shutdown(); +} + +TEST(TestRate, ros_rate_basics) { + rclcpp::init(0, nullptr); + + auto period = std::chrono::milliseconds(100); + auto offset = std::chrono::milliseconds(50); + auto epsilon = std::chrono::milliseconds(1); + double overrun_ratio = 1.5; + + rclcpp::Clock clock(RCL_ROS_TIME); + + auto start = clock.now(); + rclcpp::ROSRate r(period); + EXPECT_EQ(rclcpp::Duration(period), r.period()); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + ASSERT_FALSE(r.is_steady()); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + ASSERT_EQ(RCL_ROS_TIME, r.get_type()); + ASSERT_TRUE(r.sleep()); + auto one = clock.now(); + auto delta = one - start; + EXPECT_LT(rclcpp::Duration(period), delta); + EXPECT_GT(rclcpp::Duration(period * overrun_ratio), delta); + + clock.sleep_for(offset); + ASSERT_TRUE(r.sleep()); + auto two = clock.now(); + delta = two - start; + EXPECT_LT(rclcpp::Duration(2 * period), delta + epsilon); + EXPECT_GT(rclcpp::Duration(2 * period * overrun_ratio), delta); + + clock.sleep_for(offset); + auto two_offset = clock.now(); + r.reset(); + ASSERT_TRUE(r.sleep()); + auto three = clock.now(); + delta = three - two_offset; + EXPECT_LT(rclcpp::Duration(period), delta); + EXPECT_GT(rclcpp::Duration(period * overrun_ratio), delta); + + clock.sleep_for(offset + period); + auto four = clock.now(); + ASSERT_FALSE(r.sleep()); + auto five = clock.now(); + delta = five - four; + EXPECT_GT(rclcpp::Duration(epsilon), delta); + + rclcpp::shutdown(); +} + +/* + Basic test for the deprecated GenericRate class. + */ +TEST(TestRate, generic_rate) { + auto period = std::chrono::milliseconds(100); + auto offset = std::chrono::milliseconds(50); + auto epsilon = std::chrono::milliseconds(1); + double overrun_ratio = 1.5; + + { + auto start = std::chrono::system_clock::now(); + +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + + rclcpp::GenericRate gr(period); + ASSERT_FALSE(gr.is_steady()); + +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + + ASSERT_EQ(gr.get_type(), RCL_SYSTEM_TIME); + EXPECT_EQ(period, gr.period()); + ASSERT_TRUE(gr.sleep()); + auto one = std::chrono::system_clock::now(); + auto delta = one - start; + EXPECT_LT(period, delta + epsilon); + EXPECT_GT(period * overrun_ratio, delta); + + rclcpp::sleep_for(offset); + ASSERT_TRUE(gr.sleep()); + auto two = std::chrono::system_clock::now(); + delta = two - start; + EXPECT_LT(2 * period, delta); + EXPECT_GT(2 * period * overrun_ratio, delta); + + rclcpp::sleep_for(offset); + auto two_offset = std::chrono::system_clock::now(); + gr.reset(); + ASSERT_TRUE(gr.sleep()); + auto three = std::chrono::system_clock::now(); + delta = three - two_offset; + EXPECT_LT(period, delta + epsilon); + EXPECT_GT(period * overrun_ratio, delta); + + rclcpp::sleep_for(offset + period); + auto four = std::chrono::system_clock::now(); + ASSERT_FALSE(gr.sleep()); + auto five = std::chrono::system_clock::now(); + delta = five - four; + ASSERT_TRUE(epsilon > delta); + } + + { + auto start = std::chrono::steady_clock::now(); + +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + + rclcpp::GenericRate gr(period); + ASSERT_TRUE(gr.is_steady()); + +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + + ASSERT_EQ(gr.get_type(), RCL_STEADY_TIME); + EXPECT_EQ(period, gr.period()); + ASSERT_TRUE(gr.sleep()); + auto one = std::chrono::steady_clock::now(); + auto delta = one - start; + EXPECT_LT(period, delta); + EXPECT_GT(period * overrun_ratio, delta); + + rclcpp::sleep_for(offset); + ASSERT_TRUE(gr.sleep()); + auto two = std::chrono::steady_clock::now(); + delta = two - start; + EXPECT_LT(2 * period, delta + epsilon); + EXPECT_GT(2 * period * overrun_ratio, delta); + + rclcpp::sleep_for(offset); + auto two_offset = std::chrono::steady_clock::now(); + gr.reset(); + ASSERT_TRUE(gr.sleep()); + auto three = std::chrono::steady_clock::now(); + delta = three - two_offset; + EXPECT_LT(period, delta); + EXPECT_GT(period * overrun_ratio, delta); + + rclcpp::sleep_for(offset + period); + auto four = std::chrono::steady_clock::now(); + ASSERT_FALSE(gr.sleep()); + auto five = std::chrono::steady_clock::now(); + delta = five - four; + EXPECT_GT(epsilon, delta); + } } TEST(TestRate, from_double) { { - rclcpp::WallRate rate(1.0); - EXPECT_EQ(std::chrono::seconds(1), rate.period()); + rclcpp::Rate rate(1.0); + EXPECT_EQ(rclcpp::Duration(std::chrono::seconds(1)), rate.period()); } { rclcpp::WallRate rate(2.0); - EXPECT_EQ(std::chrono::milliseconds(500), rate.period()); + EXPECT_EQ(rclcpp::Duration(std::chrono::milliseconds(500)), rate.period()); } { rclcpp::WallRate rate(0.5); - EXPECT_EQ(std::chrono::seconds(2), rate.period()); + EXPECT_EQ(rclcpp::Duration(std::chrono::seconds(2)), rate.period()); } { - rclcpp::WallRate rate(4.0); - EXPECT_EQ(std::chrono::milliseconds(250), rate.period()); + rclcpp::ROSRate rate(4.0); + EXPECT_EQ(rclcpp::Duration(std::chrono::milliseconds(250)), rate.period()); } } + +TEST(TestRate, clock_types) { + { + rclcpp::Rate rate(1.0, std::make_shared(RCL_SYSTEM_TIME)); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + EXPECT_FALSE(rate.is_steady()); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + EXPECT_EQ(RCL_SYSTEM_TIME, rate.get_type()); + } + { + rclcpp::Rate rate(1.0, std::make_shared(RCL_STEADY_TIME)); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + EXPECT_TRUE(rate.is_steady()); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + EXPECT_EQ(RCL_STEADY_TIME, rate.get_type()); + } + { + rclcpp::Rate rate(1.0, std::make_shared(RCL_ROS_TIME)); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + EXPECT_FALSE(rate.is_steady()); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + EXPECT_EQ(RCL_ROS_TIME, rate.get_type()); + } +} + +TEST(TestRate, incorrect_constuctor) { + // Constructor with 0-frequency + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Rate rate(0.0), + std::invalid_argument("rate must be greater than 0")); + + // Constructor with negative frequency + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Rate rate(-1.0), + std::invalid_argument("rate must be greater than 0")); + + // Constructor with 0-duration + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Rate rate(rclcpp::Duration(0, 0)), + std::invalid_argument("period must be greater than 0")); + + // Constructor with negative duration + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Rate rate(rclcpp::Duration(-1, 0)), + std::invalid_argument("period must be greater than 0")); +} From fa732b9ee8000714831568e14486099f2714003d Mon Sep 17 00:00:00 2001 From: AiVerisimilitude <133206333+AiVerisimilitude@users.noreply.github.com> Date: Sat, 2 Sep 2023 02:17:00 +0200 Subject: [PATCH 271/455] Fix C++20 allocator construct deprecation (#2292) Signed-off-by: Guilherme Rodrigues --- rclcpp/include/rclcpp/experimental/intra_process_manager.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index a152632a53..4c3a72b5d6 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -488,8 +488,8 @@ class IntraProcessManager if constexpr (rclcpp::TypeAdapter::is_specialized::value) { ROSMessageTypeAllocator ros_message_alloc(allocator); - auto ptr = ros_message_alloc.allocate(1); - ros_message_alloc.construct(ptr); + auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_alloc, 1); + ROSMessageTypeAllocatorTraits::construct(ros_message_alloc, ptr); ROSMessageTypeDeleter deleter; allocator::set_allocator_for_deleter(&deleter, &allocator); rclcpp::TypeAdapter::convert_to_ros_message(*message, *ptr); From 5e152d77d8144c074894de9a5bc7025aac7f5813 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Wed, 6 Sep 2023 22:24:51 +0800 Subject: [PATCH 272/455] Topic correct typeadapter deduction (#2294) * fix TypeAdapter deduction Signed-off-by: Chen Lihui --- .../experimental/intra_process_manager.hpp | 4 +- .../test_publisher_with_type_adapter.cpp | 57 ++++++++++++++----- 2 files changed, 45 insertions(+), 16 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 4c3a72b5d6..f5e00fa4f8 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -486,13 +486,13 @@ class IntraProcessManager "subscription use different allocator types, which is not supported"); } - if constexpr (rclcpp::TypeAdapter::is_specialized::value) { + if constexpr (rclcpp::TypeAdapter::is_specialized::value) { ROSMessageTypeAllocator ros_message_alloc(allocator); auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_alloc, 1); ROSMessageTypeAllocatorTraits::construct(ros_message_alloc, ptr); ROSMessageTypeDeleter deleter; allocator::set_allocator_for_deleter(&deleter, &allocator); - rclcpp::TypeAdapter::convert_to_ros_message(*message, *ptr); + rclcpp::TypeAdapter::convert_to_ros_message(*message, *ptr); auto ros_msg = std::unique_ptr(ptr, deleter); ros_message_subscription->provide_intra_process_message(std::move(ros_msg)); } else { diff --git a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp index c041d86aee..e96e1e3a38 100644 --- a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp @@ -152,33 +152,54 @@ TEST_F(TestPublisher, conversion_exception_is_passed_up) { } } +using UseTakeSharedMethod = bool; +class TestPublisherFixture + : public TestPublisher, + public ::testing::WithParamInterface +{ +}; + /* * Testing that publisher sends type adapted types and ROS message types with intra proccess communications. */ -TEST_F( - TestPublisher, +TEST_P( + TestPublisherFixture, check_type_adapted_message_is_sent_and_received_intra_process) { using StringTypeAdapter = rclcpp::TypeAdapter; const std::string message_data = "Message Data"; const std::string topic_name = "topic_name"; bool is_received; - auto callback = - [message_data, &is_received]( - const rclcpp::msg::String::ConstSharedPtr msg, - const rclcpp::MessageInfo & message_info - ) -> void - { - is_received = true; - ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); - ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); - }; - auto node = rclcpp::Node::make_shared( "test_intra_process", rclcpp::NodeOptions().use_intra_process_comms(true)); auto pub = node->create_publisher(topic_name, 10); - auto sub = node->create_subscription(topic_name, 1, callback); + rclcpp::Subscription::SharedPtr sub; + if (GetParam()) { + auto callback = + [message_data, &is_received]( + const rclcpp::msg::String::ConstSharedPtr msg, + const rclcpp::MessageInfo & message_info + ) -> void + { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + sub = node->create_subscription(topic_name, 1, callback); + } else { + auto callback_unique = + [message_data, &is_received]( + rclcpp::msg::String::UniquePtr msg, + const rclcpp::MessageInfo & message_info + ) -> void + { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + sub = node->create_subscription(topic_name, 1, callback_unique); + } auto wait_for_message_to_be_received = [&is_received, &node]() { rclcpp::executors::SingleThreadedExecutor executor; @@ -239,6 +260,14 @@ TEST_F( } } +INSTANTIATE_TEST_SUITE_P( + TestPublisherFixtureWithParam, + TestPublisherFixture, + ::testing::Values( + true, // use take shared method + false // not use take shared method +)); + /* * Testing that publisher sends type adapted types and ROS message types with inter proccess communications. */ From a0148dfd5db71e3f0c73b2436418e4aa4a5fe88c Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 7 Sep 2023 14:59:26 +0000 Subject: [PATCH 273/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 11 +++++++++++ rclcpp_action/CHANGELOG.rst | 7 +++++++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 5 +++++ 4 files changed, 26 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index f2a84b1463..dc84313a64 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Topic correct typeadapter deduction (`#2294 `_) +* Fix C++20 allocator construct deprecation (`#2292 `_) +* Make Rate to select the clock to work with (`#2123 `_) +* Correct the position of a comment. (`#2290 `_) +* Remove unnecessary lambda captures in the tests. (`#2289 `_) +* Add rcl_logging_interface as an explicit dependency. (`#2284 `_) +* Revamp list_parameters to be more efficient and easier to read. (`#2282 `_) +* Contributors: AiVerisimilitude, Alexey Merzlyakov, Chen Lihui, Chris Lalancette, Jiaqi Li + 22.1.0 (2023-08-21) ------------------- * Do not crash Executor when send_response fails due to client failure. (`#2276 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 3e80fa6c92..698ee7b152 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,13 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Correct the position of a comment. (`#2290 `_) +* Fix a typo in a comment. (`#2283 `_) +* doc fix: call `canceled` only after goal state is in canceling. (`#2266 `_) +* Contributors: Chris Lalancette, Jiaqi Li, Tomoya Fujita + 22.1.0 (2023-08-21) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 336d432ba9..d9c95f70cf 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 22.1.0 (2023-08-21) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 5472a87a93..dcaa18713f 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* add logger level service to lifecycle node. (`#2277 `_) +* Contributors: Tomoya Fujita + 22.1.0 (2023-08-21) ------------------- * Stop using constref signature of benchmark DoNotOptimize. (`#2238 `_) From d5e5141b3de5b73515297567a3fa92f4996cbf51 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 7 Sep 2023 14:59:44 +0000 Subject: [PATCH 274/455] 22.2.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index dc84313a64..fb141ef7b1 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +22.2.0 (2023-09-07) +------------------- * Topic correct typeadapter deduction (`#2294 `_) * Fix C++20 allocator construct deprecation (`#2292 `_) * Make Rate to select the clock to work with (`#2123 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index e1c272d949..717790cf92 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 22.1.0 + 22.2.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 698ee7b152..d181a917d4 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +22.2.0 (2023-09-07) +------------------- * Correct the position of a comment. (`#2290 `_) * Fix a typo in a comment. (`#2283 `_) * doc fix: call `canceled` only after goal state is in canceling. (`#2266 `_) diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index b3539ee95f..1b1f1d20e5 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 22.1.0 + 22.2.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index d9c95f70cf..ab2048e2d2 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +22.2.0 (2023-09-07) +------------------- 22.1.0 (2023-08-21) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 0e00db2b56..68539562d3 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 22.1.0 + 22.2.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index dcaa18713f..e06a330e52 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +22.2.0 (2023-09-07) +------------------- * add logger level service to lifecycle node. (`#2277 `_) * Contributors: Tomoya Fujita diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index aece0e1916..fc416ee584 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 22.1.0 + 22.2.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 253d395d4e3dc6f9491f24e924f88805cd9407a2 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 7 Sep 2023 15:17:16 -0400 Subject: [PATCH 275/455] Cleanup flaky timers_manager tests. (#2299) * Cleanup flaky timers_manager tests. The timers_manager tests were relying too heavily on specific timings; this caused them to be flaky on the buildfarm, particularly on Windows. Here, we increase the timeouts, and remove one test which just relies too heavily on specific timeouts. This should make this test much less flaky on the buildfarm. Signed-off-by: Chris Lalancette --- .../rclcpp/experimental/timers_manager.hpp | 2 +- .../rclcpp/experimental/timers_manager.cpp | 4 +- rclcpp/test/rclcpp/test_timers_manager.cpp | 80 +++---------------- 3 files changed, 14 insertions(+), 72 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/timers_manager.hpp b/rclcpp/include/rclcpp/experimental/timers_manager.hpp index e8830c01a0..688684376a 100644 --- a/rclcpp/include/rclcpp/experimental/timers_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/timers_manager.hpp @@ -527,7 +527,7 @@ class TimersManager void execute_ready_timers_unsafe(); // Callback to be called when timer is ready - std::function on_ready_callback_ = nullptr; + std::function on_ready_callback_; // Thread used to run the timers execution task std::thread timers_thread_; diff --git a/rclcpp/src/rclcpp/experimental/timers_manager.cpp b/rclcpp/src/rclcpp/experimental/timers_manager.cpp index f4ecd16b76..e179787b36 100644 --- a/rclcpp/src/rclcpp/experimental/timers_manager.cpp +++ b/rclcpp/src/rclcpp/experimental/timers_manager.cpp @@ -28,9 +28,9 @@ using rclcpp::experimental::TimersManager; TimersManager::TimersManager( std::shared_ptr context, std::function on_ready_callback) +: on_ready_callback_(on_ready_callback), + context_(context) { - context_ = context; - on_ready_callback_ = on_ready_callback; } TimersManager::~TimersManager() diff --git a/rclcpp/test/rclcpp/test_timers_manager.cpp b/rclcpp/test/rclcpp/test_timers_manager.cpp index 635ec322c0..167523934c 100644 --- a/rclcpp/test/rclcpp/test_timers_manager.cpp +++ b/rclcpp/test/rclcpp/test_timers_manager.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include @@ -65,8 +66,10 @@ TEST_F(TestTimersManager, empty_manager) TEST_F(TestTimersManager, add_run_remove_timer) { size_t t_runs = 0; + std::chrono::milliseconds timer_period(10); + auto t = TimerT::make_shared( - 10ms, + timer_period, [&t_runs]() { t_runs++; }, @@ -79,7 +82,7 @@ TEST_F(TestTimersManager, add_run_remove_timer) timers_manager->add_timer(t); // Sleep for more 3 times the timer period - std::this_thread::sleep_for(30ms); + std::this_thread::sleep_for(3 * timer_period); // The timer is executed only once, even if we slept 3 times the period execute_all_ready_timers(timers_manager); @@ -191,67 +194,6 @@ TEST_F(TestTimersManager, head_not_ready) EXPECT_EQ(0u, t_runs); } -TEST_F(TestTimersManager, timers_order) -{ - auto timers_manager = std::make_shared( - rclcpp::contexts::get_global_default_context()); - - size_t t1_runs = 0; - auto t1 = TimerT::make_shared( - 10ms, - [&t1_runs]() { - t1_runs++; - }, - rclcpp::contexts::get_global_default_context()); - - size_t t2_runs = 0; - auto t2 = TimerT::make_shared( - 30ms, - [&t2_runs]() { - t2_runs++; - }, - rclcpp::contexts::get_global_default_context()); - - size_t t3_runs = 0; - auto t3 = TimerT::make_shared( - 100ms, - [&t3_runs]() { - t3_runs++; - }, - rclcpp::contexts::get_global_default_context()); - - // Add timers in a random order - timers_manager->add_timer(t2); - timers_manager->add_timer(t3); - timers_manager->add_timer(t1); - - std::this_thread::sleep_for(10ms); - execute_all_ready_timers(timers_manager); - EXPECT_EQ(1u, t1_runs); - EXPECT_EQ(0u, t2_runs); - EXPECT_EQ(0u, t3_runs); - - std::this_thread::sleep_for(30ms); - execute_all_ready_timers(timers_manager); - EXPECT_EQ(2u, t1_runs); - EXPECT_EQ(1u, t2_runs); - EXPECT_EQ(0u, t3_runs); - - std::this_thread::sleep_for(100ms); - execute_all_ready_timers(timers_manager); - EXPECT_EQ(3u, t1_runs); - EXPECT_EQ(2u, t2_runs); - EXPECT_EQ(1u, t3_runs); - - timers_manager->remove_timer(t1); - - std::this_thread::sleep_for(30ms); - execute_all_ready_timers(timers_manager); - EXPECT_EQ(3u, t1_runs); - EXPECT_EQ(3u, t2_runs); - EXPECT_EQ(1u, t3_runs); -} - TEST_F(TestTimersManager, start_stop_timers_thread) { auto timers_manager = std::make_shared( @@ -274,7 +216,7 @@ TEST_F(TestTimersManager, timers_thread) auto timers_manager = std::make_shared( rclcpp::contexts::get_global_default_context()); - size_t t1_runs = 0; + int t1_runs = 0; auto t1 = TimerT::make_shared( 1ms, [&t1_runs]() { @@ -282,7 +224,7 @@ TEST_F(TestTimersManager, timers_thread) }, rclcpp::contexts::get_global_default_context()); - size_t t2_runs = 0; + int t2_runs = 0; auto t2 = TimerT::make_shared( 1ms, [&t2_runs]() { @@ -296,12 +238,12 @@ TEST_F(TestTimersManager, timers_thread) // Run timers thread for a while timers_manager->start(); - std::this_thread::sleep_for(5ms); + std::this_thread::sleep_for(50ms); timers_manager->stop(); EXPECT_LT(1u, t1_runs); EXPECT_LT(1u, t2_runs); - EXPECT_EQ(t1_runs, t2_runs); + EXPECT_LE(std::abs(t1_runs - t2_runs), 1); } TEST_F(TestTimersManager, destructor) @@ -365,13 +307,13 @@ TEST_F(TestTimersManager, add_remove_while_thread_running) timers_manager->start(); // After a while remove t1 and add t2 - std::this_thread::sleep_for(5ms); + std::this_thread::sleep_for(50ms); timers_manager->remove_timer(t1); size_t tmp_t1 = t1_runs; timers_manager->add_timer(t2); // Wait some more time and then stop - std::this_thread::sleep_for(5ms); + std::this_thread::sleep_for(50ms); timers_manager->stop(); // t1 has stopped running From e103b8d37ed14216b495b75c72dda82cb7f98c6f Mon Sep 17 00:00:00 2001 From: jmachowinski Date: Thu, 7 Sep 2023 23:37:35 +0200 Subject: [PATCH 276/455] fix(ClientGoalHandle): Made mutex recursive to prevent deadlocks (#2267) * fix(ClientGoalHandle): Made mutex recursive to prevent deadlocks This prevents deadlocks in cases, were e.g. get_status() would be called on the handle in a callback of the handle. * test(rclcpp_action): Added test for deadlocks during access of a goal handle This test checks, if the code deadlocks, if methods on the goal handle are called from the callbacks. Signed-off-by: Janosch Machowinski Co-authored-by: Tomoya Fujita --- .../rclcpp_action/client_goal_handle.hpp | 2 +- .../rclcpp_action/client_goal_handle_impl.hpp | 22 ++--- rclcpp_action/test/test_client.cpp | 80 +++++++++++++++++++ 3 files changed, 92 insertions(+), 12 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index dd3a40671c..73987ec887 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -163,7 +163,7 @@ class ClientGoalHandle ResultCallback result_callback_{nullptr}; int8_t status_{GoalStatus::STATUS_ACCEPTED}; - std::mutex handle_mutex_; + std::recursive_mutex handle_mutex_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 0c25e52433..d12b4fc5b3 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -59,7 +59,7 @@ template std::shared_future::WrappedResult> ClientGoalHandle::async_get_result() { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); if (!is_result_aware_) { throw exceptions::UnawareGoalHandleError(); } @@ -70,7 +70,7 @@ template void ClientGoalHandle::set_result(const WrappedResult & wrapped_result) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); status_ = static_cast(wrapped_result.code); result_promise_.set_value(wrapped_result); if (result_callback_) { @@ -82,7 +82,7 @@ template void ClientGoalHandle::set_feedback_callback(FeedbackCallback callback) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); feedback_callback_ = callback; } @@ -90,7 +90,7 @@ template void ClientGoalHandle::set_result_callback(ResultCallback callback) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); result_callback_ = callback; } @@ -98,7 +98,7 @@ template int8_t ClientGoalHandle::get_status() { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); return status_; } @@ -106,7 +106,7 @@ template void ClientGoalHandle::set_status(int8_t status) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); status_ = status; } @@ -114,7 +114,7 @@ template bool ClientGoalHandle::is_feedback_aware() { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); return feedback_callback_ != nullptr; } @@ -122,7 +122,7 @@ template bool ClientGoalHandle::is_result_aware() { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); return is_result_aware_; } @@ -130,7 +130,7 @@ template bool ClientGoalHandle::set_result_awareness(bool awareness) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); bool previous = is_result_aware_; is_result_aware_ = awareness; return previous; @@ -140,7 +140,7 @@ template void ClientGoalHandle::invalidate(const exceptions::UnawareGoalHandleError & ex) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); // Guard against multiple calls if (is_invalidated()) { return; @@ -168,7 +168,7 @@ ClientGoalHandle::call_feedback_callback( RCLCPP_ERROR(rclcpp::get_logger("rclcpp_action"), "Sent feedback to wrong goal handle."); return; } - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); if (nullptr == feedback_callback_) { // Normal, some feedback messages may arrive after the goal result. RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"), "Received feedback but goal ignores it."); diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index b94a82d500..b180c3f825 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -852,6 +852,86 @@ TEST_F(TestClientAgainstServer, async_cancel_some_goals_with_callback) EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status()); } +TEST_F(TestClientAgainstServer, deadlock_in_callbacks) +{ + std::atomic feedback_callback_called = false; + std::atomic response_callback_called = false; + std::atomic result_callback_called = false; + std::atomic no_deadlock = false; + + std::thread tr = std::thread( + [&]() { + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + + using GoalHandle = rclcpp_action::ClientGoalHandle; + rclcpp_action::Client::SendGoalOptions ops; + ops.feedback_callback = + [&feedback_callback_called](const GoalHandle::SharedPtr handle, + ActionType::Feedback::ConstSharedPtr) { + // call functions on the handle that acquire the lock + handle->get_status(); + handle->is_feedback_aware(); + handle->is_result_aware(); + + feedback_callback_called = true; + }; + ops.goal_response_callback = [&response_callback_called]( + const GoalHandle::SharedPtr & handle) { + // call functions on the handle that acquire the lock + handle->get_status(); + handle->is_feedback_aware(); + handle->is_result_aware(); + + response_callback_called = true; + }; + ops.result_callback = [&result_callback_called]( + const GoalHandle::WrappedResult &) { + result_callback_called = true; + }; + + goal.order = 6; + auto future_goal_handle = action_client->async_send_goal(goal, ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + + ASSERT_TRUE(goal_handle); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); + + auto result_future = action_client->async_get_result(goal_handle); + dual_spin_until_future_complete(result_future); + + EXPECT_TRUE(result_future.valid()); + auto result = result_future.get(); + + no_deadlock = true; + }); + + auto start_time = std::chrono::system_clock::now(); + + while (std::chrono::system_clock::now() - start_time < std::chrono::milliseconds(2000) && + !no_deadlock) + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + if (no_deadlock) { + tr.join(); + } else { + // In case of a failure, the thread is assumed to be in a deadlock. + // We detach the thread so we don't block further tests. + tr.detach(); + } + + EXPECT_TRUE(no_deadlock); + EXPECT_TRUE(response_callback_called); + EXPECT_TRUE(result_callback_called); + EXPECT_TRUE(feedback_callback_called); +} + TEST_F(TestClientAgainstServer, send_rcl_errors) { auto action_client = rclcpp_action::create_client(client_node, action_name); From 38734d769afcc53163c7a8efbca9946999ba8020 Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Fri, 8 Sep 2023 10:59:15 -0700 Subject: [PATCH 277/455] Update API docs links in package READMEs (#2302) Signed-off-by: Christophe Bedard --- rclcpp/README.md | 2 +- rclcpp_action/README.md | 3 ++- rclcpp_components/README.md | 2 +- rclcpp_lifecycle/README.md | 3 ++- 4 files changed, 6 insertions(+), 4 deletions(-) diff --git a/rclcpp/README.md b/rclcpp/README.md index 55c25f4780..d3c390b449 100644 --- a/rclcpp/README.md +++ b/rclcpp/README.md @@ -2,7 +2,7 @@ The ROS client library in C++. -Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components and features. +The link to the latest rclcpp API documentation, which includes a complete list of its main components and features, can be found on the rclcpp package info page, at the [ROS Index](https://index.ros.org/p/rclcpp/). ## Quality Declaration diff --git a/rclcpp_action/README.md b/rclcpp_action/README.md index c98987c922..dbb91a6ba3 100644 --- a/rclcpp_action/README.md +++ b/rclcpp_action/README.md @@ -2,7 +2,8 @@ Adds action APIs for C++. -Visit the [rclcpp_action API documentation](http://docs.ros2.org/latest/api/rclcpp_action/) for a complete list of its main components and features. For more information about Actions in ROS 2, see the [design document](http://design.ros2.org/articles/actions.html). +The link to the latest rclcpp_action API documentation, which includes a complete list of its main components and features, can be found on the rclcpp_action package info page, at the [ROS Index](https://index.ros.org/p/rclcpp_action/). +For more information about Actions in ROS 2, see the [design document](http://design.ros2.org/articles/actions.html). ## Quality Declaration diff --git a/rclcpp_components/README.md b/rclcpp_components/README.md index 829bcbc888..0b0e0a7aec 100644 --- a/rclcpp_components/README.md +++ b/rclcpp_components/README.md @@ -2,7 +2,7 @@ Package containing tools for dynamically loadable components. -Visit the [rclcpp_components API documentation](http://docs.ros2.org/latest/api/rclcpp_components/) for a complete list of its main components and features. +The link to the latest rclcpp_components API documentation, which includes a complete list of its main components and features, can be found on the rclcpp_components package info page, at the [ROS Index](https://index.ros.org/p/rclcpp_components/). ## Quality Declaration diff --git a/rclcpp_lifecycle/README.md b/rclcpp_lifecycle/README.md index d5bcd96479..e182390136 100644 --- a/rclcpp_lifecycle/README.md +++ b/rclcpp_lifecycle/README.md @@ -2,7 +2,8 @@ Package containing a prototype for lifecycle implementation. -Visit the [rclcpp_lifecycle API documentation](http://docs.ros2.org/latest/api/rclcpp_lifecycle/) for a complete list of its main components and features. For more information about LifeCycle in ROS 2, see the [design document](http://design.ros2.org/articles/node_lifecycle.html). +The link to the latest rclcpp_lifecycle API documentation, which includes a complete list of its main components and features, can be found on the rclcpp_lifecycle package info page, at the [ROS Index](https://index.ros.org/p/rclcpp_lifecycle/). +For more information about LifeCycle in ROS 2, see the [design document](http://design.ros2.org/articles/node_lifecycle.html). ## Quality Declaration From dd6fad6d4280939d920a708987b38593aaf36538 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 8 Sep 2023 16:40:09 -0400 Subject: [PATCH 278/455] Fix the return type of Rate::period. (#2301) In a recent commit (bc435776a257fcf76c5b0124bec26f6824342e34), we reworked how the Rate class worked so it could be used with ROS time as well. Unfortunately, we also accidentally broke the API of it by changing the return type of Rate::period to a Duration instead of a std::chrono::nanoseconds . Put this back to a std::chrono::nanoseconds; if we want to change it to a Duration, we'll have to add a new method and deprecate this one. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/rate.hpp | 2 +- rclcpp/src/rclcpp/rate.cpp | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 884e462a76..381e8d7908 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -163,7 +163,7 @@ class Rate : public RateBase reset(); RCLCPP_PUBLIC - Duration + std::chrono::nanoseconds period() const; private: diff --git a/rclcpp/src/rclcpp/rate.cpp b/rclcpp/src/rclcpp/rate.cpp index 04a1f57185..9a6e3d486b 100644 --- a/rclcpp/src/rclcpp/rate.cpp +++ b/rclcpp/src/rclcpp/rate.cpp @@ -14,6 +14,7 @@ #include "rclcpp/rate.hpp" +#include #include namespace rclcpp @@ -87,10 +88,10 @@ Rate::reset() last_interval_ = clock_->now(); } -Duration +std::chrono::nanoseconds Rate::period() const { - return period_; + return std::chrono::nanoseconds(period_.nanoseconds()); } WallRate::WallRate(const double rate) From f496291e81b3e925e118f2c0d4ab7e0f8092df8f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 8 Sep 2023 20:46:51 +0000 Subject: [PATCH 279/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 7 +++++++ rclcpp_action/CHANGELOG.rst | 6 ++++++ rclcpp_components/CHANGELOG.rst | 5 +++++ rclcpp_lifecycle/CHANGELOG.rst | 5 +++++ 4 files changed, 23 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index fb141ef7b1..c90eda9e02 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix the return type of Rate::period. (`#2301 `_) +* Update API docs links in package READMEs (`#2302 `_) +* Cleanup flaky timers_manager tests. (`#2299 `_) +* Contributors: Chris Lalancette, Christophe Bedard + 22.2.0 (2023-09-07) ------------------- * Topic correct typeadapter deduction (`#2294 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index d181a917d4..e3ef128dd0 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,12 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update API docs links in package READMEs (`#2302 `_) +* fix(ClientGoalHandle): Made mutex recursive to prevent deadlocks (`#2267 `_) +* Contributors: Christophe Bedard, jmachowinski + 22.2.0 (2023-09-07) ------------------- * Correct the position of a comment. (`#2290 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index ab2048e2d2..cf65aa83f7 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update API docs links in package READMEs (`#2302 `_) +* Contributors: Christophe Bedard + 22.2.0 (2023-09-07) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index e06a330e52..5b1443bba3 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update API docs links in package READMEs (`#2302 `_) +* Contributors: Christophe Bedard + 22.2.0 (2023-09-07) ------------------- * add logger level service to lifecycle node. (`#2277 `_) From ea31f94eb453acf3f34b37b85e38284abc514d57 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 8 Sep 2023 20:47:00 +0000 Subject: [PATCH 280/455] 23.0.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index c90eda9e02..ba690c6d24 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +23.0.0 (2023-09-08) +------------------- * Fix the return type of Rate::period. (`#2301 `_) * Update API docs links in package READMEs (`#2302 `_) * Cleanup flaky timers_manager tests. (`#2299 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 717790cf92..65250590e3 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 22.2.0 + 23.0.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index e3ef128dd0..447f07b0cc 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +23.0.0 (2023-09-08) +------------------- * Update API docs links in package READMEs (`#2302 `_) * fix(ClientGoalHandle): Made mutex recursive to prevent deadlocks (`#2267 `_) * Contributors: Christophe Bedard, jmachowinski diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 1b1f1d20e5..6c6ddb7f93 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 22.2.0 + 23.0.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index cf65aa83f7..58cfffac7e 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +23.0.0 (2023-09-08) +------------------- * Update API docs links in package READMEs (`#2302 `_) * Contributors: Christophe Bedard diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 68539562d3..ad90b595a4 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 22.2.0 + 23.0.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 5b1443bba3..275bb3428a 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +23.0.0 (2023-09-08) +------------------- * Update API docs links in package READMEs (`#2302 `_) * Contributors: Christophe Bedard diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index fc416ee584..850923df60 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 22.2.0 + 23.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From c42745c5baea249ec6159f1100a9a0d745510872 Mon Sep 17 00:00:00 2001 From: "Minju, Lee" <70446214+leeminju531@users.noreply.github.com> Date: Tue, 19 Sep 2023 06:13:11 +0900 Subject: [PATCH 281/455] fix the depth to relative in list_parameters (#2300) * fix the depth to relative in list_parameters Signed-off-by: leeminju531 --- rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp | 2 +- rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index b4c899ff9b..922ce9e4d1 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -1059,7 +1059,7 @@ NodeParameters::list_parameters(const std::vector & prefixes, uint6 if (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) { return true; } - std::string substr = kv.first.substr(prefix.length()); + std::string substr = kv.first.substr(prefix.length() + 1); return separators_less_than_depth(substr); } return false; diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp index 9113c96ca5..16884489be 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp @@ -95,15 +95,15 @@ TEST_F(TestNodeParameters, list_parameters) std::find(list_result2.names.begin(), list_result2.names.end(), parameter_name), list_result2.names.end()); - // Check prefixes + // Check prefixes and the depth relative to the given prefixes const std::string parameter_name2 = "prefix.new_parameter"; const rclcpp::ParameterValue value2(true); const rcl_interfaces::msg::ParameterDescriptor descriptor2; const auto added_parameter_value2 = node_parameters->declare_parameter(parameter_name2, value2, descriptor2, false); - EXPECT_EQ(value.get(), added_parameter_value.get()); + EXPECT_EQ(value2.get(), added_parameter_value2.get()); prefixes = {"prefix"}; - auto list_result3 = node_parameters->list_parameters(prefixes, 2u); + auto list_result3 = node_parameters->list_parameters(prefixes, 1u); EXPECT_EQ(1u, list_result3.names.size()); EXPECT_NE( std::find(list_result3.names.begin(), list_result3.names.end(), parameter_name2), From 9284d7cefa9e37ce3b6926a44e6177d6e4a4b62a Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Thu, 21 Sep 2023 18:44:13 -0700 Subject: [PATCH 282/455] Decouple rosout publisher init from node init. (#2174) Signed-off-by: Tomoya Fujita --- .../src/rclcpp/node_interfaces/node_base.cpp | 45 ++++++++++++------- 1 file changed, 29 insertions(+), 16 deletions(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 36e1afb932..7b13880157 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -21,6 +21,8 @@ #include "rcl/arguments.h" #include "rcl/node_type_cache.h" +#include "rcl/logging.h" +#include "rcl/logging_rosout.h" #include "rclcpp/exceptions.hpp" #include "rcutils/logging_macros.h" #include "rmw/validate_namespace.h" @@ -55,17 +57,12 @@ NodeBase::NodeBase( std::shared_ptr logging_mutex = get_global_logging_mutex(); rcl_ret_t ret; - { - std::lock_guard guard(*logging_mutex); - // TODO(ivanpauno): /rosout Qos should be reconfigurable. - // TODO(ivanpauno): Instead of mutually excluding rcl_node_init with the global logger mutex, - // rcl_logging_rosout_init_publisher_for_node could be decoupled from there and be called - // here directly. - ret = rcl_node_init( - rcl_node.get(), - node_name.c_str(), namespace_.c_str(), - context_->get_rcl_context().get(), &rcl_node_options); - } + + // TODO(ivanpauno): /rosout Qos should be reconfigurable. + ret = rcl_node_init( + rcl_node.get(), + node_name.c_str(), namespace_.c_str(), + context_->get_rcl_context().get(), &rcl_node_options); if (ret != RCL_RET_OK) { if (ret == RCL_RET_NODE_INVALID_NAME) { rcl_reset_error(); // discard rcl_node_init error @@ -115,13 +112,29 @@ NodeBase::NodeBase( throw_from_rcl_error(ret, "failed to initialize rcl node"); } + // The initialization for the rosout publisher + if (rcl_logging_rosout_enabled() && rcl_node_options.enable_rosout) { + std::lock_guard guard(*logging_mutex); + ret = rcl_logging_rosout_init_publisher_for_node(rcl_node.get()); + if (ret != RCL_RET_OK) { + throw_from_rcl_error(ret, "failed to initialize rosout publisher"); + } + } + node_handle_.reset( rcl_node.release(), - [logging_mutex](rcl_node_t * node) -> void { - std::lock_guard guard(*logging_mutex); - // TODO(ivanpauno): Instead of mutually excluding rcl_node_fini with the global logger mutex, - // rcl_logging_rosout_fini_publisher_for_node could be decoupled from there and be called - // here directly. + [logging_mutex, rcl_node_options](rcl_node_t * node) -> void { + { + std::lock_guard guard(*logging_mutex); + if (rcl_logging_rosout_enabled() && rcl_node_options.enable_rosout) { + rcl_ret_t ret = rcl_logging_rosout_fini_publisher_for_node(node); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rosout publisher: %s", rcl_get_error_string().str); + } + } + } if (rcl_node_fini(node) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", From 13182b5aad7b9aadfd77b30de5d5534f7f2b833f Mon Sep 17 00:00:00 2001 From: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Date: Wed, 27 Sep 2023 17:14:53 -0400 Subject: [PATCH 283/455] Documentation for list_parameters (#2315) * list_parameters documentation Signed-off-by: CursedRock17 --- rclcpp/include/rclcpp/node.hpp | 11 ++++++++++- .../rclcpp/node_interfaces/test_node_parameters.cpp | 7 +++++++ 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 19abf4a602..c1ad85b417 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -972,7 +972,16 @@ class Node : public std::enable_shared_from_this /// Return a list of parameters with any of the given prefixes, up to the given depth. /** - * \todo: properly document and test this method. + * Parameters are separated into a hierarchy using the "." (dot) character. + * The "prefixes" argument is a way to select only particular parts of the hierarchy. + * + * \param[in] prefixes The list of prefixes that should be searched for within the + * current parameters. If this vector of prefixes is empty, then list_parameters + * will return all parameters. + * \param[in] depth An unsigned integer that represents the recursive depth to search. + * If this depth = 0, then all parameters that fit the prefixes will be returned. + * \returns A ListParametersResult message which contains both an array of unique prefixes + * and an array of names that were matched to the prefixes given. */ RCLCPP_PUBLIC rcl_interfaces::msg::ListParametersResult diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp index 16884489be..d262c67a9a 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp @@ -116,6 +116,13 @@ TEST_F(TestNodeParameters, list_parameters) EXPECT_NE( std::find(list_result4.names.begin(), list_result4.names.end(), parameter_name), list_result4.names.end()); + + // Return all parameters when the depth = 0 + auto list_result5 = node_parameters->list_parameters(prefixes, 0u); + EXPECT_EQ(1u, list_result5.names.size()); + EXPECT_NE( + std::find(list_result5.names.begin(), list_result5.names.end(), parameter_name), + list_result5.names.end()); } TEST_F(TestNodeParameters, parameter_overrides) From 9ff864c6ae2e2eb72f8975cff074a79ca56128d5 Mon Sep 17 00:00:00 2001 From: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Date: Thu, 28 Sep 2023 08:29:47 -0400 Subject: [PATCH 284/455] Removing Old Connext Tests (#2313) * Removing Old Connext Tests Signed-off-by: CursedRock17 --- rclcpp/test/rclcpp/CMakeLists.txt | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 8c31a95415..159f990f32 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -34,13 +34,7 @@ if(TARGET test_exceptions) target_link_libraries(test_exceptions ${PROJECT_NAME} mimick) endif() -# Increasing timeout because connext can take a long time to destroy nodes -# TODO(brawner) remove when destroying Node for Connext is resolved. See: -# https://github.com/ros2/rclcpp/issues/1250 -ament_add_gtest( - test_allocator_memory_strategy - strategies/test_allocator_memory_strategy.cpp - TIMEOUT 360) +ament_add_gtest(test_allocator_memory_strategy strategies/test_allocator_memory_strategy.cpp) if(TARGET test_allocator_memory_strategy) ament_target_dependencies(test_allocator_memory_strategy "rcl" @@ -669,8 +663,6 @@ if(TARGET test_interface_traits) target_link_libraries(test_interface_traits ${PROJECT_NAME}) endif() -# TODO(brawner) remove when destroying Node for Connext is resolved. See: -# https://github.com/ros2/rclcpp/issues/1250 ament_add_gtest( test_executors executors/test_executors.cpp From 7c1143dc1502d5dda99a3dfa17124a72dbaea90c Mon Sep 17 00:00:00 2001 From: Tully Foote Date: Fri, 29 Sep 2023 16:13:43 -0700 Subject: [PATCH 285/455] Update SignalHandler get_global_signal_handler to avoid complex types in static memory (#2316) * Update SignalHandler get_global_signal_handler to avoid complex types in static memory This was flagged by msan as a problem. There's a description of why this is a potential problem here: https://google.github.io/styleguide/cppguide.html#Static_and_Global_Variables Signed-off-by: Tully Foote Co-authored-by: William Woodall --- rclcpp/src/rclcpp/signal_handler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/signal_handler.cpp b/rclcpp/src/rclcpp/signal_handler.cpp index 7085c63bdf..cf26d06df4 100644 --- a/rclcpp/src/rclcpp/signal_handler.cpp +++ b/rclcpp/src/rclcpp/signal_handler.cpp @@ -113,7 +113,7 @@ SignalHandler::get_logger() SignalHandler & SignalHandler::get_global_signal_handler() { - static SignalHandler signal_handler; + static SignalHandler & signal_handler = *new SignalHandler(); return signal_handler; } From 623c3eb8747f4aa55985f0d2d29694c33a81767d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 3 Oct 2023 14:16:49 -0400 Subject: [PATCH 286/455] Add locking to protect the TimeSource::NodeState::node_base_ (#2320) We need this because it is possible for one thread to be handling the on_parameter_event callback while another one is detaching the node. This lock will protect that from happening. Signed-off-by: Chris Lalancette --- rclcpp/src/rclcpp/time_source.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index a0e1d00853..465ceaf5a7 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -236,6 +236,7 @@ class TimeSource::NodeState final rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface) { + std::lock_guard guard(node_base_lock_); node_base_ = node_base_interface; node_topics_ = node_topics_interface; node_graph_ = node_graph_interface; @@ -280,17 +281,14 @@ class TimeSource::NodeState final parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event( node_topics_, [this](std::shared_ptr event) { - if (node_base_ != nullptr) { - this->on_parameter_event(event); - } - // Do nothing if node_base_ is nullptr because it means the TimeSource is now - // without an attached node + this->on_parameter_event(event); }); } // Detach the attached node void detachNode() { + std::lock_guard guard(node_base_lock_); // destroy_clock_sub() *must* be first here, to ensure that the executor // can't possibly call any of the callbacks as we are cleaning up. destroy_clock_sub(); @@ -327,6 +325,7 @@ class TimeSource::NodeState final std::thread clock_executor_thread_; // Preserve the node reference + std::mutex node_base_lock_; rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_{nullptr}; rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_{nullptr}; rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_{nullptr}; @@ -464,6 +463,14 @@ class TimeSource::NodeState final // Callback for parameter updates void on_parameter_event(std::shared_ptr event) { + std::lock_guard guard(node_base_lock_); + + if (node_base_ == nullptr) { + // Do nothing if node_base_ is nullptr because it means the TimeSource is now + // without an attached node + return; + } + // Filter out events on 'use_sim_time' parameter instances in other nodes. if (event->node != node_base_->get_fully_qualified_name()) { return; From d6bd8baac5bc050ab31e4e7e8ee8b482fd469c14 Mon Sep 17 00:00:00 2001 From: Ignacio Vizzo Date: Wed, 4 Oct 2023 08:18:21 -0400 Subject: [PATCH 287/455] Add missing header required by the rclcpp::NodeOptions type (#2324) Signed-off-by: Ignacio Vizzo --- rclcpp_components/include/rclcpp_components/node_factory.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp_components/include/rclcpp_components/node_factory.hpp b/rclcpp_components/include/rclcpp_components/node_factory.hpp index 67e6cd7331..7b1f2dcae6 100644 --- a/rclcpp_components/include/rclcpp_components/node_factory.hpp +++ b/rclcpp_components/include/rclcpp_components/node_factory.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP_COMPONENTS__NODE_FACTORY_HPP__ #define RCLCPP_COMPONENTS__NODE_FACTORY_HPP__ +#include "rclcpp/node_options.hpp" #include "rclcpp_components/node_instance_wrapper.hpp" namespace rclcpp_components From 32438a6a6760e9f2f764f77cbf568fa6180cc75e Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 4 Oct 2023 13:08:56 +0000 Subject: [PATCH 288/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 10 ++++++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 5 +++++ rclcpp_lifecycle/CHANGELOG.rst | 3 +++ 4 files changed, 21 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index ba690c6d24..c176173ff6 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add locking to protect the TimeSource::NodeState::node_base\_ (`#2320 `_) +* Update SignalHandler get_global_signal_handler to avoid complex types in static memory (`#2316 `_) +* Removing Old Connext Tests (`#2313 `_) +* Documentation for list_parameters (`#2315 `_) +* Decouple rosout publisher init from node init. (`#2174 `_) +* fix the depth to relative in list_parameters (`#2300 `_) +* Contributors: Chris Lalancette, Lucas Wendland, Minju, Lee, Tomoya Fujita, Tully Foote + 23.0.0 (2023-09-08) ------------------- * Fix the return type of Rate::period. (`#2301 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 447f07b0cc..04dae886a7 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 23.0.0 (2023-09-08) ------------------- * Update API docs links in package READMEs (`#2302 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 58cfffac7e..181066c83c 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing header required by the rclcpp::NodeOptions type (`#2324 `_) +* Contributors: Ignacio Vizzo + 23.0.0 (2023-09-08) ------------------- * Update API docs links in package READMEs (`#2302 `_) diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 275bb3428a..3325952cb0 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 23.0.0 (2023-09-08) ------------------- * Update API docs links in package READMEs (`#2302 `_) From 0644f220a2f84f1f1f56c61b065228f6cc32420d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 4 Oct 2023 13:09:05 +0000 Subject: [PATCH 289/455] 23.1.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index c176173ff6..75185ede87 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +23.1.0 (2023-10-04) +------------------- * Add locking to protect the TimeSource::NodeState::node_base\_ (`#2320 `_) * Update SignalHandler get_global_signal_handler to avoid complex types in static memory (`#2316 `_) * Removing Old Connext Tests (`#2313 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 65250590e3..110cc63e72 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 23.0.0 + 23.1.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 04dae886a7..5a32cf54a9 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +23.1.0 (2023-10-04) +------------------- 23.0.0 (2023-09-08) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 6c6ddb7f93..4d96b76d52 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 23.0.0 + 23.1.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 181066c83c..f3734be502 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +23.1.0 (2023-10-04) +------------------- * Add missing header required by the rclcpp::NodeOptions type (`#2324 `_) * Contributors: Ignacio Vizzo diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index ad90b595a4..18c6eaec17 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 23.0.0 + 23.1.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 3325952cb0..f94a150d9a 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +23.1.0 (2023-10-04) +------------------- 23.0.0 (2023-09-08) ------------------- diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 850923df60..328d6d4db9 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 23.0.0 + 23.1.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 9019a8d9b7710bd405a5a31c8d74fdc9ffa686dd Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 5 Oct 2023 13:00:16 -0700 Subject: [PATCH 290/455] Adding API to copy all parameters from one node to another (#2304) Signed-off-by: stevemacenski --- .../rclcpp/copy_all_parameter_values.hpp | 82 +++++++++++++++++ rclcpp/include/rclcpp/rclcpp.hpp | 2 + rclcpp/test/rclcpp/CMakeLists.txt | 7 ++ .../rclcpp/test_copy_all_parameter_values.cpp | 88 +++++++++++++++++++ 4 files changed, 179 insertions(+) create mode 100644 rclcpp/include/rclcpp/copy_all_parameter_values.hpp create mode 100644 rclcpp/test/rclcpp/test_copy_all_parameter_values.cpp diff --git a/rclcpp/include/rclcpp/copy_all_parameter_values.hpp b/rclcpp/include/rclcpp/copy_all_parameter_values.hpp new file mode 100644 index 0000000000..cc61b621e1 --- /dev/null +++ b/rclcpp/include/rclcpp/copy_all_parameter_values.hpp @@ -0,0 +1,82 @@ +// Copyright 2023 Open Navigation LLC +// +// 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__COPY_ALL_PARAMETER_VALUES_HPP_ +#define RCLCPP__COPY_ALL_PARAMETER_VALUES_HPP_ + +#include +#include + +#include "rcl_interfaces/srv/list_parameters.hpp" +#include "rcl_interfaces/msg/parameter_descriptor.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" + +#include "rclcpp/parameter.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" + +namespace rclcpp +{ + +/** + * Copy all parameters from one source node to another destination node. + * May throw exceptions if parameters from source are uninitialized or undeclared. + * \param source Node to copy parameters from + * \param destination Node to copy parameters to + * \param override_existing_params Default false. Whether to override existing destination params + * if both the source and destination contain the same parameter. + */ +template +void +copy_all_parameter_values( + const NodeT1 & source, const NodeT2 & destination, const bool override_existing_params = false) +{ + using Parameters = std::vector; + using Descriptions = std::vector; + auto source_params = source->get_node_parameters_interface(); + auto dest_params = destination->get_node_parameters_interface(); + rclcpp::Logger logger = destination->get_node_logging_interface()->get_logger(); + + std::vector param_names = source_params->list_parameters({}, 0).names; + Parameters params = source_params->get_parameters(param_names); + Descriptions descriptions = source_params->describe_parameters(param_names); + + for (unsigned int idx = 0; idx != params.size(); idx++) { + if (!dest_params->has_parameter(params[idx].get_name())) { + dest_params->declare_parameter( + params[idx].get_name(), params[idx].get_parameter_value(), descriptions[idx]); + } else if (override_existing_params) { + try { + rcl_interfaces::msg::SetParametersResult result = + dest_params->set_parameters_atomically({params[idx]}); + if (!result.successful) { + // Parameter update rejected or read-only + RCLCPP_WARN( + logger, + "Unable to set parameter (%s): %s!", + params[idx].get_name().c_str(), result.reason.c_str()); + } + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + RCLCPP_WARN( + logger, + "Unable to set parameter (%s): incompatable parameter type (%s)!", + params[idx].get_name().c_str(), e.what()); + } + } + } +} + +} // namespace rclcpp + +#endif // RCLCPP__COPY_ALL_PARAMETER_VALUES_HPP_ diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index ef587578e2..1d5b9113b5 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -54,6 +54,7 @@ * - rclcpp::ParameterValue * - rclcpp::AsyncParametersClient * - rclcpp::SyncParametersClient + * - rclcpp::copy_all_parameter_values() * - rclcpp/parameter.hpp * - rclcpp/parameter_value.hpp * - rclcpp/parameter_client.hpp @@ -164,6 +165,7 @@ #include #include +#include "rclcpp/copy_all_parameter_values.hpp" #include "rclcpp/executors.hpp" #include "rclcpp/guard_condition.hpp" #include "rclcpp/logging.hpp" diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 159f990f32..afa12cd24d 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -75,6 +75,13 @@ if(TARGET test_client) ) target_link_libraries(test_client ${PROJECT_NAME} mimick) endif() +ament_add_gtest(test_copy_all_parameter_values test_copy_all_parameter_values.cpp) +if(TARGET test_copy_all_parameter_values) + ament_target_dependencies(test_copy_all_parameter_values + "rcl_interfaces" + ) + target_link_libraries(test_copy_all_parameter_values ${PROJECT_NAME}) +endif() ament_add_gtest(test_create_timer test_create_timer.cpp) if(TARGET test_create_timer) ament_target_dependencies(test_create_timer diff --git a/rclcpp/test/rclcpp/test_copy_all_parameter_values.cpp b/rclcpp/test/rclcpp/test_copy_all_parameter_values.cpp new file mode 100644 index 0000000000..e3020efd1e --- /dev/null +++ b/rclcpp/test/rclcpp/test_copy_all_parameter_values.cpp @@ -0,0 +1,88 @@ +// Copyright 2023 Open Navigation LLC +// +// 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. + +#include +#include "rclcpp/copy_all_parameter_values.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestNode : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNode, TestParamCopying) +{ + auto node1 = std::make_shared("test_node1"); + auto node2 = std::make_shared("test_node2"); + + // Tests for (1) multiple types, (2) recursion, (3) overriding values + node1->declare_parameter("Foo1", rclcpp::ParameterValue(std::string(("bar1")))); + node1->declare_parameter("Foo2", rclcpp::ParameterValue(0.123)); + node1->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("bar")))); + node1->declare_parameter("Foo.bar", rclcpp::ParameterValue(std::string(("steve")))); + node2->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("barz2")))); + + // Show Node2 is empty of Node1's parameters, but contains its own + EXPECT_FALSE(node2->has_parameter("Foo1")); + EXPECT_FALSE(node2->has_parameter("Foo2")); + EXPECT_FALSE(node2->has_parameter("Foo.bar")); + EXPECT_TRUE(node2->has_parameter("Foo")); + EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2")); + + bool override = false; + rclcpp::copy_all_parameter_values(node1, node2, override); + + // Test new parameters exist, of expected value, and original param is not overridden + EXPECT_TRUE(node2->has_parameter("Foo1")); + EXPECT_EQ(node2->get_parameter("Foo1").as_string(), std::string("bar1")); + EXPECT_TRUE(node2->has_parameter("Foo2")); + EXPECT_EQ(node2->get_parameter("Foo2").as_double(), 0.123); + EXPECT_TRUE(node2->has_parameter("Foo.bar")); + EXPECT_EQ(node2->get_parameter("Foo.bar").as_string(), std::string("steve")); + EXPECT_TRUE(node2->has_parameter("Foo")); + EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2")); + + // Test if parameter overrides are permissible that Node2's value is overridden + override = true; + rclcpp::copy_all_parameter_values(node1, node2, override); + EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("bar")); +} + +TEST_F(TestNode, TestParamCopyingExceptions) +{ + auto node1 = std::make_shared("test_node1"); + auto node2 = std::make_shared("test_node2"); + + // Tests for Parameter value conflicts handled + node1->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("bar")))); + node2->declare_parameter("Foo", rclcpp::ParameterValue(0.123)); + + bool override = true; + EXPECT_NO_THROW( + rclcpp::copy_all_parameter_values(node1, node2, override)); + + // Tests for Parameter read-only handled + node1->declare_parameter("Foo1", rclcpp::ParameterValue(std::string(("bar")))); + node2->declare_parameter("Foo1", rclcpp::ParameterValue(0.123)); + EXPECT_NO_THROW(rclcpp::copy_all_parameter_values(node1, node2, override)); +} From 77c7aaf9178268c16dee8fc412537e9f67d2afe1 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Fri, 6 Oct 2023 15:15:34 -0700 Subject: [PATCH 291/455] remove invalid sized allocation test for SerializedMessage. (#2330) Signed-off-by: Tomoya Fujita --- rclcpp/test/rclcpp/test_serialized_message.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/rclcpp/test/rclcpp/test_serialized_message.cpp b/rclcpp/test/rclcpp/test_serialized_message.cpp index 0a020bb7c0..67a2d9b2be 100644 --- a/rclcpp/test/rclcpp/test_serialized_message.cpp +++ b/rclcpp/test/rclcpp/test_serialized_message.cpp @@ -145,11 +145,6 @@ TEST(TestSerializedMessage, reserve) { // Resize using reserve method serialized_msg.reserve(15); EXPECT_EQ(15u, serialized_msg.capacity()); - - // Use invalid value throws exception - EXPECT_THROW( - {serialized_msg.reserve(-1);}, - rclcpp::exceptions::RCLBadAlloc); } TEST(TestSerializedMessage, serialization) { From 00b9d0a0189df442ef3f6044cf183eb8c2f42900 Mon Sep 17 00:00:00 2001 From: "Minju, Lee" <70446214+leeminju531@users.noreply.github.com> Date: Mon, 9 Oct 2023 23:36:00 +0900 Subject: [PATCH 292/455] add clients & services count (#2072) * add clients & services count * add count clients,services tests Signed-off-by: leeminju531 --- rclcpp/include/rclcpp/node.hpp | 20 +++++++++ .../rclcpp/node_interfaces/node_graph.hpp | 8 ++++ .../node_interfaces/node_graph_interface.hpp | 18 ++++++++ rclcpp/include/rclcpp/rclcpp.hpp | 3 ++ rclcpp/src/rclcpp/node.cpp | 12 +++++ .../src/rclcpp/node_interfaces/node_graph.cpp | 44 +++++++++++++++++++ .../node_interfaces/test_node_graph.cpp | 19 ++++++++ rclcpp/test/rclcpp/test_node.cpp | 3 ++ .../rclcpp_lifecycle/lifecycle_node.hpp | 16 +++++++ rclcpp_lifecycle/src/lifecycle_node.cpp | 12 +++++ rclcpp_lifecycle/test/test_lifecycle_node.cpp | 11 +++++ 11 files changed, 166 insertions(+) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index c1ad85b417..6521264b86 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -1314,6 +1314,26 @@ class Node : public std::enable_shared_from_this size_t count_subscribers(const std::string & topic_name) const; + /// Return the number of clients created for a given service. + /** + * \param[in] service_name the actual service name used; it will not be automatically remapped. + * \return number of clients that have been created for the given service. + * \throws std::runtime_error if clients could not be counted + */ + RCLCPP_PUBLIC + size_t + count_clients(const std::string & service_name) const; + + /// Return the number of services created for a given service. + /** + * \param[in] service_name the actual service name used; it will not be automatically remapped. + * \return number of services that have been created for the given service. + * \throws std::runtime_error if services could not be counted + */ + RCLCPP_PUBLIC + size_t + count_services(const std::string & service_name) const; + /// Return the topic endpoint information about publishers on a given topic. /** * The returned parameter is a list of topic endpoint information, where each item will contain diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp index d167515784..863dbee1bf 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -113,6 +113,14 @@ class NodeGraph : public NodeGraphInterface size_t count_subscribers(const std::string & topic_name) const override; + RCLCPP_PUBLIC + size_t + count_clients(const std::string & service_name) const override; + + RCLCPP_PUBLIC + size_t + count_services(const std::string & service_name) const override; + RCLCPP_PUBLIC const rcl_guard_condition_t * get_graph_guard_condition() const override; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp index 5967997ac7..80abc308c1 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -305,6 +305,24 @@ class NodeGraphInterface size_t count_subscribers(const std::string & topic_name) const = 0; + /// Return the number of clients created for a given service. + /* + * \param[in] service_name the actual service name used; it will not be automatically remapped. + */ + RCLCPP_PUBLIC + virtual + size_t + count_clients(const std::string & service_name) const = 0; + + /// Return the number of services created for a given service. + /* + * \param[in] service_name the actual service name used; it will not be automatically remapped. + */ + RCLCPP_PUBLIC + virtual + size_t + count_services(const std::string & service_name) const = 0; + /// Return the rcl guard condition which is triggered when the ROS graph changes. RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 1d5b9113b5..50af3f1a89 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -96,6 +96,9 @@ * - Get the number of publishers or subscribers on a topic: * - rclcpp::Node::count_publishers() * - rclcpp::Node::count_subscribers() + * - Get the number of clients or servers on a service: + * - rclcpp::Node::count_clients() + * - rclcpp::Node::count_services() * * And components related to logging: * diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index c31f4ee1f0..c31903f2fe 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -522,6 +522,18 @@ Node::count_subscribers(const std::string & topic_name) const return node_graph_->count_subscribers(topic_name); } +size_t +Node::count_clients(const std::string & service_name) const +{ + return node_graph_->count_clients(service_name); +} + +size_t +Node::count_services(const std::string & service_name) const +{ + return node_graph_->count_services(service_name); +} + std::vector Node::get_publishers_info_by_topic(const std::string & topic_name, bool no_mangle) const { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index 1228703cb6..f6e9e1fda0 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -498,6 +498,50 @@ NodeGraph::count_subscribers(const std::string & topic_name) const return count; } +size_t +NodeGraph::count_clients(const std::string & service_name) const +{ + auto rcl_node_handle = node_base_->get_rcl_node_handle(); + + auto fqdn = rclcpp::expand_topic_or_service_name( + service_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + true); + + size_t count; + auto ret = rcl_count_clients(rcl_node_handle, fqdn.c_str(), &count); + if (ret != RMW_RET_OK) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not count clients: ") + rmw_get_error_string().str); + // *INDENT-ON* + } + return count; +} + +size_t +NodeGraph::count_services(const std::string & service_name) const +{ + auto rcl_node_handle = node_base_->get_rcl_node_handle(); + + auto fqdn = rclcpp::expand_topic_or_service_name( + service_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + true); + + size_t count; + auto ret = rcl_count_services(rcl_node_handle, fqdn.c_str(), &count); + if (ret != RMW_RET_OK) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not count services: ") + rmw_get_error_string().str); + // *INDENT-ON* + } + return count; +} + const rcl_guard_condition_t * NodeGraph::get_graph_guard_condition() const { diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp index 70ccb5622d..3882d2b71c 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -129,6 +129,9 @@ TEST_F(TestNodeGraph, construct_from_node) EXPECT_EQ(0u, node_graph()->count_publishers("not_a_topic")); EXPECT_EQ(0u, node_graph()->count_subscribers("not_a_topic")); + EXPECT_EQ(0u, node_graph()->count_clients("not_a_service")); + EXPECT_EQ(0u, node_graph()->count_services("not_a_service")); + EXPECT_NE(nullptr, node_graph()->get_graph_guard_condition()); // get_graph_event is non-const @@ -534,6 +537,22 @@ TEST_F(TestNodeGraph, count_subscribers_rcl_error) std::runtime_error("could not count subscribers: error not set")); } +TEST_F(TestNodeGraph, count_clients_rcl_error) +{ + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_clients, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->count_clients("service"), + std::runtime_error("could not count clients: error not set")); +} + +TEST_F(TestNodeGraph, count_services_rcl_error) +{ + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_services, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->count_services("service"), + std::runtime_error("could not count services: error not set")); +} + TEST_F(TestNodeGraph, notify_shutdown) { EXPECT_NO_THROW(node()->get_node_graph_interface()->notify_shutdown()); diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index 2f74a998c3..ad73aadc2a 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -3311,6 +3311,9 @@ TEST_F(TestNode, get_entity_names) { const auto service_names_and_types = node->get_service_names_and_types(); EXPECT_EQ(service_names_and_types.end(), service_names_and_types.find("service")); + EXPECT_EQ(0u, node->count_clients("service")); + EXPECT_EQ(0u, node->count_services("service")); + const auto service_names_and_types_by_node = node->get_service_names_and_types_by_node("node", "/ns"); EXPECT_EQ( diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 097537b53a..657ddddc34 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -690,6 +690,22 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, size_t count_subscribers(const std::string & topic_name) const; + /// Return the number of clients created for a given service. + /** + * \sa rclcpp::Node::count_clients + */ + RCLCPP_LIFECYCLE_PUBLIC + size_t + count_clients(const std::string & service_name) const; + + /// Return the number of services created for a given service. + /** + * \sa rclcpp::Node::count_services + */ + RCLCPP_LIFECYCLE_PUBLIC + size_t + count_services(const std::string & service_name) const; + /// Return the topic endpoint information about publishers on a given topic. /** * \sa rclcpp::Node::get_publishers_info_by_topic diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index a85d661e63..0c448cf5e6 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -386,6 +386,18 @@ LifecycleNode::count_subscribers(const std::string & topic_name) const return node_graph_->count_subscribers(topic_name); } +size_t +LifecycleNode::count_clients(const std::string & service_name) const +{ + return node_graph_->count_clients(service_name); +} + +size_t +LifecycleNode::count_services(const std::string & service_name) const +{ + return node_graph_->count_services(service_name); +} + std::vector LifecycleNode::get_publishers_info_by_topic(const std::string & topic_name, bool no_mangle) const { diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index ddcc926527..fdb9be6153 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -726,6 +726,17 @@ TEST_F(TestDefaultStateMachine, test_graph_services) { EXPECT_STREQ( service_names_and_types["/testnode/get_transition_graph"][0].c_str(), "lifecycle_msgs/srv/GetAvailableTransitions"); + + EXPECT_EQ(0u, test_node->count_clients("/testnode/change_state")); + EXPECT_EQ(0u, test_node->count_clients("/testnode/get_available_states")); + EXPECT_EQ(0u, test_node->count_clients("/testnode/get_available_transitions")); + EXPECT_EQ(0u, test_node->count_clients("/testnode/get_state")); + EXPECT_EQ(0u, test_node->count_clients("/testnode/get_transition_graph")); + EXPECT_EQ(1u, test_node->count_services("/testnode/change_state")); + EXPECT_EQ(1u, test_node->count_services("/testnode/get_available_states")); + EXPECT_EQ(1u, test_node->count_services("/testnode/get_available_transitions")); + EXPECT_EQ(1u, test_node->count_services("/testnode/get_state")); + EXPECT_EQ(1u, test_node->count_services("/testnode/get_transition_graph")); } TEST_F(TestDefaultStateMachine, test_graph_services_by_node) { From e77c1fe5504312920241ca94a977b483162674ee Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 9 Oct 2023 15:31:47 +0000 Subject: [PATCH 293/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 7 +++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 5 +++++ 4 files changed, 18 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 75185ede87..f84322c35b 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* add clients & services count (`#2072 `_) +* remove invalid sized allocation test for SerializedMessage. (`#2330 `_) +* Adding API to copy all parameters from one node to another (`#2304 `_) +* Contributors: Minju, Lee, Steve Macenski, Tomoya Fujita + 23.1.0 (2023-10-04) ------------------- * Add locking to protect the TimeSource::NodeState::node_base\_ (`#2320 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 5a32cf54a9..cc62b9c12a 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 23.1.0 (2023-10-04) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index f3734be502..c613cc4b8a 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 23.1.0 (2023-10-04) ------------------- * Add missing header required by the rclcpp::NodeOptions type (`#2324 `_) diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index f94a150d9a..0d3b1dc06a 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* add clients & services count (`#2072 `_) +* Contributors: Minju, Lee + 23.1.0 (2023-10-04) ------------------- From 13abc1beed02a995863c5c115ef758d35485b46b Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 9 Oct 2023 15:31:54 +0000 Subject: [PATCH 294/455] 23.2.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index f84322c35b..daa087ed24 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +23.2.0 (2023-10-09) +------------------- * add clients & services count (`#2072 `_) * remove invalid sized allocation test for SerializedMessage. (`#2330 `_) * Adding API to copy all parameters from one node to another (`#2304 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 110cc63e72..d5f7028ceb 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 23.1.0 + 23.2.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index cc62b9c12a..14f55e5fe2 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +23.2.0 (2023-10-09) +------------------- 23.1.0 (2023-10-04) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 4d96b76d52..10383cd2ba 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 23.1.0 + 23.2.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index c613cc4b8a..331768bae7 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +23.2.0 (2023-10-09) +------------------- 23.1.0 (2023-10-04) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 18c6eaec17..d8bccf8e7f 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 23.1.0 + 23.2.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 0d3b1dc06a..e0837dd0cd 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +23.2.0 (2023-10-09) +------------------- * add clients & services count (`#2072 `_) * Contributors: Minju, Lee diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 328d6d4db9..3dbb62ae27 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 23.1.0 + 23.2.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 7f7ffcf6ed1143b4d4a9e0cf248e190731b2e5f8 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 10 Oct 2023 13:25:55 -0400 Subject: [PATCH 295/455] Fix rclcpp_lifecycle inclusion on Windows. (#2331) The comment in the commit explains this clearly, but on Windows ERROR is a macro. The reuse of it, even as an enum, causes compilation errors on downstream users. Push the macro and undefine it so downstream consumers can freely include it. Signed-off-by: Chris Lalancette --- .../node_interfaces/lifecycle_node_interface.hpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp index 272c4def27..45748ea55d 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp @@ -21,6 +21,14 @@ #include "rclcpp_lifecycle/visibility_control.h" #include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" +// When windows.h is included, ERROR is defined as a macro. So the use of it later in this file, +// even as an enum, causes compilation errors. Work around this by undefining the macro here, +// and then redefining when this header is finished being included. +#if defined(_WIN32) +#pragma push_macro("ERROR") +#undef ERROR +#endif + namespace rclcpp_lifecycle { namespace node_interfaces @@ -108,6 +116,10 @@ class LifecycleNodeInterface } // namespace node_interfaces } // namespace rclcpp_lifecycle +#if defined(_WIN32) +#pragma pop_macro("ERROR") +#endif + RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT( rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface, lifecycle_node) From 5ffc963e1aef97928a6cc895db18d40673c655f1 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Mon, 16 Oct 2023 16:38:38 +0300 Subject: [PATCH 296/455] Remove useless ROSRate class (#2326) Signed-off-by: Alexey Merzlyakov --- rclcpp/include/rclcpp/rate.hpp | 10 ----- rclcpp/src/rclcpp/rate.cpp | 8 ---- rclcpp/test/rclcpp/test_rate.cpp | 69 ++------------------------------ 3 files changed, 3 insertions(+), 84 deletions(-) diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 381e8d7908..5b04b72b81 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -184,16 +184,6 @@ class WallRate : public Rate explicit WallRate(const Duration & period); }; -class ROSRate : public Rate -{ -public: - RCLCPP_PUBLIC - explicit ROSRate(const double rate); - - RCLCPP_PUBLIC - explicit ROSRate(const Duration & period); -}; - } // namespace rclcpp #endif // RCLCPP__RATE_HPP_ diff --git a/rclcpp/src/rclcpp/rate.cpp b/rclcpp/src/rclcpp/rate.cpp index 9a6e3d486b..cd071d2cb0 100644 --- a/rclcpp/src/rclcpp/rate.cpp +++ b/rclcpp/src/rclcpp/rate.cpp @@ -102,12 +102,4 @@ WallRate::WallRate(const Duration & period) : Rate(period, std::make_shared(RCL_STEADY_TIME)) {} -ROSRate::ROSRate(const double rate) -: Rate(rate, std::make_shared(RCL_ROS_TIME)) -{} - -ROSRate::ROSRate(const Duration & period) -: Rate(period, std::make_shared(RCL_ROS_TIME)) -{} - } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp index 5ab64ee608..4f067b2bd8 100644 --- a/rclcpp/test/rclcpp/test_rate.cpp +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -22,7 +22,7 @@ #include "../utils/rclcpp_gtest_macros.hpp" /* - Basic tests for the Rate, WallRate and ROSRate classes. + Basic tests for the Rate and WallRate classes. */ TEST(TestRate, rate_basics) { rclcpp::init(0, nullptr); @@ -144,69 +144,6 @@ TEST(TestRate, wall_rate_basics) { rclcpp::shutdown(); } -TEST(TestRate, ros_rate_basics) { - rclcpp::init(0, nullptr); - - auto period = std::chrono::milliseconds(100); - auto offset = std::chrono::milliseconds(50); - auto epsilon = std::chrono::milliseconds(1); - double overrun_ratio = 1.5; - - rclcpp::Clock clock(RCL_ROS_TIME); - - auto start = clock.now(); - rclcpp::ROSRate r(period); - EXPECT_EQ(rclcpp::Duration(period), r.period()); -// suppress deprecated warnings -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else // !defined(_WIN32) -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - ASSERT_FALSE(r.is_steady()); -// suppress deprecated warnings -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else // !defined(_WIN32) -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - ASSERT_EQ(RCL_ROS_TIME, r.get_type()); - ASSERT_TRUE(r.sleep()); - auto one = clock.now(); - auto delta = one - start; - EXPECT_LT(rclcpp::Duration(period), delta); - EXPECT_GT(rclcpp::Duration(period * overrun_ratio), delta); - - clock.sleep_for(offset); - ASSERT_TRUE(r.sleep()); - auto two = clock.now(); - delta = two - start; - EXPECT_LT(rclcpp::Duration(2 * period), delta + epsilon); - EXPECT_GT(rclcpp::Duration(2 * period * overrun_ratio), delta); - - clock.sleep_for(offset); - auto two_offset = clock.now(); - r.reset(); - ASSERT_TRUE(r.sleep()); - auto three = clock.now(); - delta = three - two_offset; - EXPECT_LT(rclcpp::Duration(period), delta); - EXPECT_GT(rclcpp::Duration(period * overrun_ratio), delta); - - clock.sleep_for(offset + period); - auto four = clock.now(); - ASSERT_FALSE(r.sleep()); - auto five = clock.now(); - delta = five - four; - EXPECT_GT(rclcpp::Duration(epsilon), delta); - - rclcpp::shutdown(); -} - /* Basic test for the deprecated GenericRate class. */ @@ -331,7 +268,7 @@ TEST(TestRate, from_double) { EXPECT_EQ(rclcpp::Duration(std::chrono::seconds(1)), rate.period()); } { - rclcpp::WallRate rate(2.0); + rclcpp::Rate rate(2.0); EXPECT_EQ(rclcpp::Duration(std::chrono::milliseconds(500)), rate.period()); } { @@ -339,7 +276,7 @@ TEST(TestRate, from_double) { EXPECT_EQ(rclcpp::Duration(std::chrono::seconds(2)), rate.period()); } { - rclcpp::ROSRate rate(4.0); + rclcpp::WallRate rate(4.0); EXPECT_EQ(rclcpp::Duration(std::chrono::milliseconds(250)), rate.period()); } } From c366e531fafcf662cadfcef5d4966d974171b74c Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 23 Oct 2023 11:10:18 -0500 Subject: [PATCH 297/455] Fixes pointed out by the clang analyzer. (#2339) 1. Remove the default Logger copy constructor without copy assignment (rule of three -> rule of zero). 2. Remove an unnecessary capture in a lambda. 3. Mark a variable unused. 4. Mark a method as override. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/logger.hpp | 3 --- rclcpp/src/rclcpp/context.cpp | 2 +- rclcpp/test/rclcpp/executors/test_executors.cpp | 1 + rclcpp/test/rclcpp/test_intra_process_manager.cpp | 2 +- 4 files changed, 3 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/logger.hpp b/rclcpp/include/rclcpp/logger.hpp index 77f7f8d670..3b8e8a1625 100644 --- a/rclcpp/include/rclcpp/logger.hpp +++ b/rclcpp/include/rclcpp/logger.hpp @@ -126,9 +126,6 @@ class Logger std::shared_ptr> logger_sublogger_pairname_ = nullptr; public: - RCLCPP_PUBLIC - Logger(const Logger &) = default; - /// Get the name of this logger. /** * \return the full name of the logger including any prefixes, or diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 35a11730ab..75c451a861 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -462,7 +462,7 @@ template std::vector Context::get_shutdown_callback() const { - const auto get_callback_vector = [this](auto & mutex, auto & callback_set) { + const auto get_callback_vector = [](auto & mutex, auto & callback_set) { const std::lock_guard lock(mutex); std::vector callbacks; for (auto & callback : callback_set) { diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index fbd410bcab..653f06fb9c 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -644,6 +644,7 @@ TYPED_TEST(TestExecutors, testRaceConditionAddNode) break; } total += k * (i + 42); + (void)total; } }); } diff --git a/rclcpp/test/rclcpp/test_intra_process_manager.cpp b/rclcpp/test/rclcpp/test_intra_process_manager.cpp index 83cf586e99..da863f3e3c 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager.cpp @@ -289,7 +289,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase } bool - use_take_shared_method() const + use_take_shared_method() const override { return take_shared_method; } From fcbe64cff4bea3109531254ceb2955dc4b1bb320 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Thu, 26 Oct 2023 00:08:18 -0700 Subject: [PATCH 298/455] address rate related flaky tests. (#2329) Signed-off-by: Tomoya Fujita --- rclcpp/src/rclcpp/context.cpp | 2 +- rclcpp/src/rclcpp/rate.cpp | 3 ++- rclcpp/test/rclcpp/test_rate.cpp | 34 +++++++++++++++++++------------- 3 files changed, 23 insertions(+), 16 deletions(-) diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 75c451a861..bbc1d29d0f 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -496,7 +496,7 @@ Context::sleep_for(const std::chrono::nanoseconds & nanoseconds) std::unique_lock lock(interrupt_mutex_); auto start = std::chrono::steady_clock::now(); // this will release the lock while waiting - interrupt_condition_variable_.wait_for(lock, nanoseconds); + interrupt_condition_variable_.wait_for(lock, time_left); time_left -= std::chrono::steady_clock::now() - start; } } while (time_left > std::chrono::nanoseconds::zero() && this->is_valid()); diff --git a/rclcpp/src/rclcpp/rate.cpp b/rclcpp/src/rclcpp/rate.cpp index cd071d2cb0..7c98d6d4fe 100644 --- a/rclcpp/src/rclcpp/rate.cpp +++ b/rclcpp/src/rclcpp/rate.cpp @@ -67,7 +67,8 @@ Rate::sleep() // Calculate the time to sleep auto time_to_sleep = next_interval - now; // Sleep (will get interrupted by ctrl-c, may not sleep full time) - return clock_->sleep_for(time_to_sleep); + clock_->sleep_for(time_to_sleep); + return true; } bool diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp index 4f067b2bd8..b80789c853 100644 --- a/rclcpp/test/rclcpp/test_rate.cpp +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -21,12 +21,24 @@ #include "../utils/rclcpp_gtest_macros.hpp" +class TestRate : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + /* Basic tests for the Rate and WallRate classes. */ -TEST(TestRate, rate_basics) { - rclcpp::init(0, nullptr); - +TEST_F(TestRate, rate_basics) { auto period = std::chrono::milliseconds(1000); auto offset = std::chrono::milliseconds(500); auto epsilon = std::chrono::milliseconds(100); @@ -79,13 +91,9 @@ TEST(TestRate, rate_basics) { auto five = std::chrono::system_clock::now(); delta = five - four; ASSERT_TRUE(epsilon > delta); - - rclcpp::shutdown(); } -TEST(TestRate, wall_rate_basics) { - rclcpp::init(0, nullptr); - +TEST_F(TestRate, wall_rate_basics) { auto period = std::chrono::milliseconds(100); auto offset = std::chrono::milliseconds(50); auto epsilon = std::chrono::milliseconds(1); @@ -140,14 +148,12 @@ TEST(TestRate, wall_rate_basics) { auto five = std::chrono::steady_clock::now(); delta = five - four; EXPECT_GT(epsilon, delta); - - rclcpp::shutdown(); } /* Basic test for the deprecated GenericRate class. */ -TEST(TestRate, generic_rate) { +TEST_F(TestRate, generic_rate) { auto period = std::chrono::milliseconds(100); auto offset = std::chrono::milliseconds(50); auto epsilon = std::chrono::milliseconds(1); @@ -262,7 +268,7 @@ TEST(TestRate, generic_rate) { } } -TEST(TestRate, from_double) { +TEST_F(TestRate, from_double) { { rclcpp::Rate rate(1.0); EXPECT_EQ(rclcpp::Duration(std::chrono::seconds(1)), rate.period()); @@ -281,7 +287,7 @@ TEST(TestRate, from_double) { } } -TEST(TestRate, clock_types) { +TEST_F(TestRate, clock_types) { { rclcpp::Rate rate(1.0, std::make_shared(RCL_SYSTEM_TIME)); // suppress deprecated warnings @@ -341,7 +347,7 @@ TEST(TestRate, clock_types) { } } -TEST(TestRate, incorrect_constuctor) { +TEST_F(TestRate, incorrect_constuctor) { // Constructor with 0-frequency RCLCPP_EXPECT_THROW_EQ( rclcpp::Rate rate(0.0), From 2204e44305636a3603a4c4b41ed625d7a90193a1 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 30 Oct 2023 12:24:24 -0700 Subject: [PATCH 299/455] Adjust rclcpp usage of type description service (#2344) Signed-off-by: Michael Carroll --- .../node_type_descriptions.cpp | 25 +++---------------- .../test_node_type_descriptions.cpp | 16 ++++++------ 2 files changed, 11 insertions(+), 30 deletions(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp index f4b5e20d30..2a50230bbe 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp @@ -84,8 +84,10 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl } if (enabled) { - auto rcl_node = node_base->get_rcl_node_handle(); - rcl_ret_t rcl_ret = rcl_node_type_description_service_init(rcl_node); + auto * rcl_node = node_base->get_rcl_node_handle(); + auto rcl_srv = std::make_shared(); + rcl_ret_t rcl_ret = rcl_node_type_description_service_init(rcl_srv.get(), rcl_node); + if (rcl_ret != RCL_RET_OK) { RCLCPP_ERROR( logger_, "Failed to initialize ~/get_type_description_service: %s", @@ -94,13 +96,6 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl "Failed to initialize ~/get_type_description service."); } - rcl_service_t * rcl_srv = nullptr; - rcl_ret = rcl_node_get_type_description_service(rcl_node, &rcl_srv); - if (rcl_ret != RCL_RET_OK) { - throw std::runtime_error( - "Failed to get initialized ~/get_type_description service from rcl."); - } - rclcpp::AnyServiceCallback cb; cb.set( [this]( @@ -124,18 +119,6 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl nullptr); } } - - ~NodeTypeDescriptionsImpl() - { - if ( - type_description_srv_ && - RCL_RET_OK != rcl_node_type_description_service_fini(node_base_->get_rcl_node_handle())) - { - RCLCPP_ERROR( - logger_, - "Error in shutdown of get_type_description service: %s", rcl_get_error_string().str); - } - } }; NodeTypeDescriptions::NodeTypeDescriptions( diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_type_descriptions.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_type_descriptions.cpp index 79ef6c3bcf..1a4603528a 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_type_descriptions.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_type_descriptions.cpp @@ -43,10 +43,9 @@ TEST_F(TestNodeTypeDescriptions, disabled_no_service) node_options.append_parameter_override("start_type_description_service", false); rclcpp::Node node{"node", "ns", node_options}; - rcl_node_t * rcl_node = node.get_node_base_interface()->get_rcl_node_handle(); - rcl_service_t * srv = nullptr; - rcl_ret_t ret = rcl_node_get_type_description_service(rcl_node, &srv); - ASSERT_EQ(RCL_RET_NOT_INIT, ret); + auto services = node.get_node_graph_interface()->get_service_names_and_types_by_node( + "node", "/ns"); + EXPECT_TRUE(services.find("/ns/node/get_type_description") == services.end()); } TEST_F(TestNodeTypeDescriptions, enabled_creates_service) @@ -55,9 +54,8 @@ TEST_F(TestNodeTypeDescriptions, enabled_creates_service) node_options.append_parameter_override("start_type_description_service", true); rclcpp::Node node{"node", "ns", node_options}; - rcl_node_t * rcl_node = node.get_node_base_interface()->get_rcl_node_handle(); - rcl_service_t * srv = nullptr; - rcl_ret_t ret = rcl_node_get_type_description_service(rcl_node, &srv); - ASSERT_EQ(RCL_RET_OK, ret); - ASSERT_NE(nullptr, srv); + auto services = node.get_node_graph_interface()->get_service_names_and_types_by_node( + "node", "/ns"); + + EXPECT_TRUE(services.find("/ns/node/get_type_description") != services.end()); } From fff009a75100f2afd8ef1c3863620bf5ebe67708 Mon Sep 17 00:00:00 2001 From: Jiaqi Li Date: Tue, 31 Oct 2023 15:21:28 +0800 Subject: [PATCH 300/455] Add missing 'enable_rosout' comments (#2345) Signed-off-by: Jiaqi Li --- rclcpp/include/rclcpp/node_options.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp/include/rclcpp/node_options.hpp b/rclcpp/include/rclcpp/node_options.hpp index a30ce4cf11..71b0d997cf 100644 --- a/rclcpp/include/rclcpp/node_options.hpp +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -43,6 +43,7 @@ class NodeOptions * - arguments = {} * - parameter_overrides = {} * - use_global_arguments = true + * - enable_rosout = true * - use_intra_process_comms = false * - enable_topic_statistics = false * - start_parameter_services = true From 8a02e3934c74b6c3355d24af76d1f035ce15573f Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Thu, 2 Nov 2023 10:26:33 -0700 Subject: [PATCH 301/455] Use message_info in SubscriptionTopicStatistics instead of typed message (#2337) * Use message_info in SubscriptionTopicStatistics instead of typed message - Untemplatize the rclcpp::topic_statistics::SubscriptionTopicStatistics class. Now we will be using message_info instead of typed deserialized messages in the handle_message callbacks. * Fix test_receive_stats_include_window_reset by using publisher emulator - Emulate publishing messages by directly calling rclcpp::Subscription::handle_message(msg_shared_ptr, message_info) and settling up needed message_info.source_timestamp Signed-off-by: Michael Orlov Co-authored-by: Tomoya Fujita --- rclcpp/include/rclcpp/create_subscription.hpp | 17 +- rclcpp/include/rclcpp/subscription.hpp | 6 +- .../include/rclcpp/subscription_factory.hpp | 5 +- .../subscription_topic_statistics.hpp | 23 +- .../test_subscription_topic_statistics.cpp | 314 +++++------------- 5 files changed, 95 insertions(+), 270 deletions(-) diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 5b84930ff7..016c966b62 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -50,8 +50,8 @@ template< typename SubscriptionT, typename MessageMemoryStrategyT, typename NodeParametersT, - typename NodeTopicsT, - typename ROSMessageType = typename SubscriptionT::ROSMessageType> + typename NodeTopicsT +> typename std::shared_ptr create_subscription( NodeParametersT & node_parameters, @@ -70,7 +70,7 @@ create_subscription( using rclcpp::node_interfaces::get_node_topics_interface; auto node_topics_interface = get_node_topics_interface(node_topics); - std::shared_ptr> + std::shared_ptr subscription_topic_stats = nullptr; if (rclcpp::detail::resolve_enable_topic_statistics( @@ -80,8 +80,7 @@ create_subscription( if (options.topic_stats_options.publish_period <= std::chrono::milliseconds(0)) { throw std::invalid_argument( "topic_stats_options.publish_period must be greater than 0, specified value of " + - std::to_string(options.topic_stats_options.publish_period.count()) + - " ms"); + std::to_string(options.topic_stats_options.publish_period.count()) + " ms"); } std::shared_ptr> @@ -91,12 +90,12 @@ create_subscription( options.topic_stats_options.publish_topic, qos); - subscription_topic_stats = std::make_shared< - rclcpp::topic_statistics::SubscriptionTopicStatistics - >(node_topics_interface->get_node_base_interface()->get_name(), publisher); + subscription_topic_stats = + std::make_shared( + node_topics_interface->get_node_base_interface()->get_name(), publisher); std::weak_ptr< - rclcpp::topic_statistics::SubscriptionTopicStatistics + rclcpp::topic_statistics::SubscriptionTopicStatistics > weak_subscription_topic_stats(subscription_topic_stats); auto sub_call_back = [weak_subscription_topic_stats]() { auto subscription_topic_stats = weak_subscription_topic_stats.lock(); diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index b0857fb102..8e1aac2aa7 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -104,7 +104,7 @@ class Subscription : public SubscriptionBase private: using SubscriptionTopicStatisticsSharedPtr = - std::shared_ptr>; + std::shared_ptr; public: RCLCPP_SMART_PTR_DEFINITIONS(Subscription) @@ -316,7 +316,7 @@ class Subscription : public SubscriptionBase if (subscription_topic_statistics_) { const auto nanos = std::chrono::time_point_cast(now); const auto time = rclcpp::Time(nanos.time_since_epoch().count()); - subscription_topic_statistics_->handle_message(*typed_message, time); + subscription_topic_statistics_->handle_message(message_info.get_rmw_message_info(), time); } } @@ -357,7 +357,7 @@ class Subscription : public SubscriptionBase if (subscription_topic_statistics_) { const auto nanos = std::chrono::time_point_cast(now); const auto time = rclcpp::Time(nanos.time_since_epoch().count()); - subscription_topic_statistics_->handle_message(*typed_message, time); + subscription_topic_statistics_->handle_message(message_info.get_rmw_message_info(), time); } } diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index a1727eab5a..0e9d9fefe5 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -75,15 +75,14 @@ template< typename CallbackT, typename AllocatorT, typename SubscriptionT = rclcpp::Subscription, - typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType, - typename ROSMessageType = typename SubscriptionT::ROSMessageType + typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType > SubscriptionFactory create_subscription_factory( CallbackT && callback, const rclcpp::SubscriptionOptionsWithAllocator & options, typename MessageMemoryStrategyT::SharedPtr msg_mem_strat, - std::shared_ptr> + std::shared_ptr subscription_topic_stats = nullptr ) { diff --git a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp index 4b9221406f..781e2c86fc 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp @@ -48,21 +48,12 @@ using libstatistics_collector::moving_average_statistics::StatisticData; /** * Class used to collect, measure, and publish topic statistics data. Current statistics * supported for subscribers are received message age and received message period. - * - * \tparam CallbackMessageT the subscribed message type - */ -template + */ class SubscriptionTopicStatistics { - using TopicStatsCollector = - libstatistics_collector::topic_statistics_collector::TopicStatisticsCollector< - CallbackMessageT>; - using ReceivedMessageAge = - libstatistics_collector::topic_statistics_collector::ReceivedMessageAgeCollector< - CallbackMessageT>; - using ReceivedMessagePeriod = - libstatistics_collector::topic_statistics_collector::ReceivedMessagePeriodCollector< - CallbackMessageT>; + using TopicStatsCollector = libstatistics_collector::TopicStatisticsCollector; + using ReceivedMessageAge = libstatistics_collector::ReceivedMessageAgeCollector; + using ReceivedMessagePeriod = libstatistics_collector::ReceivedMessagePeriodCollector; public: /// Construct a SubscriptionTopicStatistics object. @@ -101,16 +92,16 @@ class SubscriptionTopicStatistics /** * This method acquires a lock to prevent race conditions to collectors list. * - * \param received_message the message received by the subscription + * \param message_info the message info corresponding to the received message * \param now_nanoseconds current time in nanoseconds */ virtual void handle_message( - const CallbackMessageT & received_message, + const rmw_message_info_t & message_info, const rclcpp::Time now_nanoseconds) const { std::lock_guard lock(mutex_); for (const auto & collector : subscriber_statistics_collectors_) { - collector->OnMessageReceived(received_message, now_nanoseconds.nanoseconds()); + collector->OnMessageReceived(message_info, now_nanoseconds.nanoseconds()); } } diff --git a/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp index ce6887c631..9166272207 100644 --- a/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp @@ -14,7 +14,6 @@ #include -#include #include #include #include @@ -22,12 +21,12 @@ #include #include #include +#include #include #include "libstatistics_collector/moving_average_statistics/types.hpp" #include "rclcpp/create_publisher.hpp" -#include "rclcpp/msg/message_with_header.hpp" #include "rclcpp/node.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/rclcpp.hpp" @@ -36,10 +35,10 @@ #include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" #include "statistics_msgs/msg/metrics_message.hpp" -#include "statistics_msgs/msg/statistic_data_point.hpp" #include "statistics_msgs/msg/statistic_data_type.hpp" #include "test_msgs/msg/empty.hpp" +#include "test_msgs/msg/strings.hpp" #include "test_topic_stats_utils.hpp" @@ -67,7 +66,6 @@ constexpr const std::chrono::seconds kUnstableMessageAgeWindowDuration{ constexpr const std::chrono::seconds kUnstableMessageAgeOffset{std::chrono::seconds{1}}; } // namespace -using rclcpp::msg::MessageWithHeader; using test_msgs::msg::Empty; using rclcpp::topic_statistics::SubscriptionTopicStatistics; using statistics_msgs::msg::MetricsMessage; @@ -76,114 +74,73 @@ using statistics_msgs::msg::StatisticDataType; using libstatistics_collector::moving_average_statistics::StatisticData; /** - * Wrapper class to test and expose parts of the SubscriptionTopicStatistics class. - * \tparam CallbackMessageT + * Wrapper class to test and expose parts of the SubscriptionTopicStatistics class. */ -template -class TestSubscriptionTopicStatistics : public SubscriptionTopicStatistics +class TestSubscriptionTopicStatistics : public SubscriptionTopicStatistics { public: TestSubscriptionTopicStatistics( const std::string & node_name, rclcpp::Publisher::SharedPtr publisher) - : SubscriptionTopicStatistics(node_name, publisher) + : SubscriptionTopicStatistics(node_name, std::move(publisher)) { } - virtual ~TestSubscriptionTopicStatistics() = default; + ~TestSubscriptionTopicStatistics() override = default; /// Exposed for testing - std::vector get_current_collector_data() const - { - return SubscriptionTopicStatistics::get_current_collector_data(); - } + using SubscriptionTopicStatistics::get_current_collector_data; }; /** - * Empty publisher node: used to publish empty messages + * PublisherNode wrapper: used to create publisher node */ -class EmptyPublisher : public rclcpp::Node +template +class PublisherNode : public rclcpp::Node { public: - EmptyPublisher( + PublisherNode( const std::string & name, const std::string & topic, const std::chrono::milliseconds & publish_period = std::chrono::milliseconds{100}) : Node(name) { - publisher_ = create_publisher(topic, 10); + publisher_ = create_publisher(topic, 10); publish_timer_ = this->create_wall_timer( publish_period, [this]() { this->publish_message(); }); } - virtual ~EmptyPublisher() = default; + ~PublisherNode() override = default; private: void publish_message() { - auto msg = Empty{}; + auto msg = MessageT{}; publisher_->publish(msg); } - rclcpp::Publisher::SharedPtr publisher_; + typename rclcpp::Publisher::SharedPtr publisher_; rclcpp::TimerBase::SharedPtr publish_timer_; }; /** - * MessageWithHeader publisher node: used to publish MessageWithHeader with `header` value set - */ -class MessageWithHeaderPublisher : public rclcpp::Node -{ -public: - MessageWithHeaderPublisher( - const std::string & name, const std::string & topic, - const std::chrono::milliseconds & publish_period = std::chrono::milliseconds{100}) - : Node(name) - { - publisher_ = create_publisher(topic, 10); - publish_timer_ = this->create_wall_timer( - publish_period, [this]() { - this->publish_message(); - }); - uniform_dist_ = std::uniform_int_distribution{1000000, 100000000}; - } - - virtual ~MessageWithHeaderPublisher() = default; - -private: - void publish_message() - { - std::random_device rd; - std::mt19937 gen{rd()}; - uint32_t d = uniform_dist_(gen); - auto msg = MessageWithHeader{}; - // Subtract ~1 second (add some noise for a non-zero standard deviation) - // so the received message age calculation is always > 0 - msg.header.stamp = this->now() - rclcpp::Duration{1, d}; - publisher_->publish(msg); - } - - rclcpp::Publisher::SharedPtr publisher_; - rclcpp::TimerBase::SharedPtr publish_timer_; - std::uniform_int_distribution uniform_dist_; -}; - -/** - * TransitionMessageStamp publisher node : used to publish MessageWithHeader with `header` value set + * TransitionMessageStamp publisher emulator node : used to emulate publishing messages by + * directly calling rclcpp::Subscription::handle_message(msg_shared_ptr, message_info). * The message age results change during the test. */ - -class TransitionMessageStampPublisher : public rclcpp::Node +template +class TransitionMessageStampPublisherEmulator : public rclcpp::Node { public: - TransitionMessageStampPublisher( - const std::string & name, const std::string & topic, + TransitionMessageStampPublisherEmulator( + const std::string & name, const std::chrono::seconds transition_duration, const std::chrono::seconds message_age_offset, + typename rclcpp::Subscription::SharedPtr subscription, const std::chrono::milliseconds & publish_period = std::chrono::milliseconds{100}) - : Node(name), transition_duration_(transition_duration), message_age_offset_(message_age_offset) + : Node(name), transition_duration_(transition_duration), message_age_offset_(message_age_offset), + subscription_(std::move(subscription)) { - publisher_ = create_publisher(topic, 10); publish_timer_ = this->create_wall_timer(publish_period, [this]() {this->publish_message();}); start_time_ = this->now(); } @@ -191,84 +148,66 @@ class TransitionMessageStampPublisher : public rclcpp::Node private: void publish_message() { - auto msg = MessageWithHeader{}; + std::shared_ptr msg_shared_ptr = std::make_shared(); + rmw_message_info_t rmw_message_info = rmw_get_zero_initialized_message_info(); + auto now = this->now(); auto elapsed_time = now - start_time_; if (elapsed_time < transition_duration_) { // Apply only to the topic statistics in the first half // Subtract offset so message_age is always >= offset. - msg.header.stamp = now - message_age_offset_; + rmw_message_info.source_timestamp = (now - message_age_offset_).nanoseconds(); } else { - msg.header.stamp = now; + rmw_message_info.source_timestamp = now.nanoseconds(); } - publisher_->publish(msg); + rclcpp::MessageInfo message_info{rmw_message_info}; + subscription_->handle_message(msg_shared_ptr, message_info); } std::chrono::seconds transition_duration_; std::chrono::seconds message_age_offset_; + typename rclcpp::Subscription::SharedPtr subscription_; rclcpp::Time start_time_; - - rclcpp::Publisher::SharedPtr publisher_; rclcpp::TimerBase::SharedPtr publish_timer_; }; /** - * Empty subscriber node: used to create subscriber topic statistics requirements + * Message subscriber node: used to create subscriber with enabled topic statistics collectors + * */ -class EmptySubscriber : public rclcpp::Node +template +class SubscriberWithTopicStatistics : public rclcpp::Node { public: - EmptySubscriber(const std::string & name, const std::string & topic) + SubscriberWithTopicStatistics( + const std::string & name, const std::string & topic, + std::chrono::milliseconds publish_period = defaultStatisticsPublishPeriod) : Node(name) { - // manually enable topic statistics via options + // Manually enable topic statistics via options auto options = rclcpp::SubscriptionOptions(); options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; + options.topic_stats_options.publish_period = publish_period; - auto callback = [](Empty::UniquePtr msg) { + auto callback = [](typename MessageT::UniquePtr msg) { (void) msg; }; - subscription_ = create_subscription>( + subscription_ = create_subscription>( topic, rclcpp::QoS(rclcpp::KeepAll()), callback, options); } - virtual ~EmptySubscriber() = default; - -private: - rclcpp::Subscription::SharedPtr subscription_; -}; + ~SubscriberWithTopicStatistics() override = default; -/** - * MessageWithHeader subscriber node: used to create subscriber topic statistics requirements - */ -class MessageWithHeaderSubscriber : public rclcpp::Node -{ -public: - MessageWithHeaderSubscriber(const std::string & name, const std::string & topic) - : Node(name) + typename rclcpp::Subscription::SharedPtr get_subscription() { - // manually enable topic statistics via options - auto options = rclcpp::SubscriptionOptions(); - options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; - options.topic_stats_options.publish_period = defaultStatisticsPublishPeriod; - - auto callback = [](MessageWithHeader::UniquePtr msg) { - (void) msg; - }; - subscription_ = create_subscription>( - topic, - rclcpp::QoS(rclcpp::KeepAll()), - callback, - options); + return subscription_; } - virtual ~MessageWithHeaderSubscriber() = default; private: - rclcpp::Subscription::SharedPtr subscription_; + typename rclcpp::Subscription::SharedPtr subscription_; }; /** @@ -277,43 +216,17 @@ class MessageWithHeaderSubscriber : public rclcpp::Node class TestSubscriptionTopicStatisticsFixture : public ::testing::Test { protected: - void SetUp() + void SetUp() override { rclcpp::init(0 /* argc */, nullptr /* argv */); } - void TearDown() + void TearDown() override { rclcpp::shutdown(); } }; -/** - * Check if a received statistics message is empty (no data was observed) - * \param message_to_check - */ -void check_if_statistics_message_is_empty(const MetricsMessage & message_to_check) -{ - for (const auto & stats_point : message_to_check.statistics) { - const auto type = stats_point.data_type; - switch (type) { - case StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT: - EXPECT_EQ(0, stats_point.data) << "unexpected sample count" << stats_point.data; - break; - case StatisticDataType::STATISTICS_DATA_TYPE_AVERAGE: - case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM: - case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM: - case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV: - EXPECT_TRUE(std::isnan(stats_point.data)) << "unexpected value" << stats_point.data << - " for type:" << type; - break; - default: - FAIL() << "received unknown statistics type: " << std::dec << - static_cast(type); - } - } -} - /** * Check if a received statistics message observed data and contains some calculation * \param message_to_check @@ -348,28 +261,13 @@ void check_if_statistic_message_is_populated(const MetricsMessage & message_to_c /** * Test an invalid argument is thrown for a bad input publish period. */ -TEST(TestSubscriptionTopicStatistics, test_invalid_publish_period) +TEST_F(TestSubscriptionTopicStatisticsFixture, test_invalid_publish_period) { - rclcpp::init(0 /* argc */, nullptr /* argv */); - - auto node = std::make_shared("test_period_node"); - - auto options = rclcpp::SubscriptionOptions(); - options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; - options.topic_stats_options.publish_period = std::chrono::milliseconds(0); - - auto callback = [](Empty::UniquePtr msg) { - (void) msg; - }; - ASSERT_THROW( - (node->create_subscription>( - "should_throw_invalid_arg", - rclcpp::QoS(rclcpp::KeepAll()), - callback, - options)), std::invalid_argument); - - rclcpp::shutdown(); + SubscriberWithTopicStatistics( + "test_period_node", "should_throw_invalid_arg", std::chrono::milliseconds(0) + ), + std::invalid_argument); } /** @@ -378,7 +276,7 @@ TEST(TestSubscriptionTopicStatistics, test_invalid_publish_period) */ TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) { - auto empty_subscriber = std::make_shared( + auto empty_subscriber = std::make_shared>( kTestSubNodeName, kTestSubStatsEmptyTopic); @@ -389,7 +287,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) 10); // Construct a separate instance - auto sub_topic_stats = std::make_unique>( + auto sub_topic_stats = std::make_unique( empty_subscriber->get_name(), topic_stats_publisher); @@ -410,7 +308,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no_header) { // Create an empty publisher - auto empty_publisher = std::make_shared( + auto empty_publisher = std::make_shared>( kTestPubNodeName, kTestSubStatsEmptyTopic); // empty_subscriber has a topic statistics instance as part of its subscription @@ -422,7 +320,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no "/statistics", kNumExpectedMessages); - auto empty_subscriber = std::make_shared( + auto empty_subscriber = std::make_shared>( kTestSubNodeName, kTestSubStatsEmptyTopic); @@ -432,74 +330,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no ex.add_node(empty_subscriber); // Spin and get future - ex.spin_until_future_complete( - statistics_listener->GetFuture(), - kTestTimeout); - - // Compare message counts, sample count should be the same as published and received count - EXPECT_EQ(kNumExpectedMessages, statistics_listener->GetNumberOfMessagesReceived()); - - // Check the received message total count - const auto received_messages = statistics_listener->GetReceivedMessages(); - EXPECT_EQ(kNumExpectedMessages, received_messages.size()); - - // check the type of statistics that were received and their counts - uint64_t message_age_count{0}; - uint64_t message_period_count{0}; - - std::set received_metrics; - for (const auto & msg : received_messages) { - if (msg.metrics_source == "message_age") { - message_age_count++; - } - if (msg.metrics_source == "message_period") { - message_period_count++; - } - } - EXPECT_EQ(kNumExpectedMessageAgeMessages, message_age_count); - EXPECT_EQ(kNumExpectedMessagePeriodMessages, message_period_count); - - // Check the collected statistics for message period. - // Message age statistics will not be calculated because Empty messages - // don't have a `header` with timestamp. This means that we expect to receive a `message_age` - // and `message_period` message for each empty message published. - for (const auto & msg : received_messages) { - if (msg.metrics_source == kMessageAgeSourceLabel) { - check_if_statistics_message_is_empty(msg); - } else if (msg.metrics_source == kMessagePeriodSourceLabel) { - check_if_statistic_message_is_populated(msg); - } - } -} - -TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_with_header) -{ - // Create a MessageWithHeader publisher - auto msg_with_header_publisher = std::make_shared( - kTestPubNodeName, - kTestSubStatsTopic); - // empty_subscriber has a topic statistics instance as part of its subscription - // this will listen to and generate statistics for the empty message - - // Create a listener for topic statistics messages - auto statistics_listener = std::make_shared( - "test_receive_stats_for_message_with_header", - "/statistics", - kNumExpectedMessages); - - auto msg_with_header_subscriber = std::make_shared( - kTestSubNodeName, - kTestSubStatsTopic); - - rclcpp::executors::SingleThreadedExecutor ex; - ex.add_node(msg_with_header_publisher); - ex.add_node(statistics_listener); - ex.add_node(msg_with_header_subscriber); - - // Spin and get future - ex.spin_until_future_complete( - statistics_listener->GetFuture(), - kTestTimeout); + ex.spin_until_future_complete(statistics_listener->GetFuture(), kTestTimeout); // Compare message counts, sample count should be the same as published and received count EXPECT_EQ(kNumExpectedMessages, statistics_listener->GetNumberOfMessagesReceived()); @@ -524,6 +355,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi EXPECT_EQ(kNumExpectedMessageAgeMessages, message_age_count); EXPECT_EQ(kNumExpectedMessagePeriodMessages, message_period_count); + // Check the collected statistics for message period. for (const auto & msg : received_messages) { check_if_statistic_message_is_populated(msg); } @@ -531,23 +363,27 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_include_window_reset) { - // Create a MessageWithHeader publisher - auto msg_with_header_publisher = std::make_shared( - kTestPubNodeName, kTestSubStatsTopic, kUnstableMessageAgeWindowDuration, - kUnstableMessageAgeOffset); - - // msg_with_header_subscriber has a topic statistics instance as part of its + // msg_subscriber_with_topic_statistics has a topic statistics instance as part of its // subscription this will listen to and generate statistics - auto msg_with_header_subscriber = - std::make_shared(kTestSubNodeName, kTestSubStatsTopic); + auto msg_subscriber_with_topic_statistics = + std::make_shared>( + kTestSubNodeName, + kTestSubStatsTopic); + + // Create a message publisher + auto msg_publisher = + std::make_shared>( + kTestPubNodeName, kUnstableMessageAgeWindowDuration, + kUnstableMessageAgeOffset, msg_subscriber_with_topic_statistics->get_subscription()); + // Create a listener for topic statistics messages auto statistics_listener = std::make_shared( "test_receive_stats_include_window_reset", "/statistics", kNumExpectedMessages); rclcpp::executors::SingleThreadedExecutor ex; - ex.add_node(msg_with_header_publisher); + ex.add_node(msg_publisher); ex.add_node(statistics_listener); - ex.add_node(msg_with_header_subscriber); + ex.add_node(msg_subscriber_with_topic_statistics); // Spin and get future ex.spin_until_future_complete(statistics_listener->GetFuture(), kTestTimeout); From f294488e17921034ebbbca75e8604a56684874e7 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 2 Nov 2023 18:29:12 -0400 Subject: [PATCH 302/455] Disable the loaned messages inside the executor. (#2335) * Disable the loaned messages inside the executor. They are currently unsafe to use; see the comment in the commit for more information. Signed-off-by: Chris Lalancette --- rclcpp/src/rclcpp/executor.cpp | 5 +++++ rclcpp/src/rclcpp/subscription_base.cpp | 15 ++++++++++++++- 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 9de5883a34..4dd0c9a358 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -669,6 +669,11 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) subscription->get_topic_name(), [&]() {return subscription->take_type_erased(message.get(), message_info);}, [&]() {subscription->handle_message(message, message_info);}); + // TODO(clalancette): In the case that the user is using the MessageMemoryPool, + // and they take a shared_ptr reference to the message in the callback, this can + // inadvertently return the message to the pool when the user is still using it. + // This is a bug that needs to be fixed in the pool, and we should probably have + // a custom deleter for the message that actually does the return_message(). subscription->return_message(message); } break; diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 67a9a6c56f..e8510b6444 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -298,7 +298,20 @@ SubscriptionBase::setup_intra_process( bool SubscriptionBase::can_loan_messages() const { - return rcl_subscription_can_loan_messages(subscription_handle_.get()); + bool retval = rcl_subscription_can_loan_messages(subscription_handle_.get()); + if (retval) { + // TODO(clalancette): The loaned message interface is currently not safe to use with + // shared_ptr callbacks. If a user takes a copy of the shared_ptr, it can get freed from + // underneath them via rcl_return_loaned_message_from_subscription(). The correct solution is + // to return the loaned message in a custom deleter, but that needs to be carefully handled + // with locking. Warn the user about this until we fix it. + RCLCPP_WARN_ONCE( + this->node_logger_, + "Loaned messages are only safe with const ref subscription callbacks. " + "If you are using any other kind of subscriptions, " + "set the ROS_DISABLE_LOANED_MESSAGES environment variable to 1 (the default)."); + } + return retval; } rclcpp::Waitable::SharedPtr From 4691063a61d39357d2c8ea08359e21bd3da42a29 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 3 Nov 2023 05:59:06 -0700 Subject: [PATCH 303/455] Add a custom deleter when constructing rcl_service_t (#2351) * Add a custom deleter when constructing rcl_service_t In the type description service construction, we were previously passing the shared_ptr to the rcl_service_t with the assumption that rclcpp::Service would do the clean up. This was an incorrect assumption, and so I have added a custom deleter to fini the service and delete when the shared_ptr is cleaned up. Signed-off-by: Michael Carroll --- .../node_interfaces/node_type_descriptions.cpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp index 2a50230bbe..fdac4652e0 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp @@ -85,12 +85,25 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl if (enabled) { auto * rcl_node = node_base->get_rcl_node_handle(); - auto rcl_srv = std::make_shared(); + std::shared_ptr rcl_srv( + new rcl_service_t, + [rcl_node, logger = this->logger_](rcl_service_t * service) + { + if (rcl_service_fini(service, rcl_node) != RCL_RET_OK) { + RCLCPP_ERROR( + logger, + "Error in destruction of rcl service handle [~/get_type_description]: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete service; + }); + *rcl_srv = rcl_get_zero_initialized_service(); rcl_ret_t rcl_ret = rcl_node_type_description_service_init(rcl_srv.get(), rcl_node); if (rcl_ret != RCL_RET_OK) { RCLCPP_ERROR( - logger_, "Failed to initialize ~/get_type_description_service: %s", + logger_, "Failed to initialize ~/get_type_description service: %s", rcl_get_error_string().str); throw std::runtime_error( "Failed to initialize ~/get_type_description service."); From 5049c45f854f5f71a7b0fe8845541b31b4f367ec Mon Sep 17 00:00:00 2001 From: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Date: Fri, 3 Nov 2023 18:06:25 -0400 Subject: [PATCH 304/455] Serialized Messages with Topic Statistics (#2274) Signed-off-by: CursedRock17 --- rclcpp/include/rclcpp/subscription.hpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 8e1aac2aa7..d6e0f536ed 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -325,8 +325,20 @@ class Subscription : public SubscriptionBase const std::shared_ptr & serialized_message, const rclcpp::MessageInfo & message_info) override { - // TODO(wjwwood): enable topic statistics for serialized messages + std::chrono::time_point now; + if (subscription_topic_statistics_) { + // get current time before executing callback to + // exclude callback duration from topic statistics result. + now = std::chrono::system_clock::now(); + } + any_callback_.dispatch(serialized_message, message_info); + + if (subscription_topic_statistics_) { + const auto nanos = std::chrono::time_point_cast(now); + const auto time = rclcpp::Time(nanos.time_since_epoch().count()); + subscription_topic_statistics_->handle_message(message_info.get_rmw_message_info(), time); + } } void From 76e2b2677b529329db85ed535ddcac2d045473b9 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Sat, 4 Nov 2023 09:08:11 -0700 Subject: [PATCH 305/455] rclcpp::Time::max() clock type support. (#2352) Signed-off-by: Tomoya Fujita --- rclcpp/include/rclcpp/time.hpp | 2 +- rclcpp/src/rclcpp/time.cpp | 4 ++-- rclcpp/test/rclcpp/test_time.cpp | 25 +++++++++++++++++++++---- 3 files changed, 24 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/time.hpp b/rclcpp/include/rclcpp/time.hpp index 15533f39ef..b6aee4c972 100644 --- a/rclcpp/include/rclcpp/time.hpp +++ b/rclcpp/include/rclcpp/time.hpp @@ -189,7 +189,7 @@ class Time */ RCLCPP_PUBLIC static Time - max(); + max(rcl_clock_type_t clock_type = RCL_SYSTEM_TIME); // NOLINT /// Get the seconds since epoch /** diff --git a/rclcpp/src/rclcpp/time.cpp b/rclcpp/src/rclcpp/time.cpp index 556a5e69ad..e5ff2af8d0 100644 --- a/rclcpp/src/rclcpp/time.cpp +++ b/rclcpp/src/rclcpp/time.cpp @@ -276,9 +276,9 @@ Time::operator-=(const rclcpp::Duration & rhs) } Time -Time::max() +Time::max(rcl_clock_type_t clock_type) { - return Time(std::numeric_limits::max(), 999999999); + return Time(std::numeric_limits::max(), 999999999, clock_type); } diff --git a/rclcpp/test/rclcpp/test_time.cpp b/rclcpp/test/rclcpp/test_time.cpp index f3969d3886..7d656fb9c4 100644 --- a/rclcpp/test/rclcpp/test_time.cpp +++ b/rclcpp/test/rclcpp/test_time.cpp @@ -363,10 +363,27 @@ TEST_F(TestTime, seconds) { } TEST_F(TestTime, test_max) { - const rclcpp::Time time_max = rclcpp::Time::max(); - const rclcpp::Time max_time(std::numeric_limits::max(), 999999999); - EXPECT_DOUBLE_EQ(max_time.seconds(), time_max.seconds()); - EXPECT_EQ(max_time.nanoseconds(), time_max.nanoseconds()); + // Same clock types + for (rcl_clock_type_t type = RCL_ROS_TIME; + type != RCL_STEADY_TIME; type = static_cast(type + 1)) + { + const rclcpp::Time time_max = rclcpp::Time::max(type); + const rclcpp::Time max_time(std::numeric_limits::max(), 999999999, type); + EXPECT_DOUBLE_EQ(max_time.seconds(), time_max.seconds()); + EXPECT_EQ(max_time.nanoseconds(), time_max.nanoseconds()); + } + // Different clock types + { + const rclcpp::Time time_max = rclcpp::Time::max(RCL_ROS_TIME); + const rclcpp::Time max_time(std::numeric_limits::max(), 999999999, RCL_STEADY_TIME); + EXPECT_ANY_THROW((void)(time_max == max_time)); + EXPECT_ANY_THROW((void)(time_max != max_time)); + EXPECT_ANY_THROW((void)(time_max <= max_time)); + EXPECT_ANY_THROW((void)(time_max >= max_time)); + EXPECT_ANY_THROW((void)(time_max < max_time)); + EXPECT_ANY_THROW((void)(time_max > max_time)); + EXPECT_ANY_THROW((void)(time_max - max_time)); + } } TEST_F(TestTime, test_constructor_from_rcl_time_point) { From 7f411371b367ea402e2066c2af1ccbac6b4eb5bc Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Sun, 5 Nov 2023 14:27:40 -0500 Subject: [PATCH 306/455] Updates to not use std::move in some places. (#2353) gcc 13.1.1 complains that these uses inhibit copy elision. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/experimental/intra_process_manager.hpp | 4 ++-- rclcpp/include/rclcpp/publisher.hpp | 2 +- rclcpp/test/rclcpp/test_publisher.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index f5e00fa4f8..54ff0c0dee 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -467,7 +467,7 @@ class IntraProcessManager auto ptr = MessageAllocTraits::allocate(allocator, 1); MessageAllocTraits::construct(allocator, ptr, *message); - subscription->provide_intra_process_data(std::move(MessageUniquePtr(ptr, deleter))); + subscription->provide_intra_process_data(MessageUniquePtr(ptr, deleter)); } continue; @@ -510,7 +510,7 @@ class IntraProcessManager MessageAllocTraits::construct(allocator, ptr, *message); ros_message_subscription->provide_intra_process_message( - std::move(MessageUniquePtr(ptr, deleter))); + MessageUniquePtr(ptr, deleter)); } } } diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 18229c7a4e..c03220f38b 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -390,7 +390,7 @@ class Publisher : public PublisherBase if (this->can_loan_messages()) { // we release the ownership from the rclpp::LoanedMessage instance // and let the middleware clean up the memory. - this->do_loaned_message_publish(std::move(loaned_msg.release())); + this->do_loaned_message_publish(loaned_msg.release()); } else { // we don't release the ownership, let the middleware copy the ros message // and thus the destructor of rclcpp::LoanedMessage cleans up the memory. diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index a0c3ec8b75..3edb24c0dd 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -482,7 +482,7 @@ class TestPublisherProtectedMethods : public rclcpp::Publisher && loaned_msg) { - this->do_loaned_message_publish(std::move(loaned_msg.release())); + this->do_loaned_message_publish(loaned_msg.release()); } void call_default_incompatible_qos_callback(rclcpp::QOSOfferedIncompatibleQoSInfo & event) const From d407a5e3313d90642f1dd7b1de2e95b833242ff7 Mon Sep 17 00:00:00 2001 From: Zard-C <58285320+Zard-C@users.noreply.github.com> Date: Mon, 6 Nov 2023 03:28:17 +0800 Subject: [PATCH 307/455] fix (signal_handler.hpp): spelling (#2356) Signed-off-by: Zard-C --- rclcpp/src/rclcpp/signal_handler.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/src/rclcpp/signal_handler.hpp b/rclcpp/src/rclcpp/signal_handler.hpp index 0d3c399ccb..db608b0d10 100644 --- a/rclcpp/src/rclcpp/signal_handler.hpp +++ b/rclcpp/src/rclcpp/signal_handler.hpp @@ -75,7 +75,7 @@ class SignalHandler final bool install(SignalHandlerOptions signal_handler_options = SignalHandlerOptions::All); - /// Uninstall the signal handler for SIGINT/SIGTERM and join the dedicated singal handling + /// Uninstall the signal handler for SIGINT/SIGTERM and join the dedicated signal handling /// thread. /** * Also restores the previous signal handler. @@ -189,7 +189,7 @@ class SignalHandler final // Whether or not a signal has been received. std::atomic_bool signal_received_ = false; - // A thread to which singal handling tasks are deferred. + // A thread to which signal handling tasks are deferred. std::thread signal_handler_thread_; // A mutex used to synchronize the install() and uninstall() methods. From 620fcf1e0588514dbac637f9d157c54e9414472c Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 6 Nov 2023 17:36:40 +0000 Subject: [PATCH 308/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 16 ++++++++++++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 5 +++++ 4 files changed, 27 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index daa087ed24..8fd886d27f 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,22 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix (signal_handler.hpp): spelling (`#2356 `_) +* Updates to not use std::move in some places. (`#2353 `_) +* rclcpp::Time::max() clock type support. (`#2352 `_) +* Serialized Messages with Topic Statistics (`#2274 `_) +* Add a custom deleter when constructing rcl_service_t (`#2351 `_) +* Disable the loaned messages inside the executor. (`#2335 `_) +* Use message_info in SubscriptionTopicStatistics instead of typed message (`#2337 `_) +* Add missing 'enable_rosout' comments (`#2345 `_) +* Adjust rclcpp usage of type description service (`#2344 `_) +* address rate related flaky tests. (`#2329 `_) +* Fixes pointed out by the clang analyzer. (`#2339 `_) +* Remove useless ROSRate class (`#2326 `_) +* Contributors: Alexey Merzlyakov, Chris Lalancette, Jiaqi Li, Lucas Wendland, Michael Carroll, Michael Orlov, Tomoya Fujita, Zard-C + 23.2.0 (2023-10-09) ------------------- * add clients & services count (`#2072 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 14f55e5fe2..7b3fa697e6 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 23.2.0 (2023-10-09) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 331768bae7..8290c58720 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 23.2.0 (2023-10-09) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index e0837dd0cd..2224abf17e 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix rclcpp_lifecycle inclusion on Windows. (`#2331 `_) +* Contributors: Chris Lalancette + 23.2.0 (2023-10-09) ------------------- * add clients & services count (`#2072 `_) From 0f331f90a99da40f7b7a69b4f55b30b7245d294f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 6 Nov 2023 17:36:49 +0000 Subject: [PATCH 309/455] 24.0.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 8fd886d27f..1a8ffdbbd5 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +24.0.0 (2023-11-06) +------------------- * fix (signal_handler.hpp): spelling (`#2356 `_) * Updates to not use std::move in some places. (`#2353 `_) * rclcpp::Time::max() clock type support. (`#2352 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index d5f7028ceb..76480d10f2 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 23.2.0 + 24.0.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 7b3fa697e6..7bd386a3a7 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +24.0.0 (2023-11-06) +------------------- 23.2.0 (2023-10-09) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 10383cd2ba..105ead1894 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 23.2.0 + 24.0.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 8290c58720..9371eec9b4 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +24.0.0 (2023-11-06) +------------------- 23.2.0 (2023-10-09) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index d8bccf8e7f..0a50dd3486 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 23.2.0 + 24.0.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 2224abf17e..41b0a22c62 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +24.0.0 (2023-11-06) +------------------- * Fix rclcpp_lifecycle inclusion on Windows. (`#2331 `_) * Contributors: Chris Lalancette diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 3dbb62ae27..0c1b19c011 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 23.2.0 + 24.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 9c098e544ecf191b7c61f63f7f4fac6f6449cedd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?= Date: Thu, 9 Nov 2023 23:09:18 +0300 Subject: [PATCH 310/455] fix(rclcpp_components): increase the service queue sizes in component_container (#2363) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * fix(rclcpp_components): increase the service queue sizes in component_container Signed-off-by: M. Fatih Cırıt --- rclcpp_components/src/component_manager.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index 036350a64f..7b77af9c92 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -39,10 +39,12 @@ ComponentManager::ComponentManager( { loadNode_srv_ = create_service( "~/_container/load_node", - std::bind(&ComponentManager::on_load_node, this, _1, _2, _3)); + std::bind(&ComponentManager::on_load_node, this, _1, _2, _3), + rclcpp::ServicesQoS().keep_last(200)); unloadNode_srv_ = create_service( "~/_container/unload_node", - std::bind(&ComponentManager::on_unload_node, this, _1, _2, _3)); + std::bind(&ComponentManager::on_unload_node, this, _1, _2, _3), + rclcpp::ServicesQoS().keep_last(200)); listNodes_srv_ = create_service( "~/_container/list_nodes", std::bind(&ComponentManager::on_list_nodes, this, _1, _2, _3)); From de841d9c8bfe30310ec2c4c5d11a712f07f6ab47 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 13 Nov 2023 15:32:42 -0500 Subject: [PATCH 311/455] Support users holding onto shared pointers in the message memory pool (#2336) * Support users holding onto shared pointers in the message memory pool Before this commit, the MessageMemoryPool would actually reuse messages in the pool, even if the user had taken additional shared_ptr copies. This commit fixes things so that we properly handle that situation. In particular, we allocate memory during class initialization, and delete it during destruction. We then run the constructor when we hand the pointer out, and the destructor (only) when we return it to the pool. This keeps things consistent. We also add in locks, since in a multi-threaded scenario we need to protect against multiple threads accessing the pool at the same time. With this in place, things work as expected when users hold shared_ptr copies. We also add in a test for this situation. One note about performance: this update preserves the "no-allocations-at-runtime" aspect of the MessagePool. However, there are some tradeoffs with CPU time here, particularly with very large message pools. This could probably be optimized further to do less work when trying to add items back to the free_list, but I view that as a further enhancement. Signed-off-by: Chris Lalancette --- .../message_pool_memory_strategy.hpp | 110 ++++++++++++++---- .../test_message_pool_memory_strategy.cpp | 30 +++-- 2 files changed, 105 insertions(+), 35 deletions(-) diff --git a/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp index 0e7d4366e5..703066fa3f 100644 --- a/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp @@ -15,10 +15,17 @@ #ifndef RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_ #define RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_ +#include +#include #include +#include +#include +#include #include "rosidl_runtime_cpp/traits.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" #include "rclcpp/visibility_control.hpp" @@ -50,13 +57,24 @@ class MessagePoolMemoryStrategy public: RCLCPP_SMART_PTR_DEFINITIONS(MessagePoolMemoryStrategy) - /// Default constructor MessagePoolMemoryStrategy() - : next_array_index_(0) { + pool_mutex_ = std::make_shared(); + + pool_ = std::shared_ptr>( + new std::array, + [](std::array * arr) { + for (size_t i = 0; i < Size; ++i) { + free((*arr)[i]); + } + delete arr; + }); + + free_list_ = std::make_shared>(); + for (size_t i = 0; i < Size; ++i) { - pool_[i].msg_ptr_ = std::make_shared(); - pool_[i].used = false; + (*pool_)[i] = static_cast(malloc(sizeof(MessageT))); + free_list_->push_back(i); } } @@ -68,43 +86,85 @@ class MessagePoolMemoryStrategy */ std::shared_ptr borrow_message() { - size_t current_index = next_array_index_; - next_array_index_ = (next_array_index_ + 1) % Size; - if (pool_[current_index].used) { - throw std::runtime_error("Tried to access message that was still in use! Abort."); + std::lock_guard lock(*pool_mutex_); + if (free_list_->size() == 0) { + throw std::runtime_error("No more free slots in the pool"); } - pool_[current_index].msg_ptr_->~MessageT(); - new (pool_[current_index].msg_ptr_.get())MessageT; - pool_[current_index].used = true; - return pool_[current_index].msg_ptr_; + size_t current_index = free_list_->pop_front(); + + return std::shared_ptr( + new((*pool_)[current_index]) MessageT(), + [pool = this->pool_, pool_mutex = this->pool_mutex_, + free_list = this->free_list_](MessageT * p) { + std::lock_guard lock(*pool_mutex); + for (size_t i = 0; i < Size; ++i) { + if ((*pool)[i] == p) { + p->~MessageT(); + free_list->push_back(i); + break; + } + } + }); } /// Return a message to the message pool. /** - * Manage metadata in the message pool ring buffer to release the message. + * This does nothing since the message isn't returned to the pool until the user has dropped + * all references. * \param[in] msg Shared pointer to the message to return. */ void return_message(std::shared_ptr & msg) { - for (size_t i = 0; i < Size; ++i) { - if (pool_[i].msg_ptr_ == msg) { - pool_[i].used = false; - return; - } - } - throw std::runtime_error("Unrecognized message ptr in return_message."); + (void)msg; } protected: - struct PoolMember + template + class CircularArray { - std::shared_ptr msg_ptr_; - bool used; +public: + void push_back(const size_t v) + { + if (size_ + 1 > N) { + throw std::runtime_error("Tried to push too many items into the array"); + } + array_[(front_ + size_) % N] = v; + ++size_; + } + + size_t pop_front() + { + if (size_ < 1) { + throw std::runtime_error("Tried to pop item from empty array"); + } + + size_t val = array_[front_]; + + front_ = (front_ + 1) % N; + --size_; + + return val; + } + + size_t size() const + { + return size_; + } + +private: + size_t front_ = 0; + size_t size_ = 0; + std::array array_; }; - std::array pool_; - size_t next_array_index_; + // It's very important that these are shared_ptrs, since users of this class might hold a + // reference to a pool item longer than the lifetime of the class. In that scenario, the + // shared_ptr ensures that the lifetime of these variables outlives this class, and hence ensures + // the custom destructor for each pool item can successfully run. + std::shared_ptr pool_mutex_; + std::shared_ptr> pool_; + std::shared_ptr> free_list_; }; } // namespace message_pool_memory_strategy diff --git a/rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp index b68bc96ab6..69198adf59 100644 --- a/rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp @@ -56,18 +56,28 @@ TEST_F(TestMessagePoolMemoryStrategy, borrow_too_many) { // Size is 1, borrowing second time should fail RCLCPP_EXPECT_THROW_EQ( message_memory_strategy_->borrow_message(), - std::runtime_error("Tried to access message that was still in use! Abort.")); + std::runtime_error("No more free slots in the pool")); EXPECT_NO_THROW(message_memory_strategy_->return_message(message)); } -TEST_F(TestMessagePoolMemoryStrategy, return_unrecognized) { - auto message = message_memory_strategy_->borrow_message(); - ASSERT_NE(nullptr, message); +TEST_F(TestMessagePoolMemoryStrategy, borrow_hold_reference) { + { + auto message = message_memory_strategy_->borrow_message(); + ASSERT_NE(nullptr, message); - auto unrecognized = std::make_shared(); - // Unrecognized does not belong to pool - RCLCPP_EXPECT_THROW_EQ( - message_memory_strategy_->return_message(unrecognized), - std::runtime_error("Unrecognized message ptr in return_message.")); - EXPECT_NO_THROW(message_memory_strategy_->return_message(message)); + // Return it. + EXPECT_NO_THROW(message_memory_strategy_->return_message(message)); + + // But we are still holding the reference, so we expect that there is still no room in the pool. + RCLCPP_EXPECT_THROW_EQ( + message_memory_strategy_->borrow_message(), + std::runtime_error("No more free slots in the pool")); + } + + // Now that we've dropped the reference (left the scope), we expect to be able to borrow again. + + auto message2 = message_memory_strategy_->borrow_message(); + ASSERT_NE(nullptr, message2); + + EXPECT_NO_THROW(message_memory_strategy_->return_message(message2)); } From 411dbe8212dd753ee60871f8605d0a8beaae5e37 Mon Sep 17 00:00:00 2001 From: mauropasse Date: Tue, 14 Nov 2023 02:24:07 +0000 Subject: [PATCH 312/455] Fix data race in EventHandlerBase (#2349) Both the EventHandler and its associated pubs/subs share the same underlying rmw event listener. When a pub/sub is destroyed, the listener is destroyed. There is a data race when the ~EventHandlerBase wants to access the listener after it has been destroyed. The EventHandler stores a shared_ptr of its associated pub/sub. But since we were clearing the listener event callbacks on the base class destructor ~EventHandlerBase, the pub/sub was already destroyed, which means the rmw event listener was also destroyed, thus causing a segfault when trying to obtain it to clear the callbacks. Clearing the callbacks on ~EventHandler instead of ~EventHandlerBase avoids the race, since the pub/sub are still valid. Signed-off-by: Mauro Passerino --- rclcpp/include/rclcpp/event_handler.hpp | 10 ++++++++++ rclcpp/src/rclcpp/event_handler.cpp | 7 ------- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/event_handler.hpp b/rclcpp/include/rclcpp/event_handler.hpp index 3f41de469c..f9b75eb7cf 100644 --- a/rclcpp/include/rclcpp/event_handler.hpp +++ b/rclcpp/include/rclcpp/event_handler.hpp @@ -260,6 +260,16 @@ class EventHandler : public EventHandlerBase } } + ~EventHandler() + { + // Since the rmw event listener holds a reference to the + // "on ready" callback, we need to clear it on destruction of this class. + // This clearing is not needed for other rclcpp entities like pub/subs, since + // they do own the underlying rmw entities, which are destroyed + // on their rclcpp destructors, thus no risk of dangling pointers. + clear_on_ready_callback(); + } + /// Take data so that the callback cannot be scheduled again std::shared_ptr take_data() override diff --git a/rclcpp/src/rclcpp/event_handler.cpp b/rclcpp/src/rclcpp/event_handler.cpp index 40ae6c030d..d4b4d57b08 100644 --- a/rclcpp/src/rclcpp/event_handler.cpp +++ b/rclcpp/src/rclcpp/event_handler.cpp @@ -39,13 +39,6 @@ UnsupportedEventTypeException::UnsupportedEventTypeException( EventHandlerBase::~EventHandlerBase() { - // Since the rmw event listener holds a reference to - // this callback, we need to clear it on destruction of this class. - // This clearing is not needed for other rclcpp entities like pub/subs, since - // they do own the underlying rmw entities, which are destroyed - // on their rclcpp destructors, thus no risk of dangling pointers. - clear_on_ready_callback(); - if (rcl_event_fini(&event_handle_) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", From 2446fd000bad7dd20b8f480476eef1f25d913e27 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Wed, 15 Nov 2023 23:44:25 +0800 Subject: [PATCH 313/455] aligh with rcl that a rosout publisher of a node might not exist (#2357) * aligh with rcl * keep same behavior with rclpy 1. to not throw exception durning rcl_logging_rosout_remove_sublogger 2. reset error message for RCL_RET_NOT_FOUND * test for a node with rosout disabled Signed-off-by: Chen Lihui --- rclcpp/src/rclcpp/logger.cpp | 11 ++++++----- rclcpp/test/rclcpp/test_rosout_subscription.cpp | 10 ++++++++++ 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/rclcpp/src/rclcpp/logger.cpp b/rclcpp/src/rclcpp/logger.cpp index 874858b180..2e83c07013 100644 --- a/rclcpp/src/rclcpp/logger.cpp +++ b/rclcpp/src/rclcpp/logger.cpp @@ -17,6 +17,7 @@ #include #include "rcl_logging_interface/rcl_logging_interface.h" +#include "rcl/error_handling.h" #include "rcl/logging_rosout.h" #include "rclcpp/exceptions.hpp" @@ -80,10 +81,12 @@ Logger::get_child(const std::string & suffix) { std::lock_guard guard(*logging_mutex); rcl_ret = rcl_logging_rosout_add_sublogger((*name_).c_str(), suffix.c_str()); - if (RCL_RET_OK != rcl_ret) { + if (RCL_RET_NOT_FOUND == rcl_ret) { + rcl_reset_error(); + } else if (RCL_RET_OK != rcl_ret) { exceptions::throw_from_rcl_error( rcl_ret, "failed to call rcl_logging_rosout_add_sublogger", - rcutils_get_error_state(), rcutils_reset_error); + rcl_get_error_state(), rcl_reset_error); } } @@ -98,9 +101,7 @@ Logger::get_child(const std::string & suffix) logger_sublogger_pairname_ptr->second.c_str()); delete logger_sublogger_pairname_ptr; if (RCL_RET_OK != rcl_ret) { - exceptions::throw_from_rcl_error( - rcl_ret, "failed to call rcl_logging_rosout_remove_sublogger", - rcutils_get_error_state(), rcutils_reset_error); + rcl_reset_error(); } }); } diff --git a/rclcpp/test/rclcpp/test_rosout_subscription.cpp b/rclcpp/test/rclcpp/test_rosout_subscription.cpp index c5f00bb26d..08786a3fcb 100644 --- a/rclcpp/test/rclcpp/test_rosout_subscription.cpp +++ b/rclcpp/test/rclcpp/test_rosout_subscription.cpp @@ -178,3 +178,13 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild_hierarchy) { EXPECT_TRUE(future.get()); received_msg_promise = {}; } + +TEST_F(TestRosoutSubscription, test_rosoutsubscription_node_rosout_disabled) { + rclcpp::NodeOptions options = rclcpp::NodeOptions().enable_rosout(false); + auto node = std::make_shared("my_node", options); + auto log_func = [&node] { + auto logger = node->get_logger().get_child("child"); + RCLCPP_INFO(logger, "test"); + }; + ASSERT_NO_THROW(log_func()); +} From d08d9cf5ffd5ecb80c1ef4085573c411eecbd737 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Fri, 17 Nov 2023 22:20:30 +0900 Subject: [PATCH 314/455] feat(rclcpp_components): support events executor in node main template (#2366) Signed-off-by: wep21 --- rclcpp_components/src/node_main.cpp.in | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/rclcpp_components/src/node_main.cpp.in b/rclcpp_components/src/node_main.cpp.in index 71754d1f82..d2a0e84601 100644 --- a/rclcpp_components/src/node_main.cpp.in +++ b/rclcpp_components/src/node_main.cpp.in @@ -23,11 +23,14 @@ #define NODE_MAIN_LOGGER_NAME "@node@" +using namespace rclcpp::executors; +using namespace rclcpp::experimental::executors; + int main(int argc, char * argv[]) { auto args = rclcpp::init_and_remove_ros_arguments(argc, argv); rclcpp::Logger logger = rclcpp::get_logger(NODE_MAIN_LOGGER_NAME); - rclcpp::executors::@executor@ exec; + @executor@ exec; rclcpp::NodeOptions options; options.arguments(args); std::vector node_wrappers; From 26f6efa840cbac047a93dc21d055cfe9cde7270f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 27 Nov 2023 16:39:20 -0500 Subject: [PATCH 315/455] Switch to target_link_libraries. (#2374) That way we can hide more of the implementation by using the PRIVATE keyword. Signed-off-by: Chris Lalancette --- rclcpp/CMakeLists.txt | 72 +-- rclcpp/package.xml | 2 + rclcpp/test/benchmark/CMakeLists.txt | 9 +- rclcpp/test/rclcpp/CMakeLists.txt | 437 ++++-------------- rclcpp_action/CMakeLists.txt | 62 +-- rclcpp_action/package.xml | 1 + rclcpp_action/test/benchmark/CMakeLists.txt | 6 +- rclcpp_components/CMakeLists.txt | 32 +- .../rclcpp_components_register_node.cmake | 9 +- 9 files changed, 170 insertions(+), 460 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index c30ad4d818..ffae9cc7ee 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -16,6 +16,8 @@ find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) find_package(rmw REQUIRED) find_package(rosgraph_msgs REQUIRED) +find_package(rosidl_dynamic_typesupport REQUIRED) +find_package(rosidl_runtime_c REQUIRED) find_package(rosidl_runtime_cpp REQUIRED) find_package(rosidl_typesupport_c REQUIRED) find_package(rosidl_typesupport_cpp REQUIRED) @@ -202,23 +204,28 @@ target_include_directories(${PROJECT_NAME} PUBLIC "$" "$" "$") -target_link_libraries(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT}) -# specific order: dependents before dependencies -ament_target_dependencies(${PROJECT_NAME} - "ament_index_cpp" - "libstatistics_collector" - "rcl" - "rcl_interfaces" - "rcl_logging_interface" - "rcl_yaml_param_parser" - "rcpputils" - "rcutils" - "builtin_interfaces" - "rosgraph_msgs" - "rosidl_typesupport_cpp" - "rosidl_runtime_cpp" - "statistics_msgs" - "tracetools" +target_link_libraries(${PROJECT_NAME} PUBLIC + ${builtin_interfaces_TARGETS} + libstatistics_collector::libstatistics_collector + rcl::rcl + ${rcl_interfaces_TARGETS} + rcl_yaml_param_parser::rcl_yaml_param_parser + rcpputils::rcpputils + rcutils::rcutils + rmw::rmw + ${rosgraph_msgs_TARGETS} + rosidl_dynamic_typesupport::rosidl_dynamic_typesupport + rosidl_runtime_c::rosidl_runtime_c + rosidl_runtime_cpp::rosidl_runtime_cpp + rosidl_typesupport_cpp::rosidl_typesupport_cpp + ${statistics_msgs_TARGETS} + tracetools::tracetools + ${CMAKE_THREAD_LIBS_INIT} +) + +target_link_libraries(${PROJECT_NAME} PRIVATE + ament_index_cpp::ament_index_cpp + rcl_logging_interface::rcl_logging_interface ) # Causes the visibility macros to use dllexport rather than dllimport, @@ -240,20 +247,23 @@ ament_export_libraries(${PROJECT_NAME}) # Export modern CMake targets ament_export_targets(${PROJECT_NAME}) -# specific order: dependents before dependencies -ament_export_dependencies(ament_index_cpp) -ament_export_dependencies(libstatistics_collector) -ament_export_dependencies(rcl) -ament_export_dependencies(rcpputils) -ament_export_dependencies(rcutils) -ament_export_dependencies(builtin_interfaces) -ament_export_dependencies(rosgraph_msgs) -ament_export_dependencies(rosidl_typesupport_cpp) -ament_export_dependencies(rosidl_typesupport_c) -ament_export_dependencies(rosidl_runtime_cpp) -ament_export_dependencies(rcl_yaml_param_parser) -ament_export_dependencies(statistics_msgs) -ament_export_dependencies(tracetools) +ament_export_dependencies( + builtin_interfaces + libstatistics_collector + rcl + rcl_interfaces + rcl_yaml_param_parser + rcpputils + rcutils + rmw + rosgraph_msgs + rosidl_dynamic_typesupport + rosidl_runtime_c + rosidl_runtime_cpp + rosidl_typesupport_cpp + statistics_msgs + tracetools +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 76480d10f2..0996336d01 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -22,6 +22,7 @@ builtin_interfaces rcl_interfaces rosgraph_msgs + rosidl_runtime_c rosidl_runtime_cpp rosidl_typesupport_c rosidl_typesupport_cpp @@ -29,6 +30,7 @@ builtin_interfaces rcl_interfaces rosgraph_msgs + rosidl_runtime_c rosidl_runtime_cpp rosidl_typesupport_c rosidl_typesupport_cpp diff --git a/rclcpp/test/benchmark/CMakeLists.txt b/rclcpp/test/benchmark/CMakeLists.txt index a88c43e3b2..6b93711df2 100644 --- a/rclcpp/test/benchmark/CMakeLists.txt +++ b/rclcpp/test/benchmark/CMakeLists.txt @@ -7,14 +7,12 @@ find_package(performance_test_fixture REQUIRED) add_performance_test(benchmark_client benchmark_client.cpp) if(TARGET benchmark_client) - target_link_libraries(benchmark_client ${PROJECT_NAME}) - ament_target_dependencies(benchmark_client test_msgs rcl_interfaces) + target_link_libraries(benchmark_client ${PROJECT_NAME} ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS}) endif() add_performance_test(benchmark_executor benchmark_executor.cpp) if(TARGET benchmark_executor) - target_link_libraries(benchmark_executor ${PROJECT_NAME}) - ament_target_dependencies(benchmark_executor test_msgs) + target_link_libraries(benchmark_executor ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() add_performance_test(benchmark_init_shutdown benchmark_init_shutdown.cpp) @@ -39,6 +37,5 @@ endif() add_performance_test(benchmark_service benchmark_service.cpp) if(TARGET benchmark_service) - target_link_libraries(benchmark_service ${PROJECT_NAME}) - ament_target_dependencies(benchmark_service test_msgs rcl_interfaces) + target_link_libraries(benchmark_service ${PROJECT_NAME} ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS}) endif() diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index afa12cd24d..ca6c8f2adf 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -36,70 +36,36 @@ endif() ament_add_gtest(test_allocator_memory_strategy strategies/test_allocator_memory_strategy.cpp) if(TARGET test_allocator_memory_strategy) - ament_target_dependencies(test_allocator_memory_strategy - "rcl" - "test_msgs" - ) - target_link_libraries(test_allocator_memory_strategy ${PROJECT_NAME}) + target_link_libraries(test_allocator_memory_strategy ${PROJECT_NAME} rcpputils::rcpputils ${test_msgs_TARGETS}) endif() ament_add_gtest(test_message_pool_memory_strategy strategies/test_message_pool_memory_strategy.cpp) if(TARGET test_message_pool_memory_strategy) - ament_target_dependencies(test_message_pool_memory_strategy - "rcl" - "test_msgs" - ) - target_link_libraries(test_message_pool_memory_strategy ${PROJECT_NAME}) + target_link_libraries(test_message_pool_memory_strategy ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_any_service_callback test_any_service_callback.cpp) if(TARGET test_any_service_callback) - ament_target_dependencies(test_any_service_callback - "test_msgs" - ) - target_link_libraries(test_any_service_callback ${PROJECT_NAME}) + target_link_libraries(test_any_service_callback ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_any_subscription_callback test_any_subscription_callback.cpp) if(TARGET test_any_subscription_callback) - ament_target_dependencies(test_any_subscription_callback - "test_msgs" - ) - target_link_libraries(test_any_subscription_callback ${PROJECT_NAME}) + target_link_libraries(test_any_subscription_callback ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_client test_client.cpp) if(TARGET test_client) - ament_target_dependencies(test_client - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_client ${PROJECT_NAME} mimick) + target_link_libraries(test_client ${PROJECT_NAME} mimick ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_copy_all_parameter_values test_copy_all_parameter_values.cpp) if(TARGET test_copy_all_parameter_values) - ament_target_dependencies(test_copy_all_parameter_values - "rcl_interfaces" - ) target_link_libraries(test_copy_all_parameter_values ${PROJECT_NAME}) endif() ament_add_gtest(test_create_timer test_create_timer.cpp) if(TARGET test_create_timer) - ament_target_dependencies(test_create_timer - "rcl_interfaces" - "rmw" - "rcl" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) target_link_libraries(test_create_timer ${PROJECT_NAME}) target_include_directories(test_create_timer PRIVATE ./) endif() ament_add_gtest(test_create_subscription test_create_subscription.cpp) if(TARGET test_create_subscription) - target_link_libraries(test_create_subscription ${PROJECT_NAME}) - ament_target_dependencies(test_create_subscription - "test_msgs" - ) + target_link_libraries(test_create_subscription ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() function(test_add_callback_groups_to_executor_for_rmw_implementation) set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) @@ -108,32 +74,17 @@ function(test_add_callback_groups_to_executor_for_rmw_implementation) TIMEOUT 120 ) if(TARGET test_add_callback_groups_to_executor${target_suffix}) - target_link_libraries(test_add_callback_groups_to_executor${target_suffix} ${PROJECT_NAME}) - ament_target_dependencies(test_add_callback_groups_to_executor${target_suffix} - "test_msgs" - ) + target_link_libraries(test_add_callback_groups_to_executor${target_suffix} ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() endfunction() call_for_each_rmw_implementation(test_add_callback_groups_to_executor_for_rmw_implementation) ament_add_gtest(test_expand_topic_or_service_name test_expand_topic_or_service_name.cpp) if(TARGET test_expand_topic_or_service_name) - ament_target_dependencies(test_expand_topic_or_service_name - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME} mimick) + target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME} mimick rcl::rcl rmw::rmw) endif() ament_add_gtest(test_function_traits test_function_traits.cpp) if(TARGET test_function_traits) - target_include_directories(test_function_traits PUBLIC ../../include) - ament_target_dependencies(test_function_traits - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) + target_link_libraries(test_function_traits ${PROJECT_NAME}) endif() ament_add_gtest( test_future_return_code @@ -143,72 +94,33 @@ if(TARGET test_future_return_code) endif() ament_add_gmock(test_intra_process_manager test_intra_process_manager.cpp) if(TARGET test_intra_process_manager) - ament_target_dependencies(test_intra_process_manager - "rcl" - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_intra_process_manager ${PROJECT_NAME}) + target_link_libraries(test_intra_process_manager ${PROJECT_NAME} ${rcl_interfaces_TARGETS} rmw::rmw) endif() ament_add_gmock(test_intra_process_manager_with_allocators test_intra_process_manager_with_allocators.cpp) if(TARGET test_intra_process_manager_with_allocators) - ament_target_dependencies(test_intra_process_manager_with_allocators - "test_msgs" - ) - target_link_libraries(test_intra_process_manager_with_allocators ${PROJECT_NAME}) + target_link_libraries(test_intra_process_manager_with_allocators ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_ring_buffer_implementation test_ring_buffer_implementation.cpp) if(TARGET test_ring_buffer_implementation) - ament_target_dependencies(test_ring_buffer_implementation - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME}) endif() ament_add_gtest(test_intra_process_buffer test_intra_process_buffer.cpp) if(TARGET test_intra_process_buffer) - ament_target_dependencies(test_intra_process_buffer - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) target_link_libraries(test_intra_process_buffer ${PROJECT_NAME}) endif() ament_add_gtest(test_loaned_message test_loaned_message.cpp) -ament_target_dependencies(test_loaned_message - "test_msgs" -) -target_link_libraries(test_loaned_message ${PROJECT_NAME} mimick) +target_link_libraries(test_loaned_message ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) ament_add_gtest(test_memory_strategy test_memory_strategy.cpp) -ament_target_dependencies(test_memory_strategy - "test_msgs" -) -target_link_libraries(test_memory_strategy ${PROJECT_NAME}) +target_link_libraries(test_memory_strategy ${PROJECT_NAME} ${test_msgs_TARGETS}) ament_add_gtest(test_message_memory_strategy test_message_memory_strategy.cpp) -ament_target_dependencies(test_message_memory_strategy - "test_msgs" -) -target_link_libraries(test_message_memory_strategy ${PROJECT_NAME}) +target_link_libraries(test_message_memory_strategy ${PROJECT_NAME} ${test_msgs_TARGETS}) ament_add_gtest(test_node test_node.cpp TIMEOUT 240) if(TARGET test_node) - ament_target_dependencies(test_node - "rcl_interfaces" - "rcpputils" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_node ${PROJECT_NAME} mimick) + target_link_libraries(test_node ${PROJECT_NAME} mimick rcpputils::rcpputils rmw::rmw ${test_msgs_TARGETS}) endif() ament_add_gtest(test_node_interfaces__get_node_interfaces @@ -219,7 +131,7 @@ endif() ament_add_gtest(test_node_interfaces__node_base node_interfaces/test_node_base.cpp) if(TARGET test_node_interfaces__node_base) - target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME} mimick) + target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME} mimick rcl::rcl rmw::rmw) endif() ament_add_gtest(test_node_interfaces__node_clock node_interfaces/test_node_clock.cpp) @@ -230,48 +142,42 @@ ament_add_gtest(test_node_interfaces__node_graph node_interfaces/test_node_graph.cpp TIMEOUT 120) if(TARGET test_node_interfaces__node_graph) - ament_target_dependencies( - test_node_interfaces__node_graph - "test_msgs") - target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME} mimick) + target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME} mimick rcl::rcl ${test_msgs_TARGETS}) endif() ament_add_gtest(test_node_interfaces__node_interfaces node_interfaces/test_node_interfaces.cpp) if(TARGET test_node_interfaces__node_interfaces) - target_link_libraries(test_node_interfaces__node_interfaces ${PROJECT_NAME} mimick) + target_link_libraries(test_node_interfaces__node_interfaces ${PROJECT_NAME}) endif() ament_add_gtest(test_node_interfaces__node_parameters node_interfaces/test_node_parameters.cpp) if(TARGET test_node_interfaces__node_parameters) - target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME} mimick) + target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME} mimick rcpputils::rcpputils) endif() ament_add_gtest(test_node_interfaces__node_services node_interfaces/test_node_services.cpp) if(TARGET test_node_interfaces__node_services) - target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME} mimick) + target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME} mimick rcl::rcl) endif() ament_add_gtest(test_node_interfaces__node_timers node_interfaces/test_node_timers.cpp) if(TARGET test_node_interfaces__node_timers) - target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME} mimick) + target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME} mimick rcl::rcl) endif() ament_add_gtest(test_node_interfaces__node_topics node_interfaces/test_node_topics.cpp) if(TARGET test_node_interfaces__node_topics) - ament_target_dependencies( - test_node_interfaces__node_topics - "test_msgs") - target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME} mimick) + target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME} mimick rcl::rcl ${test_msgs_TARGETS}) endif() ament_add_gtest(test_node_interfaces__node_type_descriptions node_interfaces/test_node_type_descriptions.cpp) if(TARGET test_node_interfaces__node_type_descriptions) - target_link_libraries(test_node_interfaces__node_type_descriptions ${PROJECT_NAME} mimick) + target_link_libraries(test_node_interfaces__node_type_descriptions ${PROJECT_NAME}) endif() ament_add_gtest(test_node_interfaces__node_waitables node_interfaces/test_node_waitables.cpp) if(TARGET test_node_interfaces__node_waitables) - target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME} mimick) + target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME} mimick rcl::rcl) endif() ament_add_gtest(test_node_interfaces__test_template_utils # Compile time test node_interfaces/detail/test_template_utils.cpp) @@ -302,82 +208,43 @@ endif() ament_add_gtest(test_node_global_args test_node_global_args.cpp) if(TARGET test_node_global_args) - ament_target_dependencies(test_node_global_args - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) target_link_libraries(test_node_global_args ${PROJECT_NAME}) endif() ament_add_gtest(test_node_options test_node_options.cpp) if(TARGET test_node_options) - ament_target_dependencies(test_node_options "rcl") - target_link_libraries(test_node_options ${PROJECT_NAME} mimick) + target_link_libraries(test_node_options ${PROJECT_NAME} mimick rcl::rcl) endif() ament_add_gtest(test_init_options test_init_options.cpp) if(TARGET test_init_options) - ament_target_dependencies(test_init_options "rcl") - target_link_libraries(test_init_options ${PROJECT_NAME} mimick) + target_link_libraries(test_init_options ${PROJECT_NAME} mimick rcl::rcl) endif() ament_add_gmock(test_parameter_client test_parameter_client.cpp) if(TARGET test_parameter_client) - ament_target_dependencies(test_parameter_client - "rcl_interfaces" - ) - target_link_libraries(test_parameter_client ${PROJECT_NAME}) + target_link_libraries(test_parameter_client ${PROJECT_NAME} ${rcl_interfaces_TARGETS}) endif() ament_add_gtest(test_parameter_service test_parameter_service.cpp) if(TARGET test_parameter_service) - ament_target_dependencies(test_parameter_service - "rcl_interfaces" - ) target_link_libraries(test_parameter_service ${PROJECT_NAME}) endif() ament_add_gtest(test_parameter_events_filter test_parameter_events_filter.cpp) if(TARGET test_parameter_events_filter) - ament_target_dependencies(test_parameter_events_filter - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_parameter_events_filter ${PROJECT_NAME}) + target_link_libraries(test_parameter_events_filter ${PROJECT_NAME} ${rcl_interfaces_TARGETS}) endif() ament_add_gtest(test_parameter test_parameter.cpp) if(TARGET test_parameter) - ament_target_dependencies(test_parameter - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) target_link_libraries(test_parameter ${PROJECT_NAME}) endif() ament_add_gtest(test_parameter_event_handler test_parameter_event_handler.cpp) if(TARGET test_parameter_event_handler) - ament_target_dependencies(test_parameter_event_handler - "rcl_interfaces" - "rmw" - "rosidl_generator_cpp" - "rosidl_typesupport_cpp" - ) target_link_libraries(test_parameter_event_handler ${PROJECT_NAME}) endif() ament_add_gtest(test_parameter_map test_parameter_map.cpp) if(TARGET test_parameter_map) - target_link_libraries(test_parameter_map ${PROJECT_NAME}) + target_link_libraries(test_parameter_map ${PROJECT_NAME} rcl::rcl rcl_yaml_param_parser::rcl_yaml_param_parser rcutils::rcutils) endif() ament_add_gtest(test_publisher test_publisher.cpp TIMEOUT 120) if(TARGET test_publisher) - ament_target_dependencies(test_publisher - "rcl" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_publisher ${PROJECT_NAME} mimick) + target_link_libraries(test_publisher ${PROJECT_NAME} mimick rcl::rcl rcutils::rcutils ${test_msgs_TARGETS}) endif() set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}") @@ -407,32 +274,22 @@ ament_add_gtest(test_subscription_publisher_with_same_type_adapter test_subscrip APPEND_LIBRARY_DIRS "${append_library_dirs}" ) if(TARGET test_subscription_publisher_with_same_type_adapter) - ament_target_dependencies(test_subscription_publisher_with_same_type_adapter - "statistics_msgs" - ) target_link_libraries(test_subscription_publisher_with_same_type_adapter ${PROJECT_NAME} - ${cpp_typesupport_target}) + ${cpp_typesupport_target} + ${statistics_msgs_TARGETS} + ) endif() ament_add_gtest(test_publisher_subscription_count_api test_publisher_subscription_count_api.cpp) if(TARGET test_publisher_subscription_count_api) - ament_target_dependencies(test_publisher_subscription_count_api - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME}) + target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_qos test_qos.cpp) if(TARGET test_qos) - ament_target_dependencies(test_qos - "rmw" - ) target_link_libraries(test_qos ${PROJECT_NAME} + rmw::rmw ) endif() function(test_generic_pubsub_for_rmw_implementation) @@ -441,12 +298,7 @@ function(test_generic_pubsub_for_rmw_implementation) ENV ${rmw_implementation_env_var} ) if(TARGET test_generic_pubsub${target_suffix}) - target_link_libraries(test_generic_pubsub${target_suffix} ${PROJECT_NAME}) - ament_target_dependencies(test_generic_pubsub${target_suffix} - "rcpputils" - "rosidl_typesupport_cpp" - "test_msgs" - ) + target_link_libraries(test_generic_pubsub${target_suffix} ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) endif() endfunction() call_for_each_rmw_implementation(test_generic_pubsub_for_rmw_implementation) @@ -457,153 +309,79 @@ function(test_qos_event_for_rmw_implementation) ENV ${rmw_implementation_env_var} ) if(TARGET test_qos_event${target_suffix}) - target_link_libraries(test_qos_event${target_suffix} ${PROJECT_NAME} mimick) - ament_target_dependencies(test_qos_event${target_suffix} - "rmw" - "rosidl_typesupport_cpp" - "test_msgs" - ) + target_link_libraries(test_qos_event${target_suffix} ${PROJECT_NAME} mimick rcutils::rcutils rmw::rmw ${test_msgs_TARGETS}) endif() endfunction() call_for_each_rmw_implementation(test_qos_event_for_rmw_implementation) ament_add_gmock(test_qos_overriding_options test_qos_overriding_options.cpp) if(TARGET test_qos_overriding_options) - target_link_libraries(test_qos_overriding_options - ${PROJECT_NAME} - ) + target_link_libraries(test_qos_overriding_options ${PROJECT_NAME}) endif() ament_add_gmock(test_qos_parameters test_qos_parameters.cpp) if(TARGET test_qos_parameters) - target_link_libraries(test_qos_parameters - ${PROJECT_NAME} - ) + target_link_libraries(test_qos_parameters ${PROJECT_NAME}) endif() ament_add_gtest(test_rate test_rate.cpp) if(TARGET test_rate) - ament_target_dependencies(test_rate - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_rate - ${PROJECT_NAME} - ) + target_link_libraries(test_rate ${PROJECT_NAME}) endif() ament_add_gtest(test_serialized_message_allocator test_serialized_message_allocator.cpp) if(TARGET test_serialized_message_allocator) - ament_target_dependencies(test_serialized_message_allocator - test_msgs - ) - target_link_libraries(test_serialized_message_allocator - ${PROJECT_NAME} - ) + target_link_libraries(test_serialized_message_allocator ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) endif() ament_add_gtest(test_serialized_message test_serialized_message.cpp) if(TARGET test_serialized_message) - ament_target_dependencies(test_serialized_message - test_msgs - ) - target_link_libraries(test_serialized_message - ${PROJECT_NAME} - ) + target_link_libraries(test_serialized_message ${PROJECT_NAME} rcpputils::rcpputils ${test_msgs_TARGETS}) endif() ament_add_gtest(test_service test_service.cpp) if(TARGET test_service) - ament_target_dependencies(test_service - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_service ${PROJECT_NAME} mimick) + target_link_libraries(test_service ${PROJECT_NAME} mimick ${rcl_interfaces_TARGES} ${test_msgs_TARGETS}) endif() ament_add_gmock(test_service_introspection test_service_introspection.cpp) -if(TARGET test_service) - ament_target_dependencies(test_service_introspection - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_service_introspection ${PROJECT_NAME} mimick) +if(TARGET test_service_introspection) + target_link_libraries(test_service_introspection ${PROJECT_NAME} mimick ${service_msgs_TARGETS} ${test_msgs_TARGETS}) endif() # Creating and destroying nodes is slow with Connext, so this needs larger timeout. ament_add_gtest(test_subscription test_subscription.cpp TIMEOUT 120) if(TARGET test_subscription) - ament_target_dependencies(test_subscription - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_subscription ${PROJECT_NAME} mimick) + target_link_libraries(test_subscription ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) endif() ament_add_gtest(test_subscription_publisher_count_api test_subscription_publisher_count_api.cpp) if(TARGET test_subscription_publisher_count_api) - ament_target_dependencies(test_subscription_publisher_count_api - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME}) + target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_subscription_traits test_subscription_traits.cpp) if(TARGET test_subscription_traits) - ament_target_dependencies(test_subscription_traits - "rcl" - "test_msgs" - ) - target_link_libraries(test_subscription_traits ${PROJECT_NAME}) + target_link_libraries(test_subscription_traits ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) endif() ament_add_gtest(test_type_support test_type_support.cpp) if(TARGET test_type_support) - ament_target_dependencies(test_type_support - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_type_support ${PROJECT_NAME}) + target_link_libraries(test_type_support ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gmock(test_typesupport_helpers test_typesupport_helpers.cpp) if(TARGET test_typesupport_helpers) - target_link_libraries(test_typesupport_helpers ${PROJECT_NAME}) + target_link_libraries(test_typesupport_helpers ${PROJECT_NAME} rcpputils::rcpputils) endif() ament_add_gtest(test_find_weak_nodes test_find_weak_nodes.cpp) if(TARGET test_find_weak_nodes) - ament_target_dependencies(test_find_weak_nodes - "rcl" - ) target_link_libraries(test_find_weak_nodes ${PROJECT_NAME}) endif() ament_add_gtest(test_externally_defined_services test_externally_defined_services.cpp) -ament_target_dependencies(test_externally_defined_services - "rcl" - "test_msgs" -) -target_link_libraries(test_externally_defined_services ${PROJECT_NAME}) +target_link_libraries(test_externally_defined_services ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) ament_add_gtest(test_duration test_duration.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_duration) - ament_target_dependencies(test_duration - "rcl") - target_link_libraries(test_duration ${PROJECT_NAME}) + target_link_libraries(test_duration ${PROJECT_NAME} rcl::rcl) endif() ament_add_gtest(test_logger test_logger.cpp) -target_link_libraries(test_logger ${PROJECT_NAME}) +target_link_libraries(test_logger ${PROJECT_NAME} rcutils::rcutils) ament_add_gmock(test_logging test_logging.cpp) -target_link_libraries(test_logging ${PROJECT_NAME}) +target_link_libraries(test_logging ${PROJECT_NAME} rcutils::rcutils) ament_add_gmock(test_context test_context.cpp) target_link_libraries(test_context ${PROJECT_NAME}) @@ -611,62 +389,46 @@ target_link_libraries(test_context ${PROJECT_NAME}) ament_add_gtest(test_time test_time.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_time) - ament_target_dependencies(test_time - "rcl") - target_link_libraries(test_time ${PROJECT_NAME}) + target_link_libraries(test_time ${PROJECT_NAME} rcl::rcl rcutils::rcutils) endif() ament_add_gtest(test_timer test_timer.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_timer) - ament_target_dependencies(test_timer - "rcl") - target_link_libraries(test_timer ${PROJECT_NAME} mimick) + target_link_libraries(test_timer ${PROJECT_NAME} mimick rcl::rcl) endif() ament_add_gtest(test_timers_manager test_timers_manager.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_timers_manager) - ament_target_dependencies(test_timers_manager - "rcl") - target_link_libraries(test_timers_manager ${PROJECT_NAME} mimick) + target_link_libraries(test_timers_manager ${PROJECT_NAME}) endif() ament_add_gtest(test_time_source test_time_source.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_time_source) - ament_target_dependencies(test_time_source - "rcl") - target_link_libraries(test_time_source ${PROJECT_NAME}) + target_link_libraries(test_time_source ${PROJECT_NAME} rcl::rcl) endif() ament_add_gtest(test_utilities test_utilities.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_utilities) - ament_target_dependencies(test_utilities - "rcl") - target_link_libraries(test_utilities ${PROJECT_NAME} mimick) + target_link_libraries(test_utilities ${PROJECT_NAME} mimick rcl::rcl) endif() ament_add_gtest(test_wait_for_message test_wait_for_message.cpp) if(TARGET test_wait_for_message) - ament_target_dependencies(test_wait_for_message - "test_msgs") - target_link_libraries(test_wait_for_message ${PROJECT_NAME}) + target_link_libraries(test_wait_for_message ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_logger_service test_logger_service.cpp) if(TARGET test_logger_service) - ament_target_dependencies(test_logger_service - "rcl_interfaces") - target_link_libraries(test_logger_service ${PROJECT_NAME}) + target_link_libraries(test_logger_service ${PROJECT_NAME} ${rcl_interfaces_TARGETS}) endif() ament_add_gtest(test_interface_traits test_interface_traits.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_interface_traits) - ament_target_dependencies(test_interface_traits - "rcl") target_link_libraries(test_interface_traits ${PROJECT_NAME}) endif() @@ -676,67 +438,47 @@ ament_add_gtest( APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 180) if(TARGET test_executors) - ament_target_dependencies(test_executors - "rcl" - "test_msgs") - target_link_libraries(test_executors ${PROJECT_NAME}) + target_link_libraries(test_executors ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) endif() ament_add_gtest(test_static_single_threaded_executor executors/test_static_single_threaded_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_static_single_threaded_executor) - ament_target_dependencies(test_static_single_threaded_executor - "test_msgs") - target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME} mimick) + target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) endif() ament_add_gtest(test_multi_threaded_executor executors/test_multi_threaded_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_multi_threaded_executor) - ament_target_dependencies(test_multi_threaded_executor - "rcl") target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME}) endif() ament_add_gtest(test_static_executor_entities_collector executors/test_static_executor_entities_collector.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) if(TARGET test_static_executor_entities_collector) - ament_target_dependencies(test_static_executor_entities_collector - "rcl" - "test_msgs") - target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME} mimick) + target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME} mimick rcpputils::rcpputils ${test_msgs_TARGETS}) endif() ament_add_gtest(test_entities_collector executors/test_entities_collector.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) if(TARGET test_entities_collector) - ament_target_dependencies(test_entities_collector - "rcl" - "test_msgs") - target_link_libraries(test_entities_collector ${PROJECT_NAME} mimick) + target_link_libraries(test_entities_collector ${PROJECT_NAME}) endif() ament_add_gtest(test_executor_notify_waitable executors/test_executor_notify_waitable.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) if(TARGET test_executor_notify_waitable) - ament_target_dependencies(test_executor_notify_waitable - "rcl" - "test_msgs") - target_link_libraries(test_executor_notify_waitable ${PROJECT_NAME} mimick) + target_link_libraries(test_executor_notify_waitable ${PROJECT_NAME} mimick rcpputils::rcpputils) endif() ament_add_gtest(test_events_executor executors/test_events_executor.cpp TIMEOUT 5) if(TARGET test_events_executor) - ament_target_dependencies(test_events_executor - "test_msgs") - target_link_libraries(test_events_executor ${PROJECT_NAME}) + target_link_libraries(test_events_executor ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_events_queue executors/test_events_queue.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_events_queue) - ament_target_dependencies(test_events_queue - "test_msgs") target_link_libraries(test_events_queue ${PROJECT_NAME}) endif() @@ -749,76 +491,60 @@ endif() ament_add_gtest(test_wait_set test_wait_set.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_wait_set) - ament_target_dependencies(test_wait_set "test_msgs") - target_link_libraries(test_wait_set ${PROJECT_NAME}) + target_link_libraries(test_wait_set ${PROJECT_NAME} ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_subscription_topic_statistics topic_statistics/test_subscription_topic_statistics.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" ) if(TARGET test_subscription_topic_statistics) - ament_target_dependencies(test_subscription_topic_statistics - "builtin_interfaces" - "libstatistics_collector" - "rcl_interfaces" - "rcutils" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "statistics_msgs" - "test_msgs") target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME} - ${cpp_typesupport_target}) + libstatistics_collector::libstatistics_collector + ${statistics_msgs_TARGETS} + ${test_msgs_TARGETS} + ) endif() ament_add_gtest(test_subscription_options test_subscription_options.cpp) if(TARGET test_subscription_options) - ament_target_dependencies(test_subscription_options "rcl") target_link_libraries(test_subscription_options ${PROJECT_NAME}) endif() ament_add_gtest(test_dynamic_storage wait_set_policies/test_dynamic_storage.cpp) if(TARGET test_dynamic_storage) - ament_target_dependencies(test_dynamic_storage "rcl" "test_msgs") - target_link_libraries(test_dynamic_storage ${PROJECT_NAME}) + target_link_libraries(test_dynamic_storage ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_storage_policy_common wait_set_policies/test_storage_policy_common.cpp) if(TARGET test_storage_policy_common) - ament_target_dependencies(test_storage_policy_common "rcl" "test_msgs") - target_link_libraries(test_storage_policy_common ${PROJECT_NAME} mimick) + target_link_libraries(test_storage_policy_common ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) endif() ament_add_gtest(test_static_storage wait_set_policies/test_static_storage.cpp) if(TARGET test_static_storage) - ament_target_dependencies(test_static_storage "rcl" "test_msgs") - target_link_libraries(test_static_storage ${PROJECT_NAME}) + target_link_libraries(test_static_storage ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_thread_safe_synchronization wait_set_policies/test_thread_safe_synchronization.cpp) if(TARGET test_thread_safe_synchronization) - ament_target_dependencies(test_thread_safe_synchronization "rcl" "test_msgs") - target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME}) + target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_rosout_qos test_rosout_qos.cpp) if(TARGET test_rosout_qos) - ament_target_dependencies(test_rosout_qos "rcl") - target_link_libraries(test_rosout_qos ${PROJECT_NAME}) + target_link_libraries(test_rosout_qos ${PROJECT_NAME} rcl::rcl rmw::rmw) endif() ament_add_gtest(test_rosout_subscription test_rosout_subscription.cpp) if(TARGET test_rosout_subscription) - ament_target_dependencies(test_rosout_subscription "rcl") - target_link_libraries(test_rosout_subscription ${PROJECT_NAME}) + target_link_libraries(test_rosout_subscription ${PROJECT_NAME} ${rcl_interfaces_TARGETS}) endif() ament_add_gtest(test_executor test_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) if(TARGET test_executor) - ament_target_dependencies(test_executor "rcl") target_link_libraries(test_executor ${PROJECT_NAME} mimick) endif() @@ -835,12 +561,7 @@ function(test_subscription_content_filter_for_rmw_implementation) TIMEOUT 120 ) if(TARGET test_subscription_content_filter${target_suffix}) - target_link_libraries(test_subscription_content_filter${target_suffix} ${PROJECT_NAME} mimick) - ament_target_dependencies(test_subscription_content_filter${target_suffix} - "rcpputils" - "rosidl_typesupport_cpp" - "test_msgs" - ) + target_link_libraries(test_subscription_content_filter${target_suffix} ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) endif() endfunction() call_for_each_rmw_implementation(test_subscription_content_filter_for_rmw_implementation) diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index 785ed0e5e1..647a820969 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -5,6 +5,7 @@ project(rclcpp_action) find_package(ament_cmake_ros REQUIRED) find_package(action_msgs REQUIRED) find_package(rclcpp REQUIRED) +find_package(rcl REQUIRED) find_package(rcl_action REQUIRED) find_package(rcpputils REQUIRED) find_package(rosidl_runtime_c REQUIRED) @@ -21,28 +22,26 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") ) endif() -set(${PROJECT_NAME}_SRCS +add_library(${PROJECT_NAME} src/client.cpp src/qos.cpp src/server.cpp src/server_goal_handle.cpp src/types.cpp ) - -add_library(${PROJECT_NAME} - ${${PROJECT_NAME}_SRCS}) - target_include_directories(${PROJECT_NAME} PUBLIC "$" "$") - -ament_target_dependencies(${PROJECT_NAME} - "action_msgs" - "rcl_action" - "rclcpp" - "rcpputils" - "rosidl_runtime_c" +target_link_libraries(${PROJECT_NAME} PUBLIC + ${action_msgs_TARGETS} + rcl::rcl + rcl_action::rcl_action + rclcpp::rclcpp + rosidl_runtime_c::rosidl_runtime_c +) +target_link_libraries(${PROJECT_NAME} PRIVATE + rcpputils::rcpputils ) # Causes the visibility macros to use dllexport rather than dllimport, @@ -69,12 +68,7 @@ ament_export_libraries(${PROJECT_NAME}) # Export modern CMake targets ament_export_targets(${PROJECT_NAME}) -# specific order: dependents before dependencies -ament_export_dependencies(ament_cmake) -ament_export_dependencies(action_msgs) -ament_export_dependencies(rclcpp) -ament_export_dependencies(rcl_action) -ament_export_dependencies(rosidl_runtime_c) +ament_export_dependencies(action_msgs rcl_action rclcpp rosidl_runtime_c) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) @@ -87,58 +81,52 @@ if(BUILD_TESTING) ament_add_gtest(test_client test/test_client.cpp TIMEOUT 180) if(TARGET test_client) - ament_target_dependencies(test_client - "rcutils" - "test_msgs" - ) target_link_libraries(test_client ${PROJECT_NAME} mimick + rcl::rcl + rcl_action::rcl_action + rclcpp::rclcpp + rcutils::rcutils + ${test_msgs_TARGETS} ) endif() ament_add_gtest(test_server test/test_server.cpp TIMEOUT 180) if(TARGET test_server) - ament_target_dependencies(test_server - "rcpputils" - "rcutils" - "test_msgs" - ) target_link_libraries(test_server ${PROJECT_NAME} mimick + rcl_action::rcl_action + rclcpp::rclcpp + ${test_msgs_TARGETS} ) endif() ament_add_gtest(test_server_goal_handle test/test_server_goal_handle.cpp) if(TARGET test_server_goal_handle) - ament_target_dependencies(test_server_goal_handle - "rcutils" - "test_msgs" - ) target_link_libraries(test_server_goal_handle ${PROJECT_NAME} + ${action_msgs_TARGETS} mimick + rclcpp::rclcpp + ${test_msgs_TARGETS} ) endif() ament_add_gtest(test_traits test/test_traits.cpp) if(TARGET test_traits) - ament_target_dependencies(test_traits - "test_msgs" - ) target_link_libraries(test_traits ${PROJECT_NAME} + ${test_msgs_TARGETS} ) endif() ament_add_gtest(test_types test/test_types.cpp) if(TARGET test_types) - ament_target_dependencies(test_types - "test_msgs" - ) target_link_libraries(test_types ${PROJECT_NAME} + ${test_msgs_TARGETS} ) endif() endif() diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 105ead1894..2cd65a86f1 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -23,6 +23,7 @@ action_msgs rclcpp rcl_action + rcl rcpputils ament_cmake diff --git a/rclcpp_action/test/benchmark/CMakeLists.txt b/rclcpp_action/test/benchmark/CMakeLists.txt index dc87c553af..0ce6c6245c 100644 --- a/rclcpp_action/test/benchmark/CMakeLists.txt +++ b/rclcpp_action/test/benchmark/CMakeLists.txt @@ -9,8 +9,7 @@ add_performance_test( benchmark_action_client.cpp TIMEOUT 240) if(TARGET benchmark_action_client) - target_link_libraries(benchmark_action_client ${PROJECT_NAME}) - ament_target_dependencies(benchmark_action_client rclcpp test_msgs) + target_link_libraries(benchmark_action_client ${PROJECT_NAME} rclcpp::rclcpp ${test_msgs_TARGETS}) endif() add_performance_test( @@ -18,6 +17,5 @@ add_performance_test( benchmark_action_server.cpp TIMEOUT 120) if(TARGET benchmark_action_server) - target_link_libraries(benchmark_action_server ${PROJECT_NAME}) - ament_target_dependencies(benchmark_action_server rclcpp test_msgs) + target_link_libraries(benchmark_action_server ${PROJECT_NAME} rclcpp::rclcpp ${test_msgs_TARGETS}) endif() diff --git a/rclcpp_components/CMakeLists.txt b/rclcpp_components/CMakeLists.txt index 121ef434c2..7d4135051c 100644 --- a/rclcpp_components/CMakeLists.txt +++ b/rclcpp_components/CMakeLists.txt @@ -21,7 +21,7 @@ find_package(composition_interfaces REQUIRED) find_package(rclcpp REQUIRED) find_package(rcpputils REQUIRED) -# Add an interface library that can be dependend upon by libraries who register components +# Add an interface library that can be depended upon by libraries who register components add_library(component INTERFACE) target_include_directories(component INTERFACE "$" @@ -38,12 +38,14 @@ add_library( target_include_directories(component_manager PUBLIC "$" "$") -ament_target_dependencies(component_manager - "ament_index_cpp" - "class_loader" - "composition_interfaces" - "rclcpp" - "rcpputils" +target_link_libraries(component_manager PUBLIC + ${composition_interfaces_TARGETS} + rclcpp::rclcpp +) +target_link_libraries(component_manager PRIVATE + ament_index_cpp::ament_index_cpp + class_loader::class_loader + rcpputils::rcpputils ) target_compile_definitions(component_manager PRIVATE "RCLCPP_COMPONENTS_BUILDING_LIBRARY") @@ -52,10 +54,7 @@ add_executable( component_container src/component_container.cpp ) -target_link_libraries(component_container component_manager) -ament_target_dependencies(component_container - "rclcpp" -) +target_link_libraries(component_container component_manager rclcpp::rclcpp) set(node_main_template_install_dir "share/${PROJECT_NAME}") install(FILES @@ -66,20 +65,13 @@ add_executable( component_container_mt src/component_container_mt.cpp ) -target_link_libraries(component_container_mt component_manager) -ament_target_dependencies(component_container_mt - "rclcpp" -) +target_link_libraries(component_container_mt component_manager rclcpp::rclcpp) add_executable( component_container_isolated src/component_container_isolated.cpp ) -target_link_libraries(component_container_isolated component_manager) -ament_target_dependencies(component_container_isolated - "rclcpp" -) - +target_link_libraries(component_container_isolated component_manager rclcpp::rclcpp) if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") target_link_libraries(component_container "stdc++fs") diff --git a/rclcpp_components/cmake/rclcpp_components_register_node.cmake b/rclcpp_components/cmake/rclcpp_components_register_node.cmake index c5b87af667..e78bb3650c 100644 --- a/rclcpp_components/cmake/rclcpp_components_register_node.cmake +++ b/rclcpp_components/cmake/rclcpp_components_register_node.cmake @@ -70,10 +70,11 @@ macro(rclcpp_components_register_node target) file(GENERATE OUTPUT ${PROJECT_BINARY_DIR}/rclcpp_components/node_main_${node}.cpp INPUT ${PROJECT_BINARY_DIR}/rclcpp_components/node_main_configured_${node}.cpp.in) add_executable(${node} ${PROJECT_BINARY_DIR}/rclcpp_components/node_main_${node}.cpp) - ament_target_dependencies(${node} - "rclcpp" - "class_loader" - "rclcpp_components") + target_link_libraries(${node} + class_loader::class_loader + rclcpp::rclcpp + rclcpp_components::component + ) install(TARGETS ${node} DESTINATION lib/${PROJECT_NAME}) From a19ad2134b17d4f00dc600aed4d897bb7b32b77f Mon Sep 17 00:00:00 2001 From: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Date: Fri, 1 Dec 2023 10:01:27 -0500 Subject: [PATCH 316/455] Adding QoS to subscription options (#2323) * Adding QoS to subscription options Signed-off-by: CursedRock17 --- rclcpp/include/rclcpp/create_subscription.hpp | 2 +- rclcpp/include/rclcpp/subscription_options.hpp | 4 ++++ rclcpp/test/rclcpp/test_subscription_options.cpp | 3 +++ 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 016c966b62..3a1e4b1a13 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -88,7 +88,7 @@ create_subscription( node_parameters, node_topics_interface, options.topic_stats_options.publish_topic, - qos); + options.topic_stats_options.qos); subscription_topic_stats = std::make_shared( diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 43b9ec034c..71f90fe15d 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -77,6 +77,10 @@ struct SubscriptionOptionsBase // Topic statistics publication period in ms. Defaults to one second. // Only values greater than zero are allowed. std::chrono::milliseconds publish_period{std::chrono::seconds(1)}; + + // An optional QoS which can provide topic_statistics with a stable QoS separate from + // the subscription's current QoS settings which could be unstable. + rclcpp::QoS qos = SystemDefaultsQoS(); }; TopicStatisticsOptions topic_stats_options; diff --git a/rclcpp/test/rclcpp/test_subscription_options.cpp b/rclcpp/test/rclcpp/test_subscription_options.cpp index c4cfb6b4c4..d1122333bc 100644 --- a/rclcpp/test/rclcpp/test_subscription_options.cpp +++ b/rclcpp/test/rclcpp/test_subscription_options.cpp @@ -60,14 +60,17 @@ TEST_F(TestSubscriptionOptions, topic_statistics_options_default_and_set) { EXPECT_EQ(options.topic_stats_options.state, rclcpp::TopicStatisticsState::NodeDefault); EXPECT_EQ(options.topic_stats_options.publish_topic, defaultPublishTopic); EXPECT_EQ(options.topic_stats_options.publish_period, 1s); + EXPECT_EQ(options.topic_stats_options.qos, rclcpp::SystemDefaultsQoS()); options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; options.topic_stats_options.publish_topic = "topic_statistics"; options.topic_stats_options.publish_period = 5min; + options.topic_stats_options.qos = rclcpp::BestAvailableQoS(); EXPECT_EQ(options.topic_stats_options.state, rclcpp::TopicStatisticsState::Enable); EXPECT_EQ(options.topic_stats_options.publish_topic, "topic_statistics"); EXPECT_EQ(options.topic_stats_options.publish_period, 5min); + EXPECT_EQ(options.topic_stats_options.qos, rclcpp::BestAvailableQoS()); } TEST_F(TestSubscriptionOptions, topic_statistics_options_node_default_mode) { From d9b2744057b8fd230f2c71c739fcdf7e27219d85 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 12 Dec 2023 09:33:28 +0800 Subject: [PATCH 317/455] make type support helper supported for service (#2209) * make type support helper supported for service and action as well Signed-off-by: Chen Lihui * not to use template and only add the necessary service type currently Signed-off-by: Chen Lihui * update comment Signed-off-by: Chen Lihui * add deprecated cycle for `get_typesupport_handle` Signed-off-by: Chen Lihui --------- Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/generic_publisher.hpp | 2 +- .../include/rclcpp/generic_subscription.hpp | 2 +- rclcpp/include/rclcpp/typesupport_helpers.hpp | 39 +++++++++ rclcpp/src/rclcpp/typesupport_helpers.cpp | 80 ++++++++++++++----- .../test/rclcpp/test_typesupport_helpers.cpp | 53 +++++++++++- 5 files changed, 154 insertions(+), 22 deletions(-) diff --git a/rclcpp/include/rclcpp/generic_publisher.hpp b/rclcpp/include/rclcpp/generic_publisher.hpp index 7cd2d8bc39..292e6900d3 100644 --- a/rclcpp/include/rclcpp/generic_publisher.hpp +++ b/rclcpp/include/rclcpp/generic_publisher.hpp @@ -77,7 +77,7 @@ class GenericPublisher : public rclcpp::PublisherBase : rclcpp::PublisherBase( node_base, topic_name, - *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), + *rclcpp::get_message_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), options.template to_rcl_publisher_options(qos), // NOTE(methylDragon): Passing these args separately is necessary for event binding options.event_callbacks, diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp index 975a9d0d0d..dfbae0467b 100644 --- a/rclcpp/include/rclcpp/generic_subscription.hpp +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -79,7 +79,7 @@ class GenericSubscription : public rclcpp::SubscriptionBase const rclcpp::SubscriptionOptionsWithAllocator & options) : SubscriptionBase( node_base, - *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), + *rclcpp::get_message_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), topic_name, options.to_rcl_subscription_options(qos), options.event_callbacks, diff --git a/rclcpp/include/rclcpp/typesupport_helpers.hpp b/rclcpp/include/rclcpp/typesupport_helpers.hpp index 2fad84cf3b..c93b318440 100644 --- a/rclcpp/include/rclcpp/typesupport_helpers.hpp +++ b/rclcpp/include/rclcpp/typesupport_helpers.hpp @@ -22,6 +22,7 @@ #include "rcpputils/shared_library.hpp" #include "rosidl_runtime_cpp/message_type_support_decl.hpp" +#include "rosidl_runtime_cpp/service_type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" @@ -40,11 +41,15 @@ get_typesupport_library(const std::string & type, const std::string & typesuppor /// Extract the type support handle from the library. /** * The library needs to match the topic type. The shared library must stay loaded for the lifetime of the result. + * + * \deprecated Use get_message_typesupport_handle() instead + * * \param[in] type The topic type, e.g. "std_msgs/msg/String" * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp" * \param[in] library The shared type support library * \return A type support handle */ +[[deprecated("Use `get_message_typesupport_handle` instead")]] RCLCPP_PUBLIC const rosidl_message_type_support_t * get_typesupport_handle( @@ -52,6 +57,40 @@ get_typesupport_handle( const std::string & typesupport_identifier, rcpputils::SharedLibrary & library); +/// Extract the message type support handle from the library. +/** + * The library needs to match the topic type. The shared library must stay loaded for the lifetime of the result. + * + * \param[in] type The topic type, e.g. "std_msgs/msg/String" + * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp" + * \param[in] library The shared type support library + * \throws std::runtime_error if the symbol of type not found in the library. + * \return A message type support handle + */ +RCLCPP_PUBLIC +const rosidl_message_type_support_t * +get_message_typesupport_handle( + const std::string & type, + const std::string & typesupport_identifier, + rcpputils::SharedLibrary & library); + +/// Extract the service type support handle from the library. +/** + * The library needs to match the topic type. The shared library must stay loaded for the lifetime of the result. + * + * \param[in] type The service type, e.g. "std_srvs/srv/Empty" + * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp" + * \param[in] library The shared type support library + * \throws std::runtime_error if the symbol of type not found in the library. + * \return A service type support handle + */ +RCLCPP_PUBLIC +const rosidl_service_type_support_t * +get_service_typesupport_handle( + const std::string & type, + const std::string & typesupport_identifier, + rcpputils::SharedLibrary & library); + } // namespace rclcpp #endif // RCLCPP__TYPESUPPORT_HELPERS_HPP_ diff --git a/rclcpp/src/rclcpp/typesupport_helpers.cpp b/rclcpp/src/rclcpp/typesupport_helpers.cpp index 7286c35baa..04dd8c4c9f 100644 --- a/rclcpp/src/rclcpp/typesupport_helpers.cpp +++ b/rclcpp/src/rclcpp/typesupport_helpers.cpp @@ -91,20 +91,12 @@ extract_type_identifier(const std::string & full_type) return std::make_tuple(package_name, middle_module, type_name); } -} // anonymous namespace - -std::shared_ptr -get_typesupport_library(const std::string & type, const std::string & typesupport_identifier) -{ - auto package_name = std::get<0>(extract_type_identifier(type)); - auto library_path = get_typesupport_library_path(package_name, typesupport_identifier); - return std::make_shared(library_path); -} - -const rosidl_message_type_support_t * -get_typesupport_handle( +const void * get_typesupport_handle_impl( const std::string & type, const std::string & typesupport_identifier, + const std::string & typesupport_name, + const std::string & symbol_part_name, + const std::string & middle_module_additional, rcpputils::SharedLibrary & library) { std::string package_name; @@ -112,19 +104,23 @@ get_typesupport_handle( std::string type_name; std::tie(package_name, middle_module, type_name) = extract_type_identifier(type); - auto mk_error = [&package_name, &type_name](auto reason) { + if (middle_module.empty()) { + middle_module = middle_module_additional; + } + + auto mk_error = [&package_name, &type_name, &typesupport_name](auto reason) { std::stringstream rcutils_dynamic_loading_error; rcutils_dynamic_loading_error << - "Something went wrong loading the typesupport library for message type " << package_name << + "Something went wrong loading the typesupport library for " << + typesupport_name << " type " << package_name << "/" << type_name << ". " << reason; return rcutils_dynamic_loading_error.str(); }; try { - std::string symbol_name = typesupport_identifier + "__get_message_type_support_handle__" + - package_name + "__" + (middle_module.empty() ? "msg" : middle_module) + "__" + type_name; - - const rosidl_message_type_support_t * (* get_ts)() = nullptr; + std::string symbol_name = typesupport_identifier + symbol_part_name + + package_name + "__" + middle_module + "__" + type_name; + const void * (* get_ts)() = nullptr; // This will throw runtime_error if the symbol was not found. get_ts = reinterpret_cast(library.get_symbol(symbol_name)); return get_ts(); @@ -133,4 +129,52 @@ get_typesupport_handle( } } +} // anonymous namespace + +std::shared_ptr +get_typesupport_library(const std::string & type, const std::string & typesupport_identifier) +{ + auto package_name = std::get<0>(extract_type_identifier(type)); + auto library_path = get_typesupport_library_path(package_name, typesupport_identifier); + return std::make_shared(library_path); +} + +const rosidl_message_type_support_t * get_typesupport_handle( + const std::string & type, + const std::string & typesupport_identifier, + rcpputils::SharedLibrary & library) +{ + return get_message_typesupport_handle(type, typesupport_identifier, library); +} + +const rosidl_message_type_support_t * get_message_typesupport_handle( + const std::string & type, + const std::string & typesupport_identifier, + rcpputils::SharedLibrary & library) +{ + static const std::string typesupport_name = "message"; + static const std::string symbol_part_name = "__get_message_type_support_handle__"; + static const std::string middle_module_additional = "msg"; + + return static_cast(get_typesupport_handle_impl( + type, typesupport_identifier, typesupport_name, symbol_part_name, + middle_module_additional, library + )); +} + +const rosidl_service_type_support_t * get_service_typesupport_handle( + const std::string & type, + const std::string & typesupport_identifier, + rcpputils::SharedLibrary & library) +{ + static const std::string typesupport_name = "service"; + static const std::string symbol_part_name = "__get_service_type_support_handle__"; + static const std::string middle_module_additional = "srv"; + + return static_cast(get_typesupport_handle_impl( + type, typesupport_identifier, typesupport_name, symbol_part_name, + middle_module_additional, library + )); +} + } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/test_typesupport_helpers.cpp b/rclcpp/test/rclcpp/test_typesupport_helpers.cpp index 8cdcfc19c0..2117b89455 100644 --- a/rclcpp/test/rclcpp/test_typesupport_helpers.cpp +++ b/rclcpp/test/rclcpp/test_typesupport_helpers.cpp @@ -50,7 +50,7 @@ TEST(TypesupportHelpersTest, returns_c_type_info_for_valid_legacy_library) { try { auto library = rclcpp::get_typesupport_library( "test_msgs/BasicTypes", "rosidl_typesupport_cpp"); - auto string_typesupport = rclcpp::get_typesupport_handle( + auto string_typesupport = rclcpp::get_message_typesupport_handle( "test_msgs/BasicTypes", "rosidl_typesupport_cpp", *library); EXPECT_THAT( @@ -65,7 +65,7 @@ TEST(TypesupportHelpersTest, returns_c_type_info_for_valid_library) { try { auto library = rclcpp::get_typesupport_library( "test_msgs/msg/BasicTypes", "rosidl_typesupport_cpp"); - auto string_typesupport = rclcpp::get_typesupport_handle( + auto string_typesupport = rclcpp::get_message_typesupport_handle( "test_msgs/msg/BasicTypes", "rosidl_typesupport_cpp", *library); EXPECT_THAT( @@ -75,3 +75,52 @@ TEST(TypesupportHelpersTest, returns_c_type_info_for_valid_library) { FAIL() << e.what(); } } + +TEST(TypesupportHelpersTest, returns_service_type_info_for_valid_legacy_library) { + try { + auto library = rclcpp::get_typesupport_library( + "test_msgs/Empty", "rosidl_typesupport_cpp"); + auto empty_typesupport = rclcpp::get_service_typesupport_handle( + "test_msgs/Empty", "rosidl_typesupport_cpp", *library); + + EXPECT_THAT( + std::string(empty_typesupport->typesupport_identifier), + ContainsRegex("rosidl_typesupport")); + } catch (const std::runtime_error & e) { + FAIL() << e.what(); + } +} + +TEST(TypesupportHelpersTest, returns_service_type_info_for_valid_library) { + try { + auto library = rclcpp::get_typesupport_library( + "test_msgs/srv/Empty", "rosidl_typesupport_cpp"); + auto empty_typesupport = rclcpp::get_service_typesupport_handle( + "test_msgs/srv/Empty", "rosidl_typesupport_cpp", *library); + + EXPECT_THAT( + std::string(empty_typesupport->typesupport_identifier), + ContainsRegex("rosidl_typesupport")); + } catch (const std::runtime_error & e) { + FAIL() << e.what(); + } +} + +TEST(TypesupportHelpersTest, test_throw_exception_with_invalid_type) { + // message + std::string invalid_type = "test_msgs/msg/InvalidType"; + auto library = rclcpp::get_typesupport_library(invalid_type, "rosidl_typesupport_cpp"); + EXPECT_THROW( + rclcpp::get_message_typesupport_handle(invalid_type, "rosidl_typesupport_cpp", *library), + std::runtime_error); + EXPECT_THROW( + rclcpp::get_service_typesupport_handle(invalid_type, "rosidl_typesupport_cpp", *library), + std::runtime_error); + + // service + invalid_type = "test_msgs/srv/InvalidType"; + library = rclcpp::get_typesupport_library(invalid_type, "rosidl_typesupport_cpp"); + EXPECT_THROW( + rclcpp::get_service_typesupport_handle(invalid_type, "rosidl_typesupport_cpp", *library), + std::runtime_error); +} From 2bb1dc2c716ae12a24145891724f8436aa4e52bd Mon Sep 17 00:00:00 2001 From: DensoADAS <46967124+DensoADAS@users.noreply.github.com> Date: Tue, 19 Dec 2023 17:35:13 +0100 Subject: [PATCH 318/455] Updated GenericSubscription to AnySubscriptionCallback (#1928) * added rclcpp::SerializedMessage support for AnySubscriptionCallback Signed-off-by: Joshua Hampp Signed-off-by: Joshua Hampp * using AnySubscription callback for generic subscriptiion Signed-off-by: Joshua Hampp Signed-off-by: Joshua Hampp * updated tests Signed-off-by: Joshua Hampp Signed-off-by: Joshua Hampp * Remove comment Signed-off-by: Joshua Hampp --------- Signed-off-by: Joshua Hampp Signed-off-by: Joshua Hampp Co-authored-by: Joshua Hampp Co-authored-by: Jacob Perron --- .../rclcpp/any_subscription_callback.hpp | 34 +++++++++++-- .../rclcpp/create_generic_subscription.hpp | 15 ++++-- .../include/rclcpp/generic_subscription.hpp | 13 +++-- rclcpp/include/rclcpp/node.hpp | 6 ++- rclcpp/include/rclcpp/node_impl.hpp | 6 +-- rclcpp/src/rclcpp/generic_subscription.cpp | 4 +- rclcpp/test/rclcpp/test_generic_pubsub.cpp | 50 ++++++++++++++++++- 7 files changed, 107 insertions(+), 21 deletions(-) diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 4fd912fd10..9c5e7928e0 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -30,6 +30,7 @@ #include "rclcpp/detail/subscription_callback_type_helper.hpp" #include "rclcpp/function_traits.hpp" #include "rclcpp/message_info.hpp" +#include "rclcpp/serialization.hpp" #include "rclcpp/serialized_message.hpp" #include "rclcpp/type_adapter.hpp" @@ -158,13 +159,14 @@ struct AnySubscriptionCallbackPossibleTypes template< typename MessageT, typename AllocatorT, - bool is_adapted_type = rclcpp::TypeAdapter::is_specialized::value + bool is_adapted_type = rclcpp::TypeAdapter::is_specialized::value, + bool is_serialized_type = serialization_traits::is_serialized_message_class::value > struct AnySubscriptionCallbackHelper; /// Specialization for when MessageT is not a TypeAdapter. template -struct AnySubscriptionCallbackHelper +struct AnySubscriptionCallbackHelper { using CallbackTypes = AnySubscriptionCallbackPossibleTypes; @@ -194,7 +196,7 @@ struct AnySubscriptionCallbackHelper /// Specialization for when MessageT is a TypeAdapter. template -struct AnySubscriptionCallbackHelper +struct AnySubscriptionCallbackHelper { using CallbackTypes = AnySubscriptionCallbackPossibleTypes; @@ -232,6 +234,26 @@ struct AnySubscriptionCallbackHelper >; }; +/// Specialization for when MessageT is a SerializedMessage to exclude duplicated declarations. +template +struct AnySubscriptionCallbackHelper +{ + using CallbackTypes = AnySubscriptionCallbackPossibleTypes; + + using variant_type = std::variant< + typename CallbackTypes::ConstRefSerializedMessageCallback, + typename CallbackTypes::ConstRefSerializedMessageWithInfoCallback, + typename CallbackTypes::UniquePtrSerializedMessageCallback, + typename CallbackTypes::UniquePtrSerializedMessageWithInfoCallback, + typename CallbackTypes::SharedConstPtrSerializedMessageCallback, + typename CallbackTypes::SharedConstPtrSerializedMessageWithInfoCallback, + typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageCallback, + typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageWithInfoCallback, + typename CallbackTypes::SharedPtrSerializedMessageCallback, + typename CallbackTypes::SharedPtrSerializedMessageWithInfoCallback + >; +}; + } // namespace detail template< @@ -487,7 +509,9 @@ class AnySubscriptionCallback } // Dispatch when input is a ros message and the output could be anything. - void + template + typename std::enable_if::value, + void>::type dispatch( std::shared_ptr message, const rclcpp::MessageInfo & message_info) @@ -589,7 +613,7 @@ class AnySubscriptionCallback // Dispatch when input is a serialized message and the output could be anything. void dispatch( - std::shared_ptr serialized_message, + std::shared_ptr serialized_message, const rclcpp::MessageInfo & message_info) { TRACETOOLS_TRACEPOINT(callback_start, static_cast(this), false); diff --git a/rclcpp/include/rclcpp/create_generic_subscription.hpp b/rclcpp/include/rclcpp/create_generic_subscription.hpp index f5281cc673..c2549721b5 100644 --- a/rclcpp/include/rclcpp/create_generic_subscription.hpp +++ b/rclcpp/include/rclcpp/create_generic_subscription.hpp @@ -45,13 +45,15 @@ namespace rclcpp * Not all publisher options are currently respected, the only relevant options for this * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`. */ -template> +template< + typename CallbackT, + typename AllocatorT = std::allocator> std::shared_ptr create_generic_subscription( rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface, const std::string & topic_name, const std::string & topic_type, const rclcpp::QoS & qos, - std::function)> callback, + CallbackT && callback, const rclcpp::SubscriptionOptionsWithAllocator & options = ( rclcpp::SubscriptionOptionsWithAllocator() ) @@ -60,13 +62,20 @@ std::shared_ptr create_generic_subscription( auto ts_lib = rclcpp::get_typesupport_library( topic_type, "rosidl_typesupport_cpp"); + auto allocator = options.get_allocator(); + + using rclcpp::AnySubscriptionCallback; + AnySubscriptionCallback + any_subscription_callback(*allocator); + any_subscription_callback.set(std::forward(callback)); + auto subscription = std::make_shared( topics_interface->get_node_base_interface(), std::move(ts_lib), topic_name, topic_type, qos, - callback, + any_subscription_callback, options); topics_interface->add_subscription(subscription, options.callback_group); diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp index dfbae0467b..e9bf79deea 100644 --- a/rclcpp/include/rclcpp/generic_subscription.hpp +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -74,8 +74,7 @@ class GenericSubscription : public rclcpp::SubscriptionBase const std::string & topic_name, const std::string & topic_type, const rclcpp::QoS & qos, - // TODO(nnmm): Add variant for callback with message info. See issue #1604. - std::function)> callback, + AnySubscriptionCallback callback, const rclcpp::SubscriptionOptionsWithAllocator & options) : SubscriptionBase( node_base, @@ -85,7 +84,11 @@ class GenericSubscription : public rclcpp::SubscriptionBase options.event_callbacks, options.use_default_callbacks, DeliveredMessageKind::SERIALIZED_MESSAGE), - callback_(callback), + callback_([callback]( + std::shared_ptr serialized_message, + const rclcpp::MessageInfo & message_info) mutable { + callback.dispatch(serialized_message, message_info); + }), ts_lib_(ts_lib) {} @@ -151,7 +154,9 @@ class GenericSubscription : public rclcpp::SubscriptionBase private: RCLCPP_DISABLE_COPY(GenericSubscription) - std::function)> callback_; + std::function, + const rclcpp::MessageInfo)> callback_; // The type support library should stay loaded, so it is stored in the GenericSubscription std::shared_ptr ts_lib_; }; diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 6521264b86..50b96bbeaf 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -358,12 +358,14 @@ class Node : public std::enable_shared_from_this * `%callback_group`. * \return Shared pointer to the created generic subscription. */ - template> + template< + typename CallbackT, + typename AllocatorT = std::allocator> std::shared_ptr create_generic_subscription( const std::string & topic_name, const std::string & topic_type, const rclcpp::QoS & qos, - std::function)> callback, + CallbackT && callback, const rclcpp::SubscriptionOptionsWithAllocator & options = ( rclcpp::SubscriptionOptionsWithAllocator() ) diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 5f1fb7b3df..d55a23f9c1 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -221,13 +221,13 @@ Node::create_generic_publisher( ); } -template +template std::shared_ptr Node::create_generic_subscription( const std::string & topic_name, const std::string & topic_type, const rclcpp::QoS & qos, - std::function)> callback, + CallbackT && callback, const rclcpp::SubscriptionOptionsWithAllocator & options) { return rclcpp::create_generic_subscription( @@ -235,7 +235,7 @@ Node::create_generic_subscription( extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), topic_type, qos, - std::move(callback), + std::forward(callback), options ); } diff --git a/rclcpp/src/rclcpp/generic_subscription.cpp b/rclcpp/src/rclcpp/generic_subscription.cpp index e6e61add24..2d2be61277 100644 --- a/rclcpp/src/rclcpp/generic_subscription.cpp +++ b/rclcpp/src/rclcpp/generic_subscription.cpp @@ -49,9 +49,9 @@ GenericSubscription::handle_message( void GenericSubscription::handle_serialized_message( const std::shared_ptr & message, - const rclcpp::MessageInfo &) + const rclcpp::MessageInfo & message_info) { - callback_(message); + callback_(message, message_info); } void diff --git a/rclcpp/test/rclcpp/test_generic_pubsub.cpp b/rclcpp/test/rclcpp/test_generic_pubsub.cpp index f4cef0b757..79fbfcc33a 100644 --- a/rclcpp/test/rclcpp/test_generic_pubsub.cpp +++ b/rclcpp/test/rclcpp/test_generic_pubsub.cpp @@ -69,7 +69,7 @@ class RclcppGenericNodeFixture : public Test size_t counter = 0; auto subscription = node_->create_generic_subscription( topic_name, type, rclcpp::QoS(1), - [&counter, &messages, this](std::shared_ptr message) { + [&counter, &messages, this](const std::shared_ptr message) { T2 deserialized_message; rclcpp::Serialization serializer; serializer.deserialize_message(message.get(), &deserialized_message); @@ -236,7 +236,7 @@ TEST_F(RclcppGenericNodeFixture, generic_subscription_uses_qos) auto publisher = node_->create_publisher(topic_name, qos); auto subscription = node_->create_generic_subscription( topic_name, topic_type, qos, - [](std::shared_ptr/* message */) {}); + [](std::shared_ptr/* message */) {}); auto connected = [publisher, subscription]() -> bool { return publisher->get_subscription_count() && subscription->get_publisher_count(); }; @@ -263,3 +263,49 @@ TEST_F(RclcppGenericNodeFixture, generic_publisher_uses_qos) // It normally takes < 20ms, 5s chosen as "a very long time" ASSERT_TRUE(wait_for(connected, 5s)); } + +TEST_F(RclcppGenericNodeFixture, generic_subscription_different_callbacks) +{ + using namespace std::chrono_literals; + std::string topic_name = "string_topic"; + std::string topic_type = "test_msgs/msg/Strings"; + rclcpp::QoS qos = rclcpp::QoS(1); + + auto publisher = node_->create_publisher(topic_name, qos); + + // Test shared_ptr for const messages + { + auto subscription = node_->create_generic_subscription( + topic_name, topic_type, qos, + [](const std::shared_ptr/* message */) {}); + auto connected = [publisher, subscription]() -> bool { + return publisher->get_subscription_count() && subscription->get_publisher_count(); + }; + // It normally takes < 20ms, 5s chosen as "a very long time" + ASSERT_TRUE(wait_for(connected, 5s)); + } + + // Test unique_ptr + { + auto subscription = node_->create_generic_subscription( + topic_name, topic_type, qos, + [](std::unique_ptr/* message */) {}); + auto connected = [publisher, subscription]() -> bool { + return publisher->get_subscription_count() && subscription->get_publisher_count(); + }; + // It normally takes < 20ms, 5s chosen as "a very long time" + ASSERT_TRUE(wait_for(connected, 5s)); + } + + // Test message callback + { + auto subscription = node_->create_generic_subscription( + topic_name, topic_type, qos, + [](rclcpp::SerializedMessage /* message */) {}); + auto connected = [publisher, subscription]() -> bool { + return publisher->get_subscription_count() && subscription->get_publisher_count(); + }; + // It normally takes < 20ms, 5s chosen as "a very long time" + ASSERT_TRUE(wait_for(connected, 5s)); + } +} From 21f8a22311d5ff9f50886d264bab811603bbc5d7 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Dec 2023 17:17:15 +0000 Subject: [PATCH 319/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 11 +++++++++++ rclcpp_action/CHANGELOG.rst | 5 +++++ rclcpp_components/CHANGELOG.rst | 7 +++++++ rclcpp_lifecycle/CHANGELOG.rst | 3 +++ 4 files changed, 26 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 1a8ffdbbd5..c830316c15 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Updated GenericSubscription to AnySubscriptionCallback (`#1928 `_) +* make type support helper supported for service (`#2209 `_) +* Adding QoS to subscription options (`#2323 `_) +* Switch to target_link_libraries. (`#2374 `_) +* aligh with rcl that a rosout publisher of a node might not exist (`#2357 `_) +* Fix data race in EventHandlerBase (`#2349 `_) +* Support users holding onto shared pointers in the message memory pool (`#2336 `_) +* Contributors: Chen Lihui, Chris Lalancette, DensoADAS, Lucas Wendland, mauropasse + 24.0.0 (2023-11-06) ------------------- * fix (signal_handler.hpp): spelling (`#2356 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 7bd386a3a7..a1766c32d2 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Switch to target_link_libraries. (`#2374 `_) +* Contributors: Chris Lalancette + 24.0.0 (2023-11-06) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 9371eec9b4..e475ed26ba 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Switch to target_link_libraries. (`#2374 `_) +* feat(rclcpp_components): support events executor in node main template (`#2366 `_) +* fix(rclcpp_components): increase the service queue sizes in component_container (`#2363 `_) +* Contributors: Chris Lalancette, Daisuke Nishimatsu, M. Fatih Cırıt + 24.0.0 (2023-11-06) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 41b0a22c62..6bffd3cd66 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 24.0.0 (2023-11-06) ------------------- * Fix rclcpp_lifecycle inclusion on Windows. (`#2331 `_) From 5d2b32b5c3e5bbf538807779a7151ac1363002d2 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Dec 2023 17:17:23 +0000 Subject: [PATCH 320/455] 25.0.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index c830316c15..53683e53dc 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +25.0.0 (2023-12-26) +------------------- * Updated GenericSubscription to AnySubscriptionCallback (`#1928 `_) * make type support helper supported for service (`#2209 `_) * Adding QoS to subscription options (`#2323 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 0996336d01..2b5c9bb74c 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 24.0.0 + 25.0.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index a1766c32d2..711a346441 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +25.0.0 (2023-12-26) +------------------- * Switch to target_link_libraries. (`#2374 `_) * Contributors: Chris Lalancette diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 2cd65a86f1..e8b3cb98a3 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 24.0.0 + 25.0.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index e475ed26ba..df05905df3 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +25.0.0 (2023-12-26) +------------------- * Switch to target_link_libraries. (`#2374 `_) * feat(rclcpp_components): support events executor in node main template (`#2366 `_) * fix(rclcpp_components): increase the service queue sizes in component_container (`#2363 `_) diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 0a50dd3486..f3f5f4135d 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 24.0.0 + 25.0.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 6bffd3cd66..e0eaac5d9f 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +25.0.0 (2023-12-26) +------------------- 24.0.0 (2023-11-06) ------------------- diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 0c1b19c011..5a543a9f0c 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 24.0.0 + 25.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 126d517193b6df8177680898e23906a85792eaf6 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Wed, 3 Jan 2024 10:17:47 -0300 Subject: [PATCH 321/455] Increase timeout for rclcpp_lifecycle to 360 (#2395) * Increase timeout for rclcpp_lifecycle to 360 Signed-off-by: Jorge Perez Co-authored-by: Chris Lalancette --- rclcpp_lifecycle/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index f46f993b21..66b1990ec5 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -60,8 +60,13 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # Give cppcheck hints about macro definitions coming from outside this package set(ament_cmake_cppcheck_ADDITIONAL_INCLUDE_DIRS ${rclcpp_INCLUDE_DIRS}) + list(APPEND AMENT_LINT_AUTO_EXCLUDE "ament_cmake_cppcheck") ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_cppcheck REQUIRED) + ament_cppcheck() + set_tests_properties(cppcheck PROPERTIES TIMEOUT 360) + find_package(performance_test_fixture REQUIRED) add_performance_test( From 6f6b5f2e3608eafeadb908aa4f6eded5b38c3043 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 8 Jan 2024 08:23:33 -0500 Subject: [PATCH 322/455] Stop storing the context in the guard condition. (#2400) * Stop storing the context in the guard condition. This was creating a circular reference between GuardCondition and Context, so that Context would never be cleaned up. Since we never really need the GuardCondition to know about its own Context, remove that part of the circular reference. While we are in here, we also change the get_context() lambda to a straight weak_ptr; there is no reason for the indirection since the context for the guard condition cannot change at runtime. We also remove the deprecated version of the get_notify_guard_condition(). That's because there is no way to properly implement it in the new scheme, and it seems to be unused outside of rclcpp. Finally, we add in a test that guarantees the use_count is what we expect when inside and leaving a scope, ensuring that contexts will properly be destroyed. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/callback_group.hpp | 17 ++-------- rclcpp/include/rclcpp/guard_condition.hpp | 8 +---- rclcpp/src/rclcpp/callback_group.cpp | 34 ++----------------- rclcpp/src/rclcpp/guard_condition.cpp | 15 +++----- .../src/rclcpp/node_interfaces/node_base.cpp | 7 +--- rclcpp/test/rclcpp/test_context.cpp | 22 ++++++++++++ rclcpp/test/rclcpp/test_guard_condition.cpp | 10 ------ 7 files changed, 35 insertions(+), 78 deletions(-) diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index 43c7daa888..3f60ab5cbc 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -129,8 +129,7 @@ class CallbackGroup * added to the executor in either case. * * \param[in] group_type The type of the callback group. - * \param[in] get_node_context Lambda to retrieve the node context when - * checking that the creating node is valid and using the guard condition. + * \param[in] context A weak pointer to the context associated with this callback group. * \param[in] automatically_add_to_executor_with_node A boolean that * determines whether a callback group is automatically added to an executor * with the node with which it is associated. @@ -138,7 +137,7 @@ class CallbackGroup RCLCPP_PUBLIC explicit CallbackGroup( CallbackGroupType group_type, - std::function get_node_context, + rclcpp::Context::WeakPtr context, bool automatically_add_to_executor_with_node = true); /// Default destructor. @@ -228,16 +227,6 @@ class CallbackGroup bool automatically_add_to_executor_with_node() const; - /// Retrieve the guard condition used to signal changes to this callback group. - /** - * \param[in] context_ptr context to use when creating the guard condition - * \return guard condition if it is valid, otherwise nullptr. - */ - [[deprecated("Use get_notify_guard_condition() without arguments")]] - RCLCPP_PUBLIC - rclcpp::GuardCondition::SharedPtr - get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr); - /// Retrieve the guard condition used to signal changes to this callback group. /** * \return guard condition if it is valid, otherwise nullptr. @@ -297,7 +286,7 @@ class CallbackGroup std::shared_ptr notify_guard_condition_ = nullptr; std::recursive_mutex notify_guard_condition_mutex_; - std::function get_context_; + rclcpp::Context::WeakPtr context_; private: template diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp index 350f306010..0a57bace22 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -48,7 +48,7 @@ class GuardCondition */ RCLCPP_PUBLIC explicit GuardCondition( - rclcpp::Context::SharedPtr context = + const rclcpp::Context::SharedPtr & context = rclcpp::contexts::get_global_default_context(), rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options()); @@ -57,11 +57,6 @@ class GuardCondition virtual ~GuardCondition(); - /// Return the context used when creating this guard condition. - RCLCPP_PUBLIC - rclcpp::Context::SharedPtr - get_context() const; - /// Return the underlying rcl guard condition structure. RCLCPP_PUBLIC rcl_guard_condition_t & @@ -128,7 +123,6 @@ class GuardCondition 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_; diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index 77f6c87bd9..e2bc473d54 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -31,12 +31,12 @@ using rclcpp::CallbackGroupType; CallbackGroup::CallbackGroup( CallbackGroupType group_type, - std::function get_context, + rclcpp::Context::WeakPtr context, bool automatically_add_to_executor_with_node) : type_(group_type), associated_with_executor_(false), can_be_taken_from_(true), automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node), - get_context_(get_context) + context_(context) {} CallbackGroup::~CallbackGroup() @@ -123,40 +123,12 @@ CallbackGroup::automatically_add_to_executor_with_node() const return automatically_add_to_executor_with_node_; } -// \TODO(mjcarroll) Deprecated, remove on tock -rclcpp::GuardCondition::SharedPtr -CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr) -{ - std::lock_guard lock(notify_guard_condition_mutex_); - if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) { - if (associated_with_executor_) { - trigger_notify_guard_condition(); - } - notify_guard_condition_ = nullptr; - } - - if (!notify_guard_condition_) { - notify_guard_condition_ = std::make_shared(context_ptr); - } - - return notify_guard_condition_; -} - rclcpp::GuardCondition::SharedPtr CallbackGroup::get_notify_guard_condition() { std::lock_guard lock(notify_guard_condition_mutex_); - if (!this->get_context_) { - throw std::runtime_error("Callback group was created without context and not passed context"); - } - auto context_ptr = this->get_context_(); + rclcpp::Context::SharedPtr context_ptr = context_.lock(); if (context_ptr && context_ptr->is_valid()) { - if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) { - if (associated_with_executor_) { - trigger_notify_guard_condition(); - } - notify_guard_condition_ = nullptr; - } if (!notify_guard_condition_) { notify_guard_condition_ = std::make_shared(context_ptr); } diff --git a/rclcpp/src/rclcpp/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp index 627644e602..4f9a85d959 100644 --- a/rclcpp/src/rclcpp/guard_condition.cpp +++ b/rclcpp/src/rclcpp/guard_condition.cpp @@ -23,16 +23,17 @@ namespace rclcpp { GuardCondition::GuardCondition( - rclcpp::Context::SharedPtr context, + const rclcpp::Context::SharedPtr & context, rcl_guard_condition_options_t guard_condition_options) -: context_(context), rcl_guard_condition_{rcl_get_zero_initialized_guard_condition()} +: rcl_guard_condition_{rcl_get_zero_initialized_guard_condition()} { - if (!context_) { + if (!context) { throw std::invalid_argument("context argument unexpectedly nullptr"); } + rcl_ret_t ret = rcl_guard_condition_init( &this->rcl_guard_condition_, - context_->get_rcl_context().get(), + context->get_rcl_context().get(), guard_condition_options); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to create guard condition"); @@ -53,12 +54,6 @@ GuardCondition::~GuardCondition() } } -rclcpp::Context::SharedPtr -GuardCondition::get_context() const -{ - return context_; -} - rcl_guard_condition_t & GuardCondition::get_rcl_guard_condition() { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 7b13880157..5648290654 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -218,14 +218,9 @@ NodeBase::create_callback_group( rclcpp::CallbackGroupType group_type, bool automatically_add_to_executor_with_node) { - auto weak_context = this->get_context()->weak_from_this(); - auto get_node_context = [weak_context]() -> rclcpp::Context::SharedPtr { - return weak_context.lock(); - }; - auto group = std::make_shared( group_type, - get_node_context, + context_->weak_from_this(), automatically_add_to_executor_with_node); std::lock_guard lock(callback_groups_mutex_); callback_groups_.push_back(group); diff --git a/rclcpp/test/rclcpp/test_context.cpp b/rclcpp/test/rclcpp/test_context.cpp index 9d736f9db2..c8779371fe 100644 --- a/rclcpp/test/rclcpp/test_context.cpp +++ b/rclcpp/test/rclcpp/test_context.cpp @@ -214,3 +214,25 @@ TEST(TestContext, check_on_shutdown_callback_order_after_del) { EXPECT_TRUE(result[0] == 1 && result[1] == 3 && result[2] == 4 && result[3] == 0); } + +// This test checks that contexts will be properly destroyed when leaving a scope, after a +// guard condition has been created. +TEST(TestContext, check_context_destroyed) { + rclcpp::Context::SharedPtr ctx; + { + ctx = std::make_shared(); + ctx->init(0, nullptr); + + auto group = std::make_shared( + rclcpp::CallbackGroupType::MutuallyExclusive, + ctx->weak_from_this(), + false); + + rclcpp::GuardCondition::SharedPtr gc = group->get_notify_guard_condition(); + ASSERT_NE(gc, nullptr); + + ASSERT_EQ(ctx.use_count(), 1u); + } + + ASSERT_EQ(ctx.use_count(), 1u); +} diff --git a/rclcpp/test/rclcpp/test_guard_condition.cpp b/rclcpp/test/rclcpp/test_guard_condition.cpp index 1e72264869..90ac9ae2d9 100644 --- a/rclcpp/test/rclcpp/test_guard_condition.cpp +++ b/rclcpp/test/rclcpp/test_guard_condition.cpp @@ -66,16 +66,6 @@ TEST_F(TestGuardCondition, construction_and_destruction) { } } -/* - * Testing context accessor. - */ -TEST_F(TestGuardCondition, get_context) { - { - auto gc = std::make_shared(); - gc->get_context(); - } -} - /* * Testing rcl guard condition accessor. */ From 7901bb9da4e1addf71a307230afa60bd8d72f4f6 Mon Sep 17 00:00:00 2001 From: Jeffery Hsu Date: Thu, 18 Jan 2024 10:39:59 -0600 Subject: [PATCH 323/455] Add transient local durability support to publisher and subscriptions when using intra-process communication (#2303) * Add intra process transient local durability support to publisher and subscription Signed-off-by: Jeffery Hsu * Remove durability_is_transient_local_ from publisher_base Signed-off-by: Jeffery Hsu * Design changes that move most transient local publish functionalities out of intra process manager into intra process manager Signed-off-by: Jeffery Hsu * Move transient local publish to a separate function Signed-off-by: Jeffery Hsu * Remove publisher buffer weak ptr from intra process manager when it associated publisher is removed. Signed-off-by: Jeffery Hsu * Remove incorrectly placed RCLCPP_PUBLIC Signed-off-by: Jeffery Hsu * Add missing RCLCPP_PUBLIC Signed-off-by: Jeffery Hsu * Expand RingBufferImplementation beyond shared_ptr and unique_ptr Signed-off-by: Jeffery Hsu * Comment and format fix Signed-off-by: Jeffery Hsu --------- Signed-off-by: Jeffery Hsu --- rclcpp/CMakeLists.txt | 1 + .../resolve_intra_process_buffer_type.hpp | 5 + .../buffers/buffer_implementation_base.hpp | 4 + .../buffers/intra_process_buffer.hpp | 79 +++ .../buffers/ring_buffer_implementation.hpp | 77 +++ .../experimental/intra_process_manager.hpp | 125 ++++- .../subscription_intra_process_base.hpp | 4 + rclcpp/include/rclcpp/publisher.hpp | 47 +- rclcpp/include/rclcpp/publisher_base.hpp | 6 + rclcpp/include/rclcpp/publisher_options.hpp | 4 + rclcpp/include/rclcpp/subscription.hpp | 7 +- .../resolve_intra_process_buffer_type.cpp | 37 ++ rclcpp/src/rclcpp/intra_process_manager.cpp | 38 +- rclcpp/src/rclcpp/publisher_base.cpp | 7 + .../subscription_intra_process_base.cpp | 6 + .../test/rclcpp/test_intra_process_buffer.cpp | 54 +++ .../rclcpp/test_intra_process_manager.cpp | 456 ++++++++++++------ rclcpp/test/rclcpp/test_publisher.cpp | 99 +++- .../test_ring_buffer_implementation.cpp | 70 ++- rclcpp/test/rclcpp/test_subscription.cpp | 6 +- 20 files changed, 921 insertions(+), 211 deletions(-) create mode 100644 rclcpp/src/rclcpp/detail/resolve_intra_process_buffer_type.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index ffae9cc7ee..bf3651df81 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -46,6 +46,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/context.cpp src/rclcpp/contexts/default_context.cpp src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp + src/rclcpp/detail/resolve_intra_process_buffer_type.cpp src/rclcpp/detail/resolve_parameter_overrides.cpp src/rclcpp/detail/rmw_implementation_specific_payload.cpp src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp diff --git a/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp b/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp index e7196a1e11..234316a8f0 100644 --- a/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp +++ b/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp @@ -47,6 +47,11 @@ resolve_intra_process_buffer_type( return resolved_buffer_type; } +RCLCPP_PUBLIC +rclcpp::IntraProcessBufferType +resolve_intra_process_buffer_type( + const rclcpp::IntraProcessBufferType buffer_type); + } // namespace detail } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp b/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp index 83a6c7462c..1d50b1659e 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp @@ -15,6 +15,8 @@ #ifndef RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ #define RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ +#include + namespace rclcpp { namespace experimental @@ -31,6 +33,8 @@ class BufferImplementationBase virtual BufferT dequeue() = 0; virtual void enqueue(BufferT request) = 0; + virtual std::vector get_all_data() = 0; + virtual void clear() = 0; virtual bool has_data() const = 0; virtual size_t available_capacity() const = 0; diff --git a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp index 114e9196d2..268c3f6649 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" @@ -66,6 +67,9 @@ class IntraProcessBuffer : public IntraProcessBufferBase virtual MessageSharedPtr consume_shared() = 0; virtual MessageUniquePtr consume_unique() = 0; + + virtual std::vector get_all_data_shared() = 0; + virtual std::vector get_all_data_unique() = 0; }; template< @@ -129,6 +133,16 @@ class TypedIntraProcessBuffer : public IntraProcessBuffer(); } + std::vector get_all_data_shared() override + { + return get_all_data_shared_impl(); + } + + std::vector get_all_data_unique() override + { + return get_all_data_unique_impl(); + } + bool has_data() const override { return buffer_->has_data(); @@ -243,6 +257,71 @@ class TypedIntraProcessBuffer : public IntraProcessBufferdequeue(); } + + // MessageSharedPtr to MessageSharedPtr + template + typename std::enable_if< + std::is_same::value, + std::vector + >::type + get_all_data_shared_impl() + { + return buffer_->get_all_data(); + } + + // MessageUniquePtr to MessageSharedPtr + template + typename std::enable_if< + std::is_same::value, + std::vector + >::type + get_all_data_shared_impl() + { + std::vector result; + auto uni_ptr_vec = buffer_->get_all_data(); + result.reserve(uni_ptr_vec.size()); + for (MessageUniquePtr & uni_ptr : uni_ptr_vec) { + result.emplace_back(std::move(uni_ptr)); + } + return result; + } + + // MessageSharedPtr to MessageUniquePtr + template + typename std::enable_if< + std::is_same::value, + std::vector + >::type + get_all_data_unique_impl() + { + std::vector result; + auto shared_ptr_vec = buffer_->get_all_data(); + result.reserve(shared_ptr_vec.size()); + for (MessageSharedPtr shared_msg : shared_ptr_vec) { + MessageUniquePtr unique_msg; + MessageDeleter * deleter = std::get_deleter(shared_msg); + auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); + MessageAllocTraits::construct(*message_allocator_.get(), ptr, *shared_msg); + if (deleter) { + unique_msg = MessageUniquePtr(ptr, *deleter); + } else { + unique_msg = MessageUniquePtr(ptr); + } + result.push_back(std::move(unique_msg)); + } + return result; + } + + // MessageUniquePtr to MessageUniquePtr + template + typename std::enable_if< + std::is_same::value, + std::vector + >::type + get_all_data_unique_impl() + { + return buffer_->get_all_data(); + } }; } // namespace buffers diff --git a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp index e67aae973a..f476fa556a 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ #define RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ +#include #include #include #include @@ -113,6 +114,17 @@ class RingBufferImplementation : public BufferImplementationBase return request; } + /// Get all the elements from the ring buffer + /** + * This member function is thread-safe. + * + * \return a vector containing all the elements from the ring buffer + */ + std::vector get_all_data() override + { + return get_all_data_impl(); + } + /// Get the next index value for the ring buffer /** * This member function is thread-safe. @@ -215,6 +227,71 @@ class RingBufferImplementation : public BufferImplementationBase return capacity_ - size_; } + /// Traits for checking if a type is std::unique_ptr + template + struct is_std_unique_ptr final : std::false_type {}; + template + struct is_std_unique_ptr> final : std::true_type + { + typedef T Ptr_type; + }; + + /// Get all the elements from the ring buffer + /** + * This member function is thread-safe. + * Two versions for the implementation of the function. + * One for buffer containing unique_ptr and the other for other types + * + * \return a vector containing all the elements from the ring buffer + */ + template::value && + std::is_copy_constructible< + typename is_std_unique_ptr::Ptr_type + >::value, + void> * = nullptr> + std::vector get_all_data_impl() + { + std::lock_guard lock(mutex_); + std::vector result_vtr; + result_vtr.reserve(size_); + for (size_t id = 0; id < size_; ++id) { + result_vtr.emplace_back( + new typename is_std_unique_ptr::Ptr_type( + *(ring_buffer_[(read_index_ + id) % capacity_]))); + } + return result_vtr; + } + + template::value, void> * = nullptr> + std::vector get_all_data_impl() + { + std::lock_guard lock(mutex_); + std::vector result_vtr; + result_vtr.reserve(size_); + for (size_t id = 0; id < size_; ++id) { + result_vtr.emplace_back(ring_buffer_[(read_index_ + id) % capacity_]); + } + return result_vtr; + } + + template::value && + !std::is_copy_constructible::value, void> * = nullptr> + std::vector get_all_data_impl() + { + throw std::logic_error("Underlined type results in invalid get_all_data_impl()"); + return {}; + } + + template::value && + !std::is_copy_constructible::Ptr_type>::value, + void> * = nullptr> + std::vector get_all_data_impl() + { + throw std::logic_error("Underlined type in unique_ptr results in invalid get_all_data_impl()"); + return {}; + } + size_t capacity_; std::vector ring_buffer_; diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 54ff0c0dee..ffd77cb99c 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -28,6 +28,7 @@ #include #include "rclcpp/allocator/allocator_deleter.hpp" +#include "rclcpp/experimental/buffers/intra_process_buffer.hpp" #include "rclcpp/experimental/ros_message_intra_process_buffer.hpp" #include "rclcpp/experimental/subscription_intra_process.hpp" #include "rclcpp/experimental/subscription_intra_process_base.hpp" @@ -112,9 +113,40 @@ class IntraProcessManager * \param subscription the SubscriptionIntraProcess to register. * \return an unsigned 64-bit integer which is the subscription's unique id. */ - RCLCPP_PUBLIC + template< + typename ROSMessageType, + typename Alloc = std::allocator + > uint64_t - add_subscription(rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription); + add_subscription(rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription) + { + std::unique_lock lock(mutex_); + + uint64_t sub_id = IntraProcessManager::get_next_unique_id(); + + subscriptions_[sub_id] = subscription; + + // adds the subscription id to all the matchable publishers + for (auto & pair : publishers_) { + auto publisher = pair.second.lock(); + if (!publisher) { + continue; + } + if (can_communicate(publisher, subscription)) { + uint64_t pub_id = pair.first; + insert_sub_id_for_pub(sub_id, pub_id, subscription->use_take_shared_method()); + if (publisher->is_durability_transient_local() && + subscription->is_durability_transient_local()) + { + do_transient_local_publish( + pub_id, sub_id, + subscription->use_take_shared_method()); + } + } + } + + return sub_id; + } /// Unregister a subscription using the subscription's unique id. /** @@ -131,14 +163,21 @@ class IntraProcessManager * This method stores the publisher intra process object, together with * the information of its wrapped publisher (i.e. topic name and QoS). * + * If the publisher's durability is transient local, its buffer pointer should + * be passed and the method will store it as well. + * * In addition this generates a unique intra process id for the publisher. * * \param publisher publisher to be registered with the manager. + * \param buffer publisher's buffer to be stored if its duability is transient local. * \return an unsigned 64-bit integer which is the publisher's unique id. */ RCLCPP_PUBLIC uint64_t - add_publisher(rclcpp::PublisherBase::SharedPtr publisher); + add_publisher( + rclcpp::PublisherBase::SharedPtr publisher, + rclcpp::experimental::buffers::IntraProcessBufferBase::SharedPtr buffer = + rclcpp::experimental::buffers::IntraProcessBufferBase::SharedPtr()); /// Unregister a publisher using the publisher's unique id. /** @@ -292,6 +331,34 @@ class IntraProcessManager } } + template< + typename MessageT, + typename Alloc, + typename Deleter, + typename ROSMessageType> + void + add_shared_msg_to_buffer( + std::shared_ptr message, + uint64_t subscription_id) + { + add_shared_msg_to_buffers(message, {subscription_id}); + } + + template< + typename MessageT, + typename Alloc, + typename Deleter, + typename ROSMessageType> + void + add_owned_msg_to_buffer( + std::unique_ptr message, + uint64_t subscription_id, + typename allocator::AllocRebind::allocator_type & allocator) + { + add_owned_msg_to_buffers( + std::move(message), {subscription_id}, allocator); + } + /// Return true if the given rmw_gid_t matches any stored Publishers. RCLCPP_PUBLIC bool @@ -324,6 +391,9 @@ class IntraProcessManager using PublisherMap = std::unordered_map; + using PublisherBufferMap = + std::unordered_map; + using PublisherToSubscriptionIdsMap = std::unordered_map; @@ -342,6 +412,54 @@ class IntraProcessManager rclcpp::PublisherBase::SharedPtr pub, rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr sub) const; + template< + typename ROSMessageType, + typename Alloc = std::allocator + > + void do_transient_local_publish( + const uint64_t pub_id, const uint64_t sub_id, + const bool use_take_shared_method) + { + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + + auto publisher_buffer = publisher_buffers_[pub_id].lock(); + if (!publisher_buffer) { + throw std::runtime_error("publisher buffer has unexpectedly gone out of scope"); + } + auto buffer = std::dynamic_pointer_cast< + rclcpp::experimental::buffers::IntraProcessBuffer< + ROSMessageType, + ROSMessageTypeAllocator, + ROSMessageTypeDeleter + > + >(publisher_buffer); + if (!buffer) { + throw std::runtime_error( + "failed to dynamic cast publisher's IntraProcessBufferBase to " + "IntraProcessBuffer which can happen when the publisher and " + "subscription use different allocator types, which is not supported"); + } + if (use_take_shared_method) { + auto data_vec = buffer->get_all_data_shared(); + for (auto shared_data : data_vec) { + this->template add_shared_msg_to_buffer< + ROSMessageType, ROSMessageTypeAllocator, ROSMessageTypeDeleter, ROSMessageType>( + shared_data, sub_id); + } + } else { + auto data_vec = buffer->get_all_data_unique(); + for (auto & owned_data : data_vec) { + auto allocator = ROSMessageTypeAllocator(); + this->template add_owned_msg_to_buffer< + ROSMessageType, ROSMessageTypeAllocator, ROSMessageTypeDeleter, ROSMessageType>( + std::move(owned_data), sub_id, allocator); + } + } + } + template< typename MessageT, typename Alloc, @@ -520,6 +638,7 @@ class IntraProcessManager PublisherToSubscriptionIdsMap pub_to_subs_; SubscriptionMap subscriptions_; PublisherMap publishers_; + PublisherBufferMap publisher_buffers_; mutable std::shared_timed_mutex mutex_; }; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index c590207b90..6a14aac0c7 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -67,6 +67,10 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable size_t available_capacity() const = 0; + RCLCPP_PUBLIC + bool + is_durability_transient_local() const; + bool is_ready(rcl_wait_set_t * wait_set) override = 0; diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index c03220f38b..08a98bdf1a 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -32,6 +32,9 @@ #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" #include "rclcpp/detail/resolve_use_intra_process.hpp" +#include "rclcpp/detail/resolve_intra_process_buffer_type.hpp" +#include "rclcpp/experimental/buffers/intra_process_buffer.hpp" +#include "rclcpp/experimental/create_intra_process_buffer.hpp" #include "rclcpp/experimental/intra_process_manager.hpp" #include "rclcpp/get_message_type_support_handle.hpp" #include "rclcpp/is_ros_compatible_type.hpp" @@ -109,6 +112,12 @@ class Publisher : public PublisherBase [[deprecated("use std::shared_ptr")]] = std::shared_ptr; + using BufferSharedPtr = typename rclcpp::experimental::buffers::IntraProcessBuffer< + ROSMessageType, + ROSMessageTypeAllocator, + ROSMessageTypeDeleter + >::SharedPtr; + RCLCPP_SMART_PTR_DEFINITIONS(Publisher) /// Default constructor. @@ -171,11 +180,14 @@ class Publisher : public PublisherBase throw std::invalid_argument( "intraprocess communication is not allowed with a zero qos history depth value"); } - if (qos.durability() != rclcpp::DurabilityPolicy::Volatile) { - throw std::invalid_argument( - "intraprocess communication allowed only with volatile durability"); + if (qos.durability() == rclcpp::DurabilityPolicy::TransientLocal) { + buffer_ = rclcpp::experimental::create_intra_process_buffer< + ROSMessageType, ROSMessageTypeAllocator, ROSMessageTypeDeleter>( + rclcpp::detail::resolve_intra_process_buffer_type(options_.intra_process_buffer_type), + qos, + std::make_shared(ros_message_type_allocator_)); } - uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this()); + uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this(), buffer_); this->setup_intra_process( intra_process_publisher_id, ipm); @@ -242,9 +254,18 @@ class Publisher : public PublisherBase if (inter_process_publish_needed) { auto shared_msg = this->do_intra_process_ros_message_publish_and_return_shared(std::move(msg)); + if (buffer_) { + buffer_->add_shared(shared_msg); + } this->do_inter_process_publish(*shared_msg); } else { - this->do_intra_process_ros_message_publish(std::move(msg)); + if (buffer_) { + auto shared_msg = + this->do_intra_process_ros_message_publish_and_return_shared(std::move(msg)); + buffer_->add_shared(shared_msg); + } else { + this->do_intra_process_ros_message_publish(std::move(msg)); + } } } @@ -309,14 +330,22 @@ class Publisher : public PublisherBase get_subscription_count() > get_intra_process_subscription_count(); if (inter_process_publish_needed) { - ROSMessageType ros_msg; + auto ros_msg_ptr = std::make_shared(); // TODO(clalancette): This is unnecessarily doing an additional conversion // that may have already been done in do_intra_process_publish_and_return_shared(). // We should just reuse that effort. - rclcpp::TypeAdapter::convert_to_ros_message(*msg, ros_msg); + rclcpp::TypeAdapter::convert_to_ros_message(*msg, *ros_msg_ptr); this->do_intra_process_publish(std::move(msg)); - this->do_inter_process_publish(ros_msg); + this->do_inter_process_publish(*ros_msg_ptr); + if (buffer_) { + buffer_->add_shared(ros_msg_ptr); + } } else { + if (buffer_) { + auto ros_msg_ptr = std::make_shared(); + rclcpp::TypeAdapter::convert_to_ros_message(*msg, *ros_msg_ptr); + buffer_->add_shared(ros_msg_ptr); + } this->do_intra_process_publish(std::move(msg)); } } @@ -581,6 +610,8 @@ class Publisher : public PublisherBase PublishedTypeDeleter published_type_deleter_; ROSMessageTypeAllocator ros_message_type_allocator_; ROSMessageTypeDeleter ros_message_type_deleter_; + + BufferSharedPtr buffer_{nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index 3253595361..9a6c398eeb 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -139,6 +139,12 @@ class PublisherBase : public std::enable_shared_from_this size_t get_intra_process_subscription_count() const; + /// Get if durability is transient local + /** \return If durability is transient local*/ + RCLCPP_PUBLIC + bool + is_durability_transient_local() const; + /// Manually assert that this Publisher is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC). /** * If the rmw Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, the creator diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index 3501834dd1..01fd314f49 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -24,6 +24,7 @@ #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp" +#include "rclcpp/intra_process_buffer_type.hpp" #include "rclcpp/intra_process_setting.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/event_handler.hpp" @@ -40,6 +41,9 @@ struct PublisherOptionsBase /// Setting to explicitly set intraprocess communications. IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault; + /// Setting the data-type stored in the intraprocess buffer + IntraProcessBufferType intra_process_buffer_type = IntraProcessBufferType::SharedPtr; + /// Callbacks for various events related to publishers. PublisherEventCallbacks event_callbacks; diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index d6e0f536ed..58f9a8ce6d 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -163,10 +163,6 @@ class Subscription : public SubscriptionBase throw std::invalid_argument( "intraprocess communication is not allowed with 0 depth qos policy"); } - if (qos_profile.durability() != rclcpp::DurabilityPolicy::Volatile) { - throw std::invalid_argument( - "intraprocess communication allowed only with volatile durability"); - } using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< MessageT, @@ -193,7 +189,8 @@ class Subscription : public SubscriptionBase // Add it to the intra process manager. using rclcpp::experimental::IntraProcessManager; auto ipm = context->get_sub_context(); - uint64_t intra_process_subscription_id = ipm->add_subscription(subscription_intra_process_); + uint64_t intra_process_subscription_id = ipm->template add_subscription< + ROSMessageType, ROSMessageTypeAllocator>(subscription_intra_process_); this->setup_intra_process(intra_process_subscription_id, ipm); } diff --git a/rclcpp/src/rclcpp/detail/resolve_intra_process_buffer_type.cpp b/rclcpp/src/rclcpp/detail/resolve_intra_process_buffer_type.cpp new file mode 100644 index 0000000000..1ca9892ac4 --- /dev/null +++ b/rclcpp/src/rclcpp/detail/resolve_intra_process_buffer_type.cpp @@ -0,0 +1,37 @@ +// Copyright 2023 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. + +#include + +namespace rclcpp +{ + +namespace detail +{ +rclcpp::IntraProcessBufferType +resolve_intra_process_buffer_type( + const rclcpp::IntraProcessBufferType buffer_type) +{ + if (buffer_type == IntraProcessBufferType::CallbackDefault) { + throw std::invalid_argument( + "IntraProcessBufferType::CallbackDefault is not allowed " + "when there is no callback function"); + } + + return buffer_type; +} + +} // namespace detail + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/intra_process_manager.cpp b/rclcpp/src/rclcpp/intra_process_manager.cpp index ea068f03ae..831ffdf2ca 100644 --- a/rclcpp/src/rclcpp/intra_process_manager.cpp +++ b/rclcpp/src/rclcpp/intra_process_manager.cpp @@ -32,13 +32,24 @@ IntraProcessManager::~IntraProcessManager() {} uint64_t -IntraProcessManager::add_publisher(rclcpp::PublisherBase::SharedPtr publisher) +IntraProcessManager::add_publisher( + rclcpp::PublisherBase::SharedPtr publisher, + rclcpp::experimental::buffers::IntraProcessBufferBase::SharedPtr buffer) { std::unique_lock lock(mutex_); uint64_t pub_id = IntraProcessManager::get_next_unique_id(); publishers_[pub_id] = publisher; + if (publisher->is_durability_transient_local()) { + if (buffer) { + publisher_buffers_[pub_id] = buffer; + } else { + throw std::runtime_error( + "transient_local publisher needs to pass" + "a valid publisher buffer ptr when calling add_publisher()"); + } + } // Initialize the subscriptions storage for this publisher. pub_to_subs_[pub_id] = SplittedSubscriptions(); @@ -58,30 +69,6 @@ IntraProcessManager::add_publisher(rclcpp::PublisherBase::SharedPtr publisher) return pub_id; } -uint64_t -IntraProcessManager::add_subscription(SubscriptionIntraProcessBase::SharedPtr subscription) -{ - std::unique_lock lock(mutex_); - - uint64_t sub_id = IntraProcessManager::get_next_unique_id(); - - subscriptions_[sub_id] = subscription; - - // adds the subscription id to all the matchable publishers - for (auto & pair : publishers_) { - auto publisher = pair.second.lock(); - if (!publisher) { - continue; - } - if (can_communicate(publisher, subscription)) { - uint64_t pub_id = pair.first; - insert_sub_id_for_pub(sub_id, pub_id, subscription->use_take_shared_method()); - } - } - - return sub_id; -} - void IntraProcessManager::remove_subscription(uint64_t intra_process_subscription_id) { @@ -112,6 +99,7 @@ IntraProcessManager::remove_publisher(uint64_t intra_process_publisher_id) std::unique_lock lock(mutex_); publishers_.erase(intra_process_publisher_id); + publisher_buffers_.erase(intra_process_publisher_id); pub_to_subs_.erase(intra_process_publisher_id); } diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index 0e37dcf3fa..111246ed96 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -270,6 +270,13 @@ PublisherBase::get_intra_process_subscription_count() const return ipm->get_subscription_count(intra_process_publisher_id_); } +bool +PublisherBase::is_durability_transient_local() const +{ + return rcl_publisher_get_actual_qos(publisher_handle_.get())->durability == + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; +} + rclcpp::QoS PublisherBase::get_actual_qos() const { diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index b13123b65b..7ecafda36c 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -34,3 +34,9 @@ SubscriptionIntraProcessBase::get_actual_qos() const { return qos_profile_; } + +bool +SubscriptionIntraProcessBase::is_durability_transient_local() const +{ + return qos_profile_.durability() == rclcpp::DurabilityPolicy::TransientLocal; +} diff --git a/rclcpp/test/rclcpp/test_intra_process_buffer.cpp b/rclcpp/test/rclcpp/test_intra_process_buffer.cpp index eace6445a9..fa427de74b 100644 --- a/rclcpp/test/rclcpp/test_intra_process_buffer.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_buffer.cpp @@ -190,6 +190,33 @@ TEST(TestIntraProcessBuffer, shared_buffer_consume) { EXPECT_EQ(1L, original_shared_msg.use_count()); EXPECT_EQ(*original_shared_msg, *popped_unique_msg); EXPECT_NE(original_message_pointer, popped_message_pointer); + + original_shared_msg = std::make_shared('c'); + original_message_pointer = reinterpret_cast(original_shared_msg.get()); + auto original_shared_msg_2 = std::make_shared('d'); + auto original_message_pointer_2 = reinterpret_cast(original_shared_msg_2.get()); + intra_process_buffer.add_shared(original_shared_msg); + intra_process_buffer.add_shared(original_shared_msg_2); + + auto shared_data_vec = intra_process_buffer.get_all_data_shared(); + EXPECT_EQ(2L, shared_data_vec.size()); + EXPECT_EQ(3L, original_shared_msg.use_count()); + EXPECT_EQ(original_shared_msg.use_count(), shared_data_vec[0].use_count()); + EXPECT_EQ(*original_shared_msg, *shared_data_vec[0]); + EXPECT_EQ(original_message_pointer, reinterpret_cast(shared_data_vec[0].get())); + EXPECT_EQ(3L, original_shared_msg_2.use_count()); + EXPECT_EQ(original_shared_msg_2.use_count(), shared_data_vec[1].use_count()); + EXPECT_EQ(*original_shared_msg_2, *shared_data_vec[1]); + EXPECT_EQ(original_message_pointer_2, reinterpret_cast(shared_data_vec[1].get())); + + auto unique_data_vec = intra_process_buffer.get_all_data_unique(); + EXPECT_EQ(2L, unique_data_vec.size()); + EXPECT_EQ(3L, original_shared_msg.use_count()); + EXPECT_EQ(*original_shared_msg, *unique_data_vec[0]); + EXPECT_NE(original_message_pointer, reinterpret_cast(unique_data_vec[0].get())); + EXPECT_EQ(3L, original_shared_msg_2.use_count()); + EXPECT_EQ(*original_shared_msg_2, *unique_data_vec[1]); + EXPECT_NE(original_message_pointer_2, reinterpret_cast(unique_data_vec[1].get())); } /* @@ -237,6 +264,33 @@ TEST(TestIntraProcessBuffer, unique_buffer_consume) { EXPECT_EQ(original_value, *popped_unique_msg); EXPECT_EQ(original_message_pointer, popped_message_pointer); + + original_unique_msg = std::make_unique('c'); + original_message_pointer = reinterpret_cast(original_unique_msg.get()); + original_value = *original_unique_msg; + auto original_unique_msg_2 = std::make_unique('d'); + auto original_message_pointer_2 = reinterpret_cast(original_unique_msg.get()); + auto original_value_2 = *original_unique_msg_2; + intra_process_buffer.add_unique(std::move(original_unique_msg)); + intra_process_buffer.add_unique(std::move(original_unique_msg_2)); + + auto shared_data_vec = intra_process_buffer.get_all_data_shared(); + EXPECT_EQ(2L, shared_data_vec.size()); + EXPECT_EQ(1L, shared_data_vec[0].use_count()); + EXPECT_EQ(original_value, *shared_data_vec[0]); + EXPECT_NE(original_message_pointer, reinterpret_cast(shared_data_vec[0].get())); + EXPECT_EQ(1L, shared_data_vec[1].use_count()); + EXPECT_EQ(original_value_2, *shared_data_vec[1]); + EXPECT_NE(original_message_pointer_2, reinterpret_cast(shared_data_vec[1].get())); + + auto unique_data_vec = intra_process_buffer.get_all_data_unique(); + EXPECT_EQ(2L, unique_data_vec.size()); + EXPECT_EQ(1L, shared_data_vec[0].use_count()); + EXPECT_EQ(original_value, *unique_data_vec[0]); + EXPECT_NE(original_message_pointer, reinterpret_cast(unique_data_vec[0].get())); + EXPECT_EQ(1L, shared_data_vec[1].use_count()); + EXPECT_EQ(original_value_2, *unique_data_vec[1]); + EXPECT_NE(original_message_pointer_2, reinterpret_cast(unique_data_vec[1].get())); } /* diff --git a/rclcpp/test/rclcpp/test_intra_process_manager.cpp b/rclcpp/test/rclcpp/test_intra_process_manager.cpp index da863f3e3c..046a5228da 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager.cpp @@ -31,6 +31,111 @@ // NOLINTNEXTLINE(build/include_order) #include +namespace rclcpp +{ +namespace experimental +{ +namespace buffers +{ +namespace mock +{ + +class IntraProcessBufferBase +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(IntraProcessBufferBase) + + virtual ~IntraProcessBufferBase() {} +}; + +} // namespace mock +} // namespace buffers +} // namespace experimental +} // namespace rclcpp + +namespace rclcpp +{ +namespace experimental +{ +namespace buffers +{ +namespace mock +{ +template< + typename MessageT, + typename Alloc = std::allocator, + typename MessageDeleter = std::default_delete> +class IntraProcessBuffer : public IntraProcessBufferBase +{ +public: + using ConstMessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessBuffer) + + IntraProcessBuffer() + {} + + void add(ConstMessageSharedPtr msg) + { + message_ptr = reinterpret_cast(msg.get()); + shared_msg = msg; + ++num_msgs; + } + + void add(MessageUniquePtr msg) + { + message_ptr = reinterpret_cast(msg.get()); + unique_msg = std::move(msg); + ++num_msgs; + } + + void pop(std::uintptr_t & msg_ptr) + { + msg_ptr = message_ptr; + message_ptr = 0; + --num_msgs; + } + + size_t size() const + { + return num_msgs; + } + + std::vector get_all_data_shared() + { + if (shared_msg) { + return {shared_msg}; + } else if (unique_msg) { + return {std::make_shared(*unique_msg)}; + } + return {}; + } + + std::vector get_all_data_unique() + { + std::vector result; + if (shared_msg) { + result.push_back(std::make_unique(*shared_msg)); + } else if (unique_msg) { + result.push_back(std::make_unique(*unique_msg)); + } + return result; + } + + // need to store the messages somewhere otherwise the memory address will be reused + ConstMessageSharedPtr shared_msg; + MessageUniquePtr unique_msg; + + std::uintptr_t message_ptr; + // count add and pop + size_t num_msgs = 0u; +}; + +} // namespace mock +} // namespace buffers +} // namespace experimental +} // namespace rclcpp namespace rclcpp { // forward declaration @@ -80,6 +185,12 @@ class PublisherBase return qos_profile; } + bool + is_durability_transient_local() const + { + return qos_profile.durability() == rclcpp::DurabilityPolicy::TransientLocal; + } + bool operator==(const rmw_gid_t & gid) const { @@ -117,6 +228,9 @@ class Publisher : public PublisherBase { auto allocator = std::make_shared(); message_allocator_ = std::make_shared(*allocator.get()); + if (qos.durability() == rclcpp::DurabilityPolicy::TransientLocal) { + buffer = std::make_shared>(); + } } // The following functions use the IntraProcessManager @@ -124,75 +238,12 @@ class Publisher : public PublisherBase void publish(MessageUniquePtr msg); std::shared_ptr message_allocator_; + typename rclcpp::experimental::buffers::mock::IntraProcessBuffer::SharedPtr buffer{nullptr}; }; } // namespace mock } // namespace rclcpp -namespace rclcpp -{ -namespace experimental -{ -namespace buffers -{ -namespace mock -{ -template< - typename MessageT, - typename Alloc = std::allocator, - typename MessageDeleter = std::default_delete> -class IntraProcessBuffer -{ -public: - using ConstMessageSharedPtr = std::shared_ptr; - using MessageUniquePtr = std::unique_ptr; - - RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessBuffer) - - IntraProcessBuffer() - {} - - void add(ConstMessageSharedPtr msg) - { - message_ptr = reinterpret_cast(msg.get()); - shared_msg = msg; - ++num_msgs; - } - - void add(MessageUniquePtr msg) - { - message_ptr = reinterpret_cast(msg.get()); - unique_msg = std::move(msg); - ++num_msgs; - } - - void pop(std::uintptr_t & msg_ptr) - { - msg_ptr = message_ptr; - message_ptr = 0; - --num_msgs; - } - - size_t size() const - { - return num_msgs; - } - - // need to store the messages somewhere otherwise the memory address will be reused - ConstMessageSharedPtr shared_msg; - MessageUniquePtr unique_msg; - - std::uintptr_t message_ptr; - // count add and pop - size_t num_msgs = 0u; -}; - -} // namespace mock -} // namespace buffers -} // namespace experimental -} // namespace rclcpp - - namespace rclcpp { namespace experimental @@ -231,6 +282,12 @@ class SubscriptionIntraProcessBase return topic_name.c_str(); } + bool + is_durability_transient_local() const + { + return qos_profile.durability() == rclcpp::DurabilityPolicy::TransientLocal; + } + virtual size_t available_capacity() const = 0; @@ -331,12 +388,14 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< // Prevent the header files of the mocked classes to be included #define RCLCPP__PUBLISHER_HPP_ #define RCLCPP__PUBLISHER_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_ #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_ #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ // Force ipm to use our mock publisher class. #define Publisher mock::Publisher #define PublisherBase mock::PublisherBase +#define IntraProcessBufferBase mock::IntraProcessBufferBase #define IntraProcessBuffer mock::IntraProcessBuffer #define SubscriptionIntraProcessBase mock::SubscriptionIntraProcessBase #define SubscriptionIntraProcessBuffer mock::SubscriptionIntraProcessBuffer @@ -368,28 +427,36 @@ void Publisher::publish(MessageUniquePtr msg) throw std::runtime_error("cannot publish msg which is a null pointer"); } - ipm->template do_intra_process_publish( - intra_process_publisher_id_, - std::move(msg), - *message_allocator_); + if (buffer) { + auto shared_msg = ipm->template do_intra_process_publish_and_return_shared( + intra_process_publisher_id_, + std::move(msg), + *message_allocator_); + buffer->add(shared_msg); + } else { + ipm->template do_intra_process_publish( + intra_process_publisher_id_, + std::move(msg), + *message_allocator_); + } } } // namespace mock } // namespace rclcpp /* - This tests how the class connects and disconnects publishers and subscriptions: - - Creates 2 publishers on different topics and a subscription to one of them. - Add everything to the intra-process manager. - - All the entities are expected to have different ids. - - Check the subscriptions count for each publisher. - - One of the publishers is expected to have 1 subscription, while the other 0. - - Check the subscription count for a non existing publisher id, should return 0. - - Add a new publisher and a new subscription both with reliable QoS. - - The subscriptions count of the previous publisher is expected to remain unchanged, - while the new publisher is expected to have 2 subscriptions (it's compatible with both QoS). - - Remove the just added subscriptions. - - The count for the last publisher is expected to decrease to 1. + * This tests how the class connects and disconnects publishers and subscriptions: + * - Creates 2 publishers on different topics and a subscription to one of them. + * Add everything to the intra-process manager. + * - All the entities are expected to have different ids. + * - Check the subscriptions count for each publisher. + * - One of the publishers is expected to have 1 subscription, while the other 0. + * - Check the subscription count for a non existing publisher id, should return 0. + * - Add a new publisher and a new subscription both with reliable QoS. + * - The subscriptions count of the previous publisher is expected to remain unchanged, + * while the new publisher is expected to have 2 subscriptions (it's compatible with both QoS). + * - Remove the just added subscriptions. + * - The count for the last publisher is expected to decrease to 1. */ TEST(TestIntraProcessManager, add_pub_sub) { using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; @@ -408,7 +475,7 @@ TEST(TestIntraProcessManager, add_pub_sub) { auto p1_id = ipm->add_publisher(p1); auto p2_id = ipm->add_publisher(p2); - auto s1_id = ipm->add_subscription(s1); + auto s1_id = ipm->template add_subscription(s1); bool unique_ids = p1_id != p2_id && p2_id != s1_id; ASSERT_TRUE(unique_ids); @@ -424,7 +491,7 @@ TEST(TestIntraProcessManager, add_pub_sub) { auto s2 = std::make_shared(rclcpp::QoS(10).reliable()); - auto s2_id = ipm->add_subscription(s2); + auto s2_id = ipm->template add_subscription(s2); auto p3_id = ipm->add_publisher(p3); p1_subs = ipm->get_subscription_count(p1_id); @@ -444,14 +511,14 @@ TEST(TestIntraProcessManager, add_pub_sub) { } /* - This tests the minimal usage of the class where there is a single subscription per publisher: - - Publishes a unique_ptr message with a subscription requesting ownership. - - The received message is expected to be the same. - - Remove the first subscription from ipm and add a new one. - - Publishes a unique_ptr message with a subscription not requesting ownership. - - The received message is expected to be the same, the first subscription do not receive it. - - Publishes a shared_ptr message with a subscription not requesting ownership. - - The received message is expected to be the same. + * This tests the minimal usage of the class where there is a single subscription per publisher: + * - Publishes a unique_ptr message with a subscription requesting ownership. + * - The received message is expected to be the same. + * - Remove the first subscription from ipm and add a new one. + * - Publishes a unique_ptr message with a subscription not requesting ownership. + * - The received message is expected to be the same, the first subscription do not receive it. + * - Publishes a shared_ptr message with a subscription not requesting ownership. + * - The received message is expected to be the same. */ TEST(TestIntraProcessManager, single_subscription) { using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; @@ -467,7 +534,7 @@ TEST(TestIntraProcessManager, single_subscription) { auto s1 = std::make_shared(); s1->take_shared_method = false; - auto s1_id = ipm->add_subscription(s1); + auto s1_id = ipm->template add_subscription(s1); auto unique_msg = std::make_unique(); auto original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -478,7 +545,7 @@ TEST(TestIntraProcessManager, single_subscription) { ipm->remove_subscription(s1_id); auto s2 = std::make_shared(); s2->take_shared_method = true; - auto s2_id = ipm->add_subscription(s2); + auto s2_id = ipm->template add_subscription(s2); (void)s2_id; unique_msg = std::make_unique(); @@ -497,15 +564,15 @@ TEST(TestIntraProcessManager, single_subscription) { } /* - This tests the usage of the class where there are multiple subscriptions of the same type: - - Publishes a unique_ptr message with 2 subscriptions requesting ownership. - - One is expected to receive the published message, while the other will receive a copy. - - Publishes a unique_ptr message with 2 subscriptions not requesting ownership. - - Both received messages are expected to be the same as the published one. - - Publishes a shared_ptr message with 2 subscriptions requesting ownership. - - Both received messages are expected to be a copy of the published one. - - Publishes a shared_ptr message with 2 subscriptions not requesting ownership. - - Both received messages are expected to be the same as the published one. + * This tests the usage of the class where there are multiple subscriptions of the same type: + * - Publishes a unique_ptr message with 2 subscriptions requesting ownership. + * - One is expected to receive the published message, while the other will receive a copy. + * - Publishes a unique_ptr message with 2 subscriptions not requesting ownership. + * - Both received messages are expected to be the same as the published one. + * - Publishes a shared_ptr message with 2 subscriptions requesting ownership. + * - Both received messages are expected to be a copy of the published one. + * - Publishes a shared_ptr message with 2 subscriptions not requesting ownership. + * - Both received messages are expected to be the same as the published one. */ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; @@ -521,11 +588,11 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { auto s1 = std::make_shared(); s1->take_shared_method = false; - auto s1_id = ipm->add_subscription(s1); + auto s1_id = ipm->template add_subscription(s1); auto s2 = std::make_shared(); s2->take_shared_method = false; - auto s2_id = ipm->add_subscription(s2); + auto s2_id = ipm->template add_subscription(s2); auto unique_msg = std::make_unique(); auto original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -541,11 +608,11 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { auto s3 = std::make_shared(); s3->take_shared_method = true; - auto s3_id = ipm->add_subscription(s3); + auto s3_id = ipm->template add_subscription(s3); auto s4 = std::make_shared(); s4->take_shared_method = true; - auto s4_id = ipm->add_subscription(s4); + auto s4_id = ipm->template add_subscription(s4); unique_msg = std::make_unique(); original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -560,11 +627,11 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { auto s5 = std::make_shared(); s5->take_shared_method = false; - auto s5_id = ipm->add_subscription(s5); + auto s5_id = ipm->template add_subscription(s5); auto s6 = std::make_shared(); s6->take_shared_method = false; - auto s6_id = ipm->add_subscription(s6); + auto s6_id = ipm->template add_subscription(s6); unique_msg = std::make_unique(); original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -580,12 +647,12 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { auto s7 = std::make_shared(); s7->take_shared_method = true; - auto s7_id = ipm->add_subscription(s7); + auto s7_id = ipm->template add_subscription(s7); (void)s7_id; auto s8 = std::make_shared(); s8->take_shared_method = true; - auto s8_id = ipm->add_subscription(s8); + auto s8_id = ipm->template add_subscription(s8); (void)s8_id; unique_msg = std::make_unique(); @@ -598,20 +665,20 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { } /* - This tests the usage of the class where there are multiple subscriptions of different types: - - Publishes a unique_ptr message with 1 subscription requesting ownership and 1 not. - - The one requesting ownership is expected to receive the published message, - while the other is expected to receive a copy. - - Publishes a unique_ptr message with 2 subscriptions requesting ownership and 1 not. - - One of the subscriptions requesting ownership is expected to receive the published message, - while both other subscriptions are expected to receive different copies. - - Publishes a unique_ptr message with 2 subscriptions requesting ownership and 2 not. - - The 2 subscriptions not requesting ownership are expected to both receive the same copy - of the message, one of the subscription requesting ownership is expected to receive a - different copy, while the last is expected to receive the published message. - - Publishes a shared_ptr message with 1 subscription requesting ownership and 1 not. - - The subscription requesting ownership is expected to receive a copy of the message, while - the other is expected to receive the published message + * This tests the usage of the class where there are multiple subscriptions of different types: + * - Publishes a unique_ptr message with 1 subscription requesting ownership and 1 not. + * - The one requesting ownership is expected to receive the published message, + * while the other is expected to receive a copy. + * - Publishes a unique_ptr message with 2 subscriptions requesting ownership and 1 not. + * - One of the subscriptions requesting ownership is expected to receive the published message, + * while both other subscriptions are expected to receive different copies. + * - Publishes a unique_ptr message with 2 subscriptions requesting ownership and 2 not. + * - The 2 subscriptions not requesting ownership are expected to both receive the same copy + * of the message, one of the subscription requesting ownership is expected to receive a + * different copy, while the last is expected to receive the published message. + * - Publishes a shared_ptr message with 1 subscription requesting ownership and 1 not. + * - The subscription requesting ownership is expected to receive a copy of the message, while + * the other is expected to receive the published message */ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; @@ -627,11 +694,11 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { auto s1 = std::make_shared(); s1->take_shared_method = true; - auto s1_id = ipm->add_subscription(s1); + auto s1_id = ipm->template add_subscription(s1); auto s2 = std::make_shared(); s2->take_shared_method = false; - auto s2_id = ipm->add_subscription(s2); + auto s2_id = ipm->template add_subscription(s2); auto unique_msg = std::make_unique(); auto original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -646,15 +713,15 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { auto s3 = std::make_shared(); s3->take_shared_method = false; - auto s3_id = ipm->add_subscription(s3); + auto s3_id = ipm->template add_subscription(s3); auto s4 = std::make_shared(); s4->take_shared_method = false; - auto s4_id = ipm->add_subscription(s4); + auto s4_id = ipm->template add_subscription(s4); auto s5 = std::make_shared(); s5->take_shared_method = true; - auto s5_id = ipm->add_subscription(s5); + auto s5_id = ipm->template add_subscription(s5); unique_msg = std::make_unique(); original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -678,19 +745,19 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { auto s6 = std::make_shared(); s6->take_shared_method = true; - auto s6_id = ipm->add_subscription(s6); + auto s6_id = ipm->template add_subscription(s6); auto s7 = std::make_shared(); s7->take_shared_method = true; - auto s7_id = ipm->add_subscription(s7); + auto s7_id = ipm->template add_subscription(s7); auto s8 = std::make_shared(); s8->take_shared_method = false; - auto s8_id = ipm->add_subscription(s8); + auto s8_id = ipm->template add_subscription(s8); auto s9 = std::make_shared(); s9->take_shared_method = false; - auto s9_id = ipm->add_subscription(s9); + auto s9_id = ipm->template add_subscription(s9); unique_msg = std::make_unique(); original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -716,12 +783,12 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { auto s10 = std::make_shared(); s10->take_shared_method = false; - auto s10_id = ipm->add_subscription(s10); + auto s10_id = ipm->template add_subscription(s10); (void)s10_id; auto s11 = std::make_shared(); s11->take_shared_method = true; - auto s11_id = ipm->add_subscription(s11); + auto s11_id = ipm->template add_subscription(s11); (void)s11_id; unique_msg = std::make_unique(); @@ -734,25 +801,25 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { } /* - This tests the method "lowest_available_capacity": - - Creates 1 publisher. - - The available buffer capacity should be at least history size. - - Add 2 subscribers. - - Add everything to the intra-process manager. - - All the entities are expected to have different ids. - - Check the subscriptions count for the publisher. - - The available buffer capacity should be the history size. - - Publish one message (without receiving it). - - The available buffer capacity should decrease by 1. - - Publish another message (without receiving it). - - The available buffer capacity should decrease by 1. - - One subscriber receives one message. - - The available buffer capacity should stay the same, - as the other subscriber still has not freed its buffer. - - The other subscriber receives one message. - - The available buffer capacity should increase by 1. - - One subscription goes out of scope. - - The available buffer capacity should not change. + * This tests the method "lowest_available_capacity": + * - Creates 1 publisher. + * - The available buffer capacity should be at least history size. + * - Add 2 subscribers. + * - Add everything to the intra-process manager. + * - All the entities are expected to have different ids. + * - Check the subscriptions count for the publisher. + * - The available buffer capacity should be the history size. + * - Publish one message (without receiving it). + * - The available buffer capacity should decrease by 1. + * - Publish another message (without receiving it). + * - The available buffer capacity should decrease by 1. + * - One subscriber receives one message. + * - The available buffer capacity should stay the same, + * as the other subscriber still has not freed its buffer. + * - The other subscriber receives one message. + * - The available buffer capacity should increase by 1. + * - One subscription goes out of scope. + * - The available buffer capacity should not change. */ TEST(TestIntraProcessManager, lowest_available_capacity) { using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; @@ -776,8 +843,8 @@ TEST(TestIntraProcessManager, lowest_available_capacity) { ASSERT_LE(0u, c1); - auto s1_id = ipm->add_subscription(s1); - auto s2_id = ipm->add_subscription(s2); + auto s1_id = ipm->template add_subscription(s1); + auto s2_id = ipm->template add_subscription(s2); bool unique_ids = s1_id != s2_id && p1_id != s1_id; ASSERT_TRUE(unique_ids); @@ -820,3 +887,82 @@ TEST(TestIntraProcessManager, lowest_available_capacity) { c1 = ipm->lowest_available_capacity(p1_id); ASSERT_EQ(history_depth - 1u, c1); } + +/* + * This tests the check inside add_publisher for transient_local + * durability publishers + * - add_publisher should throw runtime_error when no valid buffer ptr + * is passed with a transient_local publisher + */ +TEST(TestIntraProcessManager, transient_local_invalid_buffer) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + constexpr auto history_depth = 10u; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared(rclcpp::QoS(history_depth).transient_local()); + + ASSERT_THROW( + { + ipm->add_publisher(p1, nullptr); + }, std::runtime_error); +} + +/* + * This tests publishing function for transient_local durability publihers + * - A message is published before three transient_local subscriptions are added to + * ipm. Two of the subscriptions use take_shared method. Delivery of the message is verified + * along with the contents and pointer addresses from the subscriptions. + */ +TEST(TestIntraProcessManager, transient_local) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; + + constexpr auto history_depth = 10u; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared(rclcpp::QoS(history_depth).transient_local()); + + auto s1 = + std::make_shared(rclcpp::QoS(history_depth).transient_local()); + auto s2 = + std::make_shared(rclcpp::QoS(history_depth).transient_local()); + auto s3 = + std::make_shared(rclcpp::QoS(history_depth).transient_local()); + + s1->take_shared_method = false; + s2->take_shared_method = true; + s3->take_shared_method = true; + + auto p1_id = ipm->add_publisher(p1, p1->buffer); + + p1->set_intra_process_manager(p1_id, ipm); + + auto unique_msg = std::make_unique(); + unique_msg->msg = "Test"; + p1->publish(std::move(unique_msg)); + + ipm->template add_subscription(s1); + ipm->template add_subscription(s2); + ipm->template add_subscription(s3); + + auto received_message_pointer_1 = s1->pop(); + auto received_message_pointer_2 = s2->pop(); + auto received_message_pointer_3 = s3->pop(); + ASSERT_NE(0u, received_message_pointer_1); + ASSERT_NE(0u, received_message_pointer_2); + ASSERT_NE(0u, received_message_pointer_3); + ASSERT_EQ(received_message_pointer_3, received_message_pointer_2); + ASSERT_EQ( + reinterpret_cast(received_message_pointer_1)->msg, + reinterpret_cast(received_message_pointer_2)->msg); + ASSERT_EQ( + reinterpret_cast(received_message_pointer_1)->msg, + reinterpret_cast(received_message_pointer_3)->msg); + ASSERT_EQ("Test", reinterpret_cast(received_message_pointer_1)->msg); +} diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 3edb24c0dd..949e006997 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -196,11 +196,7 @@ static std::vector invalid_qos_profiles() { std::vector parameters; - parameters.reserve(2); - parameters.push_back( - TestParameters( - rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(), - "transient_local_qos")); + parameters.reserve(1); parameters.push_back( TestParameters( rclcpp::QoS(rclcpp::KeepAll()), @@ -674,3 +670,96 @@ INSTANTIATE_TEST_SUITE_P( rclcpp::QoS(1).best_effort(), rclcpp::QoS(1).best_effort()), std::pair( rclcpp::QoS(1).reliable(), rclcpp::QoS(1).best_effort()))); + +TEST_F(TestPublisher, intra_process_transient_local) { + constexpr auto history_depth = 10u; + initialize(rclcpp::NodeOptions().use_intra_process_comms(true)); + rclcpp::PublisherOptionsWithAllocator> pub_options_ipm_disabled; + pub_options_ipm_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + + rclcpp::PublisherOptionsWithAllocator> pub_options_ipm_enabled; + pub_options_ipm_enabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + + auto pub_ipm_enabled_transient_local_enabled = node->create_publisher( + "topic1", + rclcpp::QoS(rclcpp::KeepLast(history_depth)).transient_local(), pub_options_ipm_enabled); + auto pub_ipm_disabled_transient_local_enabled = node->create_publisher( + "topic2", + rclcpp::QoS(rclcpp::KeepLast(history_depth)).transient_local(), pub_options_ipm_disabled); + auto pub_ipm_enabled_transient_local_disabled = node->create_publisher( + "topic3", + rclcpp::QoS(rclcpp::KeepLast(history_depth)), pub_options_ipm_enabled); + auto pub_ipm_disabled_transient_local_disabled = node->create_publisher( + "topic4", + rclcpp::QoS(rclcpp::KeepLast(history_depth)), pub_options_ipm_disabled); + + test_msgs::msg::Empty msg; + pub_ipm_enabled_transient_local_enabled->publish(msg); + pub_ipm_disabled_transient_local_enabled->publish(msg); + pub_ipm_enabled_transient_local_disabled->publish(msg); + pub_ipm_disabled_transient_local_disabled->publish(msg); + + auto do_nothing = [](std::shared_ptr) {}; + struct IntraProcessCallback + { + void callback_fun(size_t s) + { + (void) s; + called = true; + } + bool called = false; + }; + rclcpp::SubscriptionOptions sub_options_ipm_disabled; + sub_options_ipm_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + rclcpp::SubscriptionOptions sub_options_ipm_enabled; + sub_options_ipm_enabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + IntraProcessCallback callback1, callback2, callback3, callback4; + auto sub_ipm_enabled_transient_local_enabled = node->create_subscription( + "topic1", + rclcpp::QoS(rclcpp::KeepLast(history_depth)).transient_local(), + do_nothing, sub_options_ipm_enabled); + sub_ipm_enabled_transient_local_enabled->set_on_new_intra_process_message_callback( + std::bind(&IntraProcessCallback::callback_fun, &callback1, std::placeholders::_1)); + auto sub_ipm_disabled_transient_local_enabled = node->create_subscription( + "topic2", + rclcpp::QoS(rclcpp::KeepLast(history_depth)).transient_local(), + do_nothing, sub_options_ipm_disabled); + sub_ipm_disabled_transient_local_enabled->set_on_new_intra_process_message_callback( + std::bind(&IntraProcessCallback::callback_fun, &callback2, std::placeholders::_1)); + auto sub_ipm_enabled_transient_local_disabled = node->create_subscription( + "topic3", + rclcpp::QoS(rclcpp::KeepLast(history_depth)), + do_nothing, sub_options_ipm_enabled); + sub_ipm_enabled_transient_local_disabled->set_on_new_intra_process_message_callback( + std::bind(&IntraProcessCallback::callback_fun, &callback3, std::placeholders::_1)); + auto sub_ipm_disabled_transient_local_disabled = node->create_subscription( + "topic4", + rclcpp::QoS(rclcpp::KeepLast(history_depth)), + do_nothing, sub_options_ipm_disabled); + sub_ipm_disabled_transient_local_disabled->set_on_new_intra_process_message_callback( + std::bind(&IntraProcessCallback::callback_fun, &callback4, std::placeholders::_1)); + + EXPECT_TRUE(pub_ipm_enabled_transient_local_enabled->is_durability_transient_local()); + EXPECT_TRUE(pub_ipm_disabled_transient_local_enabled->is_durability_transient_local()); + EXPECT_FALSE(pub_ipm_enabled_transient_local_disabled->is_durability_transient_local()); + EXPECT_FALSE(pub_ipm_disabled_transient_local_disabled->is_durability_transient_local()); + + EXPECT_EQ(1, pub_ipm_enabled_transient_local_enabled->get_intra_process_subscription_count()); + EXPECT_EQ(0, pub_ipm_disabled_transient_local_enabled->get_intra_process_subscription_count()); + EXPECT_EQ(1, pub_ipm_enabled_transient_local_disabled->get_intra_process_subscription_count()); + EXPECT_EQ(0, pub_ipm_disabled_transient_local_disabled->get_intra_process_subscription_count()); + + EXPECT_EQ( + history_depth - 1u, + pub_ipm_enabled_transient_local_enabled->lowest_available_ipm_capacity()); + EXPECT_EQ(0, pub_ipm_disabled_transient_local_enabled->lowest_available_ipm_capacity()); + EXPECT_EQ( + history_depth, + pub_ipm_enabled_transient_local_disabled->lowest_available_ipm_capacity()); + EXPECT_EQ(0, pub_ipm_disabled_transient_local_disabled->lowest_available_ipm_capacity()); + + EXPECT_TRUE(callback1.called); + EXPECT_FALSE(callback2.called); + EXPECT_FALSE(callback3.called); + EXPECT_FALSE(callback4.called); +} diff --git a/rclcpp/test/rclcpp/test_ring_buffer_implementation.cpp b/rclcpp/test/rclcpp/test_ring_buffer_implementation.cpp index 07dfd8d584..0abd9b1a89 100644 --- a/rclcpp/test/rclcpp/test_ring_buffer_implementation.cpp +++ b/rclcpp/test/rclcpp/test_ring_buffer_implementation.cpp @@ -22,7 +22,7 @@ #include "rclcpp/experimental/buffers/ring_buffer_implementation.hpp" /* - Construtctor + * Construtctor */ TEST(TestRingBufferImplementation, constructor) { // Cannot create a buffer of size zero. @@ -37,10 +37,11 @@ TEST(TestRingBufferImplementation, constructor) { } /* - Basic usage - - insert data and check that it has data - - extract data - - overwrite old data writing over the buffer capacity + * Basic usage + * - insert data and check that it has data + * - get all data + * - extract data + * - overwrite old data writing over the buffer capacity */ TEST(TestRingBufferImplementation, basic_usage) { rclcpp::experimental::buffers::RingBufferImplementation rb(2); @@ -64,6 +65,12 @@ TEST(TestRingBufferImplementation, basic_usage) { rb.enqueue('d'); + const auto all_data_vec = rb.get_all_data(); + + EXPECT_EQ(2u, all_data_vec.size()); + EXPECT_EQ('c', all_data_vec[0]); + EXPECT_EQ('d', all_data_vec[1]); + EXPECT_EQ(true, rb.has_data()); EXPECT_EQ(true, rb.is_full()); @@ -79,3 +86,56 @@ TEST(TestRingBufferImplementation, basic_usage) { EXPECT_EQ(false, rb.has_data()); EXPECT_EQ(false, rb.is_full()); } + +/* + * Basic usage with unique_ptr + * - insert unique_ptr data and check that it has data + * - get all data + * - extract data + * - overwrite old data writing over the buffer capacity + */ +TEST(TestRingBufferImplementation, basic_usage_unique_ptr) { + rclcpp::experimental::buffers::RingBufferImplementation> rb(2); + + auto a = std::make_unique('a'); + auto b = std::make_unique('b'); + auto original_b_pointer = reinterpret_cast(b.get()); + auto c = std::make_unique('c'); + auto original_c_pointer = reinterpret_cast(c.get()); + + rb.enqueue(std::move(a)); + + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); + + rb.enqueue(std::move(b)); + rb.enqueue(std::move(c)); + + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(true, rb.is_full()); + + const auto all_data_vec = rb.get_all_data(); + + EXPECT_EQ(2u, all_data_vec.size()); + EXPECT_EQ('b', *all_data_vec[0]); + EXPECT_EQ('c', *all_data_vec[1]); + EXPECT_NE(original_b_pointer, reinterpret_cast(all_data_vec[0].get())); + EXPECT_NE(original_c_pointer, reinterpret_cast(all_data_vec[1].get())); + + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(true, rb.is_full()); + + auto uni_ptr = rb.dequeue(); + + EXPECT_EQ('b', *uni_ptr); + EXPECT_EQ(original_b_pointer, reinterpret_cast(uni_ptr.get())); + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); + + uni_ptr = rb.dequeue(); + + EXPECT_EQ('c', *uni_ptr); + EXPECT_EQ(original_c_pointer, reinterpret_cast(uni_ptr.get())); + EXPECT_EQ(false, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); +} diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index 867171848c..06f6f9785b 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -611,11 +611,7 @@ static std::vector invalid_qos_profiles() { std::vector parameters; - parameters.reserve(3); - parameters.push_back( - TestParameters( - rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(), - "transient_local_qos")); + parameters.reserve(1); parameters.push_back( TestParameters( rclcpp::QoS(rclcpp::KeepAll()), From a5221336f64e9754377e5391663dc6c7f3da3975 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 18 Jan 2024 16:40:13 -0500 Subject: [PATCH 324/455] Increase the cppcheck timeout to 600 seconds. (#2409) Signed-off-by: Chris Lalancette --- rclcpp/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index bf3651df81..cad17103b7 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -282,7 +282,7 @@ install( if(TEST cppcheck) # must set the property after ament_package() - set_tests_properties(cppcheck PROPERTIES TIMEOUT 500) + set_tests_properties(cppcheck PROPERTIES TIMEOUT 600) endif() if(TEST cpplint) From 6602fcf7bcd2e4c6a1b51bb2da5df2834abdc98e Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 24 Jan 2024 14:08:05 +0000 Subject: [PATCH 325/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 7 +++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 5 +++++ 4 files changed, 18 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 53683e53dc..5f3b2cc059 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase the cppcheck timeout to 600 seconds. (`#2409 `_) +* Add transient local durability support to publisher and subscriptions when using intra-process communication (`#2303 `_) +* Stop storing the context in the guard condition. (`#2400 `_) +* Contributors: Chris Lalancette, Jeffery Hsu + 25.0.0 (2023-12-26) ------------------- * Updated GenericSubscription to AnySubscriptionCallback (`#1928 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 711a346441..ce32ac7d3c 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 25.0.0 (2023-12-26) ------------------- * Switch to target_link_libraries. (`#2374 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index df05905df3..e1aa3337f0 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 25.0.0 (2023-12-26) ------------------- * Switch to target_link_libraries. (`#2374 `_) diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index e0eaac5d9f..53ba65a180 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase timeout for rclcpp_lifecycle to 360 (`#2395 `_) +* Contributors: Jorge Perez + 25.0.0 (2023-12-26) ------------------- From 676897df9de4f1c165c08e6d0480fa0eb5a69f42 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 24 Jan 2024 14:08:12 +0000 Subject: [PATCH 326/455] 26.0.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 5f3b2cc059..de1429e204 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +26.0.0 (2024-01-24) +------------------- * Increase the cppcheck timeout to 600 seconds. (`#2409 `_) * Add transient local durability support to publisher and subscriptions when using intra-process communication (`#2303 `_) * Stop storing the context in the guard condition. (`#2400 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 2b5c9bb74c..c1b28df8fe 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 25.0.0 + 26.0.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index ce32ac7d3c..a539fa42d0 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +26.0.0 (2024-01-24) +------------------- 25.0.0 (2023-12-26) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index e8b3cb98a3..f9cf67ea33 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 25.0.0 + 26.0.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index e1aa3337f0..3a4cc798df 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +26.0.0 (2024-01-24) +------------------- 25.0.0 (2023-12-26) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index f3f5f4135d..a4411a0a65 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 25.0.0 + 26.0.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 53ba65a180..5513040a7c 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +26.0.0 (2024-01-24) +------------------- * Increase timeout for rclcpp_lifecycle to 360 (`#2395 `_) * Contributors: Jorge Perez diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 5a543a9f0c..daf0ca8c20 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 25.0.0 + 26.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From a29f58edcbe3a0347d71d0fec3e2232d699ea918 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 24 Jan 2024 18:51:25 -0500 Subject: [PATCH 327/455] Make sure to mark RingBuffer methods as 'override'. (#2410) This gets rid of a warning when building under clang. Signed-off-by: Chris Lalancette --- .../buffers/ring_buffer_implementation.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp index f476fa556a..b8fe79a5ff 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp @@ -67,7 +67,7 @@ class RingBufferImplementation : public BufferImplementationBase * * \param request the element to be stored in the ring buffer */ - void enqueue(BufferT request) + void enqueue(BufferT request) override { std::lock_guard lock(mutex_); @@ -93,7 +93,7 @@ class RingBufferImplementation : public BufferImplementationBase * * \return the element that is being removed from the ring buffer */ - BufferT dequeue() + BufferT dequeue() override { std::lock_guard lock(mutex_); @@ -144,7 +144,7 @@ class RingBufferImplementation : public BufferImplementationBase * * \return `true` if there is data and `false` otherwise */ - inline bool has_data() const + inline bool has_data() const override { std::lock_guard lock(mutex_); return has_data_(); @@ -169,13 +169,13 @@ class RingBufferImplementation : public BufferImplementationBase * * \return the number of free capacity for new messages */ - size_t available_capacity() const + size_t available_capacity() const override { std::lock_guard lock(mutex_); return available_capacity_(); } - void clear() + void clear() override { TRACETOOLS_TRACEPOINT(rclcpp_ring_buffer_clear, static_cast(this)); } From 265f5ec297a6a3711007ec62e4aa21b0b26516fc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 26 Jan 2024 09:44:36 +0100 Subject: [PATCH 328/455] Removed deprecated header (#2413) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- rclcpp/include/rclcpp/qos_event.hpp | 22 ---------------------- 1 file changed, 22 deletions(-) delete mode 100644 rclcpp/include/rclcpp/qos_event.hpp diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp deleted file mode 100644 index efc0aa5739..0000000000 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ /dev/null @@ -1,22 +0,0 @@ -// Copyright 2019 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__QOS_EVENT_HPP_ -#define RCLCPP__QOS_EVENT_HPP_ - -#warning This header is obsolete, please include rclcpp/event_handler.hpp instead - -#include "rclcpp/event_handler.hpp" - -#endif // RCLCPP__QOS_EVENT_HPP_ From 99f0fc938b2d0c441787113a3920c16d3614a197 Mon Sep 17 00:00:00 2001 From: Matt Condino <36555625+mwcondino@users.noreply.github.com> Date: Tue, 30 Jan 2024 06:16:37 -0800 Subject: [PATCH 329/455] [events executor] - Fix Behavior with Timer Cancel (#2375) * fix Signed-off-by: Matt Condino * add timer cancel tests Signed-off-by: Matt Condino * cleanup header include Signed-off-by: Matt Condino * reverting change to timer_greater function Signed-off-by: Gus Brigantino * use std::optional, and handle edgecase of 1 cancelled timer Signed-off-by: Matt Condino * clean up run_timers func Signed-off-by: Gus Brigantino * some fixes and added tests for cancel then reset of timers. Signed-off-by: Matt Condino * refactor and clean up. remove cancelled timer tracking. Signed-off-by: Matt Condino * remove unused method for size() Signed-off-by: Matt Condino * linting Signed-off-by: Matt Condino * relax timing constraints in tests Signed-off-by: Matt Condino * further relax timing constraints to ensure windows tests are not flaky. Signed-off-by: Matt Condino * use sim clock for tests, pub clock at .25 realtime rate. Signed-off-by: Matt Condino --------- Signed-off-by: Matt Condino Signed-off-by: Gus Brigantino Co-authored-by: Gus Brigantino --- .../rclcpp/experimental/timers_manager.hpp | 12 +- .../events_executor/events_executor.cpp | 7 +- .../rclcpp/experimental/timers_manager.cpp | 45 ++- .../test/rclcpp/executors/test_executors.cpp | 379 ++++++++++++++++++ rclcpp/test/rclcpp/test_timers_manager.cpp | 46 +++ 5 files changed, 468 insertions(+), 21 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/timers_manager.hpp b/rclcpp/include/rclcpp/experimental/timers_manager.hpp index 688684376a..197397e8b8 100644 --- a/rclcpp/include/rclcpp/experimental/timers_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/timers_manager.hpp @@ -22,10 +22,10 @@ #include #include #include +#include #include #include #include - #include "rclcpp/context.hpp" #include "rclcpp/timer.hpp" @@ -172,13 +172,14 @@ class TimersManager * @brief Get the amount of time before the next timer triggers. * This function is thread safe. * - * @return std::chrono::nanoseconds to wait, + * @return std::optional to wait, * the returned value could be negative if the timer is already expired * or std::chrono::nanoseconds::max() if there are no timers stored in the object. + * If the head timer was cancelled, then this will return a nullopt. * @throws std::runtime_error if the timers thread was already running. */ RCLCPP_PUBLIC - std::chrono::nanoseconds get_head_timeout(); + std::optional get_head_timeout(); private: RCLCPP_DISABLE_COPY(TimersManager) @@ -512,12 +513,13 @@ class TimersManager * @brief Get the amount of time before the next timer triggers. * This function is not thread safe, acquire a mutex before calling it. * - * @return std::chrono::nanoseconds to wait, + * @return std::optional to wait, * the returned value could be negative if the timer is already expired * or std::chrono::nanoseconds::max() if the heap is empty. + * If the head timer was cancelled, then this will return a nullopt. * This function is not thread safe, acquire the timers_mutex_ before calling it. */ - std::chrono::nanoseconds get_head_timeout_unsafe(); + std::optional get_head_timeout_unsafe(); /** * @brief Executes all the timers currently ready when the function is invoked 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 64b07c0814..c977c8c904 100644 --- a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp +++ b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp @@ -206,11 +206,12 @@ EventsExecutor::spin_once_impl(std::chrono::nanoseconds timeout) timeout = std::chrono::nanoseconds::max(); } - // Select the smallest between input timeout and timer timeout + // Select the smallest between input timeout and timer timeout. + // Cancelled timers are not considered. bool is_timer_timeout = false; auto next_timer_timeout = timers_manager_->get_head_timeout(); - if (next_timer_timeout < timeout) { - timeout = next_timer_timeout; + if (next_timer_timeout.has_value() && next_timer_timeout.value() < timeout) { + timeout = next_timer_timeout.value(); is_timer_timeout = true; } diff --git a/rclcpp/src/rclcpp/experimental/timers_manager.cpp b/rclcpp/src/rclcpp/experimental/timers_manager.cpp index e179787b36..39924afa56 100644 --- a/rclcpp/src/rclcpp/experimental/timers_manager.cpp +++ b/rclcpp/src/rclcpp/experimental/timers_manager.cpp @@ -100,7 +100,7 @@ void TimersManager::stop() } } -std::chrono::nanoseconds TimersManager::get_head_timeout() +std::optional TimersManager::get_head_timeout() { // Do not allow to interfere with the thread running if (running_) { @@ -169,7 +169,7 @@ void TimersManager::execute_ready_timer(const rclcpp::TimerBase * timer_id) } } -std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe() +std::optional TimersManager::get_head_timeout_unsafe() { // If we don't have any weak pointer, then we just return maximum timeout if (weak_timers_heap_.empty()) { @@ -191,7 +191,9 @@ std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe() } head_timer = locked_heap.front(); } - + if (head_timer->is_canceled()) { + return std::nullopt; + } return head_timer->time_until_trigger(); } @@ -242,17 +244,34 @@ void TimersManager::run_timers() // Lock mutex std::unique_lock lock(timers_mutex_); - std::chrono::nanoseconds time_to_sleep = get_head_timeout_unsafe(); + std::optional time_to_sleep = get_head_timeout_unsafe(); + + // If head timer was cancelled, try to reheap and get a new head. + // This avoids an edge condition where head timer is cancelled, but other + // valid timers remain in the heap. + if (!time_to_sleep.has_value()) { + // Re-heap to (possibly) move cancelled timer from head of heap. If + // entire heap is cancelled, this will still result in a nullopt. + TimersHeap locked_heap = weak_timers_heap_.validate_and_lock(); + locked_heap.heapify(); + weak_timers_heap_.store(locked_heap); + time_to_sleep = get_head_timeout_unsafe(); + } - // No need to wait if a timer is already available - if (time_to_sleep > std::chrono::nanoseconds::zero()) { - if (time_to_sleep != std::chrono::nanoseconds::max()) { - // Wait until timeout or notification that timers have been updated - timers_cv_.wait_for(lock, time_to_sleep, [this]() {return timers_updated_;}); - } else { - // Wait until notification that timers have been updated - timers_cv_.wait(lock, [this]() {return timers_updated_;}); - } + // If no timers, or all timers cancelled, wait for an update. + if (!time_to_sleep.has_value() || (time_to_sleep.value() == std::chrono::nanoseconds::max()) ) { + // Wait until notification that timers have been updated + timers_cv_.wait(lock, [this]() {return timers_updated_;}); + + // Re-heap in case ordering changed due to a cancelled timer + // re-activating. + TimersHeap locked_heap = weak_timers_heap_.validate_and_lock(); + locked_heap.heapify(); + weak_timers_heap_.store(locked_heap); + } else if (time_to_sleep.value() != std::chrono::nanoseconds::zero()) { + // If time_to_sleep is zero, we immediately execute. Otherwise, wait + // until timeout or notification that timers have been updated + timers_cv_.wait_for(lock, time_to_sleep.value(), [this]() {return timers_updated_;}); } // Reset timers updated flag diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 653f06fb9c..35eea01038 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -36,8 +36,10 @@ #include "rclcpp/duration.hpp" #include "rclcpp/guard_condition.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/time_source.hpp" #include "test_msgs/msg/empty.hpp" +#include "rosgraph_msgs/msg/clock.hpp" using namespace std::chrono_literals; @@ -818,3 +820,380 @@ TYPED_TEST(TestIntraprocessExecutors, testIntraprocessRetrigger) { executor.spin(); EXPECT_EQ(kNumMessages, this->callback_count.load()); } + +class TimerNode : public rclcpp::Node +{ +public: + explicit TimerNode(std::string subname) + : Node("timer_node", subname) + { + timer1_ = rclcpp::create_timer( + this->get_node_base_interface(), get_node_timers_interface(), + get_clock(), 1ms, + std::bind(&TimerNode::Timer1Callback, this)); + + timer2_ = + rclcpp::create_timer( + this->get_node_base_interface(), get_node_timers_interface(), + get_clock(), 1ms, + std::bind(&TimerNode::Timer2Callback, this)); + } + + int GetTimer1Cnt() {return cnt1_;} + int GetTimer2Cnt() {return cnt2_;} + + void ResetTimer1() + { + timer1_->reset(); + } + + void ResetTimer2() + { + timer2_->reset(); + } + + void CancelTimer1() + { + RCLCPP_DEBUG(this->get_logger(), "Timer 1 cancelling!"); + timer1_->cancel(); + } + + void CancelTimer2() + { + RCLCPP_DEBUG(this->get_logger(), "Timer 2 cancelling!"); + timer2_->cancel(); + } + +private: + void Timer1Callback() + { + RCLCPP_DEBUG(this->get_logger(), "Timer 1!"); + cnt1_++; + } + + void Timer2Callback() + { + RCLCPP_DEBUG(this->get_logger(), "Timer 2!"); + cnt2_++; + } + + rclcpp::TimerBase::SharedPtr timer1_; + rclcpp::TimerBase::SharedPtr timer2_; + int cnt1_ = 0; + int cnt2_ = 0; +}; + +// Sets up a separate thread to publish /clock messages. +// Clock rate relative to real clock is controlled by realtime_update_rate. +// This is set conservatively slow to ensure unit tests are reliable on Windows +// environments, where timing performance is subpar. +// +// Use `sleep_for` in tests to advance the clock. Clock should run and be published +// in separate thread continuously to ensure correct behavior in node under test. +class ClockPublisher : public rclcpp::Node +{ +public: + explicit ClockPublisher(float simulated_clock_step = .001f, float realtime_update_rate = 0.25f) + : Node("clock_publisher"), + ros_update_duration_(0, 0), + realtime_clock_step_(0, 0), + rostime_(0, 0) + { + clock_publisher_ = this->create_publisher("clock", 10); + realtime_clock_step_ = + rclcpp::Duration::from_seconds(simulated_clock_step / realtime_update_rate); + ros_update_duration_ = rclcpp::Duration::from_seconds(simulated_clock_step); + + timer_thread_ = std::thread(&ClockPublisher::RunTimer, this); + } + + ~ClockPublisher() + { + running_ = false; + if (timer_thread_.joinable()) { + timer_thread_.join(); + } + } + + void sleep_for(rclcpp::Duration duration) + { + rclcpp::Time start_time(0, 0, RCL_ROS_TIME); + { + const std::lock_guard lock(mutex_); + start_time = rostime_; + } + rclcpp::Time current_time = start_time; + + while (true) { + { + const std::lock_guard lock(mutex_); + current_time = rostime_; + } + if ((current_time - start_time) >= duration) { + return; + } + std::this_thread::sleep_for(realtime_clock_step_.to_chrono()); + rostime_ += ros_update_duration_; + } + } + +private: + void RunTimer() + { + while (running_) { + PublishClock(); + std::this_thread::sleep_for(realtime_clock_step_.to_chrono()); + } + } + + void PublishClock() + { + const std::lock_guard lock(mutex_); + auto message = rosgraph_msgs::msg::Clock(); + message.clock = rostime_; + clock_publisher_->publish(message); + } + + rclcpp::Publisher::SharedPtr clock_publisher_; + + rclcpp::Duration ros_update_duration_; + rclcpp::Duration realtime_clock_step_; + // Rostime must be guarded by a mutex, since accessible in running thread + // as well as sleep_for + rclcpp::Time rostime_; + std::mutex mutex_; + std::thread timer_thread_; + std::atomic running_ = true; +}; + + +template +class TestTimerCancelBehavior : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); + std::stringstream test_name; + test_name << test_info->test_case_name() << "_" << test_info->name(); + node = std::make_shared(test_name.str()); + param_client = std::make_shared(node); + ASSERT_TRUE(param_client->wait_for_service(5s)); + + auto set_parameters_results = param_client->set_parameters( + {rclcpp::Parameter("use_sim_time", false)}); + for (auto & result : set_parameters_results) { + ASSERT_TRUE(result.successful); + } + + // Run standalone thread to publish clock time + sim_clock_node = std::make_shared(); + + // Spin the executor in a standalone thread + executor.add_node(this->node); + standalone_thread = std::thread( + [this]() { + executor.spin(); + }); + } + + void TearDown() + { + node.reset(); + + // Clean up thread object + if (standalone_thread.joinable()) { + standalone_thread.join(); + } + } + + std::shared_ptr node; + std::shared_ptr sim_clock_node; + rclcpp::SyncParametersClient::SharedPtr param_client; + std::thread standalone_thread; + T executor; +}; + +TYPED_TEST_SUITE(TestTimerCancelBehavior, ExecutorTypes, ExecutorTypeNames); + +TYPED_TEST(TestTimerCancelBehavior, testTimer1CancelledWithExecutorSpin) { + // Validate that cancelling one timer yields no change in behavior for other + // timers. Specifically, this tests the behavior when using spin() to run the + // executor, which is the most common usecase. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer1(); + this->sim_clock_node->sleep_for(150ms); + this->executor.cancel(); + + int t1_runs = this->node->GetTimer1Cnt(); + int t2_runs = this->node->GetTimer2Cnt(); + EXPECT_NE(t1_runs, t2_runs); + // Check that t2 has significantly more calls + EXPECT_LT(t1_runs + 50, t2_runs); +} + +TYPED_TEST(TestTimerCancelBehavior, testTimer2CancelledWithExecutorSpin) { + // Validate that cancelling one timer yields no change in behavior for other + // timers. Specifically, this tests the behavior when using spin() to run the + // executor, which is the most common usecase. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer2(); + this->sim_clock_node->sleep_for(150ms); + this->executor.cancel(); + + int t1_runs = this->node->GetTimer1Cnt(); + int t2_runs = this->node->GetTimer2Cnt(); + EXPECT_NE(t1_runs, t2_runs); + // Check that t1 has significantly more calls + EXPECT_LT(t2_runs + 50, t1_runs); +} + +TYPED_TEST(TestTimerCancelBehavior, testHeadTimerCancelThenResetBehavior) { + // Validate that cancelling timer doesn't affect operation of other timers, + // and that the cancelled timer starts executing normally once reset manually. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer1(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_initial = this->node->GetTimer1Cnt(); + int t2_runs_initial = this->node->GetTimer2Cnt(); + + // Manually reset timer 1, then sleep again + // Counts should update. + this->node->ResetTimer1(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_final = this->node->GetTimer1Cnt(); + int t2_runs_final = this->node->GetTimer2Cnt(); + + this->executor.cancel(); + + // T1 should have been restarted, and execute about 15 additional times. + // Check 10 greater than initial, to account for some timing jitter. + EXPECT_LT(t1_runs_initial + 50, t1_runs_final); + + EXPECT_LT(t1_runs_initial + 50, t2_runs_initial); + // Check that t2 has significantly more calls, and keeps getting called. + EXPECT_LT(t2_runs_initial + 50, t2_runs_final); +} + +TYPED_TEST(TestTimerCancelBehavior, testBackTimerCancelThenResetBehavior) { + // Validate that cancelling timer doesn't affect operation of other timers, + // and that the cancelled timer starts executing normally once reset manually. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_initial = this->node->GetTimer1Cnt(); + int t2_runs_initial = this->node->GetTimer2Cnt(); + + // Manually reset timer 1, then sleep again + // Counts should update. + this->node->ResetTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_final = this->node->GetTimer1Cnt(); + int t2_runs_final = this->node->GetTimer2Cnt(); + + this->executor.cancel(); + + // T2 should have been restarted, and execute about 15 additional times. + // Check 10 greater than initial, to account for some timing jitter. + EXPECT_LT(t2_runs_initial + 50, t2_runs_final); + + EXPECT_LT(t2_runs_initial + 50, t1_runs_initial); + // Check that t1 has significantly more calls, and keeps getting called. + EXPECT_LT(t1_runs_initial + 50, t1_runs_final); +} + +TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT1Behavior) { + // Validate behavior from cancelling 2 timers, then only re-enabling one of them. + // Ensure that only the reset timer is executed. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer1(); + this->node->CancelTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_initial = this->node->GetTimer1Cnt(); + int t2_runs_initial = this->node->GetTimer2Cnt(); + + // Manually reset timer 1, then sleep again + // Counts should update. + this->node->ResetTimer1(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_intermediate = this->node->GetTimer1Cnt(); + int t2_runs_intermediate = this->node->GetTimer2Cnt(); + + this->node->ResetTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_final = this->node->GetTimer1Cnt(); + int t2_runs_final = this->node->GetTimer2Cnt(); + + this->executor.cancel(); + + // T1 and T2 should have the same initial count. + EXPECT_LE(std::abs(t1_runs_initial - t2_runs_initial), 1); + + // Expect that T1 has up to 15 more calls than t2. Add some buffer + // to account for jitter. + EXPECT_EQ(t2_runs_initial, t2_runs_intermediate); + EXPECT_LT(t1_runs_initial + 50, t1_runs_intermediate); + + // Expect that by end of test, both are running properly again. + EXPECT_LT(t1_runs_intermediate + 50, t1_runs_final); + EXPECT_LT(t2_runs_intermediate + 50, t2_runs_final); +} + +TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT2Behavior) { + // Validate behavior from cancelling 2 timers, then only re-enabling one of them. + // Ensure that only the reset timer is executed. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer1(); + this->node->CancelTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_initial = this->node->GetTimer1Cnt(); + int t2_runs_initial = this->node->GetTimer2Cnt(); + + // Manually reset timer 1, then sleep again + // Counts should update. + this->node->ResetTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_intermediate = this->node->GetTimer1Cnt(); + int t2_runs_intermediate = this->node->GetTimer2Cnt(); + + this->node->ResetTimer1(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_final = this->node->GetTimer1Cnt(); + int t2_runs_final = this->node->GetTimer2Cnt(); + + this->executor.cancel(); + + // T1 and T2 should have the same initial count. + EXPECT_LE(std::abs(t1_runs_initial - t2_runs_initial), 1); + + // Expect that T1 has up to 15 more calls than t2. Add some buffer + // to account for jitter. + EXPECT_EQ(t1_runs_initial, t1_runs_intermediate); + EXPECT_LT(t2_runs_initial + 50, t2_runs_intermediate); + + // Expect that by end of test, both are running properly again. + EXPECT_LT(t1_runs_intermediate + 50, t1_runs_final); + EXPECT_LT(t2_runs_intermediate + 50, t2_runs_final); +} diff --git a/rclcpp/test/rclcpp/test_timers_manager.cpp b/rclcpp/test/rclcpp/test_timers_manager.cpp index 167523934c..0e49da08e1 100644 --- a/rclcpp/test/rclcpp/test_timers_manager.cpp +++ b/rclcpp/test/rclcpp/test_timers_manager.cpp @@ -359,3 +359,49 @@ TEST_F(TestTimersManager, infinite_loop) EXPECT_LT(0u, t1_runs); EXPECT_LT(0u, t2_runs); } + +// Validate that cancelling one timer yields no change in behavior for other +// timers. +TEST_F(TestTimersManager, check_one_timer_cancel_doesnt_affect_other_timers) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + size_t t1_runs = 0; + std::shared_ptr t1; + // After a while cancel t1. Don't remove it though. + // Simulates typical usage in a Node where a timer is cancelled but not removed, + // since typical users aren't going to mess around with the timer manager. + t1 = TimerT::make_shared( + 1ms, + [&t1_runs, &t1]() { + t1_runs++; + if (t1_runs == 5) { + t1->cancel(); + } + }, + rclcpp::contexts::get_global_default_context()); + + size_t t2_runs = 0; + auto t2 = TimerT::make_shared( + 1ms, + [&t2_runs]() { + t2_runs++; + }, + rclcpp::contexts::get_global_default_context()); + + // Add timers + timers_manager->add_timer(t1); + timers_manager->add_timer(t2); + + // Start timers thread + timers_manager->start(); + + std::this_thread::sleep_for(15ms); + + // t1 has stopped running + EXPECT_NE(t1_runs, t2_runs); + // Check that t2 has significantly more calls + EXPECT_LT(t1_runs + 5, t2_runs); + timers_manager->stop(); +} From c10764f4329f8c1494b68ff60ce55a3c74c8de9e Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 1 Feb 2024 12:05:38 -0500 Subject: [PATCH 330/455] Split test_executors up into smaller chunks. (#2421) The original reason is that on Windows Debug, we were failing to compile because the compilation unit was too large. But also this file was way too large (1200 lines), so it makes sense to split it up. Signed-off-by: Chris Lalancette --- rclcpp/test/rclcpp/CMakeLists.txt | 18 + .../test/rclcpp/executors/executor_types.hpp | 70 +++ .../test/rclcpp/executors/test_executors.cpp | 526 +----------------- .../executors/test_executors_intraprocess.cpp | 125 +++++ .../test_executors_timer_cancel_behavior.cpp | 408 ++++++++++++++ 5 files changed, 624 insertions(+), 523 deletions(-) create mode 100644 rclcpp/test/rclcpp/executors/executor_types.hpp create mode 100644 rclcpp/test/rclcpp/executors/test_executors_intraprocess.cpp create mode 100644 rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index ca6c8f2adf..0a9ba9ca97 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -441,6 +441,24 @@ if(TARGET test_executors) target_link_libraries(test_executors ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) endif() +ament_add_gtest( + test_executors_timer_cancel_behavior + executors/test_executors_timer_cancel_behavior.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) +if(TARGET test_executors) + target_link_libraries(test_executors_timer_cancel_behavior ${PROJECT_NAME} ${rosgraph_msgs_TARGETS}) +endif() + +ament_add_gtest( + test_executors_intraprocess + executors/test_executors_intraprocess.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) +if(TARGET test_executors) + target_link_libraries(test_executors_intraprocess ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() + ament_add_gtest(test_static_single_threaded_executor executors/test_static_single_threaded_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_static_single_threaded_executor) diff --git a/rclcpp/test/rclcpp/executors/executor_types.hpp b/rclcpp/test/rclcpp/executors/executor_types.hpp new file mode 100644 index 0000000000..0218a9b547 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/executor_types.hpp @@ -0,0 +1,70 @@ +// Copyright 2017 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__EXECUTORS__EXECUTOR_TYPES_HPP_ +#define RCLCPP__EXECUTORS__EXECUTOR_TYPES_HPP_ + +#include + +#include +#include + +#include "rclcpp/experimental/executors/events_executor/events_executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/executors/static_single_threaded_executor.hpp" +#include "rclcpp/executors/multi_threaded_executor.hpp" + +using ExecutorTypes = + ::testing::Types< + rclcpp::executors::SingleThreadedExecutor, + rclcpp::executors::MultiThreadedExecutor, + rclcpp::executors::StaticSingleThreadedExecutor, + rclcpp::experimental::executors::EventsExecutor>; + +class ExecutorTypeNames +{ +public: + template + static std::string GetName(int idx) + { + (void)idx; + if (std::is_same()) { + return "SingleThreadedExecutor"; + } + + if (std::is_same()) { + return "MultiThreadedExecutor"; + } + + if (std::is_same()) { + return "StaticSingleThreadedExecutor"; + } + + if (std::is_same()) { + return "EventsExecutor"; + } + + return ""; + } +}; + +// StaticSingleThreadedExecutor is not included in these tests for now, due to: +// https://github.com/ros2/rclcpp/issues/1219 +using StandardExecutors = + ::testing::Types< + rclcpp::executors::SingleThreadedExecutor, + rclcpp::executors::MultiThreadedExecutor, + rclcpp::experimental::executors::EventsExecutor>; + +#endif // RCLCPP__EXECUTORS__EXECUTOR_TYPES_HPP_ diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 35eea01038..4086c14668 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -15,8 +15,8 @@ /** * This test checks all implementations of rclcpp::executor to check they pass they basic API * tests. Anything specific to any executor in particular should go in a separate test file. - * */ + #include #include @@ -39,7 +39,8 @@ #include "rclcpp/time_source.hpp" #include "test_msgs/msg/empty.hpp" -#include "rosgraph_msgs/msg/clock.hpp" + +#include "./executor_types.hpp" using namespace std::chrono_literals; @@ -86,51 +87,8 @@ class TestExecutors : public ::testing::Test template class TestExecutorsStable : public TestExecutors {}; -using ExecutorTypes = - ::testing::Types< - rclcpp::executors::SingleThreadedExecutor, - rclcpp::executors::MultiThreadedExecutor, - rclcpp::executors::StaticSingleThreadedExecutor, - rclcpp::experimental::executors::EventsExecutor>; - -class ExecutorTypeNames -{ -public: - template - static std::string GetName(int idx) - { - (void)idx; - if (std::is_same()) { - return "SingleThreadedExecutor"; - } - - if (std::is_same()) { - return "MultiThreadedExecutor"; - } - - if (std::is_same()) { - return "StaticSingleThreadedExecutor"; - } - - if (std::is_same()) { - return "EventsExecutor"; - } - - return ""; - } -}; - -// TYPED_TEST_SUITE is deprecated as of gtest 1.9, use TYPED_TEST_SUITE when gtest dependency -// is updated. TYPED_TEST_SUITE(TestExecutors, ExecutorTypes, ExecutorTypeNames); -// StaticSingleThreadedExecutor is not included in these tests for now, due to: -// https://github.com/ros2/rclcpp/issues/1219 -using StandardExecutors = - ::testing::Types< - rclcpp::executors::SingleThreadedExecutor, - rclcpp::executors::MultiThreadedExecutor, - rclcpp::experimental::executors::EventsExecutor>; TYPED_TEST_SUITE(TestExecutorsStable, StandardExecutors, ExecutorTypeNames); // Make sure that executors detach from nodes when destructing @@ -719,481 +677,3 @@ TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) rclcpp::shutdown(); } - -template -class TestIntraprocessExecutors : public ::testing::Test -{ -public: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - static void TearDownTestCase() - { - rclcpp::shutdown(); - } - - void SetUp() - { - const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); - std::stringstream test_name; - test_name << test_info->test_case_name() << "_" << test_info->name(); - node = std::make_shared("node", test_name.str()); - - callback_count = 0u; - - const std::string topic_name = std::string("topic_") + test_name.str(); - - rclcpp::PublisherOptions po; - po.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; - publisher = node->create_publisher(topic_name, rclcpp::QoS(1), po); - - auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) { - this->callback_count.fetch_add(1u); - }; - - rclcpp::SubscriptionOptions so; - so.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; - subscription = - node->create_subscription( - topic_name, rclcpp::QoS(kNumMessages), std::move(callback), so); - } - - void TearDown() - { - publisher.reset(); - subscription.reset(); - node.reset(); - } - - const size_t kNumMessages = 100; - - rclcpp::Node::SharedPtr node; - rclcpp::Publisher::SharedPtr publisher; - rclcpp::Subscription::SharedPtr subscription; - std::atomic_size_t callback_count; -}; - -TYPED_TEST_SUITE(TestIntraprocessExecutors, ExecutorTypes, ExecutorTypeNames); - -TYPED_TEST(TestIntraprocessExecutors, testIntraprocessRetrigger) { - // This tests that executors will continue to service intraprocess subscriptions in the case - // that publishers aren't continuing to publish. - // This was previously broken in that intraprocess guard conditions were only triggered on - // publish and the test was added to prevent future regressions. - static constexpr size_t kNumMessages = 100; - - using ExecutorType = TypeParam; - ExecutorType executor; - executor.add_node(this->node); - - EXPECT_EQ(0u, this->callback_count.load()); - this->publisher->publish(test_msgs::msg::Empty()); - - // Wait for up to 5 seconds for the first message to come available. - const std::chrono::milliseconds sleep_per_loop(10); - int loops = 0; - while (1u != this->callback_count.load() && loops < 500) { - rclcpp::sleep_for(sleep_per_loop); - executor.spin_some(); - loops++; - } - EXPECT_EQ(1u, this->callback_count.load()); - - // reset counter - this->callback_count.store(0u); - - for (size_t ii = 0; ii < kNumMessages; ++ii) { - this->publisher->publish(test_msgs::msg::Empty()); - } - - // Fire a timer every 10ms up to 5 seconds waiting for subscriptions to be read. - loops = 0; - auto timer = this->node->create_wall_timer( - std::chrono::milliseconds(10), [this, &executor, &loops]() { - loops++; - if (kNumMessages == this->callback_count.load() || loops == 500) { - executor.cancel(); - } - }); - executor.spin(); - EXPECT_EQ(kNumMessages, this->callback_count.load()); -} - -class TimerNode : public rclcpp::Node -{ -public: - explicit TimerNode(std::string subname) - : Node("timer_node", subname) - { - timer1_ = rclcpp::create_timer( - this->get_node_base_interface(), get_node_timers_interface(), - get_clock(), 1ms, - std::bind(&TimerNode::Timer1Callback, this)); - - timer2_ = - rclcpp::create_timer( - this->get_node_base_interface(), get_node_timers_interface(), - get_clock(), 1ms, - std::bind(&TimerNode::Timer2Callback, this)); - } - - int GetTimer1Cnt() {return cnt1_;} - int GetTimer2Cnt() {return cnt2_;} - - void ResetTimer1() - { - timer1_->reset(); - } - - void ResetTimer2() - { - timer2_->reset(); - } - - void CancelTimer1() - { - RCLCPP_DEBUG(this->get_logger(), "Timer 1 cancelling!"); - timer1_->cancel(); - } - - void CancelTimer2() - { - RCLCPP_DEBUG(this->get_logger(), "Timer 2 cancelling!"); - timer2_->cancel(); - } - -private: - void Timer1Callback() - { - RCLCPP_DEBUG(this->get_logger(), "Timer 1!"); - cnt1_++; - } - - void Timer2Callback() - { - RCLCPP_DEBUG(this->get_logger(), "Timer 2!"); - cnt2_++; - } - - rclcpp::TimerBase::SharedPtr timer1_; - rclcpp::TimerBase::SharedPtr timer2_; - int cnt1_ = 0; - int cnt2_ = 0; -}; - -// Sets up a separate thread to publish /clock messages. -// Clock rate relative to real clock is controlled by realtime_update_rate. -// This is set conservatively slow to ensure unit tests are reliable on Windows -// environments, where timing performance is subpar. -// -// Use `sleep_for` in tests to advance the clock. Clock should run and be published -// in separate thread continuously to ensure correct behavior in node under test. -class ClockPublisher : public rclcpp::Node -{ -public: - explicit ClockPublisher(float simulated_clock_step = .001f, float realtime_update_rate = 0.25f) - : Node("clock_publisher"), - ros_update_duration_(0, 0), - realtime_clock_step_(0, 0), - rostime_(0, 0) - { - clock_publisher_ = this->create_publisher("clock", 10); - realtime_clock_step_ = - rclcpp::Duration::from_seconds(simulated_clock_step / realtime_update_rate); - ros_update_duration_ = rclcpp::Duration::from_seconds(simulated_clock_step); - - timer_thread_ = std::thread(&ClockPublisher::RunTimer, this); - } - - ~ClockPublisher() - { - running_ = false; - if (timer_thread_.joinable()) { - timer_thread_.join(); - } - } - - void sleep_for(rclcpp::Duration duration) - { - rclcpp::Time start_time(0, 0, RCL_ROS_TIME); - { - const std::lock_guard lock(mutex_); - start_time = rostime_; - } - rclcpp::Time current_time = start_time; - - while (true) { - { - const std::lock_guard lock(mutex_); - current_time = rostime_; - } - if ((current_time - start_time) >= duration) { - return; - } - std::this_thread::sleep_for(realtime_clock_step_.to_chrono()); - rostime_ += ros_update_duration_; - } - } - -private: - void RunTimer() - { - while (running_) { - PublishClock(); - std::this_thread::sleep_for(realtime_clock_step_.to_chrono()); - } - } - - void PublishClock() - { - const std::lock_guard lock(mutex_); - auto message = rosgraph_msgs::msg::Clock(); - message.clock = rostime_; - clock_publisher_->publish(message); - } - - rclcpp::Publisher::SharedPtr clock_publisher_; - - rclcpp::Duration ros_update_duration_; - rclcpp::Duration realtime_clock_step_; - // Rostime must be guarded by a mutex, since accessible in running thread - // as well as sleep_for - rclcpp::Time rostime_; - std::mutex mutex_; - std::thread timer_thread_; - std::atomic running_ = true; -}; - - -template -class TestTimerCancelBehavior : public ::testing::Test -{ -public: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - static void TearDownTestCase() - { - rclcpp::shutdown(); - } - - void SetUp() - { - const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); - std::stringstream test_name; - test_name << test_info->test_case_name() << "_" << test_info->name(); - node = std::make_shared(test_name.str()); - param_client = std::make_shared(node); - ASSERT_TRUE(param_client->wait_for_service(5s)); - - auto set_parameters_results = param_client->set_parameters( - {rclcpp::Parameter("use_sim_time", false)}); - for (auto & result : set_parameters_results) { - ASSERT_TRUE(result.successful); - } - - // Run standalone thread to publish clock time - sim_clock_node = std::make_shared(); - - // Spin the executor in a standalone thread - executor.add_node(this->node); - standalone_thread = std::thread( - [this]() { - executor.spin(); - }); - } - - void TearDown() - { - node.reset(); - - // Clean up thread object - if (standalone_thread.joinable()) { - standalone_thread.join(); - } - } - - std::shared_ptr node; - std::shared_ptr sim_clock_node; - rclcpp::SyncParametersClient::SharedPtr param_client; - std::thread standalone_thread; - T executor; -}; - -TYPED_TEST_SUITE(TestTimerCancelBehavior, ExecutorTypes, ExecutorTypeNames); - -TYPED_TEST(TestTimerCancelBehavior, testTimer1CancelledWithExecutorSpin) { - // Validate that cancelling one timer yields no change in behavior for other - // timers. Specifically, this tests the behavior when using spin() to run the - // executor, which is the most common usecase. - - // Cancel to stop the spin after some time. - this->sim_clock_node->sleep_for(50ms); - this->node->CancelTimer1(); - this->sim_clock_node->sleep_for(150ms); - this->executor.cancel(); - - int t1_runs = this->node->GetTimer1Cnt(); - int t2_runs = this->node->GetTimer2Cnt(); - EXPECT_NE(t1_runs, t2_runs); - // Check that t2 has significantly more calls - EXPECT_LT(t1_runs + 50, t2_runs); -} - -TYPED_TEST(TestTimerCancelBehavior, testTimer2CancelledWithExecutorSpin) { - // Validate that cancelling one timer yields no change in behavior for other - // timers. Specifically, this tests the behavior when using spin() to run the - // executor, which is the most common usecase. - - // Cancel to stop the spin after some time. - this->sim_clock_node->sleep_for(50ms); - this->node->CancelTimer2(); - this->sim_clock_node->sleep_for(150ms); - this->executor.cancel(); - - int t1_runs = this->node->GetTimer1Cnt(); - int t2_runs = this->node->GetTimer2Cnt(); - EXPECT_NE(t1_runs, t2_runs); - // Check that t1 has significantly more calls - EXPECT_LT(t2_runs + 50, t1_runs); -} - -TYPED_TEST(TestTimerCancelBehavior, testHeadTimerCancelThenResetBehavior) { - // Validate that cancelling timer doesn't affect operation of other timers, - // and that the cancelled timer starts executing normally once reset manually. - - // Cancel to stop the spin after some time. - this->sim_clock_node->sleep_for(50ms); - this->node->CancelTimer1(); - this->sim_clock_node->sleep_for(150ms); - int t1_runs_initial = this->node->GetTimer1Cnt(); - int t2_runs_initial = this->node->GetTimer2Cnt(); - - // Manually reset timer 1, then sleep again - // Counts should update. - this->node->ResetTimer1(); - this->sim_clock_node->sleep_for(150ms); - int t1_runs_final = this->node->GetTimer1Cnt(); - int t2_runs_final = this->node->GetTimer2Cnt(); - - this->executor.cancel(); - - // T1 should have been restarted, and execute about 15 additional times. - // Check 10 greater than initial, to account for some timing jitter. - EXPECT_LT(t1_runs_initial + 50, t1_runs_final); - - EXPECT_LT(t1_runs_initial + 50, t2_runs_initial); - // Check that t2 has significantly more calls, and keeps getting called. - EXPECT_LT(t2_runs_initial + 50, t2_runs_final); -} - -TYPED_TEST(TestTimerCancelBehavior, testBackTimerCancelThenResetBehavior) { - // Validate that cancelling timer doesn't affect operation of other timers, - // and that the cancelled timer starts executing normally once reset manually. - - // Cancel to stop the spin after some time. - this->sim_clock_node->sleep_for(50ms); - this->node->CancelTimer2(); - this->sim_clock_node->sleep_for(150ms); - int t1_runs_initial = this->node->GetTimer1Cnt(); - int t2_runs_initial = this->node->GetTimer2Cnt(); - - // Manually reset timer 1, then sleep again - // Counts should update. - this->node->ResetTimer2(); - this->sim_clock_node->sleep_for(150ms); - int t1_runs_final = this->node->GetTimer1Cnt(); - int t2_runs_final = this->node->GetTimer2Cnt(); - - this->executor.cancel(); - - // T2 should have been restarted, and execute about 15 additional times. - // Check 10 greater than initial, to account for some timing jitter. - EXPECT_LT(t2_runs_initial + 50, t2_runs_final); - - EXPECT_LT(t2_runs_initial + 50, t1_runs_initial); - // Check that t1 has significantly more calls, and keeps getting called. - EXPECT_LT(t1_runs_initial + 50, t1_runs_final); -} - -TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT1Behavior) { - // Validate behavior from cancelling 2 timers, then only re-enabling one of them. - // Ensure that only the reset timer is executed. - - // Cancel to stop the spin after some time. - this->sim_clock_node->sleep_for(50ms); - this->node->CancelTimer1(); - this->node->CancelTimer2(); - this->sim_clock_node->sleep_for(150ms); - int t1_runs_initial = this->node->GetTimer1Cnt(); - int t2_runs_initial = this->node->GetTimer2Cnt(); - - // Manually reset timer 1, then sleep again - // Counts should update. - this->node->ResetTimer1(); - this->sim_clock_node->sleep_for(150ms); - int t1_runs_intermediate = this->node->GetTimer1Cnt(); - int t2_runs_intermediate = this->node->GetTimer2Cnt(); - - this->node->ResetTimer2(); - this->sim_clock_node->sleep_for(150ms); - int t1_runs_final = this->node->GetTimer1Cnt(); - int t2_runs_final = this->node->GetTimer2Cnt(); - - this->executor.cancel(); - - // T1 and T2 should have the same initial count. - EXPECT_LE(std::abs(t1_runs_initial - t2_runs_initial), 1); - - // Expect that T1 has up to 15 more calls than t2. Add some buffer - // to account for jitter. - EXPECT_EQ(t2_runs_initial, t2_runs_intermediate); - EXPECT_LT(t1_runs_initial + 50, t1_runs_intermediate); - - // Expect that by end of test, both are running properly again. - EXPECT_LT(t1_runs_intermediate + 50, t1_runs_final); - EXPECT_LT(t2_runs_intermediate + 50, t2_runs_final); -} - -TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT2Behavior) { - // Validate behavior from cancelling 2 timers, then only re-enabling one of them. - // Ensure that only the reset timer is executed. - - // Cancel to stop the spin after some time. - this->sim_clock_node->sleep_for(50ms); - this->node->CancelTimer1(); - this->node->CancelTimer2(); - this->sim_clock_node->sleep_for(150ms); - int t1_runs_initial = this->node->GetTimer1Cnt(); - int t2_runs_initial = this->node->GetTimer2Cnt(); - - // Manually reset timer 1, then sleep again - // Counts should update. - this->node->ResetTimer2(); - this->sim_clock_node->sleep_for(150ms); - int t1_runs_intermediate = this->node->GetTimer1Cnt(); - int t2_runs_intermediate = this->node->GetTimer2Cnt(); - - this->node->ResetTimer1(); - this->sim_clock_node->sleep_for(150ms); - int t1_runs_final = this->node->GetTimer1Cnt(); - int t2_runs_final = this->node->GetTimer2Cnt(); - - this->executor.cancel(); - - // T1 and T2 should have the same initial count. - EXPECT_LE(std::abs(t1_runs_initial - t2_runs_initial), 1); - - // Expect that T1 has up to 15 more calls than t2. Add some buffer - // to account for jitter. - EXPECT_EQ(t1_runs_initial, t1_runs_intermediate); - EXPECT_LT(t2_runs_initial + 50, t2_runs_intermediate); - - // Expect that by end of test, both are running properly again. - EXPECT_LT(t1_runs_intermediate + 50, t1_runs_final); - EXPECT_LT(t2_runs_intermediate + 50, t2_runs_final); -} diff --git a/rclcpp/test/rclcpp/executors/test_executors_intraprocess.cpp b/rclcpp/test/rclcpp/executors/test_executors_intraprocess.cpp new file mode 100644 index 0000000000..af5f7e432e --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executors_intraprocess.cpp @@ -0,0 +1,125 @@ +// Copyright 2024 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. + +#include + +#include + +#include "rclcpp/node.hpp" +#include "rclcpp/utilities.hpp" + +#include "test_msgs/msg/empty.hpp" + +#include "./executor_types.hpp" + +template +class TestIntraprocessExecutors : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); + std::stringstream test_name; + test_name << test_info->test_case_name() << "_" << test_info->name(); + node = std::make_shared("node", test_name.str()); + + callback_count = 0u; + + const std::string topic_name = std::string("topic_") + test_name.str(); + + rclcpp::PublisherOptions po; + po.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + publisher = node->create_publisher(topic_name, rclcpp::QoS(1), po); + + auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) { + this->callback_count.fetch_add(1u); + }; + + rclcpp::SubscriptionOptions so; + so.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + subscription = + node->create_subscription( + topic_name, rclcpp::QoS(kNumMessages), std::move(callback), so); + } + + void TearDown() + { + publisher.reset(); + subscription.reset(); + node.reset(); + } + + const size_t kNumMessages = 100; + + rclcpp::Node::SharedPtr node; + rclcpp::Publisher::SharedPtr publisher; + rclcpp::Subscription::SharedPtr subscription; + std::atomic_size_t callback_count; +}; + +TYPED_TEST_SUITE(TestIntraprocessExecutors, ExecutorTypes, ExecutorTypeNames); + +TYPED_TEST(TestIntraprocessExecutors, testIntraprocessRetrigger) { + // This tests that executors will continue to service intraprocess subscriptions in the case + // that publishers aren't continuing to publish. + // This was previously broken in that intraprocess guard conditions were only triggered on + // publish and the test was added to prevent future regressions. + static constexpr size_t kNumMessages = 100; + + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + EXPECT_EQ(0u, this->callback_count.load()); + this->publisher->publish(test_msgs::msg::Empty()); + + // Wait for up to 5 seconds for the first message to come available. + const std::chrono::milliseconds sleep_per_loop(10); + int loops = 0; + while (1u != this->callback_count.load() && loops < 500) { + rclcpp::sleep_for(sleep_per_loop); + executor.spin_some(); + loops++; + } + EXPECT_EQ(1u, this->callback_count.load()); + + // reset counter + this->callback_count.store(0u); + + for (size_t ii = 0; ii < kNumMessages; ++ii) { + this->publisher->publish(test_msgs::msg::Empty()); + } + + // Fire a timer every 10ms up to 5 seconds waiting for subscriptions to be read. + loops = 0; + auto timer = this->node->create_wall_timer( + std::chrono::milliseconds(10), [this, &executor, &loops]() { + loops++; + if (kNumMessages == this->callback_count.load() || loops == 500) { + executor.cancel(); + } + }); + executor.spin(); + EXPECT_EQ(kNumMessages, this->callback_count.load()); +} diff --git a/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp new file mode 100644 index 0000000000..ecee459a19 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp @@ -0,0 +1,408 @@ +// Copyright 2024 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. + +#include + +#include +#include +#include +#include +#include + +#include "rclcpp/node.hpp" +#include "rclcpp/parameter_client.hpp" +#include "rclcpp/utilities.hpp" + +#include "rosgraph_msgs/msg/clock.hpp" + +#include "./executor_types.hpp" + +using namespace std::chrono_literals; + +class TimerNode : public rclcpp::Node +{ +public: + explicit TimerNode(std::string subname) + : Node("timer_node", subname) + { + timer1_ = rclcpp::create_timer( + this->get_node_base_interface(), get_node_timers_interface(), + get_clock(), 1ms, + std::bind(&TimerNode::Timer1Callback, this)); + + timer2_ = + rclcpp::create_timer( + this->get_node_base_interface(), get_node_timers_interface(), + get_clock(), 1ms, + std::bind(&TimerNode::Timer2Callback, this)); + } + + int GetTimer1Cnt() {return cnt1_;} + int GetTimer2Cnt() {return cnt2_;} + + void ResetTimer1() + { + timer1_->reset(); + } + + void ResetTimer2() + { + timer2_->reset(); + } + + void CancelTimer1() + { + RCLCPP_DEBUG(this->get_logger(), "Timer 1 cancelling!"); + timer1_->cancel(); + } + + void CancelTimer2() + { + RCLCPP_DEBUG(this->get_logger(), "Timer 2 cancelling!"); + timer2_->cancel(); + } + +private: + void Timer1Callback() + { + RCLCPP_DEBUG(this->get_logger(), "Timer 1!"); + cnt1_++; + } + + void Timer2Callback() + { + RCLCPP_DEBUG(this->get_logger(), "Timer 2!"); + cnt2_++; + } + + rclcpp::TimerBase::SharedPtr timer1_; + rclcpp::TimerBase::SharedPtr timer2_; + int cnt1_ = 0; + int cnt2_ = 0; +}; + +// Sets up a separate thread to publish /clock messages. +// Clock rate relative to real clock is controlled by realtime_update_rate. +// This is set conservatively slow to ensure unit tests are reliable on Windows +// environments, where timing performance is subpar. +// +// Use `sleep_for` in tests to advance the clock. Clock should run and be published +// in separate thread continuously to ensure correct behavior in node under test. +class ClockPublisher : public rclcpp::Node +{ +public: + explicit ClockPublisher(float simulated_clock_step = .001f, float realtime_update_rate = 0.25f) + : Node("clock_publisher"), + ros_update_duration_(0, 0), + realtime_clock_step_(0, 0), + rostime_(0, 0) + { + clock_publisher_ = this->create_publisher("clock", 10); + realtime_clock_step_ = + rclcpp::Duration::from_seconds(simulated_clock_step / realtime_update_rate); + ros_update_duration_ = rclcpp::Duration::from_seconds(simulated_clock_step); + + timer_thread_ = std::thread(&ClockPublisher::RunTimer, this); + } + + ~ClockPublisher() + { + running_ = false; + if (timer_thread_.joinable()) { + timer_thread_.join(); + } + } + + void sleep_for(rclcpp::Duration duration) + { + rclcpp::Time start_time(0, 0, RCL_ROS_TIME); + { + const std::lock_guard lock(mutex_); + start_time = rostime_; + } + rclcpp::Time current_time = start_time; + + while (true) { + { + const std::lock_guard lock(mutex_); + current_time = rostime_; + } + if ((current_time - start_time) >= duration) { + return; + } + std::this_thread::sleep_for(realtime_clock_step_.to_chrono()); + rostime_ += ros_update_duration_; + } + } + +private: + void RunTimer() + { + while (running_) { + PublishClock(); + std::this_thread::sleep_for(realtime_clock_step_.to_chrono()); + } + } + + void PublishClock() + { + const std::lock_guard lock(mutex_); + auto message = rosgraph_msgs::msg::Clock(); + message.clock = rostime_; + clock_publisher_->publish(message); + } + + rclcpp::Publisher::SharedPtr clock_publisher_; + + rclcpp::Duration ros_update_duration_; + rclcpp::Duration realtime_clock_step_; + // Rostime must be guarded by a mutex, since accessible in running thread + // as well as sleep_for + rclcpp::Time rostime_; + std::mutex mutex_; + std::thread timer_thread_; + std::atomic running_ = true; +}; + + +template +class TestTimerCancelBehavior : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); + std::stringstream test_name; + test_name << test_info->test_case_name() << "_" << test_info->name(); + node = std::make_shared(test_name.str()); + param_client = std::make_shared(node); + ASSERT_TRUE(param_client->wait_for_service(5s)); + + auto set_parameters_results = param_client->set_parameters( + {rclcpp::Parameter("use_sim_time", false)}); + for (auto & result : set_parameters_results) { + ASSERT_TRUE(result.successful); + } + + // Run standalone thread to publish clock time + sim_clock_node = std::make_shared(); + + // Spin the executor in a standalone thread + executor.add_node(this->node); + standalone_thread = std::thread( + [this]() { + executor.spin(); + }); + } + + void TearDown() + { + node.reset(); + + // Clean up thread object + if (standalone_thread.joinable()) { + standalone_thread.join(); + } + } + + std::shared_ptr node; + std::shared_ptr sim_clock_node; + rclcpp::SyncParametersClient::SharedPtr param_client; + std::thread standalone_thread; + T executor; +}; + +TYPED_TEST_SUITE(TestTimerCancelBehavior, ExecutorTypes, ExecutorTypeNames); + +TYPED_TEST(TestTimerCancelBehavior, testTimer1CancelledWithExecutorSpin) { + // Validate that cancelling one timer yields no change in behavior for other + // timers. Specifically, this tests the behavior when using spin() to run the + // executor, which is the most common usecase. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer1(); + this->sim_clock_node->sleep_for(150ms); + this->executor.cancel(); + + int t1_runs = this->node->GetTimer1Cnt(); + int t2_runs = this->node->GetTimer2Cnt(); + EXPECT_NE(t1_runs, t2_runs); + // Check that t2 has significantly more calls + EXPECT_LT(t1_runs + 50, t2_runs); +} + +TYPED_TEST(TestTimerCancelBehavior, testTimer2CancelledWithExecutorSpin) { + // Validate that cancelling one timer yields no change in behavior for other + // timers. Specifically, this tests the behavior when using spin() to run the + // executor, which is the most common usecase. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer2(); + this->sim_clock_node->sleep_for(150ms); + this->executor.cancel(); + + int t1_runs = this->node->GetTimer1Cnt(); + int t2_runs = this->node->GetTimer2Cnt(); + EXPECT_NE(t1_runs, t2_runs); + // Check that t1 has significantly more calls + EXPECT_LT(t2_runs + 50, t1_runs); +} + +TYPED_TEST(TestTimerCancelBehavior, testHeadTimerCancelThenResetBehavior) { + // Validate that cancelling timer doesn't affect operation of other timers, + // and that the cancelled timer starts executing normally once reset manually. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer1(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_initial = this->node->GetTimer1Cnt(); + int t2_runs_initial = this->node->GetTimer2Cnt(); + + // Manually reset timer 1, then sleep again + // Counts should update. + this->node->ResetTimer1(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_final = this->node->GetTimer1Cnt(); + int t2_runs_final = this->node->GetTimer2Cnt(); + + this->executor.cancel(); + + // T1 should have been restarted, and execute about 15 additional times. + // Check 10 greater than initial, to account for some timing jitter. + EXPECT_LT(t1_runs_initial + 50, t1_runs_final); + + EXPECT_LT(t1_runs_initial + 50, t2_runs_initial); + // Check that t2 has significantly more calls, and keeps getting called. + EXPECT_LT(t2_runs_initial + 50, t2_runs_final); +} + +TYPED_TEST(TestTimerCancelBehavior, testBackTimerCancelThenResetBehavior) { + // Validate that cancelling timer doesn't affect operation of other timers, + // and that the cancelled timer starts executing normally once reset manually. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_initial = this->node->GetTimer1Cnt(); + int t2_runs_initial = this->node->GetTimer2Cnt(); + + // Manually reset timer 1, then sleep again + // Counts should update. + this->node->ResetTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_final = this->node->GetTimer1Cnt(); + int t2_runs_final = this->node->GetTimer2Cnt(); + + this->executor.cancel(); + + // T2 should have been restarted, and execute about 15 additional times. + // Check 10 greater than initial, to account for some timing jitter. + EXPECT_LT(t2_runs_initial + 50, t2_runs_final); + + EXPECT_LT(t2_runs_initial + 50, t1_runs_initial); + // Check that t1 has significantly more calls, and keeps getting called. + EXPECT_LT(t1_runs_initial + 50, t1_runs_final); +} + +TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT1Behavior) { + // Validate behavior from cancelling 2 timers, then only re-enabling one of them. + // Ensure that only the reset timer is executed. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer1(); + this->node->CancelTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_initial = this->node->GetTimer1Cnt(); + int t2_runs_initial = this->node->GetTimer2Cnt(); + + // Manually reset timer 1, then sleep again + // Counts should update. + this->node->ResetTimer1(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_intermediate = this->node->GetTimer1Cnt(); + int t2_runs_intermediate = this->node->GetTimer2Cnt(); + + this->node->ResetTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_final = this->node->GetTimer1Cnt(); + int t2_runs_final = this->node->GetTimer2Cnt(); + + this->executor.cancel(); + + // T1 and T2 should have the same initial count. + EXPECT_LE(std::abs(t1_runs_initial - t2_runs_initial), 1); + + // Expect that T1 has up to 15 more calls than t2. Add some buffer + // to account for jitter. + EXPECT_EQ(t2_runs_initial, t2_runs_intermediate); + EXPECT_LT(t1_runs_initial + 50, t1_runs_intermediate); + + // Expect that by end of test, both are running properly again. + EXPECT_LT(t1_runs_intermediate + 50, t1_runs_final); + EXPECT_LT(t2_runs_intermediate + 50, t2_runs_final); +} + +TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT2Behavior) { + // Validate behavior from cancelling 2 timers, then only re-enabling one of them. + // Ensure that only the reset timer is executed. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer1(); + this->node->CancelTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_initial = this->node->GetTimer1Cnt(); + int t2_runs_initial = this->node->GetTimer2Cnt(); + + // Manually reset timer 1, then sleep again + // Counts should update. + this->node->ResetTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_intermediate = this->node->GetTimer1Cnt(); + int t2_runs_intermediate = this->node->GetTimer2Cnt(); + + this->node->ResetTimer1(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_final = this->node->GetTimer1Cnt(); + int t2_runs_final = this->node->GetTimer2Cnt(); + + this->executor.cancel(); + + // T1 and T2 should have the same initial count. + EXPECT_LE(std::abs(t1_runs_initial - t2_runs_initial), 1); + + // Expect that T1 has up to 15 more calls than t2. Add some buffer + // to account for jitter. + EXPECT_EQ(t1_runs_initial, t1_runs_intermediate); + EXPECT_LT(t2_runs_initial + 50, t2_runs_intermediate); + + // Expect that by end of test, both are running properly again. + EXPECT_LT(t1_runs_intermediate + 50, t1_runs_final); + EXPECT_LT(t2_runs_intermediate + 50, t2_runs_final); +} From e1f84baa11f8389e1dd9fbf41a7bf4752f25b09c Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 7 Feb 2024 13:50:55 +0000 Subject: [PATCH 331/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 8 ++++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 3 +++ 4 files changed, 17 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index de1429e204..2bc5bd1b3d 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Split test_executors up into smaller chunks. (`#2421 `_) +* [events executor] - Fix Behavior with Timer Cancel (`#2375 `_) +* Removed deprecated header (`#2413 `_) +* Make sure to mark RingBuffer methods as 'override'. (`#2410 `_) +* Contributors: Alejandro Hernández Cordero, Chris Lalancette, Matt Condino + 26.0.0 (2024-01-24) ------------------- * Increase the cppcheck timeout to 600 seconds. (`#2409 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index a539fa42d0..68037b1cff 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 26.0.0 (2024-01-24) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 3a4cc798df..fb8c51faeb 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 26.0.0 (2024-01-24) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 5513040a7c..1080a6d8ee 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 26.0.0 (2024-01-24) ------------------- * Increase timeout for rclcpp_lifecycle to 360 (`#2395 `_) From c500695e21dd186981533bb6fb84624e4c4f752c Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 7 Feb 2024 13:51:05 +0000 Subject: [PATCH 332/455] 27.0.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 2bc5bd1b3d..8a520a706e 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +27.0.0 (2024-02-07) +------------------- * Split test_executors up into smaller chunks. (`#2421 `_) * [events executor] - Fix Behavior with Timer Cancel (`#2375 `_) * Removed deprecated header (`#2413 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index c1b28df8fe..867e0a1bf2 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 26.0.0 + 27.0.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 68037b1cff..f2ab3fdb20 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +27.0.0 (2024-02-07) +------------------- 26.0.0 (2024-01-24) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index f9cf67ea33..7ebf6e0efe 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 26.0.0 + 27.0.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index fb8c51faeb..23689424e8 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +27.0.0 (2024-02-07) +------------------- 26.0.0 (2024-01-24) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index a4411a0a65..dbb3e720c6 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 26.0.0 + 27.0.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 1080a6d8ee..a19f4ee85d 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +27.0.0 (2024-02-07) +------------------- 26.0.0 (2024-01-24) ------------------- diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index daf0ca8c20..497aa259f6 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 26.0.0 + 27.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 38bcda4b765e3037d4ac7c4cbd1d265c4e75b883 Mon Sep 17 00:00:00 2001 From: HuaTsai Date: Wed, 14 Feb 2024 03:08:16 +0800 Subject: [PATCH 333/455] feat: add/minus for msg::Time and rclcpp::Duration (#2419) * feat: add/minus for msg::Time and rclcpp::Duration Signed-off-by: HuaTsai --- rclcpp/include/rclcpp/duration.hpp | 9 ++++++ rclcpp/include/rclcpp/time.hpp | 9 ++++++ rclcpp/src/rclcpp/duration.cpp | 48 ++++++++++++++++++++++++++++ rclcpp/src/rclcpp/time.cpp | 27 +++++++++------- rclcpp/test/rclcpp/test_duration.cpp | 29 +++++++++++++++++ 5 files changed, 111 insertions(+), 11 deletions(-) diff --git a/rclcpp/include/rclcpp/duration.hpp b/rclcpp/include/rclcpp/duration.hpp index 537e2a9bf6..6b48f441e3 100644 --- a/rclcpp/include/rclcpp/duration.hpp +++ b/rclcpp/include/rclcpp/duration.hpp @@ -18,6 +18,7 @@ #include #include "builtin_interfaces/msg/duration.hpp" +#include "builtin_interfaces/msg/time.hpp" #include "rcl/time.h" #include "rclcpp/visibility_control.hpp" @@ -158,6 +159,14 @@ class RCLCPP_PUBLIC Duration Duration() = default; }; +RCLCPP_PUBLIC +builtin_interfaces::msg::Time +operator+(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs); + +RCLCPP_PUBLIC +builtin_interfaces::msg::Time +operator-(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs); + } // namespace rclcpp #endif // RCLCPP__DURATION_HPP_ diff --git a/rclcpp/include/rclcpp/time.hpp b/rclcpp/include/rclcpp/time.hpp index b6aee4c972..15e063d1d2 100644 --- a/rclcpp/include/rclcpp/time.hpp +++ b/rclcpp/include/rclcpp/time.hpp @@ -222,6 +222,15 @@ RCLCPP_PUBLIC Time operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs); +/// Convert rcl_time_point_value_t to builtin_interfaces::msg::Time +/** + * \param[in] time_point is a rcl_time_point_value_t + * \return the builtin_interfaces::msg::Time from the time_point + */ +RCLCPP_PUBLIC +builtin_interfaces::msg::Time +convert_rcl_time_to_sec_nanos(const rcl_time_point_value_t & time_point); + } // namespace rclcpp #endif // RCLCPP__TIME_HPP_ diff --git a/rclcpp/src/rclcpp/duration.cpp b/rclcpp/src/rclcpp/duration.cpp index 2eac7f6b58..7cb678456e 100644 --- a/rclcpp/src/rclcpp/duration.cpp +++ b/rclcpp/src/rclcpp/duration.cpp @@ -28,6 +28,8 @@ #include "rcutils/logging_macros.h" +#include "rclcpp/utilities.hpp" + namespace rclcpp { @@ -316,4 +318,50 @@ Duration::from_nanoseconds(rcl_duration_value_t nanoseconds) return ret; } +builtin_interfaces::msg::Time +operator+(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs) +{ + if (lhs.sec < 0) { + throw std::runtime_error("message time is negative"); + } + + rcl_time_point_value_t rcl_time; + rcl_time = RCL_S_TO_NS(static_cast(lhs.sec)); + rcl_time += lhs.nanosec; + + if (rclcpp::add_will_overflow(rcl_time, rhs.nanoseconds())) { + throw std::overflow_error("addition leads to int64_t overflow"); + } + if (rclcpp::add_will_underflow(rcl_time, rhs.nanoseconds())) { + throw std::underflow_error("addition leads to int64_t underflow"); + } + + rcl_time += rhs.nanoseconds(); + + return convert_rcl_time_to_sec_nanos(rcl_time); +} + +builtin_interfaces::msg::Time +operator-(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs) +{ + if (lhs.sec < 0) { + throw std::runtime_error("message time is negative"); + } + + rcl_time_point_value_t rcl_time; + rcl_time = RCL_S_TO_NS(static_cast(lhs.sec)); + rcl_time += lhs.nanosec; + + if (rclcpp::sub_will_overflow(rcl_time, rhs.nanoseconds())) { + throw std::overflow_error("addition leads to int64_t overflow"); + } + if (rclcpp::sub_will_underflow(rcl_time, rhs.nanoseconds())) { + throw std::underflow_error("addition leads to int64_t underflow"); + } + + rcl_time -= rhs.nanoseconds(); + + return convert_rcl_time_to_sec_nanos(rcl_time); +} + } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/time.cpp b/rclcpp/src/rclcpp/time.cpp index e5ff2af8d0..a0be141312 100644 --- a/rclcpp/src/rclcpp/time.cpp +++ b/rclcpp/src/rclcpp/time.cpp @@ -90,17 +90,7 @@ Time::~Time() Time::operator builtin_interfaces::msg::Time() const { - builtin_interfaces::msg::Time msg_time; - constexpr rcl_time_point_value_t kRemainder = RCL_S_TO_NS(1); - const auto result = std::div(rcl_time_.nanoseconds, kRemainder); - if (result.rem >= 0) { - msg_time.sec = static_cast(result.quot); - msg_time.nanosec = static_cast(result.rem); - } else { - msg_time.sec = static_cast(result.quot - 1); - msg_time.nanosec = static_cast(kRemainder + result.rem); - } - return msg_time; + return convert_rcl_time_to_sec_nanos(rcl_time_.nanoseconds); } Time & @@ -281,5 +271,20 @@ Time::max(rcl_clock_type_t clock_type) return Time(std::numeric_limits::max(), 999999999, clock_type); } +builtin_interfaces::msg::Time +convert_rcl_time_to_sec_nanos(const rcl_time_point_value_t & time_point) +{ + builtin_interfaces::msg::Time ret; + constexpr rcl_time_point_value_t kRemainder = RCL_S_TO_NS(1); + const auto result = std::div(time_point, kRemainder); + if (result.rem >= 0) { + ret.sec = static_cast(result.quot); + ret.nanosec = static_cast(result.rem); + } else { + ret.sec = static_cast(result.quot - 1); + ret.nanosec = static_cast(kRemainder + result.rem); + } + return ret; +} } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/test_duration.cpp b/rclcpp/test/rclcpp/test_duration.cpp index d5908c7d4b..2347514d7a 100644 --- a/rclcpp/test/rclcpp/test_duration.cpp +++ b/rclcpp/test/rclcpp/test_duration.cpp @@ -79,6 +79,35 @@ TEST_F(TestDuration, operators) { EXPECT_TRUE(time == assignment_op_duration); } +TEST_F(TestDuration, operators_with_message_stamp) { + builtin_interfaces::msg::Time time_msg = rclcpp::Time(0, 100000000u); // 0.1s + rclcpp::Duration pos_duration(1, 100000000u); // 1.1s + rclcpp::Duration neg_duration(-2, 900000000u); // -1.1s + + builtin_interfaces::msg::Time res_addpos = time_msg + pos_duration; + EXPECT_EQ(res_addpos.sec, 1); + EXPECT_EQ(res_addpos.nanosec, 200000000u); + + builtin_interfaces::msg::Time res_addneg = time_msg + neg_duration; + EXPECT_EQ(res_addneg.sec, -1); + EXPECT_EQ(res_addneg.nanosec, 0); + + builtin_interfaces::msg::Time res_subpos = time_msg - pos_duration; + EXPECT_EQ(res_subpos.sec, -1); + EXPECT_EQ(res_subpos.nanosec, 0); + + builtin_interfaces::msg::Time res_subneg = time_msg - neg_duration; + EXPECT_EQ(res_subneg.sec, 1); + EXPECT_EQ(res_subneg.nanosec, 200000000u); + + builtin_interfaces::msg::Time neg_time_msg; + neg_time_msg.sec = -1; + auto max = rclcpp::Duration::from_nanoseconds(std::numeric_limits::max()); + + EXPECT_THROW(neg_time_msg + max, std::runtime_error); + EXPECT_THROW(time_msg + max, std::overflow_error); +} + TEST_F(TestDuration, chrono_overloads) { int64_t ns = 123456789l; auto chrono_ns = std::chrono::nanoseconds(ns); From 091f29fcd38461046520e3f5cabd396777df88f6 Mon Sep 17 00:00:00 2001 From: Adam Aposhian Date: Tue, 13 Feb 2024 15:18:15 -0700 Subject: [PATCH 334/455] crash on no class found (#2415) * crash on no class found * error on no class found instead of no callback groups Signed-off-by: Adam Aposhian Co-authored-by: Chris Lalancette --- rclcpp_components/src/node_main.cpp.in | 56 ++++++++++++++------------ 1 file changed, 31 insertions(+), 25 deletions(-) diff --git a/rclcpp_components/src/node_main.cpp.in b/rclcpp_components/src/node_main.cpp.in index d2a0e84601..7d621aac9f 100644 --- a/rclcpp_components/src/node_main.cpp.in +++ b/rclcpp_components/src/node_main.cpp.in @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -33,41 +34,46 @@ int main(int argc, char * argv[]) @executor@ exec; rclcpp::NodeOptions options; options.arguments(args); - std::vector node_wrappers; std::string library_name = "@library_name@"; std::string class_name = "rclcpp_components::NodeFactoryTemplate<@component@>"; RCLCPP_DEBUG(logger, "Load library %s", library_name.c_str()); auto loader = std::make_unique(library_name); - auto classes = loader->getAvailableClasses(); - for (const auto & clazz : classes) { - std::string name = clazz.c_str(); - if (name.compare(class_name) == 0) { - RCLCPP_DEBUG(logger, "Instantiate class %s", clazz.c_str()); - std::shared_ptr node_factory = nullptr; - try { - node_factory = loader->createInstance(clazz); - } catch (const std::exception & ex) { - RCLCPP_ERROR(logger, "Failed to load library %s", ex.what()); - return 1; - } catch (...) { - RCLCPP_ERROR(logger, "Failed to load library"); - return 1; - } - auto wrapper = node_factory->create_node_instance(options); - auto node = wrapper.get_node_base_interface(); - node_wrappers.push_back(wrapper); - exec.add_node(node); - } + std::vector classes = loader->getAvailableClasses(); + + if (std::find( + classes.begin(), + classes.end(), + class_name) == classes.end()) { + RCLCPP_ERROR( + logger, + "Class %s not found in library %s", + class_name.c_str(), + library_name.c_str()); + return 1; + } + RCLCPP_DEBUG(logger, "Instantiate class %s", class_name.c_str()); + std::shared_ptr node_factory = nullptr; + try { + node_factory = loader->createInstance(class_name); + } catch (const std::exception & ex) { + RCLCPP_ERROR(logger, "Failed to load library %s", ex.what()); + return 1; + } catch (...) { + RCLCPP_ERROR(logger, "Failed to load library"); + return 1; } + // Scope to destruct node_wrapper before shutdown + { + rclcpp_components::NodeInstanceWrapper node_wrapper = node_factory->create_node_instance(options); + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node = node_wrapper.get_node_base_interface(); + exec.add_node(node); - exec.spin(); + exec.spin(); - for (auto wrapper : node_wrappers) { - exec.remove_node(wrapper.get_node_base_interface()); + exec.remove_node(node_wrapper.get_node_base_interface()); } - node_wrappers.clear(); rclcpp::shutdown(); From b4571076a616037c7104393d7dec7e886842ced7 Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Fri, 16 Feb 2024 05:30:53 -0800 Subject: [PATCH 335/455] Update quality declaration documents (#2427) Signed-off-by: Christophe Bedard --- rclcpp/QUALITY_DECLARATION.md | 24 ++++++++++++------------ rclcpp_action/QUALITY_DECLARATION.md | 14 +++++++------- rclcpp_components/QUALITY_DECLARATION.md | 16 ++++++++-------- rclcpp_lifecycle/QUALITY_DECLARATION.md | 18 +++++++++--------- 4 files changed, 36 insertions(+), 36 deletions(-) diff --git a/rclcpp/QUALITY_DECLARATION.md b/rclcpp/QUALITY_DECLARATION.md index e0913167c2..53c2908f7c 100644 --- a/rclcpp/QUALITY_DECLARATION.md +++ b/rclcpp/QUALITY_DECLARATION.md @@ -4,7 +4,7 @@ This document is a declaration of software quality for the `rclcpp` package, bas The package `rclcpp` claims to be in the **Quality Level 1** category when it is used with a **Quality Level 1** middleware. -Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide. +Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Requirements for Quality Level 1 in REP-2004](https://www.ros.org/reps/rep-2004.html). ## Version Policy [1] @@ -94,7 +94,7 @@ There is an automated test which runs a linter that ensures each file has at lea ### Feature Testing [4.i] -Each feature in `rclcpp` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp/tree/master/test) directory. +Each feature in `rclcpp` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp/tree/rolling/test) directory. New features are required to have tests before being added. Currently nightly test results can be seen here: @@ -129,7 +129,7 @@ Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly `rclcpp` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#performance), and opts to do performance analysis on each release rather than each change. -The performance tests of `rclcpp` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/master/rclcpp/test/benchmark). +The performance tests of `rclcpp` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/rolling/rclcpp/test/benchmark). System level performance benchmarks that cover features of `rclcpp` can be found at: * [Benchmarks](http://build.ros2.org/view/Rci/job/Rci__benchmark_ubuntu_focal_amd64/BenchmarkTable/) @@ -139,7 +139,7 @@ Changes that introduce regressions in performance must be adequately justified i ### Linters and Static Analysis [4.v] -`rclcpp` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms. +`rclcpp` uses and passes all the ROS 2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms. Currently nightly test results can be seen here: * [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp/) @@ -163,49 +163,49 @@ It also has several test dependencies, which do not affect the resulting quality The `libstatistics_collector` package provides lightweight aggregation utilities to collect statistics and measure message metrics. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros-tooling/libstatistics_collector/tree/master/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros-tooling/libstatistics_collector/tree/rolling/QUALITY_DECLARATION.md). #### `rcl` `rcl` a library to support implementation of language specific ROS 2 Client Libraries. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/rolling/rcl/QUALITY_DECLARATION.md). #### `rcl_yaml_param_parser` The `rcl_yaml_param_parser` package provides an API that is used to parse YAML configuration files which may be used to configure ROS and specific nodes. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/tree/master/rcl_yaml_param_parser/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/tree/rolling/rcl_yaml_param_parser/QUALITY_DECLARATION.md). #### `rcpputils` The `rcpputils` package provides an API which contains common utilities and data structures useful when programming in C++. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/master/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/rolling/QUALITY_DECLARATION.md). #### `rcutils` The `rcutils` package provides an API which contains common utilities and data structures useful when programming in C. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcutils/blob/master/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcutils/blob/rolling/QUALITY_DECLARATION.md). #### `rmw` `rmw` is the ROS 2 middleware library. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/master/rmw/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/rolling/rmw/QUALITY_DECLARATION.md). #### `statistics_msgs` The `statistics_msgs` package contains ROS 2 message definitions for reporting statistics for topics and system resources. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/statistics_msgs/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/rolling/statistics_msgs/QUALITY_DECLARATION.md). #### `tracetools` The `tracetools` package provides utilities for instrumenting the code in `rclcpp` so that it may be traced for debugging and performance analysis. -It is **Quality Level 1**, see its [Quality Declaration document](https://gitlab.com/ros-tracing/ros2_tracing/-/blob/master/tracetools/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/ros2_tracing/blob/rolling/tracetools/QUALITY_DECLARATION.md). ### Direct Runtime non-ROS Dependency [5.iii] diff --git a/rclcpp_action/QUALITY_DECLARATION.md b/rclcpp_action/QUALITY_DECLARATION.md index 7512ad3312..5812429610 100644 --- a/rclcpp_action/QUALITY_DECLARATION.md +++ b/rclcpp_action/QUALITY_DECLARATION.md @@ -4,7 +4,7 @@ This document is a declaration of software quality for the `rclcpp_action` packa The package `rclcpp_action` claims to be in the **Quality Level 1** category when it is used with a **Quality Level 1** middleware. -Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide. +Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Requirements for Quality Level 1 in REP-2004](https://www.ros.org/reps/rep-2004.html). ## Version Policy [1] @@ -92,7 +92,7 @@ There is an automated test which runs a linter that ensures each file has at lea ### Feature Testing [4.i] -Each feature in `rclcpp_action` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp_action/tree/master/test) directory. +Each feature in `rclcpp_action` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp_action/tree/rolling/test) directory. New features are required to have tests before being added. Currently nightly test results can be seen here: @@ -125,7 +125,7 @@ Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly `rclcpp_action` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#performance), and opts to do performance analysis on each release rather than each change. -The performance tests of `rclcpp_action` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/master/rclcpp_action/test/benchmark). +The performance tests of `rclcpp_action` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/rolling/rclcpp_action/test/benchmark). System level performance benchmarks that cover features of `rclcpp_action` can be found at: * [Benchmarks](http://build.ros2.org/view/Rci/job/Rci__benchmark_ubuntu_focal_amd64/BenchmarkTable/) @@ -135,7 +135,7 @@ Changes that introduce regressions in performance must be adequately justified i ### Linters and Static Analysis [4.v] -`rclcpp_action` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms. +`rclcpp_action` uses and passes all the ROS 2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms. Currently nightly test results can be seen here: * [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp_action/) @@ -159,19 +159,19 @@ It also has several test dependencies, which do not affect the resulting quality `action_msgs` provides messages and services for ROS 2 actions. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/action_msgs/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/rolling/action_msgs/QUALITY_DECLARATION.md). #### `rclcpp` The `rclcpp` package provides the ROS client library in C++. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/rolling/rclcpp/QUALITY_DECLARATION.md). #### `rcl_action` The `rcl_action` package provides C-based ROS action implementation. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl_action/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/rolling/rcl_action/QUALITY_DECLARATION.md). ### Direct Runtime non-ROS Dependency [5.iii] diff --git a/rclcpp_components/QUALITY_DECLARATION.md b/rclcpp_components/QUALITY_DECLARATION.md index 5b8efa934d..2e38c0410a 100644 --- a/rclcpp_components/QUALITY_DECLARATION.md +++ b/rclcpp_components/QUALITY_DECLARATION.md @@ -4,7 +4,7 @@ This document is a declaration of software quality for the `rclcpp_components` p The package `rclcpp_components` claims to be in the **Quality Level 1** category. -Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide. +Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Requirements for Quality Level 1 in REP-2004](https://www.ros.org/reps/rep-2004.html). ## Version Policy [1] @@ -92,7 +92,7 @@ There is an automated test which runs a linter that ensures each file has at lea ### Feature Testing [4.i] -Each feature in `rclcpp_components` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp_components/tree/master/test) directory. +Each feature in `rclcpp_components` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp_components/tree/rolling/test) directory. New features are required to have tests before being added. Currently nightly test results can be seen here: @@ -125,7 +125,7 @@ Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly `rclcpp_components` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#performance), and opts to do performance analysis on each release rather than each change. -The performance tests of `rclcpp_components` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/master/rclcpp_components/test/benchmark). +The performance tests of `rclcpp_components` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/rolling/rclcpp_components/test/benchmark). Package and system level performance benchmarks that cover features of `rclcpp_components` can be found at: * [Benchmarks](http://build.ros2.org/view/Rci/job/Rci__benchmark_ubuntu_focal_amd64/BenchmarkTable/) @@ -135,7 +135,7 @@ Changes that introduce regressions in performance must be adequately justified i ### Linters and Static Analysis [4.v] -`rclcpp_components` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms. +`rclcpp_components` uses and passes all the ROS 2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms. Currently nightly test results can be seen here: * [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp_components/) @@ -159,7 +159,7 @@ It also has several test dependencies, which do not affect the resulting quality The `ament_index_cpp` package provides a C++ API to access the ament resource index. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ament/ament_index/blob/master/ament_index_cpp/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ament/ament_index/blob/rolling/ament_index_cpp/QUALITY_DECLARATION.md). #### `class_loader` @@ -171,19 +171,19 @@ It is **Quality Level 1**, see its [Quality Declaration document](https://github The `composition_interfaces` package contains message and service definitions for managing composable nodes in a container process. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/composition_interfaces/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/rolling/composition_interfaces/QUALITY_DECLARATION.md). #### `rclcpp` The `rclcpp` package provides the ROS client library in C++. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/rolling/rclcpp/QUALITY_DECLARATION.md). #### `rcpputils` The `rcpputils` package provides an API which contains common utilities and data structures useful when programming in C++. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/master/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/rolling/QUALITY_DECLARATION.md). ### Direct Runtime non-ROS Dependency [5.iii] diff --git a/rclcpp_lifecycle/QUALITY_DECLARATION.md b/rclcpp_lifecycle/QUALITY_DECLARATION.md index 5a37ad395b..73be7c6c9a 100644 --- a/rclcpp_lifecycle/QUALITY_DECLARATION.md +++ b/rclcpp_lifecycle/QUALITY_DECLARATION.md @@ -4,7 +4,7 @@ This document is a declaration of software quality for the `rclcpp_lifecycle` pa The package `rclcpp_lifecycle` claims to be in the **Quality Level 1** category when used with a **Quality Level 1** middleware. -Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide. +Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Requirements for Quality Level 1 in REP-2004](https://www.ros.org/reps/rep-2004.html). ## Version Policy [1] @@ -92,7 +92,7 @@ There is an automated test which runs a linter that ensures each file has at lea ### Feature Testing [4.i] -Each feature in `rclcpp_lifecycle` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp_lifecycle/tree/master/test) directory. +Each feature in `rclcpp_lifecycle` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp_lifecycle/tree/rolling/test) directory. New features are required to have tests before being added. Currently nightly test results can be seen here: @@ -125,7 +125,7 @@ Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly `rclcpp_lifecycle` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#performance), and opts to do performance analysis on each release rather than each change. -The performance tests of `rclcpp_lifecycle` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/master/rclcpp_lifecycle/test/benchmark). +The performance tests of `rclcpp_lifecycle` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/rolling/rclcpp_lifecycle/test/benchmark). Package and system level performance benchmarks that cover features of `rclcpp_lifecycle` can be found at: * [Benchmarks](http://build.ros2.org/view/Rci/job/Rci__benchmark_ubuntu_focal_amd64/BenchmarkTable/) @@ -135,7 +135,7 @@ Changes that introduce regressions in performance must be adequately justified i ### Linters and Static Analysis [4.v] -`rclcpp_lifecycle` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms. +`rclcpp_lifecycle` uses and passes all the ROS 2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms. Currently nightly test results can be seen here: * [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp_lifecycle/) @@ -159,31 +159,31 @@ It also has several test dependencies, which do not affect the resulting quality The `lifecycle_msgs` package contains message and service definitions for managing lifecycle nodes. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/lifecycle_msgs/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/rolling/lifecycle_msgs/QUALITY_DECLARATION.md). #### `rclcpp` The `rclcpp` package provides the ROS client library in C++. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/rolling/rclcpp/QUALITY_DECLARATION.md). #### `rcl_lifecycle` The `rcl_lifecycle` package provides functionality for ROS 2 lifecycle nodes in C. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl_lifecycle/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/rolling/rcl_lifecycle/QUALITY_DECLARATION.md). #### `rosidl_typesupport_cpp` The `rosidl_typesupport_cpp` package generates the type support for C++ messages. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rosidl_typesupport/blob/master/rosidl_typesupport_cpp/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rosidl_typesupport/blob/rolling/rosidl_typesupport_cpp/QUALITY_DECLARATION.md). #### `rmw` `rmw` is the ROS 2 middleware library. -It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/master/rmw/QUALITY_DECLARATION.md). +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/rolling/rmw/QUALITY_DECLARATION.md). ### Direct Runtime non-ROS Dependency [5.iii] From 10252e9f66ac87f3903f301b64320d32457f0658 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 16 Feb 2024 14:43:02 -0500 Subject: [PATCH 336/455] Set hints to find the python version we actually want. (#2426) The comment in the commit explains the reasoning behind it. Signed-off-by: Chris Lalancette --- rclcpp/CMakeLists.txt | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index cad17103b7..1f78d03d59 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.12) +cmake_minimum_required(VERSION 3.20) project(rclcpp) @@ -127,6 +127,21 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/waitable.cpp ) +# By default, without the settings below, find_package(Python3) will attempt +# to find the newest python version it can, and additionally will find the +# most specific version. For instance, on a system that has +# /usr/bin/python3.10, /usr/bin/python3.11, and /usr/bin/python3, it will find +# /usr/bin/python3.11, even if /usr/bin/python3 points to /usr/bin/python3.10. +# The behavior we want is to prefer the "system" installed version unless the +# user specifically tells us othewise through the Python3_EXECUTABLE hint. +# Setting CMP0094 to NEW means that the search will stop after the first +# python version is found. Setting Python3_FIND_UNVERSIONED_NAMES means that +# the search will prefer /usr/bin/python3 over /usr/bin/python3.11. And that +# latter functionality is only available in CMake 3.20 or later, so we need +# at least that version. +cmake_policy(SET CMP0094 NEW) +set(Python3_FIND_UNVERSIONED_NAMES FIRST) + find_package(Python3 REQUIRED COMPONENTS Interpreter) # "watch" template for changes From 1981e1f12acfcbdc5d9202cd3e636cb692621971 Mon Sep 17 00:00:00 2001 From: Jonas Otto Date: Tue, 20 Feb 2024 19:09:44 +0100 Subject: [PATCH 337/455] fix doxygen syntax for NodeInterfaces (#2428) Signed-off-by: Jonas Otto --- rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp index a7c7b7af96..80a20fb4a3 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp @@ -127,7 +127,9 @@ class NodeInterfaces * the RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT macro. * * Usage example: - * ```RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)``` + * ```cpp + * RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base) + * ``` * * If you choose not to use the helper macro, then you can specialize the * template yourself, but you must: From e10728cde937e621799997141d5d70bf1e1f497a Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 22 Feb 2024 21:57:29 -0500 Subject: [PATCH 338/455] Remove the set_deprecated signatures in any_subscription_callback. (#2431) These have been deprecated since April 2021, so it is safe to remove them now. Signed-off-by: Chris Lalancette --- .../rclcpp/any_subscription_callback.hpp | 49 +------------------ .../rclcpp/test_any_subscription_callback.cpp | 2 - 2 files changed, 1 insertion(+), 50 deletions(-) diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 9c5e7928e0..da5abe6c53 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -34,7 +34,6 @@ #include "rclcpp/serialized_message.hpp" #include "rclcpp/type_adapter.hpp" - namespace rclcpp { @@ -394,58 +393,12 @@ class AnySubscriptionCallback // converted to one another, e.g. shared_ptr and unique_ptr. using scbth = detail::SubscriptionCallbackTypeHelper; - // Determine if the given CallbackT is a deprecated signature or not. - constexpr auto is_deprecated = - rclcpp::function_traits::same_arguments< - typename scbth::callback_type, - std::function)> - >::value - || - rclcpp::function_traits::same_arguments< - typename scbth::callback_type, - std::function, const rclcpp::MessageInfo &)> - >::value; - - // Use the discovered type to force the type of callback when assigning - // into the variant. - if constexpr (is_deprecated) { - // If deprecated, call sub-routine that is deprecated. - set_deprecated(static_cast(callback)); - } else { - // Otherwise just assign it. - callback_variant_ = static_cast(callback); - } + callback_variant_ = static_cast(callback); // Return copy of self for easier testing, normally will be compiled out. return *this; } - /// Function for shared_ptr to non-const MessageT, which is deprecated. - template -#if !defined(RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS) - // suppress deprecation warnings in `test_any_subscription_callback.cpp` - [[deprecated("use 'void(std::shared_ptr)' instead")]] -#endif - void - set_deprecated(std::function)> callback) - { - callback_variant_ = callback; - } - - /// Function for shared_ptr to non-const MessageT with MessageInfo, which is deprecated. - template -#if !defined(RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS) - // suppress deprecation warnings in `test_any_subscription_callback.cpp` - [[deprecated( - "use 'void(std::shared_ptr, const rclcpp::MessageInfo &)' instead" - )]] -#endif - void - set_deprecated(std::function, const rclcpp::MessageInfo &)> callback) - { - callback_variant_ = callback; - } - std::unique_ptr create_ros_unique_ptr_from_ros_shared_ptr_message( const std::shared_ptr & message) diff --git a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp index 45fe091f07..a98271e931 100644 --- a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp +++ b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp @@ -19,8 +19,6 @@ #include #include -// TODO(aprotyas): Figure out better way to suppress deprecation warnings. -#define RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS 1 #include "rclcpp/any_subscription_callback.hpp" #include "test_msgs/msg/empty.hpp" #include "test_msgs/msg/empty.h" From 64f9aa158b39054d4f94ae24f897d096cc38bea6 Mon Sep 17 00:00:00 2001 From: Ruddick Lawrence <679360+mrjogo@users.noreply.github.com> Date: Thu, 29 Feb 2024 05:01:16 -0800 Subject: [PATCH 339/455] Add EXECUTOR docs (#2440) Signed-off-by: Ruddick Lawrence <679360+mrjogo@users.noreply.github.com> --- rclcpp_components/cmake/rclcpp_components_register_node.cmake | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rclcpp_components/cmake/rclcpp_components_register_node.cmake b/rclcpp_components/cmake/rclcpp_components_register_node.cmake index e78bb3650c..0c5bd33b91 100644 --- a/rclcpp_components/cmake/rclcpp_components_register_node.cmake +++ b/rclcpp_components/cmake/rclcpp_components_register_node.cmake @@ -24,6 +24,8 @@ # :type PLUGIN: string # :param EXECUTABLE: the node's executable name # :type EXECUTABLE: string +# :param EXECUTOR: the C++ class name of the executor to use (blank uses SingleThreadedExecutor) +# :type EXECUTOR: string # :param RESOURCE_INDEX: the ament resource index to register the components # :type RESOURCE_INDEX: string # From 726f2039b524821383873feb95aa82809f0fd72b Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 29 Feb 2024 08:03:43 -0500 Subject: [PATCH 340/455] Various cleanups to deal with uncrustify 0.78. (#2439) These should also work with uncrustify 0.72. Signed-off-by: Chris Lalancette --- .../detail/node_interfaces_helpers.hpp | 4 +- rclcpp/include/rclcpp/subscription.hpp | 2 + rclcpp/src/rclcpp/executor.cpp | 5 +- .../static_executor_entities_collector.cpp | 11 ++- .../test_allocator_memory_strategy.cpp | 12 +-- .../test/rclcpp/test_any_service_callback.cpp | 29 +++---- rclcpp/test/rclcpp/test_memory_strategy.cpp | 12 +-- rclcpp/test/rclcpp/test_service.cpp | 78 +++++++++---------- rclcpp/test/rclcpp/test_timer.cpp | 22 ++---- rclcpp_action/test/test_client.cpp | 22 +++--- 10 files changed, 95 insertions(+), 102 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp b/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp index b999344ca9..a243e9611a 100644 --- a/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp @@ -167,6 +167,7 @@ init_tuple(NodeT & n) * something like that, then you'll need to create your own specialization of * the NodeInterfacesSupports struct without this macro. */ +// *INDENT-OFF* #define RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(NodeInterfaceType, NodeInterfaceName) \ namespace rclcpp::node_interfaces::detail { \ template \ @@ -189,7 +190,7 @@ init_tuple(NodeT & n) /* Perfect forwarding constructor to get arguments down to StorageClassT (eventually). */ \ template \ explicit NodeInterfacesSupports(ArgsT && ... args) \ - : NodeInterfacesSupports( \ + : NodeInterfacesSupports( \ std::forward(args) ...) \ {} \ \ @@ -200,6 +201,7 @@ init_tuple(NodeT & n) } \ }; \ } // namespace rclcpp::node_interfaces::detail +// *INDENT-ON* } // namespace detail } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 58f9a8ce6d..f8a10a5c27 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -127,6 +127,7 @@ class Subscription : public SubscriptionBase * of the following conditions are true: qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL, * qos_profile.depth == 0 or qos_profile.durability != RMW_QOS_POLICY_DURABILITY_VOLATILE). */ + // *INDENT-OFF* Subscription( rclcpp::node_interfaces::NodeBaseInterface * node_base, const rosidl_message_type_support_t & type_support_handle, @@ -148,6 +149,7 @@ class Subscription : public SubscriptionBase any_callback_(callback), options_(options), message_memory_strategy_(message_memory_strategy) + // *INDENT-ON* { // Setup intra process publishing if requested. if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) { diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 4dd0c9a358..416a82be7c 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -95,8 +95,9 @@ Executor::~Executor() } // Disassociate all nodes. std::for_each( - weak_nodes_.begin(), weak_nodes_.end(), [] - (rclcpp::node_interfaces::NodeBaseInterface::WeakPtr weak_node_ptr) { + weak_nodes_.begin(), weak_nodes_.end(), + [](rclcpp::node_interfaces::NodeBaseInterface::WeakPtr weak_node_ptr) + { auto shared_node_ptr = weak_node_ptr.lock(); if (shared_node_ptr) { std::atomic_bool & has_executor = shared_node_ptr->get_associated_with_executor_atomic(); diff --git a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp index 6fd0b56a85..73926b12bc 100644 --- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp @@ -409,8 +409,10 @@ StaticExecutorEntitiesCollector::remove_node( std::for_each( weak_groups_to_nodes_associated_with_executor_.begin(), weak_groups_to_nodes_associated_with_executor_.end(), - [&found_group_ptrs, node_ptr](std::pair key_value_pair) { + [&found_group_ptrs, node_ptr]( + std::pair key_value_pair) + { auto & weak_node_ptr = key_value_pair.second; auto shared_node_ptr = weak_node_ptr.lock(); auto group_ptr = key_value_pair.first.lock(); @@ -419,8 +421,9 @@ StaticExecutorEntitiesCollector::remove_node( } }); std::for_each( - found_group_ptrs.begin(), found_group_ptrs.end(), [this] - (rclcpp::CallbackGroup::SharedPtr group_ptr) { + found_group_ptrs.begin(), found_group_ptrs.end(), + [this](rclcpp::CallbackGroup::SharedPtr group_ptr) + { this->remove_callback_group_from_map( group_ptr, weak_groups_to_nodes_associated_with_executor_); diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index 2adf907bc5..b5ff4ff56d 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -200,9 +200,9 @@ class TestAllocatorMemoryStrategy : public ::testing::Test std::shared_ptr create_node_with_service(const std::string & name) { - auto service_callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto service_callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; auto node_with_service = create_node_with_disabled_callback_groups(name); auto callback_group = @@ -949,9 +949,9 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_service_out_of_scope) { node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); - auto service_callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto service_callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; auto service = node->create_service( "service", std::move(service_callback), rclcpp::ServicesQoS(), callback_group); diff --git a/rclcpp/test/rclcpp/test_any_service_callback.cpp b/rclcpp/test/rclcpp/test_any_service_callback.cpp index 6a70b8b5da..b6e49097c3 100644 --- a/rclcpp/test/rclcpp/test_any_service_callback.cpp +++ b/rclcpp/test/rclcpp/test_any_service_callback.cpp @@ -51,8 +51,10 @@ TEST_F(TestAnyServiceCallback, no_set_and_dispatch_throw) { TEST_F(TestAnyServiceCallback, set_and_dispatch_no_header) { int callback_calls = 0; - auto callback = [&callback_calls](const std::shared_ptr, - std::shared_ptr) { + auto callback = [&callback_calls]( + const std::shared_ptr, + std::shared_ptr) + { callback_calls++; }; @@ -65,10 +67,11 @@ TEST_F(TestAnyServiceCallback, set_and_dispatch_no_header) { TEST_F(TestAnyServiceCallback, set_and_dispatch_header) { int callback_with_header_calls = 0; - auto callback_with_header = - [&callback_with_header_calls](const std::shared_ptr, - const std::shared_ptr, - std::shared_ptr) { + auto callback_with_header = [&callback_with_header_calls]( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr) + { callback_with_header_calls++; }; @@ -80,9 +83,9 @@ TEST_F(TestAnyServiceCallback, set_and_dispatch_header) { TEST_F(TestAnyServiceCallback, set_and_dispatch_defered) { int callback_with_header_calls = 0; - auto callback_with_header = - [&callback_with_header_calls](const std::shared_ptr, - const std::shared_ptr) { + auto callback_with_header = [&callback_with_header_calls]( + const std::shared_ptr, const std::shared_ptr) + { callback_with_header_calls++; }; @@ -94,10 +97,10 @@ TEST_F(TestAnyServiceCallback, set_and_dispatch_defered) { TEST_F(TestAnyServiceCallback, set_and_dispatch_defered_with_service_handle) { int callback_with_header_calls = 0; - auto callback_with_header = - [&callback_with_header_calls](std::shared_ptr>, - const std::shared_ptr, - const std::shared_ptr) + auto callback_with_header = [&callback_with_header_calls]( + std::shared_ptr>, + const std::shared_ptr, + const std::shared_ptr) { callback_with_header_calls++; }; diff --git a/rclcpp/test/rclcpp/test_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp index e96729a2a1..c08b84230a 100644 --- a/rclcpp/test/rclcpp/test_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/test_memory_strategy.cpp @@ -143,9 +143,9 @@ TEST_F(TestMemoryStrategy, get_service_by_handle) { { auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto service_callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto service_callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; const rclcpp::QoS qos(10); weak_groups_to_nodes.insert( std::paircreate_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto service_callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto service_callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; const rclcpp::QoS qos(10); service = node->create_service( diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index a3c361cfde..1a00ceb527 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -88,12 +88,11 @@ class TestServiceSub : public ::testing::Test Testing service construction and destruction. */ TEST_F(TestService, construction_and_destruction) { - using rcl_interfaces::srv::ListParameters; - auto callback = - [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { - }; + auto callback = []( + const rcl_interfaces::srv::ListParameters::Request::SharedPtr, + rcl_interfaces::srv::ListParameters::Response::SharedPtr) {}; { - auto service = node->create_service("service", callback); + auto service = node->create_service("service", callback); EXPECT_NE(nullptr, service->get_service_handle()); const rclcpp::ServiceBase * const_service_base = service.get(); EXPECT_NE(nullptr, const_service_base->get_service_handle()); @@ -102,7 +101,8 @@ TEST_F(TestService, construction_and_destruction) { { ASSERT_THROW( { - auto service = node->create_service("invalid_service?", callback); + auto service = node->create_service( + "invalid_service?", callback); }, rclcpp::exceptions::InvalidServiceNameError); } } @@ -111,27 +111,27 @@ TEST_F(TestService, construction_and_destruction) { Testing service construction and destruction for subnodes. */ TEST_F(TestServiceSub, construction_and_destruction) { - using rcl_interfaces::srv::ListParameters; - auto callback = - [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { - }; + auto callback = []( + const rcl_interfaces::srv::ListParameters::Request::SharedPtr, + rcl_interfaces::srv::ListParameters::Response::SharedPtr) {}; { - auto service = subnode->create_service("service", callback); + auto service = subnode->create_service( + "service", callback); EXPECT_STREQ(service->get_service_name(), "/ns/sub_ns/service"); } { ASSERT_THROW( { - auto service = node->create_service("invalid_service?", callback); + auto service = node->create_service( + "invalid_service?", callback); }, rclcpp::exceptions::InvalidServiceNameError); } } TEST_F(TestService, construction_and_destruction_rcl_errors) { - auto callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; { auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_service_init, RCL_RET_ERROR); @@ -149,11 +149,10 @@ TEST_F(TestService, construction_and_destruction_rcl_errors) { /* Testing basic getters */ TEST_F(TestService, basic_public_getters) { - using rcl_interfaces::srv::ListParameters; - auto callback = - [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { - }; - auto service = node->create_service("service", callback); + auto callback = []( + const rcl_interfaces::srv::ListParameters::Request::SharedPtr, + rcl_interfaces::srv::ListParameters::Response::SharedPtr) {}; + auto service = node->create_service("service", callback); EXPECT_STREQ(service->get_service_name(), "/ns/service"); std::shared_ptr service_handle = service->get_service_handle(); EXPECT_NE(nullptr, service_handle); @@ -189,9 +188,8 @@ TEST_F(TestService, basic_public_getters) { } TEST_F(TestService, take_request) { - auto callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; auto server = node->create_service("service", callback); { auto request_id = server->create_request_header(); @@ -217,9 +215,8 @@ TEST_F(TestService, take_request) { } TEST_F(TestService, send_response) { - auto callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; auto server = node->create_service("service", callback); { @@ -243,9 +240,9 @@ TEST_F(TestService, send_response) { 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_callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {FAIL();}; rclcpp::ServicesQoS service_qos; service_qos.keep_last(3); auto server = node->create_service( @@ -315,9 +312,8 @@ TEST_F(TestService, on_new_request_callback) { TEST_F(TestService, rcl_service_response_publisher_get_actual_qos_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_service_response_publisher_get_actual_qos, nullptr); - auto callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; auto server = node->create_service("service", callback); RCLCPP_EXPECT_THROW_EQ( server->get_response_publisher_actual_qos(), @@ -327,9 +323,8 @@ TEST_F(TestService, rcl_service_response_publisher_get_actual_qos_error) { TEST_F(TestService, rcl_service_request_subscription_get_actual_qos_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_service_request_subscription_get_actual_qos, nullptr); - auto callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; auto server = node->create_service("service", callback); RCLCPP_EXPECT_THROW_EQ( server->get_request_subscription_actual_qos(), @@ -345,11 +340,10 @@ TEST_F(TestService, server_qos) { qos_profile.lifespan(duration); qos_profile.liveliness_lease_duration(duration); - auto callback = [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; - auto server = node->create_service( - "service", callback, qos_profile); + auto server = node->create_service("service", callback, qos_profile); auto rs_qos = server->get_request_subscription_actual_qos(); auto rp_qos = server->get_response_publisher_actual_qos(); @@ -360,8 +354,6 @@ TEST_F(TestService, server_qos) { } TEST_F(TestService, server_qos_depth) { - using namespace std::literals::chrono_literals; - uint64_t server_cb_count_ = 0; auto server_callback = [&]( const test_msgs::srv::Empty::Request::SharedPtr, @@ -380,8 +372,8 @@ TEST_F(TestService, server_qos_depth) { ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); auto request = std::make_shared(); - using SharedFuture = rclcpp::Client::SharedFuture; - auto client_callback = [&request_result](SharedFuture future_response) { + auto client_callback = [&request_result]( + rclcpp::Client::SharedFuture future_response) { if (nullptr == future_response.get()) { request_result = ::testing::AssertionFailure() << "Future response was null"; } diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index 33ded455c3..6d26bc54b8 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -285,20 +285,14 @@ TEST_P(TestTimer, test_failures_with_exceptions) std::shared_ptr timer_to_test_destructor; // Test destructor failure, just logs a msg auto mock = mocking_utils::inject_on_return("lib:rclcpp", rcl_timer_fini, RCL_RET_ERROR); - EXPECT_NO_THROW( - { - switch (timer_type) { - case TimerType::WALL_TIMER: - timer_to_test_destructor = - test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); - break; - case TimerType::GENERIC_TIMER: - timer_to_test_destructor = - test_node->create_timer(std::chrono::milliseconds(0), [](void) {}); - break; - } - timer_to_test_destructor.reset(); - }); + if (timer_type == TimerType::WALL_TIMER) { + timer_to_test_destructor = + test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); + } else { + timer_to_test_destructor = + test_node->create_timer(std::chrono::milliseconds(0), [](void) {}); + } + timer_to_test_destructor.reset(); } { auto mock = mocking_utils::patch_and_return( diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index b180c3f825..d5be45840b 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -499,8 +499,7 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait bool goal_response_received = false; auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); send_goal_ops.goal_response_callback = - [&goal_response_received] - (typename ActionGoalHandle::SharedPtr goal_handle) + [&goal_response_received](typename ActionGoalHandle::SharedPtr goal_handle) { if (goal_handle) { goal_response_received = true; @@ -545,8 +544,7 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_feedback_callback_wait_for_ goal.order = 4; int feedback_count = 0; auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); - send_goal_ops.feedback_callback = - [&feedback_count]( + send_goal_ops.feedback_callback = [&feedback_count]( typename ActionGoalHandle::SharedPtr goal_handle, const std::shared_ptr feedback) { @@ -868,9 +866,9 @@ TEST_F(TestClientAgainstServer, deadlock_in_callbacks) using GoalHandle = rclcpp_action::ClientGoalHandle; rclcpp_action::Client::SendGoalOptions ops; - ops.feedback_callback = - [&feedback_callback_called](const GoalHandle::SharedPtr handle, - ActionType::Feedback::ConstSharedPtr) { + ops.feedback_callback = [&feedback_callback_called]( + const GoalHandle::SharedPtr handle, ActionType::Feedback::ConstSharedPtr) + { // call functions on the handle that acquire the lock handle->get_status(); handle->is_feedback_aware(); @@ -940,9 +938,8 @@ TEST_F(TestClientAgainstServer, send_rcl_errors) auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); send_goal_ops.result_callback = [](const typename ActionGoalHandle::WrappedResult &) {}; - send_goal_ops.feedback_callback = - [](typename ActionGoalHandle::SharedPtr, - const std::shared_ptr) {}; + send_goal_ops.feedback_callback = []( + typename ActionGoalHandle::SharedPtr, const std::shared_ptr) {}; { ActionGoal goal; @@ -982,9 +979,8 @@ TEST_F(TestClientAgainstServer, execute_rcl_errors) auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); send_goal_ops.result_callback = [](const typename ActionGoalHandle::WrappedResult &) {}; - send_goal_ops.feedback_callback = - [](typename ActionGoalHandle::SharedPtr, - const std::shared_ptr) {}; + send_goal_ops.feedback_callback = []( + typename ActionGoalHandle::SharedPtr, const std::shared_ptr) {}; { ActionGoal goal; From b007204fd83828c11f80bf4712d237bfb2431f6e Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Thu, 29 Feb 2024 14:08:55 +0100 Subject: [PATCH 341/455] Rule of five: implement move operators (#2425) Signed-off-by: Tim Clephas --- rclcpp/include/rclcpp/time.hpp | 12 ++++++++++++ rclcpp/src/rclcpp/time.cpp | 9 ++++++--- 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/time.hpp b/rclcpp/include/rclcpp/time.hpp index 15e063d1d2..a931f3ac9c 100644 --- a/rclcpp/include/rclcpp/time.hpp +++ b/rclcpp/include/rclcpp/time.hpp @@ -57,6 +57,10 @@ class Time RCLCPP_PUBLIC Time(const Time & rhs); + /// Move constructor + RCLCPP_PUBLIC + Time(Time && rhs) noexcept; + /// Time constructor /** * \param time_msg builtin_interfaces time message to copy @@ -84,6 +88,7 @@ class Time operator builtin_interfaces::msg::Time() const; /** + * Copy assignment operator * \throws std::runtime_error if seconds are negative */ RCLCPP_PUBLIC @@ -100,6 +105,13 @@ class Time Time & operator=(const builtin_interfaces::msg::Time & time_msg); + /** + * Move assignment operator + */ + RCLCPP_PUBLIC + Time & + operator=(Time && rhs) noexcept; + /** * \throws std::runtime_error if the time sources are different */ diff --git a/rclcpp/src/rclcpp/time.cpp b/rclcpp/src/rclcpp/time.cpp index a0be141312..978651516c 100644 --- a/rclcpp/src/rclcpp/time.cpp +++ b/rclcpp/src/rclcpp/time.cpp @@ -65,6 +65,8 @@ Time::Time(int64_t nanoseconds, rcl_clock_type_t clock_type) Time::Time(const Time & rhs) = default; +Time::Time(Time && rhs) noexcept = default; + Time::Time( const builtin_interfaces::msg::Time & time_msg, rcl_clock_type_t clock_type) @@ -84,9 +86,7 @@ Time::Time(const rcl_time_point_t & time_point) // noop } -Time::~Time() -{ -} +Time::~Time() = default; Time::operator builtin_interfaces::msg::Time() const { @@ -103,6 +103,9 @@ Time::operator=(const builtin_interfaces::msg::Time & time_msg) return *this; } +Time & +Time::operator=(Time && rhs) noexcept = default; + bool Time::operator==(const rclcpp::Time & rhs) const { From 3df73f0e3882409910ef07438bc6e75ce2156aba Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Tue, 5 Mar 2024 13:35:40 +0800 Subject: [PATCH 342/455] Implement generic client (#2358) * Implement generic client Signed-off-by: Barry Xu * Fix the incorrect parameter declaration Signed-off-by: Barry Xu * Deleted copy constructor and assignment for FutureAndRequestId Signed-off-by: Barry Xu * Update codes after rebase Signed-off-by: Barry Xu * Address review comments Signed-off-by: Barry Xu * Address review comments from iuhilnehc-ynos Signed-off-by: Barry Xu * Correct an error in a description Signed-off-by: Barry Xu * Fix window build errors Signed-off-by: Barry Xu * Address review comments from William Signed-off-by: Barry Xu * Add doc strings to create_generic_client Signed-off-by: Barry Xu --------- Signed-off-by: Barry Xu --- rclcpp/CMakeLists.txt | 2 + rclcpp/include/rclcpp/client.hpp | 41 +- .../include/rclcpp/create_generic_client.hpp | 90 +++ rclcpp/include/rclcpp/generic_client.hpp | 207 ++++++ rclcpp/include/rclcpp/node.hpp | 17 + rclcpp/src/rclcpp/create_generic_client.cpp | 44 ++ rclcpp/src/rclcpp/generic_client.cpp | 164 +++++ rclcpp/src/rclcpp/node.cpp | 18 + rclcpp/test/rclcpp/CMakeLists.txt | 22 + rclcpp/test/rclcpp/test_client.cpp | 385 ------------ rclcpp/test/rclcpp/test_client_common.cpp | 591 ++++++++++++++++++ rclcpp/test/rclcpp/test_generic_client.cpp | 230 +++++++ 12 files changed, 1413 insertions(+), 398 deletions(-) create mode 100644 rclcpp/include/rclcpp/create_generic_client.hpp create mode 100644 rclcpp/include/rclcpp/generic_client.hpp create mode 100644 rclcpp/src/rclcpp/create_generic_client.cpp create mode 100644 rclcpp/src/rclcpp/generic_client.cpp create mode 100644 rclcpp/test/rclcpp/test_client_common.cpp create mode 100644 rclcpp/test/rclcpp/test_generic_client.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 1f78d03d59..64b7caed66 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -45,6 +45,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/clock.cpp src/rclcpp/context.cpp src/rclcpp/contexts/default_context.cpp + src/rclcpp/create_generic_client.cpp src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp src/rclcpp/detail/resolve_intra_process_buffer_type.cpp src/rclcpp/detail/resolve_parameter_overrides.cpp @@ -74,6 +75,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/experimental/executors/events_executor/events_executor.cpp src/rclcpp/experimental/timers_manager.cpp src/rclcpp/future_return_code.cpp + src/rclcpp/generic_client.cpp src/rclcpp/generic_publisher.cpp src/rclcpp/generic_subscription.cpp src/rclcpp/graph_listener.cpp diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 85b0a2d5f9..f69ab0b28f 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -115,6 +115,29 @@ struct FutureAndRequestId /// Destructor. ~FutureAndRequestId() = default; }; + +template> +size_t +prune_requests_older_than_impl( + PendingRequestsT & pending_requests, + std::mutex & pending_requests_mutex, + std::chrono::time_point time_point, + std::vector * pruned_requests = nullptr) +{ + std::lock_guard guard(pending_requests_mutex); + auto old_size = pending_requests.size(); + for (auto it = pending_requests.begin(), last = pending_requests.end(); it != last; ) { + if (it->second.first < time_point) { + if (pruned_requests) { + pruned_requests->push_back(it->first); + } + it = pending_requests.erase(it); + } else { + ++it; + } + } + return old_size - pending_requests.size(); +} } // namespace detail namespace node_interfaces @@ -771,19 +794,11 @@ class Client : public ClientBase std::chrono::time_point time_point, std::vector * pruned_requests = nullptr) { - std::lock_guard guard(pending_requests_mutex_); - auto old_size = pending_requests_.size(); - for (auto it = pending_requests_.begin(), last = pending_requests_.end(); it != last; ) { - if (it->second.first < time_point) { - if (pruned_requests) { - pruned_requests->push_back(it->first); - } - it = pending_requests_.erase(it); - } else { - ++it; - } - } - return old_size - pending_requests_.size(); + return detail::prune_requests_older_than_impl( + pending_requests_, + pending_requests_mutex_, + time_point, + pruned_requests); } /// Configure client introspection. diff --git a/rclcpp/include/rclcpp/create_generic_client.hpp b/rclcpp/include/rclcpp/create_generic_client.hpp new file mode 100644 index 0000000000..eade7bd9f1 --- /dev/null +++ b/rclcpp/include/rclcpp/create_generic_client.hpp @@ -0,0 +1,90 @@ +// Copyright 2023 Sony Group Corporation. +// +// 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__CREATE_GENERIC_CLIENT_HPP_ +#define RCLCPP__CREATE_GENERIC_CLIENT_HPP_ + +#include +#include + +#include "rclcpp/generic_client.hpp" +#include "rclcpp/node_interfaces/get_node_base_interface.hpp" +#include "rclcpp/node_interfaces/get_node_graph_interface.hpp" +#include "rclcpp/node_interfaces/get_node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/qos.hpp" + +namespace rclcpp +{ +/// Create a generic service client with a name of given type. +/** + * \param[in] node_base NodeBaseInterface implementation of the node on which + * to create the client. + * \param[in] node_graph NodeGraphInterface implementation of the node on which + * to create the client. + * \param[in] node_services NodeServicesInterface implementation of the node on + * which to create the client. + * \param[in] service_name The name on which the service is accessible. + * \param[in] service_type The name of service type, e.g. "test_msgs/srv/BasicTypes" + * \param[in] qos Quality of service profile for client. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created client. + */ +RCLCPP_PUBLIC +rclcpp::GenericClient::SharedPtr +create_generic_client( + std::shared_ptr node_base, + std::shared_ptr node_graph, + std::shared_ptr node_services, + const std::string & service_name, + const std::string & service_type, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr); + +/// Create a generic service client with a name of given type. +/** + * The NodeT type needs to have NodeBaseInterface implementation, NodeGraphInterface implementation + * and NodeServicesInterface implementation of the node which to create the client. + * + * \param[in] node The node on which to create the client. + * \param[in] service_name The name on which the service is accessible. + * \param[in] service_type The name of service type, e.g. "test_msgs/srv/BasicTypes" + * \param[in] qos Quality of service profile for client. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created client. + */ +template +rclcpp::GenericClient::SharedPtr +create_generic_client( + NodeT node, + const std::string & service_name, + const std::string & service_type, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr) +{ + return create_generic_client( + rclcpp::node_interfaces::get_node_base_interface(node), + rclcpp::node_interfaces::get_node_graph_interface(node), + rclcpp::node_interfaces::get_node_services_interface(node), + service_name, + service_type, + qos, + group + ); +} +} // namespace rclcpp + +#endif // RCLCPP__CREATE_GENERIC_CLIENT_HPP_ diff --git a/rclcpp/include/rclcpp/generic_client.hpp b/rclcpp/include/rclcpp/generic_client.hpp new file mode 100644 index 0000000000..d6073decfc --- /dev/null +++ b/rclcpp/include/rclcpp/generic_client.hpp @@ -0,0 +1,207 @@ +// Copyright 2023 Sony Group Corporation. +// +// 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__GENERIC_CLIENT_HPP_ +#define RCLCPP__GENERIC_CLIENT_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rcl/client.h" + +#include "rclcpp/client.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rcpputils/shared_library.hpp" + +#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" + +namespace rclcpp +{ +class GenericClient : public ClientBase +{ +public: + using Request = void *; // Serialized data pointer of request message + using Response = void *; // Serialized data pointer of response message + + using SharedResponse = std::shared_ptr; + + using Promise = std::promise; + using SharedPromise = std::shared_ptr; + + using Future = std::future; + using SharedFuture = std::shared_future; + + RCLCPP_SMART_PTR_DEFINITIONS(GenericClient) + + /// A convenient GenericClient::Future and request id pair. + /** + * Public members: + * - future: a std::future. + * - request_id: the request id associated with the future. + * + * All the other methods are equivalent to the ones std::future provides. + */ + struct FutureAndRequestId + : detail::FutureAndRequestId + { + using detail::FutureAndRequestId::FutureAndRequestId; + + /// See std::future::share(). + SharedFuture share() noexcept {return this->future.share();} + + /// Move constructor. + FutureAndRequestId(FutureAndRequestId && other) noexcept = default; + /// Deleted copy constructor, each instance is a unique owner of the future. + FutureAndRequestId(const FutureAndRequestId & other) = delete; + /// Move assignment. + FutureAndRequestId & operator=(FutureAndRequestId && other) noexcept = default; + /// Deleted copy assignment, each instance is a unique owner of the future. + FutureAndRequestId & operator=(const FutureAndRequestId & other) = delete; + /// Destructor. + ~FutureAndRequestId() = default; + }; + + GenericClient( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + const std::string & service_name, + const std::string & service_type, + rcl_client_options_t & client_options); + + RCLCPP_PUBLIC + SharedResponse + create_response() override; + + RCLCPP_PUBLIC + std::shared_ptr + create_request_header() override; + + RCLCPP_PUBLIC + void + handle_response( + std::shared_ptr request_header, + std::shared_ptr response) override; + + /// Send a request to the service server. + /** + * This method returns a `FutureAndRequestId` instance + * that can be passed to Executor::spin_until_future_complete() to + * wait until it has been completed. + * + * If the future never completes, + * e.g. the call to Executor::spin_until_future_complete() times out, + * GenericClient::remove_pending_request() must be called to clean the client internal state. + * Not doing so will make the `Client` instance to use more memory each time a response is not + * received from the service server. + * + * ```cpp + * auto future = client->async_send_request(my_request); + * if ( + * rclcpp::FutureReturnCode::TIMEOUT == + * executor->spin_until_future_complete(future, timeout)) + * { + * client->remove_pending_request(future); + * // handle timeout + * } else { + * handle_response(future.get()); + * } + * ``` + * + * \param[in] request request to be send. + * \return a FutureAndRequestId instance. + */ + RCLCPP_PUBLIC + FutureAndRequestId + async_send_request(const Request request); + + /// Clean all pending requests older than a time_point. + /** + * \param[in] time_point Requests that were sent before this point are going to be removed. + * \param[inout] pruned_requests Removed requests id will be pushed to the vector + * if a pointer is provided. + * \return number of pending requests that were removed. + */ + template> + size_t + prune_requests_older_than( + std::chrono::time_point time_point, + std::vector * pruned_requests = nullptr) + { + return detail::prune_requests_older_than_impl( + pending_requests_, + pending_requests_mutex_, + time_point, + pruned_requests); + } + + RCLCPP_PUBLIC + size_t + prune_pending_requests(); + + RCLCPP_PUBLIC + bool + remove_pending_request( + int64_t request_id); + + /// Take the next response for this client. + /** + * \sa ClientBase::take_type_erased_response(). + * + * \param[out] response_out The reference to a Service Response into + * which the middleware will copy the response being taken. + * \param[out] request_header_out The request header to be filled by the + * middleware when taking, and which can be used to associate the response + * to a specific request. + * \returns true if the response was taken, otherwise false. + * \throws rclcpp::exceptions::RCLError based exceptions if the underlying + * rcl function fail. + */ + RCLCPP_PUBLIC + bool + take_response(Response response_out, rmw_request_id_t & request_header_out) + { + return this->take_type_erased_response(response_out, request_header_out); + } + +protected: + using CallbackInfoVariant = std::variant< + std::promise>; // Use variant for extension + + int64_t + async_send_request_impl( + const Request request, + CallbackInfoVariant value); + + std::optional + get_and_erase_pending_request( + int64_t request_number); + + RCLCPP_DISABLE_COPY(GenericClient) + + std::map, + CallbackInfoVariant>> pending_requests_; + std::mutex pending_requests_mutex_; + +private: + std::shared_ptr ts_lib_; + const rosidl_typesupport_introspection_cpp::MessageMembers * response_members_; +}; +} // namespace rclcpp + +#endif // RCLCPP__GENERIC_CLIENT_HPP_ diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 50b96bbeaf..35863abba4 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -42,6 +42,7 @@ #include "rclcpp/clock.hpp" #include "rclcpp/context.hpp" #include "rclcpp/event.hpp" +#include "rclcpp/generic_client.hpp" #include "rclcpp/generic_publisher.hpp" #include "rclcpp/generic_subscription.hpp" #include "rclcpp/logger.hpp" @@ -320,6 +321,22 @@ class Node : public std::enable_shared_from_this const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a GenericClient. + /** + * \param[in] service_name The name on which the service is accessible. + * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool" + * \param[in] qos Quality of service profile for client. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created GenericClient. + */ + RCLCPP_PUBLIC + rclcpp::GenericClient::SharedPtr + create_generic_client( + const std::string & service_name, + const std::string & service_type, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a GenericPublisher. /** * The returned pointer will never be empty, but this function can throw various exceptions, for diff --git a/rclcpp/src/rclcpp/create_generic_client.cpp b/rclcpp/src/rclcpp/create_generic_client.cpp new file mode 100644 index 0000000000..4b3b7ddc35 --- /dev/null +++ b/rclcpp/src/rclcpp/create_generic_client.cpp @@ -0,0 +1,44 @@ +// Copyright 2023 Sony Group Corporation. +// +// 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. + +#include "rclcpp/create_generic_client.hpp" +#include "rclcpp/generic_client.hpp" + +namespace rclcpp +{ +rclcpp::GenericClient::SharedPtr +create_generic_client( + std::shared_ptr node_base, + std::shared_ptr node_graph, + std::shared_ptr node_services, + const std::string & service_name, + const std::string & service_type, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + rcl_client_options_t options = rcl_client_get_default_options(); + options.qos = qos.get_rmw_qos_profile(); + + auto cli = rclcpp::GenericClient::make_shared( + node_base.get(), + node_graph, + service_name, + service_type, + options); + + auto cli_base_ptr = std::dynamic_pointer_cast(cli); + node_services->add_client(cli_base_ptr, group); + return cli; +} +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/generic_client.cpp b/rclcpp/src/rclcpp/generic_client.cpp new file mode 100644 index 0000000000..fdcfc70aab --- /dev/null +++ b/rclcpp/src/rclcpp/generic_client.cpp @@ -0,0 +1,164 @@ +// Copyright 2023 Sony Group Corporation. +// +// 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. + +#include + +#include "rclcpp/generic_client.hpp" +#include "rclcpp/typesupport_helpers.hpp" + +#include "rosidl_runtime_c/service_type_support_struct.h" +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" +#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp" + +namespace rclcpp +{ +GenericClient::GenericClient( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + const std::string & service_name, + const std::string & service_type, + rcl_client_options_t & client_options) +: ClientBase(node_base, node_graph) +{ + ts_lib_ = get_typesupport_library( + service_type, "rosidl_typesupport_cpp"); + + auto service_ts_ = get_service_typesupport_handle( + service_type, "rosidl_typesupport_cpp", *ts_lib_); + + auto response_type_support_intro = get_message_typesupport_handle( + service_ts_->response_typesupport, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + response_members_ = static_cast( + response_type_support_intro->data); + + rcl_ret_t ret = rcl_client_init( + this->get_client_handle().get(), + this->get_rcl_node_handle(), + service_ts_, + service_name.c_str(), + &client_options); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_SERVICE_NAME_INVALID) { + auto rcl_node_handle = this->get_rcl_node_handle(); + // this will throw on any validation problem + rcl_reset_error(); + expand_topic_or_service_name( + service_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + true); + } + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create generic client"); + } +} + +std::shared_ptr +GenericClient::create_response() +{ + void * response = new uint8_t[response_members_->size_of_]; + response_members_->init_function(response, rosidl_runtime_cpp::MessageInitialization::ZERO); + return std::shared_ptr( + response, + [this](void * p) + { + response_members_->fini_function(p); + delete[] reinterpret_cast(p); + }); +} + +std::shared_ptr +GenericClient::create_request_header() +{ + // TODO(wjwwood): This should probably use rmw_request_id's allocator. + // (since it is a C type) + return std::shared_ptr(new rmw_request_id_t); +} + +void +GenericClient::handle_response( + std::shared_ptr request_header, + std::shared_ptr response) +{ + auto optional_pending_request = + this->get_and_erase_pending_request(request_header->sequence_number); + if (!optional_pending_request) { + return; + } + auto & value = *optional_pending_request; + if (std::holds_alternative(value)) { + auto & promise = std::get(value); + promise.set_value(std::move(response)); + } +} + +size_t +GenericClient::prune_pending_requests() +{ + std::lock_guard guard(pending_requests_mutex_); + auto ret = pending_requests_.size(); + pending_requests_.clear(); + return ret; +} + +bool +GenericClient::remove_pending_request(int64_t request_id) +{ + std::lock_guard guard(pending_requests_mutex_); + return pending_requests_.erase(request_id) != 0u; +} + +std::optional +GenericClient::get_and_erase_pending_request(int64_t request_number) +{ + std::unique_lock lock(pending_requests_mutex_); + auto it = pending_requests_.find(request_number); + if (it == pending_requests_.end()) { + RCUTILS_LOG_DEBUG_NAMED( + "rclcpp", + "Received invalid sequence number. Ignoring..."); + return std::nullopt; + } + auto value = std::move(it->second.second); + pending_requests_.erase(request_number); + return value; +} + +GenericClient::FutureAndRequestId +GenericClient::async_send_request(const Request request) +{ + Promise promise; + auto future = promise.get_future(); + auto req_id = async_send_request_impl( + request, + std::move(promise)); + return FutureAndRequestId(std::move(future), req_id); +} + +int64_t +GenericClient::async_send_request_impl(const Request request, CallbackInfoVariant value) +{ + int64_t sequence_number; + std::lock_guard lock(pending_requests_mutex_); + rcl_ret_t ret = rcl_send_request(get_client_handle().get(), request, &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request"); + } + pending_requests_.try_emplace( + sequence_number, + std::make_pair(std::chrono::system_clock::now(), std::move(value))); + return sequence_number; +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index c31903f2fe..1a68c7f108 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -23,6 +23,7 @@ #include "rcl/arguments.h" +#include "rclcpp/create_generic_client.hpp" #include "rclcpp/detail/qos_parameters.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/graph_listener.hpp" @@ -676,3 +677,20 @@ Node::get_node_options() const { return this->node_options_; } + +rclcpp::GenericClient::SharedPtr +Node::create_generic_client( + const std::string & service_name, + const std::string & service_type, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_generic_client( + node_base_, + node_graph_, + node_services_, + service_name, + service_type, + qos, + group); +} diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 0a9ba9ca97..c17ed6fe06 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -63,6 +63,28 @@ if(TARGET test_create_timer) target_link_libraries(test_create_timer ${PROJECT_NAME}) target_include_directories(test_create_timer PRIVATE ./) endif() +ament_add_gtest(test_generic_client test_generic_client.cpp) +if(TARGET test_generic_client) + target_link_libraries(test_generic_client ${PROJECT_NAME} + mimick + ${rcl_interfaces_TARGETS} + rmw::rmw + rosidl_runtime_cpp::rosidl_runtime_cpp + rosidl_typesupport_cpp::rosidl_typesupport_cpp + ${test_msgs_TARGETS} + ) +endif() +ament_add_gtest(test_client_common test_client_common.cpp) +if(TARGET test_client_common) + target_link_libraries(test_client_common ${PROJECT_NAME} + mimick + ${rcl_interfaces_TARGETS} + rmw::rmw + rosidl_runtime_cpp::rosidl_runtime_cpp + rosidl_typesupport_cpp::rosidl_typesupport_cpp + ${test_msgs_TARGETS} + ) +endif() ament_add_gtest(test_create_subscription test_create_subscription.cpp) if(TARGET test_create_subscription) target_link_libraries(test_create_subscription ${PROJECT_NAME} ${test_msgs_TARGETS}) diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp index 9070e1caa9..5c7e5046ab 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -24,12 +24,9 @@ #include "rcl_interfaces/srv/list_parameters.hpp" #include "../mocking_utils/patch.hpp" -#include "../utils/rclcpp_gtest_macros.hpp" #include "test_msgs/srv/empty.hpp" -using namespace std::chrono_literals; - class TestClient : public ::testing::Test { protected: @@ -219,385 +216,3 @@ TEST_F(TestClientSub, construction_and_destruction) { }, rclcpp::exceptions::InvalidServiceNameError); } } - -class TestClientWithServer : public ::testing::Test -{ -protected: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - static void TearDownTestCase() - { - rclcpp::shutdown(); - } - - void SetUp() - { - node = std::make_shared("node", "ns"); - - auto callback = []( - const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; - - service = node->create_service(service_name, std::move(callback)); - } - - ::testing::AssertionResult SendEmptyRequestAndWait( - std::chrono::milliseconds timeout = std::chrono::milliseconds(1000)) - { - using SharedFuture = rclcpp::Client::SharedFuture; - - auto client = node->create_client(service_name); - if (!client->wait_for_service()) { - return ::testing::AssertionFailure() << "Waiting for service failed"; - } - - auto request = std::make_shared(); - bool received_response = false; - ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); - auto callback = [&received_response, &request_result](SharedFuture future_response) { - if (nullptr == future_response.get()) { - request_result = ::testing::AssertionFailure() << "Future response was null"; - } - received_response = true; - }; - - auto req_id = client->async_send_request(request, std::move(callback)); - - auto start = std::chrono::steady_clock::now(); - while (!received_response && - (std::chrono::steady_clock::now() - start) < timeout) - { - rclcpp::spin_some(node); - } - - if (!received_response) { - return ::testing::AssertionFailure() << "Waiting for response timed out"; - } - if (client->remove_pending_request(req_id)) { - return ::testing::AssertionFailure() << "Should not be able to remove a finished request"; - } - - return request_result; - } - - std::shared_ptr node; - std::shared_ptr> service; - const std::string service_name{"empty_service"}; -}; - -TEST_F(TestClientWithServer, async_send_request) { - EXPECT_TRUE(SendEmptyRequestAndWait()); -} - -TEST_F(TestClientWithServer, async_send_request_callback_with_request) { - using SharedFutureWithRequest = - rclcpp::Client::SharedFutureWithRequest; - - auto client = node->create_client(service_name); - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); - - auto request = std::make_shared(); - bool received_response = false; - auto callback = [&request, &received_response](SharedFutureWithRequest future) { - auto request_response_pair = future.get(); - EXPECT_EQ(request, request_response_pair.first); - EXPECT_NE(nullptr, request_response_pair.second); - received_response = true; - }; - auto req_id = client->async_send_request(request, std::move(callback)); - - auto start = std::chrono::steady_clock::now(); - while (!received_response && - (std::chrono::steady_clock::now() - start) < std::chrono::seconds(1)) - { - rclcpp::spin_some(node); - } - EXPECT_TRUE(received_response); - EXPECT_FALSE(client->remove_pending_request(req_id)); -} - -TEST_F(TestClientWithServer, test_client_remove_pending_request) { - auto client = node->create_client("no_service_server_available_here"); - auto request = std::make_shared(); - auto future = client->async_send_request(request); - - EXPECT_TRUE(client->remove_pending_request(future)); -} - -TEST_F(TestClientWithServer, prune_requests_older_than_no_pruned) { - auto client = node->create_client(service_name); - auto request = std::make_shared(); - auto future = client->async_send_request(request); - auto time = std::chrono::system_clock::now() + 1s; - - EXPECT_EQ(1u, client->prune_requests_older_than(time)); -} - -TEST_F(TestClientWithServer, prune_requests_older_than_with_pruned) { - auto client = node->create_client(service_name); - auto request = std::make_shared(); - auto future = client->async_send_request(request); - auto time = std::chrono::system_clock::now() + 1s; - - std::vector pruned_requests; - EXPECT_EQ(1u, client->prune_requests_older_than(time, &pruned_requests)); - ASSERT_EQ(1u, pruned_requests.size()); - EXPECT_EQ(future.request_id, pruned_requests[0]); -} - -TEST_F(TestClientWithServer, async_send_request_rcl_send_request_error) { - // Checking rcl_send_request in rclcpp::Client::async_send_request() - auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_request, RCL_RET_ERROR); - EXPECT_THROW(SendEmptyRequestAndWait(), rclcpp::exceptions::RCLError); -} - -TEST_F(TestClientWithServer, async_send_request_rcl_service_server_is_available_error) { - { - // Checking rcl_service_server_is_available in rclcpp::ClientBase::service_is_ready - auto client = node->create_client(service_name); - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_service_server_is_available, RCL_RET_NODE_INVALID); - EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError); - } - { - // Checking rcl_service_server_is_available exception in rclcpp::ClientBase::service_is_ready - auto client = node->create_client(service_name); - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_service_server_is_available, RCL_RET_ERROR); - EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError); - } - { - // Checking rcl_service_server_is_available exception in rclcpp::ClientBase::service_is_ready - auto client = node->create_client(service_name); - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_service_server_is_available, RCL_RET_ERROR); - EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError); - } -} - -TEST_F(TestClientWithServer, take_response) { - auto client = node->create_client(service_name); - ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); - auto request = std::make_shared(); - auto request_header = client->create_request_header(); - test_msgs::srv::Empty::Response response; - - client->async_send_request(request); - EXPECT_FALSE(client->take_response(response, *request_header.get())); - - { - // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_take_response, RCL_RET_OK); - EXPECT_TRUE(client->take_response(response, *request_header.get())); - } - { - // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_take_response, RCL_RET_CLIENT_TAKE_FAILED); - EXPECT_FALSE(client->take_response(response, *request_header.get())); - } - { - // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_take_response, RCL_RET_ERROR); - EXPECT_THROW( - client->take_response(response, *request_header.get()), - 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"); - - rclcpp::ServicesQoS client_qos; - client_qos.keep_last(3); - auto client = client_node->create_client("test_service", client_qos); - 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, client_qos); - 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 < 3 && 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); -} - -TEST_F(TestClient, rcl_client_request_publisher_get_actual_qos_error) { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_client_request_publisher_get_actual_qos, nullptr); - auto client = node->create_client("service"); - RCLCPP_EXPECT_THROW_EQ( - client->get_request_publisher_actual_qos(), - std::runtime_error("failed to get client's request publisher qos settings: error not set")); -} - -TEST_F(TestClient, rcl_client_response_subscription_get_actual_qos_error) { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_client_response_subscription_get_actual_qos, nullptr); - auto client = node->create_client("service"); - RCLCPP_EXPECT_THROW_EQ( - client->get_response_subscription_actual_qos(), - std::runtime_error("failed to get client's response subscription qos settings: error not set")); -} - -TEST_F(TestClient, client_qos) { - rclcpp::ServicesQoS qos_profile; - qos_profile.liveliness(rclcpp::LivelinessPolicy::Automatic); - rclcpp::Duration duration(std::chrono::nanoseconds(1)); - qos_profile.deadline(duration); - qos_profile.lifespan(duration); - qos_profile.liveliness_lease_duration(duration); - - auto client = - node->create_client("client", qos_profile); - - auto rp_qos = client->get_request_publisher_actual_qos(); - auto rs_qos = client->get_response_subscription_actual_qos(); - - EXPECT_EQ(qos_profile, rp_qos); - // Lifespan has no meaning for subscription/readers - rs_qos.lifespan(qos_profile.lifespan()); - EXPECT_EQ(qos_profile, rs_qos); -} - -TEST_F(TestClient, client_qos_depth) { - using namespace std::literals::chrono_literals; - - rclcpp::ServicesQoS client_qos_profile; - client_qos_profile.keep_last(2); - - auto client = node->create_client("test_qos_depth", client_qos_profile); - - uint64_t server_cb_count_ = 0; - auto server_callback = [&]( - const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {server_cb_count_++;}; - - auto server_node = std::make_shared("server_node", "/ns"); - - rclcpp::QoS server_qos(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); - - auto server = server_node->create_service( - "test_qos_depth", std::move(server_callback), server_qos); - - auto request = std::make_shared(); - ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); - - using SharedFuture = rclcpp::Client::SharedFuture; - uint64_t client_cb_count_ = 0; - auto client_callback = [&client_cb_count_, &request_result](SharedFuture future_response) { - if (nullptr == future_response.get()) { - request_result = ::testing::AssertionFailure() << "Future response was null"; - } - client_cb_count_++; - }; - - uint64_t client_requests = 5; - for (uint64_t i = 0; i < client_requests; i++) { - client->async_send_request(request, client_callback); - std::this_thread::sleep_for(10ms); - } - - auto start = std::chrono::steady_clock::now(); - while ((server_cb_count_ < client_requests) && - (std::chrono::steady_clock::now() - start) < 2s) - { - rclcpp::spin_some(server_node); - std::this_thread::sleep_for(2ms); - } - - EXPECT_GT(server_cb_count_, client_qos_profile.depth()); - - start = std::chrono::steady_clock::now(); - while ((client_cb_count_ < client_qos_profile.depth()) && - (std::chrono::steady_clock::now() - start) < 1s) - { - rclcpp::spin_some(node); - } - - // Spin an extra time to check if client QoS depth has been ignored, - // so more client callbacks might be called than expected. - rclcpp::spin_some(node); - - EXPECT_EQ(client_cb_count_, client_qos_profile.depth()); -} diff --git a/rclcpp/test/rclcpp/test_client_common.cpp b/rclcpp/test/rclcpp/test_client_common.cpp new file mode 100644 index 0000000000..65475bd8fc --- /dev/null +++ b/rclcpp/test/rclcpp/test_client_common.cpp @@ -0,0 +1,591 @@ +// Copyright 2024 Sony Group Corporation. +// +// 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. + +#include + +#include +#include +#include + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "rclcpp/create_generic_client.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "test_msgs/srv/empty.hpp" + +template +class TestAllClientTypesWithServer : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_node", "ns"); + + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + + service = node->create_service(service_name, std::move(callback)); + } + + template + auto SendEmptyRequestAndWait( + std::chrono::milliseconds timeout = std::chrono::milliseconds(1000)) + { + if constexpr (std::is_same_v) { + return GenericClientSendEmptyRequestAndWait(timeout); + } else if constexpr (std::is_same_v>) { + return ClientSendEmptyRequestAndWait(timeout); + } else { + return ::testing::AssertionFailure() << "No test for this client type"; + } + } + + ::testing::AssertionResult GenericClientSendEmptyRequestAndWait( + std::chrono::milliseconds timeout = std::chrono::milliseconds(1000)) + { + auto client = node->create_generic_client(service_name, "test_msgs/srv/Empty"); + if (!client->wait_for_service()) { + return ::testing::AssertionFailure() << "Service is not available yet"; + } + + auto request = std::make_shared(); + + auto future_and_req_id = client->async_send_request(request.get()); + + auto ret = rclcpp::spin_until_future_complete(node, future_and_req_id, timeout); + if (ret != rclcpp::FutureReturnCode::SUCCESS) { + return ::testing::AssertionFailure() << "Waiting for response timed out"; + } + + if (client->remove_pending_request(future_and_req_id.request_id)) { + return ::testing::AssertionFailure() << "Should not be able to remove a finished request"; + } + + return ::testing::AssertionSuccess(); + } + + ::testing::AssertionResult ClientSendEmptyRequestAndWait( + std::chrono::milliseconds timeout = std::chrono::milliseconds(1000)) + { + using SharedFuture = rclcpp::Client::SharedFuture; + + auto client = node->create_client(service_name); + if (!client->wait_for_service()) { + return ::testing::AssertionFailure() << "Waiting for service failed"; + } + + auto request = std::make_shared(); + bool received_response = false; + ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); + auto callback = [&received_response, &request_result](SharedFuture future_response) { + if (nullptr == future_response.get()) { + request_result = ::testing::AssertionFailure() << "Future response was null"; + } + received_response = true; + }; + + auto req_id = client->async_send_request(request, std::move(callback)); + + auto start = std::chrono::steady_clock::now(); + while (!received_response && + (std::chrono::steady_clock::now() - start) < timeout) + { + rclcpp::spin_some(node); + } + + if (!received_response) { + return ::testing::AssertionFailure() << "Waiting for response timed out"; + } + if (client->remove_pending_request(req_id)) { + return ::testing::AssertionFailure() << "Should not be able to remove a finished request"; + } + + return request_result; + } + + template + auto create_client( + rclcpp::Node::SharedPtr node, + const std::string service_name = "empty_service", + const rclcpp::QoS & qos = rclcpp::ServicesQoS()) + { + if constexpr (std::is_same_v) { + return node->create_generic_client(service_name, "test_msgs/srv/Empty", qos); + } else if constexpr (std::is_same_v>) { + return node->template create_client(service_name, qos); + } else { + ASSERT_TRUE(false) << "Not know how to create this kind of client"; + } + } + + template + auto async_send_request(std::shared_ptr client, std::shared_ptr request) + { + if constexpr (std::is_same_v) { + return client->async_send_request(request.get()); + } else if constexpr (std::is_same_v>) { + return client->async_send_request(request); + } else { + ASSERT_TRUE(false) << "Not know how to send request for this kind of client"; + } + } + + template + auto take_response( + std::shared_ptr client, + ResponseType & response, + std::shared_ptr request_header) + { + if constexpr (std::is_same_v) { + return client->take_response(static_cast(&response), *request_header.get()); + } else if constexpr (std::is_same_v>) { + return client->take_response(response, *request_header.get()); + } else { + ASSERT_TRUE(false) << "Not know how to take response for this kind of client"; + } + } + + std::shared_ptr node; + std::shared_ptr> service; + const std::string service_name{"empty_service"}; +}; + +using ClientType = + ::testing::Types< + rclcpp::Client, + rclcpp::GenericClient>; + +class ClientTypeNames +{ +public: + template + static std::string GetName(int idx) + { + (void)idx; + if (std::is_same_v>) { + return "Client"; + } + + if (std::is_same_v) { + return "GenericClient"; + } + + return ""; + } +}; + +TYPED_TEST_SUITE(TestAllClientTypesWithServer, ClientType, ClientTypeNames); + +TYPED_TEST(TestAllClientTypesWithServer, async_send_request) +{ + using ClientType = TypeParam; + EXPECT_TRUE(this->template SendEmptyRequestAndWait()); +} + +TYPED_TEST(TestAllClientTypesWithServer, test_client_remove_pending_request) +{ + using ClientType = TypeParam; + + auto client = this->template create_client(this->node); + + auto request = std::make_shared(); + + auto future_and_req_id = this->template async_send_request< + ClientType, test_msgs::srv::Empty::Request>(client, request); + + EXPECT_TRUE(client->remove_pending_request(future_and_req_id.request_id)); +} + +TYPED_TEST(TestAllClientTypesWithServer, prune_requests_older_than_no_pruned) +{ + using ClientType = TypeParam; + + auto client = this->template create_client(this->node); + + auto request = std::make_shared(); + + auto future = this->template async_send_request< + ClientType, test_msgs::srv::Empty::Request>(client, request); + auto time = std::chrono::system_clock::now() + std::chrono::seconds(1); + + EXPECT_EQ(1u, client->prune_requests_older_than(time)); +} + +TYPED_TEST(TestAllClientTypesWithServer, prune_requests_older_than_with_pruned) +{ + using ClientType = TypeParam; + + auto client = this->template create_client(this->node); + + auto request = std::make_shared(); + + auto future = this->template async_send_request< + ClientType, test_msgs::srv::Empty::Request>(client, request); + auto time = std::chrono::system_clock::now() + std::chrono::seconds(1); + + std::vector pruned_requests; + EXPECT_EQ(1u, client->prune_requests_older_than(time, &pruned_requests)); + ASSERT_EQ(1u, pruned_requests.size()); + EXPECT_EQ(future.request_id, pruned_requests[0]); +} + +TYPED_TEST(TestAllClientTypesWithServer, async_send_request_rcl_send_request_error) +{ + using ClientType = TypeParam; + + // Checking rcl_send_request in rclcpp::Client::async_send_request() or + // rclcpp::GenericClient::async_send_request() + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_request, RCL_RET_ERROR); + EXPECT_THROW(this->template SendEmptyRequestAndWait(), rclcpp::exceptions::RCLError); +} + +TYPED_TEST(TestAllClientTypesWithServer, async_send_request_rcl_service_server_is_available_error) +{ + using ClientType = TypeParam; + + { + // Checking rcl_service_server_is_available in rclcpp::ClientBase::service_is_ready + auto client = this->template create_client(this->node); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_server_is_available, RCL_RET_NODE_INVALID); + EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError); + } + { + // Checking rcl_service_server_is_available exception in rclcpp::ClientBase::service_is_ready + auto client = this->template create_client(this->node); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_server_is_available, RCL_RET_ERROR); + EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError); + } + { + // Checking rcl_service_server_is_available exception in rclcpp::ClientBase::service_is_ready + auto client = this->template create_client(this->node); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_server_is_available, RCL_RET_ERROR); + EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError); + } +} + +TYPED_TEST(TestAllClientTypesWithServer, take_response) +{ + using ClientType = TypeParam; + + auto client = this->template create_client(this->node); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + auto request = std::make_shared(); + auto request_header = client->create_request_header(); + test_msgs::srv::Empty::Response response; + + this->template async_send_request< + ClientType, test_msgs::srv::Empty::Request>(client, request); + + EXPECT_FALSE(this->take_response(client, response, request_header)); + + { + // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_response, RCL_RET_OK); + EXPECT_TRUE(this->take_response(client, response, request_header)); + } + { + // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_response, RCL_RET_CLIENT_TAKE_FAILED); + EXPECT_FALSE(this->take_response(client, response, request_header)); + } + { + // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_response, RCL_RET_ERROR); + EXPECT_THROW( + this->take_response(client, response, request_header), + rclcpp::exceptions::RCLError); + } +} + +/* + Testing on_new_response callbacks. + */ +TYPED_TEST(TestAllClientTypesWithServer, on_new_response_callback) +{ + using ClientType = TypeParam; + + auto client_node = std::make_shared("test_client_node", "ns"); + auto server_node = std::make_shared("test_server_node", "ns"); + + rclcpp::ServicesQoS client_qos; + client_qos.keep_last(3); + + auto client = this->template create_client(client_node, "test_service", client_qos); + + 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, client_qos); + 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); + + this->template async_send_request(client, request); + auto start = std::chrono::steady_clock::now(); + while (server_requests_count == 0 && + (std::chrono::steady_clock::now() - start) < std::chrono::seconds(10)) + { + rclcpp::spin_some(server_node); + } + + ASSERT_EQ(server_requests_count, 1u); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < std::chrono::seconds(10)); + + 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); + + this->template async_send_request(client, request); + start = std::chrono::steady_clock::now(); + while (server_requests_count == 1 && + (std::chrono::steady_clock::now() - start) < std::chrono::seconds(10)) + { + rclcpp::spin_some(server_node); + } + + ASSERT_EQ(server_requests_count, 2u); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < std::chrono::seconds(10)); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + + client->clear_on_new_response_callback(); + + this->template async_send_request(client, request); + this->template async_send_request(client, request); + this->template async_send_request(client, request); + start = std::chrono::steady_clock::now(); + while (server_requests_count < 5 && + (std::chrono::steady_clock::now() - start) < std::chrono::seconds(10)) + { + 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(std::chrono::milliseconds(100)); + } while (c3 < 3 && std::chrono::steady_clock::now() - start < std::chrono::seconds(10)); + + 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); +} + +TYPED_TEST(TestAllClientTypesWithServer, client_qos) +{ + using ClientType = TypeParam; + + rclcpp::ServicesQoS qos_profile; + qos_profile.liveliness(rclcpp::LivelinessPolicy::Automatic); + rclcpp::Duration duration(std::chrono::nanoseconds(1)); + qos_profile.deadline(duration); + qos_profile.lifespan(duration); + qos_profile.liveliness_lease_duration(duration); + + auto client = this->template create_client( + this->node, this->service_name, qos_profile); + + auto rp_qos = client->get_request_publisher_actual_qos(); + auto rs_qos = client->get_response_subscription_actual_qos(); + + EXPECT_EQ(qos_profile, rp_qos); + // Lifespan has no meaning for subscription/readers + rs_qos.lifespan(qos_profile.lifespan()); + EXPECT_EQ(qos_profile, rs_qos); +} + +TYPED_TEST(TestAllClientTypesWithServer, rcl_client_request_publisher_get_actual_qos_error) +{ + using ClientType = TypeParam; + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_client_request_publisher_get_actual_qos, nullptr); + auto client = this->template create_client(this->node, "service"); + RCLCPP_EXPECT_THROW_EQ( + client->get_request_publisher_actual_qos(), + std::runtime_error("failed to get client's request publisher qos settings: error not set")); +} + +TYPED_TEST(TestAllClientTypesWithServer, rcl_client_response_subscription_get_actual_qos_error) +{ + using ClientType = TypeParam; + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_client_response_subscription_get_actual_qos, nullptr); + auto client = this->template create_client(this->node, "service"); + RCLCPP_EXPECT_THROW_EQ( + client->get_response_subscription_actual_qos(), + std::runtime_error("failed to get client's response subscription qos settings: error not set")); +} + +// The following tests are only for rclcpp::Client +void client_async_send_request_callback_with_request( + rclcpp::Node::SharedPtr node, const std::string service_name) +{ + using SharedFutureWithRequest = + rclcpp::Client::SharedFutureWithRequest; + + auto client = node->create_client(service_name); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + + auto request = std::make_shared(); + bool received_response = false; + auto callback = [&request, &received_response](SharedFutureWithRequest future) { + auto request_response_pair = future.get(); + EXPECT_EQ(request, request_response_pair.first); + EXPECT_NE(nullptr, request_response_pair.second); + received_response = true; + }; + auto req_id = client->async_send_request(request, std::move(callback)); + + auto start = std::chrono::steady_clock::now(); + while (!received_response && + (std::chrono::steady_clock::now() - start) < std::chrono::seconds(1)) + { + rclcpp::spin_some(node); + } + EXPECT_TRUE(received_response); + EXPECT_FALSE(client->remove_pending_request(req_id)); +} +TYPED_TEST(TestAllClientTypesWithServer, async_send_request_callback_with_request) +{ + using ClientType = TypeParam; + + if (std::is_same_v>) { + client_async_send_request_callback_with_request(this->node, this->service_name); + } else if (std::is_same_v) { + GTEST_SKIP() << "Skipping test for GenericClient"; + } else { + GTEST_SKIP() << "Skipping test"; + } +} + +void client_qos_depth(rclcpp::Node::SharedPtr node) +{ + using namespace std::literals::chrono_literals; + + rclcpp::ServicesQoS client_qos_profile; + client_qos_profile.keep_last(2); + + auto client = node->create_client("test_qos_depth", client_qos_profile); + + uint64_t server_cb_count_ = 0; + auto server_callback = [&]( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {server_cb_count_++;}; + + auto server_node = std::make_shared("server_node", "/ns"); + + rclcpp::QoS server_qos(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + + auto server = server_node->create_service( + "test_qos_depth", std::move(server_callback), server_qos); + + auto request = std::make_shared(); + ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); + + using SharedFuture = rclcpp::Client::SharedFuture; + uint64_t client_cb_count_ = 0; + auto client_callback = [&client_cb_count_, &request_result](SharedFuture future_response) { + if (nullptr == future_response.get()) { + request_result = ::testing::AssertionFailure() << "Future response was null"; + } + client_cb_count_++; + }; + + uint64_t client_requests = 5; + for (uint64_t i = 0; i < client_requests; i++) { + client->async_send_request(request, client_callback); + std::this_thread::sleep_for(10ms); + } + + auto start = std::chrono::steady_clock::now(); + while ((server_cb_count_ < client_requests) && + (std::chrono::steady_clock::now() - start) < 2s) + { + rclcpp::spin_some(server_node); + std::this_thread::sleep_for(2ms); + } + + EXPECT_GT(server_cb_count_, client_qos_profile.depth()); + + start = std::chrono::steady_clock::now(); + while ((client_cb_count_ < client_qos_profile.depth()) && + (std::chrono::steady_clock::now() - start) < 1s) + { + rclcpp::spin_some(node); + } + + // Spin an extra time to check if client QoS depth has been ignored, + // so more client callbacks might be called than expected. + rclcpp::spin_some(node); + + EXPECT_EQ(client_cb_count_, client_qos_profile.depth()); +} + +TYPED_TEST(TestAllClientTypesWithServer, qos_depth) +{ + using ClientType = TypeParam; + + if (std::is_same_v>) { + client_qos_depth(this->node); + } else if (std::is_same_v) { + GTEST_SKIP() << "Skipping test for GenericClient"; + } else { + GTEST_SKIP() << "Skipping test"; + } +} diff --git a/rclcpp/test/rclcpp/test_generic_client.cpp b/rclcpp/test/rclcpp/test_generic_client.cpp new file mode 100644 index 0000000000..be65ea1f53 --- /dev/null +++ b/rclcpp/test/rclcpp/test_generic_client.cpp @@ -0,0 +1,230 @@ +// Copyright 2023 Sony Group Corporation. +// +// 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. + +#include + +#include +#include +#include + +#include "rclcpp/create_generic_client.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/serialization.hpp" + +#include "rcl_interfaces/srv/list_parameters.hpp" + +#include "../mocking_utils/patch.hpp" + +#include "test_msgs/srv/empty.hpp" + +using namespace std::chrono_literals; + +// All tests are from test_client + +class TestGenericClient : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_node", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +class TestGenericClientSub : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_node", "/ns"); + subnode = node->create_sub_node("sub_ns"); + } + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::Node::SharedPtr subnode; +}; + +/* + Testing client construction and destruction. + */ +TEST_F(TestGenericClient, construction_and_destruction) { + { + auto client = node->create_generic_client("test_service", "test_msgs/srv/Empty"); + } + + { + ASSERT_THROW( + { + auto client = node->create_generic_client("invalid_test_service?", "test_msgs/srv/Empty"); + }, rclcpp::exceptions::InvalidServiceNameError); + } + + { + ASSERT_THROW( + { + auto client = node->create_generic_client("test_service", "test_msgs/srv/InvalidType"); + }, std::runtime_error); + } +} + +TEST_F(TestGenericClient, construction_with_free_function) { + { + auto client = rclcpp::create_generic_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + "test_service", + "test_msgs/srv/Empty", + rclcpp::ServicesQoS(), + nullptr); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_generic_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + "invalid_?test_service", + "test_msgs/srv/Empty", + rclcpp::ServicesQoS(), + nullptr); + }, rclcpp::exceptions::InvalidServiceNameError); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_generic_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + "test_service", + "test_msgs/srv/InvalidType", + rclcpp::ServicesQoS(), + nullptr); + }, std::runtime_error); + } + { + auto client = rclcpp::create_generic_client( + node, + "test_service", + "test_msgs/srv/Empty", + rclcpp::ServicesQoS(), + nullptr); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_generic_client( + node, + "invalid_?test_service", + "test_msgs/srv/Empty", + rclcpp::ServicesQoS(), + nullptr); + }, rclcpp::exceptions::InvalidServiceNameError); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_generic_client( + node, + "invalid_?test_service", + "test_msgs/srv/InvalidType", + rclcpp::ServicesQoS(), + nullptr); + }, std::runtime_error); + } +} + +TEST_F(TestGenericClient, construct_with_rcl_error) { + { + // reset() is not necessary for this exception, but handles unused return value warning + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_client_init, RCL_RET_ERROR); + EXPECT_THROW( + node->create_generic_client("test_service", "test_msgs/srv/Empty").reset(), + rclcpp::exceptions::RCLError); + } + { + // reset() is required for this one + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_client_fini, RCL_RET_ERROR); + EXPECT_NO_THROW( + node->create_generic_client("test_service", "test_msgs/srv/Empty").reset()); + } +} + +TEST_F(TestGenericClient, wait_for_service) { + const std::string service_name = "test_service"; + + auto client = node->create_generic_client(service_name, "test_msgs/srv/Empty"); + EXPECT_FALSE(client->wait_for_service(std::chrono::nanoseconds(0))); + EXPECT_FALSE(client->wait_for_service(std::chrono::milliseconds(10))); + + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + + auto service = + node->create_service(service_name, std::move(callback)); + + EXPECT_TRUE(client->wait_for_service(std::chrono::nanoseconds(-1))); + EXPECT_TRUE(client->service_is_ready()); +} + +/* + Testing generic client construction and destruction for subnodes. + */ +TEST_F(TestGenericClientSub, construction_and_destruction) { + { + auto client = subnode->create_generic_client("test_service", "test_msgs/srv/Empty"); + EXPECT_STREQ(client->get_service_name(), "/ns/test_service"); + } + + { + ASSERT_THROW( + { + auto client = node->create_generic_client("invalid_service?", "test_msgs/srv/Empty"); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} From 51a4d2ea39af94bf55b224d7b76a05650d5bc038 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 5 Mar 2024 17:53:55 -0500 Subject: [PATCH 343/455] Fix TypeAdapted publishing with large messages. (#2443) Mostly by ensuring we aren't attempting to store large messages on the stack. Also add in tests. I verified that before these changes, the tests failed, while after them they succeed. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/publisher.hpp | 24 +++---- rclcpp/test/msg/LargeMessage.msg | 3 + rclcpp/test/rclcpp/CMakeLists.txt | 1 + .../test_publisher_with_type_adapter.cpp | 71 ++++++++++++++++++- 4 files changed, 84 insertions(+), 15 deletions(-) create mode 100644 rclcpp/test/msg/LargeMessage.msg diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 08a98bdf1a..c6f16643c6 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -290,8 +290,8 @@ class Publisher : public PublisherBase { // Avoid allocating when not using intra process. if (!intra_process_is_enabled_) { - // In this case we're not using intra process. - return this->do_inter_process_publish(msg); + this->do_inter_process_publish(msg); + return; } // Otherwise we have to allocate memory in a unique_ptr and pass it along. // As the message is not const, a copy should be made. @@ -318,12 +318,12 @@ class Publisher : public PublisherBase > publish(std::unique_ptr msg) { - // Avoid allocating when not using intra process. if (!intra_process_is_enabled_) { // In this case we're not using intra process. - ROSMessageType ros_msg; - rclcpp::TypeAdapter::convert_to_ros_message(*msg, ros_msg); - return this->do_inter_process_publish(ros_msg); + auto ros_msg_ptr = std::make_unique(); + rclcpp::TypeAdapter::convert_to_ros_message(*msg, *ros_msg_ptr); + this->do_inter_process_publish(*ros_msg_ptr); + return; } bool inter_process_publish_needed = @@ -331,9 +331,6 @@ class Publisher : public PublisherBase if (inter_process_publish_needed) { auto ros_msg_ptr = std::make_shared(); - // TODO(clalancette): This is unnecessarily doing an additional conversion - // that may have already been done in do_intra_process_publish_and_return_shared(). - // We should just reuse that effort. rclcpp::TypeAdapter::convert_to_ros_message(*msg, *ros_msg_ptr); this->do_intra_process_publish(std::move(msg)); this->do_inter_process_publish(*ros_msg_ptr); @@ -368,13 +365,12 @@ class Publisher : public PublisherBase > publish(const T & msg) { - // Avoid double allocating when not using intra process. if (!intra_process_is_enabled_) { // Convert to the ROS message equivalent and publish it. - ROSMessageType ros_msg; - rclcpp::TypeAdapter::convert_to_ros_message(msg, ros_msg); - // In this case we're not using intra process. - return this->do_inter_process_publish(ros_msg); + auto ros_msg_ptr = std::make_unique(); + rclcpp::TypeAdapter::convert_to_ros_message(msg, *ros_msg_ptr); + this->do_inter_process_publish(*ros_msg_ptr); + return; } // Otherwise we have to allocate memory in a unique_ptr and pass it along. diff --git a/rclcpp/test/msg/LargeMessage.msg b/rclcpp/test/msg/LargeMessage.msg new file mode 100644 index 0000000000..1e383c0bae --- /dev/null +++ b/rclcpp/test/msg/LargeMessage.msg @@ -0,0 +1,3 @@ +# A message with a size larger than the default Linux stack size +uint8[10485760] data +uint64 size diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index c17ed6fe06..25ca4082e1 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -6,6 +6,7 @@ add_definitions(-DTEST_RESOURCES_DIRECTORY="${TEST_RESOURCES_DIRECTORY}") rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs ../msg/Header.msg + ../msg/LargeMessage.msg ../msg/MessageWithHeader.msg ../msg/String.msg DEPENDENCIES builtin_interfaces diff --git a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp index e96e1e3a38..9367a89014 100644 --- a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. - #include +#include #include #include #include @@ -25,6 +25,7 @@ #include "rclcpp/loaned_message.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/msg/large_message.hpp" #include "rclcpp/msg/string.hpp" @@ -105,6 +106,32 @@ struct TypeAdapter } }; +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = std::string; + using ros_message_type = rclcpp::msg::LargeMessage; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + destination.size = source.size(); + std::memcpy(destination.data.data(), source.data(), source.size()); + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + destination.resize(source.size); + std::memcpy(destination.data(), source.data.data(), source.size); + } +}; + } // namespace rclcpp /* @@ -336,3 +363,45 @@ TEST_F(TestPublisher, check_type_adapted_message_is_sent_and_received) { assert_message_was_received(); } } + +TEST_F(TestPublisher, test_large_message_unique) +{ + // There have been some bugs in the past when trying to type-adapt large messages + // (larger than the stack size). Here we just make sure that a 10MB message works, + // which is larger than the default stack size on Linux. + + using StringTypeAdapter = rclcpp::TypeAdapter; + + auto node = std::make_shared("my_node", "/ns", rclcpp::NodeOptions()); + + const std::string topic_name = "topic_name"; + + auto pub = node->create_publisher(topic_name, 1); + + static constexpr size_t length = 10 * 1024 * 1024; + auto message_data = std::make_unique(); + message_data->reserve(length); + std::fill(message_data->begin(), message_data->begin() + length, '#'); + pub->publish(std::move(message_data)); +} + +TEST_F(TestPublisher, test_large_message_constref) +{ + // There have been some bugs in the past when trying to type-adapt large messages + // (larger than the stack size). Here we just make sure that a 10MB message works, + // which is larger than the default stack size on Linux. + + using StringTypeAdapter = rclcpp::TypeAdapter; + + auto node = std::make_shared("my_node", "/ns", rclcpp::NodeOptions()); + + const std::string topic_name = "topic_name"; + + auto pub = node->create_publisher(topic_name, 1); + + static constexpr size_t length = 10 * 1024 * 1024; + std::string message_data; + message_data.reserve(length); + std::fill(message_data.begin(), message_data.begin() + length, '#'); + pub->publish(message_data); +} From 5e14a283b67940526a20c365b827d8090a552bad Mon Sep 17 00:00:00 2001 From: mauropasse Date: Wed, 6 Mar 2024 13:45:58 +0000 Subject: [PATCH 344/455] Modify rclcpp_action::GoalUUID hashing algorithm (#2441) * Add unit tests for hashing rclcpp_action::GoalUUID's * Use the FNV-1a hash algorithm for Goal UUID Signed-off-by: Mauro Passerino --- rclcpp_action/include/rclcpp_action/types.hpp | 15 ++++----- rclcpp_action/test/test_types.cpp | 33 +++++++++++++++++++ 2 files changed, 40 insertions(+), 8 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index 829b6cd2c1..cf359dfaa9 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -69,14 +69,13 @@ struct hash { size_t operator()(const rclcpp_action::GoalUUID & uuid) const noexcept { - // TODO(sloretz) Use someone else's hash function and cite it - size_t result = 0; - for (size_t i = 0; i < uuid.size(); ++i) { - for (size_t b = 0; b < sizeof(size_t); ++b) { - size_t part = uuid[i]; - part <<= CHAR_BIT * b; - result ^= part; - } + // Using the FNV-1a hash algorithm + constexpr size_t FNV_prime = 1099511628211u; + size_t result = 14695981039346656037u; + + for (const auto & byte : uuid) { + result ^= byte; + result *= FNV_prime; } return result; } diff --git a/rclcpp_action/test/test_types.cpp b/rclcpp_action/test/test_types.cpp index 4922c52930..619a4f7899 100644 --- a/rclcpp_action/test/test_types.cpp +++ b/rclcpp_action/test/test_types.cpp @@ -15,6 +15,7 @@ #include #include +#include #include "rclcpp_action/types.hpp" TEST(TestActionTypes, goal_uuid_to_string) { @@ -59,3 +60,35 @@ TEST(TestActionTypes, rcl_action_goal_info_to_goal_uuid) { EXPECT_EQ(goal_info.goal_id.uuid[i], goal_id[i]); } } + +TEST(TestActionTypes, goal_uuid_to_hashed_uuid_random) { + // Use std::random_device to seed the generator of goal IDs. + std::random_device rd; + std::independent_bits_engine< + std::default_random_engine, 8, decltype(rd())> random_bytes_generator(rd()); + + std::vector hashed_guuids; + constexpr size_t iterations = 1000; + + for (size_t i = 0; i < iterations; i++) { + rclcpp_action::GoalUUID goal_id; + + // Generate random bytes for each element of the array + for (auto & element : goal_id) { + element = static_cast(random_bytes_generator()); + } + + size_t new_hashed_guuid = std::hash()(goal_id); + + // Search for any prevoius hashed goal_id with the same value + for (auto prev_hashed_guuid : hashed_guuids) { + EXPECT_NE(prev_hashed_guuid, new_hashed_guuid); + if (prev_hashed_guuid == new_hashed_guuid) { + // Fail before the first occurrence of a collision + GTEST_FAIL(); + } + } + + hashed_guuids.push_back(new_hashed_guuid); + } +} From 1c350d0d7fb9c7158e0a39057112486ddbd38e9a Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Mon, 18 Mar 2024 13:37:26 -0700 Subject: [PATCH 345/455] relax the test simulation rate for timer canceling tests. (#2453) Signed-off-by: Tomoya Fujita --- .../rclcpp/executors/test_executors_timer_cancel_behavior.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp index ecee459a19..ea938d268a 100644 --- a/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp @@ -102,7 +102,7 @@ class TimerNode : public rclcpp::Node class ClockPublisher : public rclcpp::Node { public: - explicit ClockPublisher(float simulated_clock_step = .001f, float realtime_update_rate = 0.25f) + explicit ClockPublisher(float simulated_clock_step = .001f, float realtime_update_rate = 0.10f) : Node("clock_publisher"), ros_update_duration_(0, 0), realtime_clock_step_(0, 0), From 3e470d5ac2b8d5e77d3c0c26402087f5cffa2db1 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Wed, 20 Mar 2024 12:37:56 -0700 Subject: [PATCH 346/455] Revert "relax the test simulation rate for timer canceling tests. (#2453)" (#2456) This reverts commit 1c350d0d7fb9c7158e0a39057112486ddbd38e9a. Signed-off-by: Tomoya Fujita --- .../rclcpp/executors/test_executors_timer_cancel_behavior.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp index ea938d268a..ecee459a19 100644 --- a/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp @@ -102,7 +102,7 @@ class TimerNode : public rclcpp::Node class ClockPublisher : public rclcpp::Node { public: - explicit ClockPublisher(float simulated_clock_step = .001f, float realtime_update_rate = 0.10f) + explicit ClockPublisher(float simulated_clock_step = .001f, float realtime_update_rate = 0.25f) : Node("clock_publisher"), ros_update_duration_(0, 0), realtime_clock_step_(0, 0), From 198c82ca1509b3cde7ce43b1afc94bb1b0257f8e Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Thu, 21 Mar 2024 00:10:15 -0700 Subject: [PATCH 347/455] enable simulation clock for timer canceling test. (#2458) * enable simulation clock for timer canceling test. Signed-off-by: Tomoya Fujita * move MainExecutorTypes to test_executors_timer_cancel_behavior.cpp. Signed-off-by: Tomoya Fujita --------- Signed-off-by: Tomoya Fujita --- .../test_executors_timer_cancel_behavior.cpp | 34 +++++++++++++++---- 1 file changed, 28 insertions(+), 6 deletions(-) diff --git a/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp index ecee459a19..7fd1676c5d 100644 --- a/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp @@ -35,14 +35,20 @@ class TimerNode : public rclcpp::Node public: explicit TimerNode(std::string subname) : Node("timer_node", subname) + { + } + + void CreateTimer1() { timer1_ = rclcpp::create_timer( this->get_node_base_interface(), get_node_timers_interface(), get_clock(), 1ms, std::bind(&TimerNode::Timer1Callback, this)); + } - timer2_ = - rclcpp::create_timer( + void CreateTimer2() + { + timer2_ = rclcpp::create_timer( this->get_node_base_interface(), get_node_timers_interface(), get_clock(), 1ms, std::bind(&TimerNode::Timer2Callback, this)); @@ -76,14 +82,14 @@ class TimerNode : public rclcpp::Node private: void Timer1Callback() { - RCLCPP_DEBUG(this->get_logger(), "Timer 1!"); cnt1_++; + RCLCPP_DEBUG(this->get_logger(), "Timer 1! (%d)", cnt1_); } void Timer2Callback() { - RCLCPP_DEBUG(this->get_logger(), "Timer 2!"); cnt2_++; + RCLCPP_DEBUG(this->get_logger(), "Timer 2! (%d)", cnt2_); } rclcpp::TimerBase::SharedPtr timer1_; @@ -200,11 +206,18 @@ class TestTimerCancelBehavior : public ::testing::Test ASSERT_TRUE(param_client->wait_for_service(5s)); auto set_parameters_results = param_client->set_parameters( - {rclcpp::Parameter("use_sim_time", false)}); + {rclcpp::Parameter("use_sim_time", true)}); for (auto & result : set_parameters_results) { ASSERT_TRUE(result.successful); } + // Check if the clock type is simulation time + EXPECT_EQ(RCL_ROS_TIME, node->get_clock()->get_clock_type()); + + // Create timers + this->node->CreateTimer1(); + this->node->CreateTimer2(); + // Run standalone thread to publish clock time sim_clock_node = std::make_shared(); @@ -233,7 +246,16 @@ class TestTimerCancelBehavior : public ::testing::Test T executor; }; -TYPED_TEST_SUITE(TestTimerCancelBehavior, ExecutorTypes, ExecutorTypeNames); +using MainExecutorTypes = + ::testing::Types< + rclcpp::executors::SingleThreadedExecutor, + rclcpp::executors::MultiThreadedExecutor, + rclcpp::executors::StaticSingleThreadedExecutor>; + +// TODO(@fujitatomoya): this test excludes EventExecutor because it does not +// support simulation time used for this test to relax the racy condition. +// See more details for https://github.com/ros2/rclcpp/issues/2457. +TYPED_TEST_SUITE(TestTimerCancelBehavior, MainExecutorTypes, ExecutorTypeNames); TYPED_TEST(TestTimerCancelBehavior, testTimer1CancelledWithExecutorSpin) { // Validate that cancelling one timer yields no change in behavior for other From b50f9ab41840dfec121b7d12118da6e6593d21ae Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 22 Mar 2024 12:07:30 -0700 Subject: [PATCH 348/455] refactor and improve the parameterized spin_some tests for executors (#2460) * refactor and improve the spin_some parameterized tests for executors Signed-off-by: William Woodall * disable spin_some_max_duration for the StaticSingleThreadedExecutor and EventsExecutor Signed-off-by: William Woodall * fixup and clarify the docstring for Executor::spin_some() Signed-off-by: William Woodall * style Signed-off-by: William Woodall * review comments Signed-off-by: William Woodall --------- Signed-off-by: William Woodall --- rclcpp/include/rclcpp/executor.hpp | 21 +- .../test/rclcpp/executors/test_executors.cpp | 213 +++++++++++++++--- 2 files changed, 195 insertions(+), 39 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index b4f1b9d8f9..dc6020178c 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -282,16 +282,23 @@ class Executor void spin_node_some(std::shared_ptr node); - /// Collect work once and execute all available work, optionally within a duration. + /// Collect work once and execute all available work, optionally within a max duration. /** - * This function can be overridden. The default implementation is suitable for a - * single-threaded model of execution. - * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function - * to block (which may have unintended consequences). + * This function can be overridden. + * The default implementation is suitable for a single-threaded model of execution. + * Adding subscriptions, timers, services, etc. with blocking or long running + * callbacks may cause the function exceed the max_duration significantly. + * + * If there is no work to be done when this called, it will return immediately + * because the collecting of available work is non-blocking. + * Before each piece of ready work is executed this function checks if the + * max_duration has been exceeded, and if it has it returns without starting + * the execution of the next piece of work. + * + * If a max_duration of 0 is given, then all of the collected work will be + * executed before the function returns. * * \param[in] max_duration The maximum amount of time to spend executing work, or 0 for no limit. - * Note that spin_some() may take longer than this time as it only returns once max_duration has - * been exceeded. */ RCLCPP_PUBLIC virtual void diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 4086c14668..87ed9350dd 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -374,7 +374,20 @@ class TestWaitable : public rclcpp::Waitable { (void) data; count_++; - std::this_thread::sleep_for(3ms); + if (nullptr != on_execute_callback_) { + on_execute_callback_(); + } else { + // TODO(wjwwood): I don't know why this was here, but probably it should + // not be there, or test cases where that is important should use the + // on_execute_callback? + std::this_thread::sleep_for(3ms); + } + } + + void + set_on_execute_callback(std::function on_execute_callback) + { + on_execute_callback_ = on_execute_callback; } void @@ -404,6 +417,7 @@ class TestWaitable : public rclcpp::Waitable private: size_t count_ = 0; rclcpp::GuardCondition gc_; + std::function on_execute_callback_ = nullptr; }; TYPED_TEST(TestExecutors, spinAll) @@ -448,45 +462,180 @@ TYPED_TEST(TestExecutors, spinAll) spinner.join(); } -TYPED_TEST(TestExecutors, spinSome) +// Helper function to convert chrono durations into a scalar that GoogleTest +// can more easily compare and print. +template +auto +to_nanoseconds_helper(DurationT duration) +{ + return std::chrono::duration_cast(duration).count(); +} + +// The purpose of this test is to check that the ExecutorT.spin_some() method: +// - works nominally (it can execute entities) +// - it can execute multiple items at once +// - it does not wait for work to be available before returning +TYPED_TEST(TestExecutors, spin_some) { using ExecutorType = TypeParam; - ExecutorType executor; + + // Use an isolated callback group to avoid interference from any housekeeping + // items that may be in the default callback group of the node. + constexpr bool automatically_add_to_executor_with_node = false; + auto isolated_callback_group = this->node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + automatically_add_to_executor_with_node); + + // Check that spin_some() returns quickly when there is no work to be done. + // This can be a false positive if there is somehow some work for the executor + // to do that has not been considered, but the isolated callback group should + // avoid that. + { + ExecutorType executor; + executor.add_callback_group(isolated_callback_group, this->node->get_node_base_interface()); + + auto start = std::chrono::steady_clock::now(); + // spin_some with some non-trival "max_duration" and check that it does not + // take anywhere near that long to execute. + constexpr auto max_duration = 10s; + executor.spin_some(max_duration); + EXPECT_LT( + to_nanoseconds_helper(std::chrono::steady_clock::now() - start), + to_nanoseconds_helper(max_duration / 2)) + << "spin_some() took a long time to execute when it should have done " + << "nothing and should not have blocked either, but this could be a " + << "false negative if the computer is really slow"; + } + + // Check that having one thing ready gets executed by spin_some(). auto waitable_interfaces = this->node->get_node_waitables_interface(); - auto my_waitable = std::make_shared(); - waitable_interfaces->add_waitable(my_waitable, nullptr); - executor.add_node(this->node); + auto my_waitable1 = std::make_shared(); + waitable_interfaces->add_waitable(my_waitable1, isolated_callback_group); + { + ExecutorType executor; + executor.add_callback_group(isolated_callback_group, this->node->get_node_base_interface()); + + my_waitable1->trigger(); + + // The long duration should not matter, as executing the waitable is + // non-blocking, and spin_some() should exit after completing the available + // work. + auto start = std::chrono::steady_clock::now(); + constexpr auto max_duration = 10s; + executor.spin_some(max_duration); + EXPECT_LT( + to_nanoseconds_helper(std::chrono::steady_clock::now() - start), + to_nanoseconds_helper(max_duration / 2)) + << "spin_some() took a long time to execute when it should have very " + << "little to do and should not have blocked either, but this could be a " + << "false negative if the computer is really slow"; + + EXPECT_EQ(my_waitable1->get_count(), 1u) + << "spin_some() failed to execute a waitable that was triggered"; + } - // Long timeout, doesn't block test from finishing because spin_some should exit after the - // first one completes. - bool spin_exited = false; - std::thread spinner([&spin_exited, &executor, this]() { - executor.spin_some(1s); - executor.remove_node(this->node, true); - spin_exited = true; - }); + // Check that multiple things being ready are executed by spin_some(). + auto my_waitable2 = std::make_shared(); + waitable_interfaces->add_waitable(my_waitable2, isolated_callback_group); + { + ExecutorType executor; + executor.add_callback_group(isolated_callback_group, this->node->get_node_base_interface()); + + const size_t original_my_waitable1_count = my_waitable1->get_count(); + my_waitable1->trigger(); + my_waitable2->trigger(); + + // The long duration should not matter, as executing the waitable is + // non-blocking, and spin_some() should exit after completing the available + // work. + auto start = std::chrono::steady_clock::now(); + constexpr auto max_duration = 10s; + executor.spin_some(max_duration); + EXPECT_LT( + to_nanoseconds_helper(std::chrono::steady_clock::now() - start), + to_nanoseconds_helper(max_duration / 2)) + << "spin_some() took a long time to execute when it should have very " + << "little to do and should not have blocked either, but this could be a " + << "false negative if the computer is really slow"; + + EXPECT_EQ(my_waitable1->get_count(), original_my_waitable1_count + 1) + << "spin_some() failed to execute a waitable that was triggered"; + EXPECT_EQ(my_waitable2->get_count(), 1u) + << "spin_some() failed to execute a waitable that was triggered"; + } +} - // Do some work until sufficient calls to the waitable occur, but keep going until either - // count becomes too large, spin exits, or the 1 second timeout completes. - auto start = std::chrono::steady_clock::now(); - while ( - my_waitable->get_count() <= 1 && - !spin_exited && - (std::chrono::steady_clock::now() - start < 1s)) +// The purpose of this test is to check that the ExecutorT.spin_some() method: +// - does not continue executing after max_duration has elapsed +TYPED_TEST(TestExecutors, spin_some_max_duration) +{ + using ExecutorType = TypeParam; + + // TODO(wjwwood): The `StaticSingleThreadedExecutor` and the `EventsExecutor` + // do not properly implement max_duration (it seems), so disable this test + // for them in the meantime. + // see: https://github.com/ros2/rclcpp/issues/2462 + if ( + std::is_same() || + std::is_same()) { - my_waitable->trigger(); - this->publisher->publish(test_msgs::msg::Empty()); - std::this_thread::sleep_for(1ms); + GTEST_SKIP(); } - // The count of "execute" depends on whether the executor starts spinning before (1) or after (0) - // the first iteration of the while loop - EXPECT_LE(1u, my_waitable->get_count()); - waitable_interfaces->remove_waitable(my_waitable, nullptr); - EXPECT_TRUE(spin_exited); - // Cancel if it hasn't exited already. - executor.cancel(); - spinner.join(); + // Use an isolated callback group to avoid interference from any housekeeping + // items that may be in the default callback group of the node. + constexpr bool automatically_add_to_executor_with_node = false; + auto isolated_callback_group = this->node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + automatically_add_to_executor_with_node); + + // Set up a situation with two waitables that take time to execute, such that + // the time it takes to execute two waitables far exceeds the max_duration + // given to spin_some(), which should result in spin_some() starting to + // execute one of them, have the max duration elapse, finish executing one + // of them, then returning before starting on the second. + constexpr auto max_duration = 100ms; // relatively short because we expect to exceed it + constexpr auto waitable_callback_duration = max_duration * 2; + auto long_running_callback = [&waitable_callback_duration]() { + std::this_thread::sleep_for(waitable_callback_duration); + }; + + auto waitable_interfaces = this->node->get_node_waitables_interface(); + + auto my_waitable1 = std::make_shared(); + my_waitable1->set_on_execute_callback(long_running_callback); + waitable_interfaces->add_waitable(my_waitable1, isolated_callback_group); + + auto my_waitable2 = std::make_shared(); + my_waitable2->set_on_execute_callback(long_running_callback); + waitable_interfaces->add_waitable(my_waitable2, isolated_callback_group); + + ExecutorType executor; + executor.add_callback_group(isolated_callback_group, this->node->get_node_base_interface()); + + auto start = std::chrono::steady_clock::now(); + // spin_some and check that it does not take longer than two of waitable_callback_duration, + // nor significantly less than a single waitable_callback_duration. + executor.spin_some(max_duration); + auto spin_some_run_time = std::chrono::steady_clock::now() - start; + EXPECT_GT( + to_nanoseconds_helper(spin_some_run_time), + to_nanoseconds_helper(waitable_callback_duration / 2)) + << "spin_some() took less than half the expected time to execute a single " + << "waitable, which implies it did not actually execute one when it was " + << "expected to"; + EXPECT_LT( + to_nanoseconds_helper(spin_some_run_time), + to_nanoseconds_helper(waitable_callback_duration * 2)) + << "spin_some() took longer than expected to execute by a significant margin, but " + << "this could be a false positive on a very slow computer"; + + // check that exactly one of the waitables were executed (do not depend on a specific order) + size_t number_of_waitables_executed = my_waitable1->get_count() + my_waitable2->get_count(); + EXPECT_EQ(number_of_waitables_executed, 1u) + << "expected exactly one of the two waitables to be executed, but " + << "my_waitable1->get_count(): " << my_waitable1->get_count() << " and " + << "my_waitable2->get_count(): " << my_waitable2->get_count(); } // Check spin_node_until_future_complete with node base pointer From 42810ee3be16621208e2439cd6257a584d827759 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Tue, 26 Mar 2024 06:34:28 -0700 Subject: [PATCH 349/455] fix spin_some_max_duration unit-test for events-executor (#2465) Signed-off-by: Alberto Soragna --- rclcpp/test/rclcpp/executors/test_executors.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 87ed9350dd..da4b087607 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -352,8 +352,13 @@ class TestWaitable : public rclcpp::Waitable bool is_ready(rcl_wait_set_t * wait_set) override { - (void)wait_set; - return true; + 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; + } + } + return false; } std::shared_ptr @@ -571,13 +576,12 @@ TYPED_TEST(TestExecutors, spin_some_max_duration) { using ExecutorType = TypeParam; - // TODO(wjwwood): The `StaticSingleThreadedExecutor` and the `EventsExecutor` + // TODO(wjwwood): The `StaticSingleThreadedExecutor` // do not properly implement max_duration (it seems), so disable this test // for them in the meantime. // see: https://github.com/ros2/rclcpp/issues/2462 if ( - std::is_same() || - std::is_same()) + std::is_same()) { GTEST_SKIP(); } @@ -610,6 +614,9 @@ TYPED_TEST(TestExecutors, spin_some_max_duration) my_waitable2->set_on_execute_callback(long_running_callback); waitable_interfaces->add_waitable(my_waitable2, isolated_callback_group); + my_waitable1->trigger(); + my_waitable2->trigger(); + ExecutorType executor; executor.add_callback_group(isolated_callback_group, this->node->get_node_base_interface()); From 6c7764e9681c3e82eae03262f7a595c13d5a3685 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Wed, 27 Mar 2024 09:43:54 -0700 Subject: [PATCH 350/455] Do not generate the exception when action service response timeout. (#2464) * Do not generate the exception when action service response timeout. Signed-off-by: Tomoya Fujita * address review comment. Signed-off-by: Tomoya Fujita --------- Signed-off-by: Tomoya Fujita --- rclcpp_action/src/server.cpp | 36 ++++++++++++++++++++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index c0fa96a88e..780b2ba20c 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -344,7 +344,16 @@ ServerBase::execute_goal_request_received(std::shared_ptr & data) } if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + if (ret == RCL_RET_TIMEOUT) { + RCLCPP_WARN( + pimpl_->logger_, + "Failed to send goal response %s (timeout): %s", + to_string(uuid).c_str(), rcl_get_error_string().str); + rcl_reset_error(); + return; + } else { + rclcpp::exceptions::throw_from_rcl_error(ret); + } } const auto status = response_pair.first; @@ -483,6 +492,15 @@ ServerBase::execute_cancel_request_received(std::shared_ptr & data) pimpl_->action_server_.get(), &request_header, response.get()); } + if (ret == RCL_RET_TIMEOUT) { + GoalUUID uuid = request->goal_info.goal_id.uuid; + RCLCPP_WARN( + pimpl_->logger_, + "Failed to send cancel response %s (timeout): %s", + to_string(uuid).c_str(), rcl_get_error_string().str); + rcl_reset_error(); + return; + } if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } @@ -538,6 +556,14 @@ ServerBase::execute_result_request_received(std::shared_ptr & data) std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); rcl_ret_t rcl_ret = rcl_action_send_result_response( pimpl_->action_server_.get(), &request_header, result_response.get()); + if (rcl_ret == RCL_RET_TIMEOUT) { + RCLCPP_WARN( + pimpl_->logger_, + "Failed to send result response %s (timeout): %s", + to_string(uuid).c_str(), rcl_get_error_string().str); + rcl_reset_error(); + return; + } if (RCL_RET_OK != rcl_ret) { rclcpp::exceptions::throw_from_rcl_error(rcl_ret); } @@ -671,7 +697,13 @@ ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr result_m for (auto & request_header : iter->second) { rcl_ret_t ret = rcl_action_send_result_response( pimpl_->action_server_.get(), &request_header, result_msg.get()); - if (RCL_RET_OK != ret) { + if (ret == RCL_RET_TIMEOUT) { + RCLCPP_WARN( + pimpl_->logger_, + "Failed to send result response %s (timeout): %s", + to_string(uuid).c_str(), rcl_get_error_string().str); + rcl_reset_error(); + } else if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } } From 1e34abd4b5a89c10f4412cfed8985630d62a3c31 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Thu, 28 Mar 2024 08:53:31 +0000 Subject: [PATCH 351/455] Changelog. Signed-off-by: Marco A. Gutierrez --- rclcpp/CHANGELOG.rst | 48 +++++++++++++++++++++++++++++++++ rclcpp_action/CHANGELOG.rst | 14 ++++++++++ rclcpp_components/CHANGELOG.rst | 10 +++++++ rclcpp_lifecycle/CHANGELOG.rst | 5 ++++ 4 files changed, 77 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 8a520a706e..39107a3fd3 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,54 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* fix spin_some_max_duration unit-test for events-executor (`#2465 `_) +* refactor and improve the parameterized spin_some tests for executors (`#2460 `_) + * refactor and improve the spin_some parameterized tests for executors + * disable spin_some_max_duration for the StaticSingleThreadedExecutor and EventsExecutor + * fixup and clarify the docstring for Executor::spin_some() + * style + * review comments + --------- +* enable simulation clock for timer canceling test. (`#2458 `_) + * enable simulation clock for timer canceling test. + * move MainExecutorTypes to test_executors_timer_cancel_behavior.cpp. + --------- +* Revert "relax the test simulation rate for timer canceling tests. (`#2453 `_)" (`#2456 `_) + This reverts commit 1c350d0d7fb9c7158e0a39057112486ddbd38e9a. +* relax the test simulation rate for timer canceling tests. (`#2453 `_) +* Fix TypeAdapted publishing with large messages. (`#2443 `_) + Mostly by ensuring we aren't attempting to store + large messages on the stack. Also add in tests. + I verified that before these changes, the tests failed, + while after them they succeed. +* Implement generic client (`#2358 `_) + * Implement generic client + * Fix the incorrect parameter declaration + * Deleted copy constructor and assignment for FutureAndRequestId + * Update codes after rebase + * Address review comments + * Address review comments from iuhilnehc-ynos + * Correct an error in a description + * Fix window build errors + * Address review comments from William + * Add doc strings to create_generic_client + --------- +* Rule of five: implement move operators (`#2425 `_) +* Various cleanups to deal with uncrustify 0.78. (`#2439 `_) + These should also work with uncrustify 0.72. +* Remove the set_deprecated signatures in any_subscription_callback. (`#2431 `_) + These have been deprecated since April 2021, so it is safe + to remove them now. +* fix doxygen syntax for NodeInterfaces (`#2428 `_) +* Set hints to find the python version we actually want. (`#2426 `_) + The comment in the commit explains the reasoning behind it. +* Update quality declaration documents (`#2427 `_) +* feat: add/minus for msg::Time and rclcpp::Duration (`#2419 `_) + * feat: add/minus for msg::Time and rclcpp::Duration +* Contributors: Alberto Soragna, Barry Xu, Chris Lalancette, Christophe Bedard, HuaTsai, Jonas Otto, Tim Clephas, Tomoya Fujita, William Woodall + 27.0.0 (2024-02-07) ------------------- * Split test_executors up into smaller chunks. (`#2421 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index f2ab3fdb20..3f7284e400 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,20 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Do not generate the exception when action service response timeout. (`#2464 `_) + * Do not generate the exception when action service response timeout. + * address review comment. + --------- +* Modify rclcpp_action::GoalUUID hashing algorithm (`#2441 `_) + * Add unit tests for hashing rclcpp_action::GoalUUID's + * Use the FNV-1a hash algorithm for Goal UUID +* Various cleanups to deal with uncrustify 0.78. (`#2439 `_) + These should also work with uncrustify 0.72. +* Update quality declaration documents (`#2427 `_) +* Contributors: Chris Lalancette, Christophe Bedard, Tomoya Fujita, mauropasse + 27.0.0 (2024-02-07) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 23689424e8..e4458d0ec0 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add EXECUTOR docs (`#2440 `_) +* Update quality declaration documents (`#2427 `_) +* crash on no class found (`#2415 `_) + * crash on no class found + * error on no class found instead of no callback groups + Co-authored-by: Chris Lalancette +* Contributors: Adam Aposhian, Christophe Bedard, Ruddick Lawrence + 27.0.0 (2024-02-07) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index a19f4ee85d..14135550f6 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update quality declaration documents (`#2427 `_) +* Contributors: Christophe Bedard + 27.0.0 (2024-02-07) ------------------- From bedc547f42700b9b3f8f00244a51402e1228634b Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Thu, 28 Mar 2024 08:53:45 +0000 Subject: [PATCH 352/455] 28.0.0 Signed-off-by: Marco A. Gutierrez --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 39107a3fd3..c02354634b 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.0.0 (2024-03-28) +------------------- * fix spin_some_max_duration unit-test for events-executor (`#2465 `_) * refactor and improve the parameterized spin_some tests for executors (`#2460 `_) * refactor and improve the spin_some parameterized tests for executors diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 867e0a1bf2..103b1c1c33 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 27.0.0 + 28.0.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 3f7284e400..b7b7a7e4c7 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.0.0 (2024-03-28) +------------------- * Do not generate the exception when action service response timeout. (`#2464 `_) * Do not generate the exception when action service response timeout. * address review comment. diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 7ebf6e0efe..8c1dd30127 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 27.0.0 + 28.0.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index e4458d0ec0..ad7db5096e 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.0.0 (2024-03-28) +------------------- * Add EXECUTOR docs (`#2440 `_) * Update quality declaration documents (`#2427 `_) * crash on no class found (`#2415 `_) diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index dbb3e720c6..e309698b84 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 27.0.0 + 28.0.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 14135550f6..0e8c604097 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.0.0 (2024-03-28) +------------------- * Update quality declaration documents (`#2427 `_) * Contributors: Christophe Bedard diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 497aa259f6..6a2157c002 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 27.0.0 + 28.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 9d5aaf5bae6d39f98446b928e274ffd5b986ab6e Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Thu, 28 Mar 2024 06:23:32 -0700 Subject: [PATCH 353/455] fix flakiness in TestTimersManager unit-test (#2468) the previous version of the test was relying on the assumption that a timer with 1ms period gets called at least 6 times if the main thread waits 15ms. this is true most of the times, but it's not guaranteed, especially when running the test on windows CI servers. the new version of the test makes no assumptions on how much time it takes for the timers manager to invoke the timers, but rather focuses on ensuring that they are called the right amount of times, which is what's important for the purpose of the test Signed-off-by: Alberto Soragna --- rclcpp/test/rclcpp/test_timers_manager.cpp | 50 ++++++++++++++++++---- 1 file changed, 41 insertions(+), 9 deletions(-) diff --git a/rclcpp/test/rclcpp/test_timers_manager.cpp b/rclcpp/test/rclcpp/test_timers_manager.cpp index 0e49da08e1..90d321c188 100644 --- a/rclcpp/test/rclcpp/test_timers_manager.cpp +++ b/rclcpp/test/rclcpp/test_timers_manager.cpp @@ -367,22 +367,23 @@ TEST_F(TestTimersManager, check_one_timer_cancel_doesnt_affect_other_timers) auto timers_manager = std::make_shared( rclcpp::contexts::get_global_default_context()); - size_t t1_runs = 0; + std::atomic t1_runs = 0; + const size_t cancel_iter = 5; std::shared_ptr t1; // After a while cancel t1. Don't remove it though. // Simulates typical usage in a Node where a timer is cancelled but not removed, // since typical users aren't going to mess around with the timer manager. t1 = TimerT::make_shared( 1ms, - [&t1_runs, &t1]() { + [&t1_runs, &t1, cancel_iter]() { t1_runs++; - if (t1_runs == 5) { + if (t1_runs == cancel_iter) { t1->cancel(); } }, rclcpp::contexts::get_global_default_context()); - size_t t2_runs = 0; + std::atomic t2_runs = 0; auto t2 = TimerT::make_shared( 1ms, [&t2_runs]() { @@ -397,11 +398,42 @@ TEST_F(TestTimersManager, check_one_timer_cancel_doesnt_affect_other_timers) // Start timers thread timers_manager->start(); - std::this_thread::sleep_for(15ms); + // Wait for t1 to be canceled + auto loop_start_time = std::chrono::high_resolution_clock::now(); + while (!t1->is_canceled()) { + auto now = std::chrono::high_resolution_clock::now(); + if (now - loop_start_time >= std::chrono::seconds(30)) { + FAIL() << "timeout waiting for t1 to be canceled"; + break; + } + std::this_thread::sleep_for(3ms); + } + + EXPECT_TRUE(t1->is_canceled()); + EXPECT_FALSE(t2->is_canceled()); + EXPECT_EQ(t1_runs, cancel_iter); + + // Verify that t2 is still being invoked + const size_t start_t2_runs = t2_runs; + const size_t num_t2_extra_runs = 6; + loop_start_time = std::chrono::high_resolution_clock::now(); + while (t2_runs < start_t2_runs + num_t2_extra_runs) { + auto now = std::chrono::high_resolution_clock::now(); + if (now - loop_start_time >= std::chrono::seconds(30)) { + FAIL() << "timeout waiting for t2 to do some runs"; + break; + } + std::this_thread::sleep_for(3ms); + } + + EXPECT_TRUE(t1->is_canceled()); + EXPECT_FALSE(t2->is_canceled()); + // t1 hasn't run since before + EXPECT_EQ(t1_runs, cancel_iter); + // t2 has run the expected additional number of times + EXPECT_GE(t2_runs, start_t2_runs + num_t2_extra_runs); + // the t2 runs are strictly more than the t1 runs + EXPECT_GT(t2_runs, t1_runs); - // t1 has stopped running - EXPECT_NE(t1_runs, t2_runs); - // Check that t2 has significantly more calls - EXPECT_LT(t1_runs + 5, t2_runs); timers_manager->stop(); } From 5632a09af212c624c24c3820c765ac4bab5a8ddc Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 28 Mar 2024 22:21:57 -0500 Subject: [PATCH 354/455] Utilize rclcpp::WaitSet as part of the executors (#2142) * Deprecate callback_group call taking context Signed-off-by: Michael Carroll * Add base executor objects that can be used by implementors Signed-off-by: Michael Carroll * Template common operations Signed-off-by: Michael Carroll * Address reviewer feedback: * Add callback to EntitiesCollector constructor * Make function to check automatically added callback groups take a list Signed-off-by: Michael Carroll * Lint Signed-off-by: Michael Carroll * Address reviewer feedback and fix templates Signed-off-by: Michael Carroll * Lint and docs Signed-off-by: Michael Carroll * Make executor own the notify waitable Signed-off-by: Michael Carroll * Add pending queue to collector, remove from waitable Also change node's get_guard_condition to return shared_ptr Signed-off-by: Michael Carroll * Change interrupt guard condition to shared_ptr Check if guard condition is valid before adding it to the waitable Signed-off-by: Michael Carroll * Lint and docs Signed-off-by: Michael Carroll * Utilize rclcpp::WaitSet as part of the executors Signed-off-by: Michael Carroll * Don't exchange atomic twice Signed-off-by: Michael Carroll * Fix add_node and add more tests Signed-off-by: Michael Carroll * Make get_notify_guard_condition follow API tick-tock Signed-off-by: Michael Carroll * Improve callback group tick-tocking Signed-off-by: Michael Carroll * Don't lock twice Signed-off-by: Michael Carroll * Address reviewer feedback Signed-off-by: Michael Carroll * Add thread safety annotations and make locks consistent Signed-off-by: Michael Carroll * @wip Signed-off-by: Michael Carroll * Reset callback groups for multithreaded executor Signed-off-by: Michael Carroll * Avoid many small function calls when building executables Signed-off-by: Michael Carroll * Re-trigger guard condition if buffer has data Signed-off-by: Michael Carroll * Address reviewer feedback Signed-off-by: Michael Carroll * Trace points Signed-off-by: Michael Carroll * Remove tracepoints Signed-off-by: Michael Carroll * Reducing diff Signed-off-by: Michael Carroll * Reduce diff Signed-off-by: Michael Carroll * Uncrustify Signed-off-by: Michael Carroll * Restore tests Signed-off-by: Michael Carroll * Back to weak_ptr and reduce test time Signed-off-by: Michael Carroll * reduce diff and lint Signed-off-by: Michael Carroll * Restore static single threaded tests that weren't working before Signed-off-by: Michael Carroll * Restore more tests Signed-off-by: Michael Carroll * Fix multithreaded test Signed-off-by: Michael Carroll * Fix assert Signed-off-by: Michael Carroll * Fix constructor test Signed-off-by: Michael Carroll * Change ready_executables signature back Signed-off-by: Michael Carroll * Don't enforce removing callback groups before nodes Signed-off-by: Michael Carroll * Remove the "add_valid_node" API Signed-off-by: Michael Carroll * Only notify if the trigger condition is valid Signed-off-by: Michael Carroll * Only trigger if valid and needed Signed-off-by: Michael Carroll * Fix spin_some/spin_all implementation Signed-off-by: Michael Carroll * Restore single threaded executor Signed-off-by: Michael Carroll * Picking ABI-incompatible executor changes Signed-off-by: Michael Carroll * Add PIMPL Signed-off-by: Michael Carroll * Additional waitset prune Signed-off-by: Michael Carroll * Fix bad merge Signed-off-by: Michael Carroll * Expand test timeout Signed-off-by: Michael Carroll * Introduce method to clear expired entities from a collection Signed-off-by: Michael Carroll * Make sure to call remove_expired_entities(). Signed-off-by: Chris Lalancette * Prune queued work when callback group is removed Signed-off-by: Michael Carroll * Prune subscriptions from dynamic storage Signed-off-by: Michael Carroll * Styles fixes. Signed-off-by: Chris Lalancette * Re-trigger guard conditions Signed-off-by: Michael Carroll * Condense to just use watiable.take_data Signed-off-by: Michael Carroll * Lint Signed-off-by: Michael Carroll * Address reviewer comments (nits) Signed-off-by: Michael Carroll * Lock mutex when copying Signed-off-by: Michael Carroll * Refactors to static single threaded based on reviewers Signed-off-by: Michael Carroll * More small refactoring Signed-off-by: Michael Carroll * Lint Signed-off-by: Michael Carroll * Lint Signed-off-by: Michael Carroll * Add ready executable accessors to WaitResult Signed-off-by: Michael Carroll * Make use of accessors from wait_set Signed-off-by: Michael Carroll * Fix tests Signed-off-by: Michael Carroll * Fix more tests Signed-off-by: Michael Carroll * Tidy up single threaded executor implementation Signed-off-by: Michael Carroll * Don't null out timer, rely on call Signed-off-by: Michael Carroll * change how timers are checked from wait result in executors Signed-off-by: William Woodall * peak -> peek Signed-off-by: William Woodall * fix bug in next_waitable logic Signed-off-by: William Woodall * fix bug in StaticSTE that broke the add callback groups to executor tests Signed-off-by: William Woodall * style Signed-off-by: William Woodall --------- Signed-off-by: Michael Carroll Signed-off-by: Michael Carroll Signed-off-by: Chris Lalancette Signed-off-by: William Woodall Co-authored-by: Chris Lalancette Co-authored-by: William Woodall --- rclcpp/CMakeLists.txt | 1 - rclcpp/include/rclcpp/any_executable.hpp | 6 +- rclcpp/include/rclcpp/callback_group.hpp | 27 +- rclcpp/include/rclcpp/executor.hpp | 180 +--- .../executor_entities_collection.hpp | 6 + .../executors/executor_notify_waitable.hpp | 4 +- .../static_executor_entities_collector.hpp | 357 -------- .../static_single_threaded_executor.hpp | 111 +-- .../subscription_intra_process_buffer.hpp | 10 + rclcpp/include/rclcpp/wait_result.hpp | 170 +++- rclcpp/include/rclcpp/wait_result_kind.hpp | 1 + .../detail/storage_policy_common.hpp | 78 +- .../wait_set_policies/dynamic_storage.hpp | 75 +- .../sequential_synchronization.hpp | 2 +- .../wait_set_policies/static_storage.hpp | 55 ++ rclcpp/include/rclcpp/wait_set_template.hpp | 19 + rclcpp/src/rclcpp/any_executable.cpp | 1 + rclcpp/src/rclcpp/callback_group.cpp | 1 + rclcpp/src/rclcpp/executor.cpp | 782 +++++++----------- .../executor_entities_collection.cpp | 29 +- .../executors/executor_entities_collector.cpp | 28 +- .../executors/executor_notify_waitable.cpp | 30 +- .../executors/multi_threaded_executor.cpp | 14 +- .../executors/single_threaded_executor.cpp | 5 + .../static_executor_entities_collector.cpp | 527 ------------ .../static_single_threaded_executor.cpp | 265 ++---- rclcpp/test/benchmark/benchmark_executor.cpp | 39 - rclcpp/test/rclcpp/CMakeLists.txt | 6 - .../test/rclcpp/executors/test_executors.cpp | 33 +- .../test_static_single_threaded_executor.cpp | 11 +- .../test_add_callback_groups_to_executor.cpp | 22 +- rclcpp/test/rclcpp/test_executor.cpp | 141 +--- 32 files changed, 975 insertions(+), 2061 deletions(-) delete mode 100644 rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp delete mode 100644 rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 64b7caed66..c95df3e768 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -69,7 +69,6 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/executors/executor_notify_waitable.cpp src/rclcpp/executors/multi_threaded_executor.cpp src/rclcpp/executors/single_threaded_executor.cpp - src/rclcpp/executors/static_executor_entities_collector.cpp src/rclcpp/executors/static_single_threaded_executor.cpp src/rclcpp/expand_topic_or_service_name.cpp src/rclcpp/experimental/executors/events_executor/events_executor.cpp diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index 5d4064f452..e4e9eaecb0 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -45,9 +45,9 @@ struct AnyExecutable rclcpp::ClientBase::SharedPtr client; rclcpp::Waitable::SharedPtr waitable; // These are used to keep the scope on the containing items - rclcpp::CallbackGroup::SharedPtr callback_group; - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base; - std::shared_ptr data; + rclcpp::CallbackGroup::SharedPtr callback_group {nullptr}; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base {nullptr}; + std::shared_ptr data {nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index 3f60ab5cbc..d8c529d56f 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -184,18 +184,41 @@ class CallbackGroup * \return the number of entities in the callback group. */ RCLCPP_PUBLIC - size_t size() const; + size_t + size() const; + /// Return a reference to the 'can be taken' atomic boolean. + /** + * The resulting bool will be true in the case that no executor is currently + * using an executable entity from this group. + * The resulting bool will be false in the case that an executor is currently + * using an executable entity from this group, and the group policy doesn't + * allow a second take (eg mutual exclusion) + * \return a reference to the flag + */ RCLCPP_PUBLIC std::atomic_bool & can_be_taken_from(); + /// Get the group type. + /** + * \return the group type + */ RCLCPP_PUBLIC const CallbackGroupType & type() const; + /// Collect all of the entity pointers contained in this callback group. + /** + * \param[in] sub_func Function to execute for each subscription + * \param[in] service_func Function to execute for each service + * \param[in] client_func Function to execute for each client + * \param[in] timer_func Function to execute for each timer + * \param[in] waitable_fuinc Function to execute for each waitable + */ RCLCPP_PUBLIC - void collect_all_ptrs( + void + collect_all_ptrs( std::function sub_func, std::function service_func, std::function client_func, diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index dc6020178c..cfd7ea81fd 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -29,26 +29,24 @@ #include "rcl/guard_condition.h" #include "rcl/wait.h" +#include "rclcpp/executors/executor_notify_waitable.hpp" #include "rcpputils/scope_exit.hpp" #include "rclcpp/context.hpp" #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/guard_condition.hpp" #include "rclcpp/executor_options.hpp" +#include "rclcpp/executors/executor_entities_collection.hpp" +#include "rclcpp/executors/executor_entities_collector.hpp" #include "rclcpp/future_return_code.hpp" -#include "rclcpp/memory_strategies.hpp" -#include "rclcpp/memory_strategy.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/wait_set.hpp" namespace rclcpp { -typedef std::map> WeakCallbackGroupsToNodesMap; - // Forward declaration is used in convenience method signature. class Node; class ExecutorImplementation; @@ -425,17 +423,6 @@ class Executor void cancel(); - /// Support dynamic switching of the memory strategy. - /** - * Switching the memory strategy while the executor is spinning in another threading could have - * unintended consequences. - * \param[in] memory_strategy Shared pointer to the memory strategy to set. - * \throws std::runtime_error if memory_strategy is null - */ - RCLCPP_PUBLIC - void - set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy); - /// Returns true if the executor is currently spinning. /** * This function can be called asynchronously from any thread. @@ -520,6 +507,11 @@ class Executor static void execute_client(rclcpp::ClientBase::SharedPtr client); + /// Gather all of the waitable entities from associated nodes and callback groups. + RCLCPP_PUBLIC + void + collect_entities(); + /// Block until more work becomes avilable or timeout is reached. /** * Builds a set of waitable entities, which are passed to the middleware. @@ -531,62 +523,6 @@ class Executor void wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - /// Find node associated with a callback group - /** - * \param[in] weak_groups_to_nodes map of callback groups to nodes - * \param[in] group callback group to find assocatiated node - * \return Pointer to associated node if found, else nullptr - */ - RCLCPP_PUBLIC - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr - get_node_by_group( - const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, - rclcpp::CallbackGroup::SharedPtr group); - - /// Return true if the node has been added to this executor. - /** - * \param[in] node_ptr a shared pointer that points to a node base interface - * \param[in] weak_groups_to_nodes map to nodes to lookup - * \return true if the node is associated with the executor, otherwise false - */ - RCLCPP_PUBLIC - bool - has_node( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const; - - /// Find the callback group associated with a timer - /** - * \param[in] timer Timer to find associated callback group - * \return Pointer to callback group node if found, else nullptr - */ - RCLCPP_PUBLIC - rclcpp::CallbackGroup::SharedPtr - get_group_by_timer(rclcpp::TimerBase::SharedPtr timer); - - /// Add a callback group to an executor - /** - * \see rclcpp::Executor::add_callback_group - */ - RCLCPP_PUBLIC - virtual void - add_callback_group_to_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, - bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_); - - /// Remove a callback group from the executor. - /** - * \see rclcpp::Executor::remove_callback_group - */ - RCLCPP_PUBLIC - virtual void - remove_callback_group_from_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, - bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_); - /// Check for executable in ready state and populate union structure. /** * \param[out] any_executable populated union structure of ready executable @@ -597,33 +533,6 @@ class Executor bool get_next_ready_executable(AnyExecutable & any_executable); - /// Check for executable in ready state and populate union structure. - /** - * This is the implementation of get_next_ready_executable that takes into - * account the current state of callback groups' association with nodes and - * executors. - * - * This checks in a particular order for available work: - * * Timers - * * Subscriptions - * * Services - * * Clients - * * Waitable - * - * If the next executable is not associated with this executor/node pair, - * then this method will return false. - * - * \param[out] any_executable populated union structure of ready executable - * \param[in] weak_groups_to_nodes mapping of callback groups to nodes - * \return true if an executable was ready and any_executable was populated, - * otherwise false - */ - RCLCPP_PUBLIC - bool - get_next_ready_executable_from_map( - AnyExecutable & any_executable, - const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - /// Wait for executable in ready state and populate union structure. /** * If an executable is ready, it will return immediately, otherwise @@ -641,21 +550,6 @@ class Executor AnyExecutable & any_executable, std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - /// Add all callback groups that can be automatically added from associated nodes. - /** - * The executor, before collecting entities, verifies if any callback group from - * nodes associated with the executor, which is not already associated to an executor, - * can be automatically added to this executor. - * This takes care of any callback group that has been added to a node but not explicitly added - * to the executor. - * It is important to note that in order for the callback groups to be automatically added to an - * executor through this function, the node of the callback groups needs to have been added - * through the `add_node` method. - */ - RCLCPP_PUBLIC - virtual void - add_callback_groups_from_nodes_associated_to_executor() RCPPUTILS_TSA_REQUIRES(mutex_); - /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins. std::atomic_bool spinning; @@ -665,16 +559,8 @@ class Executor /// Guard condition for signaling the rmw layer to wake up for system shutdown. std::shared_ptr shutdown_guard_condition_; - /// Wait set for managing entities that the rmw layer waits on. - rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set(); - - // Mutex to protect the subsequent memory_strategy_. mutable std::mutex mutex_; - /// The memory strategy: an interface for handling user-defined memory allocation strategies. - memory_strategy::MemoryStrategy::SharedPtr - memory_strategy_ RCPPUTILS_TSA_PT_GUARDED_BY(mutex_); - /// The context associated with this executor. std::shared_ptr context_; @@ -684,39 +570,31 @@ class Executor virtual void spin_once_impl(std::chrono::nanoseconds timeout); - typedef std::map> - WeakNodesToGuardConditionsMap; - - typedef std::map> - WeakCallbackGroupsToGuardConditionsMap; - - /// maps nodes to guard conditions - WeakNodesToGuardConditionsMap - weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + /// Waitable containing guard conditions controlling the executor flow. + /** + * This waitable contains the interrupt and shutdown guard condition, as well + * as the guard condition associated with each node and callback group. + * By default, if any change is detected in the monitored entities, the notify + * waitable will awake the executor and rebuild the collections. + */ + std::shared_ptr notify_waitable_; - /// maps callback groups to guard conditions - WeakCallbackGroupsToGuardConditionsMap - weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + std::atomic_bool entities_need_rebuild_; - /// maps callback groups associated to nodes - WeakCallbackGroupsToNodesMap - weak_groups_associated_with_executor_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + /// Collector used to associate executable entities from nodes and guard conditions + rclcpp::executors::ExecutorEntitiesCollector collector_; - /// maps callback groups to nodes associated with executor - WeakCallbackGroupsToNodesMap - weak_groups_to_nodes_associated_with_executor_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + /// WaitSet to be waited on. + rclcpp::WaitSet wait_set_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + std::optional> wait_result_ RCPPUTILS_TSA_GUARDED_BY(mutex_); - /// maps all callback groups to nodes - WeakCallbackGroupsToNodesMap - weak_groups_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + /// Hold the current state of the collection being waited on by the waitset + rclcpp::executors::ExecutorEntitiesCollection current_collection_ RCPPUTILS_TSA_GUARDED_BY( + mutex_); - /// nodes that are associated with the executor - std::list - weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + /// Hold the current state of the notify waitable being waited on by the waitset + std::shared_ptr current_notify_waitable_ + RCPPUTILS_TSA_GUARDED_BY(mutex_); /// shutdown callback handle registered to Context rclcpp::OnShutdownCallbackHandle shutdown_callback_handle_; diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp index 166bb99119..517894a2a2 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp @@ -178,6 +178,12 @@ struct ExecutorEntitiesCollection /// Clear the entities collection void clear(); + + /// Remove entities that have expired weak ownership + /** + * \return The total number of removed entities + */ + size_t remove_expired_entities(); }; /// Build an entities collection from callback groups diff --git a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp index eee8e59793..2b43fecca1 100644 --- a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp @@ -48,11 +48,11 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable ~ExecutorNotifyWaitable() override = default; RCLCPP_PUBLIC - ExecutorNotifyWaitable(const ExecutorNotifyWaitable & other); + ExecutorNotifyWaitable(ExecutorNotifyWaitable & other); RCLCPP_PUBLIC - ExecutorNotifyWaitable & operator=(const ExecutorNotifyWaitable & other); + ExecutorNotifyWaitable & operator=(ExecutorNotifyWaitable & other); /// Add conditions to the wait set /** diff --git a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp deleted file mode 100644 index f9fd2ff672..0000000000 --- a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp +++ /dev/null @@ -1,357 +0,0 @@ -// Copyright 2020 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__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_ -#define RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_ - -#include -#include -#include -#include -#include - -#include "rcl/guard_condition.h" -#include "rcl/wait.h" - -#include "rclcpp/experimental/executable_list.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/memory_strategy.hpp" -#include "rclcpp/visibility_control.hpp" -#include "rclcpp/waitable.hpp" - -namespace rclcpp -{ -namespace executors -{ -typedef std::map> WeakCallbackGroupsToNodesMap; - -class StaticExecutorEntitiesCollector final - : public rclcpp::Waitable, - public std::enable_shared_from_this -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(StaticExecutorEntitiesCollector) - - // Constructor - RCLCPP_PUBLIC - StaticExecutorEntitiesCollector() = default; - - // Destructor - RCLCPP_PUBLIC - ~StaticExecutorEntitiesCollector(); - - /// Initialize StaticExecutorEntitiesCollector - /** - * \param p_wait_set A reference to the wait set to be used in the executor - * \param memory_strategy Shared pointer to the memory strategy to set. - * \throws std::runtime_error if memory strategy is null - */ - RCLCPP_PUBLIC - void - init( - rcl_wait_set_t * p_wait_set, - rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy); - - /// Finalize StaticExecutorEntitiesCollector to clear resources - RCLCPP_PUBLIC - bool - is_init() {return initialized_;} - - RCLCPP_PUBLIC - void - fini(); - - /// Execute the waitable. - RCLCPP_PUBLIC - void - execute(std::shared_ptr & data) override; - - /// Take the data so that it can be consumed with `execute`. - /** - * For `StaticExecutorEntitiesCollector`, this always return `nullptr`. - * \sa rclcpp::Waitable::take_data() - */ - RCLCPP_PUBLIC - std::shared_ptr - take_data() override; - - /// Function to add_handles_to_wait_set and wait for work and - /** - * block until the wait set is ready or until the timeout has been exceeded. - * \throws std::runtime_error if wait set couldn't be cleared or filled. - * \throws any rcl errors from rcl_wait, \see rclcpp::exceptions::throw_from_rcl_error() - */ - RCLCPP_PUBLIC - void - refresh_wait_set(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - - /** - * \throws std::runtime_error if it couldn't add guard condition to wait set - */ - RCLCPP_PUBLIC - void - add_to_wait_set(rcl_wait_set_t * wait_set) override; - - RCLCPP_PUBLIC - size_t - get_number_of_ready_guard_conditions() override; - - /// Add a callback group to an executor. - /** - * \see rclcpp::Executor::add_callback_group - */ - RCLCPP_PUBLIC - bool - add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); - - /// Add a callback group to an executor. - /** - * \see rclcpp::Executor::add_callback_group - * \return boolean whether the node from the callback group is new - */ - RCLCPP_PUBLIC - bool - add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - - /// Remove a callback group from the executor. - /** - * \see rclcpp::Executor::remove_callback_group - */ - RCLCPP_PUBLIC - bool - remove_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr); - - /// Remove a callback group from the executor. - /** - * \see rclcpp::Executor::remove_callback_group_from_map - */ - RCLCPP_PUBLIC - bool - remove_callback_group_from_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - - /** - * \see rclcpp::Executor::add_node() - * \throw std::runtime_error if node was already added - */ - RCLCPP_PUBLIC - bool - add_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); - - /** - * \see rclcpp::Executor::remove_node() - * \throw std::runtime_error if no guard condition is associated with node. - */ - RCLCPP_PUBLIC - bool - remove_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); - - RCLCPP_PUBLIC - std::vector - get_all_callback_groups(); - - /// Get callback groups that belong to executor. - /** - * \see rclcpp::Executor::get_manually_added_callback_groups() - */ - RCLCPP_PUBLIC - std::vector - get_manually_added_callback_groups(); - - /// Get callback groups that belong to executor. - /** - * \see rclcpp::Executor::get_automatically_added_callback_groups_from_nodes() - */ - RCLCPP_PUBLIC - std::vector - get_automatically_added_callback_groups_from_nodes(); - - /// Complete all available queued work without blocking. - /** - * This function checks if after the guard condition was triggered - * (or a spurious wakeup happened) we are really ready to execute - * i.e. re-collect entities - */ - RCLCPP_PUBLIC - bool - is_ready(rcl_wait_set_t * wait_set) override; - - /// Return number of timers - /** - * \return number of timers - */ - RCLCPP_PUBLIC - size_t - get_number_of_timers() {return exec_list_.number_of_timers;} - - /// Return number of subscriptions - /** - * \return number of subscriptions - */ - RCLCPP_PUBLIC - size_t - get_number_of_subscriptions() {return exec_list_.number_of_subscriptions;} - - /// Return number of services - /** - * \return number of services - */ - RCLCPP_PUBLIC - size_t - get_number_of_services() {return exec_list_.number_of_services;} - - /// Return number of clients - /** - * \return number of clients - */ - RCLCPP_PUBLIC - size_t - get_number_of_clients() {return exec_list_.number_of_clients;} - - /// Return number of waitables - /** - * \return number of waitables - */ - RCLCPP_PUBLIC - size_t - get_number_of_waitables() {return exec_list_.number_of_waitables;} - - /** Return a SubscritionBase Sharedptr by index. - * \param[in] i The index of the SubscritionBase - * \return a SubscritionBase shared pointer - * \throws std::out_of_range if the argument is higher than the size of the structrue. - */ - RCLCPP_PUBLIC - rclcpp::SubscriptionBase::SharedPtr - get_subscription(size_t i) {return exec_list_.subscription[i];} - - /** Return a TimerBase Sharedptr by index. - * \param[in] i The index of the TimerBase - * \return a TimerBase shared pointer - * \throws std::out_of_range if the argument is higher than the size. - */ - RCLCPP_PUBLIC - rclcpp::TimerBase::SharedPtr - get_timer(size_t i) {return exec_list_.timer[i];} - - /** Return a ServiceBase Sharedptr by index. - * \param[in] i The index of the ServiceBase - * \return a ServiceBase shared pointer - * \throws std::out_of_range if the argument is higher than the size. - */ - RCLCPP_PUBLIC - rclcpp::ServiceBase::SharedPtr - get_service(size_t i) {return exec_list_.service[i];} - - /** Return a ClientBase Sharedptr by index - * \param[in] i The index of the ClientBase - * \return a ClientBase shared pointer - * \throws std::out_of_range if the argument is higher than the size. - */ - RCLCPP_PUBLIC - rclcpp::ClientBase::SharedPtr - get_client(size_t i) {return exec_list_.client[i];} - - /** Return a Waitable Sharedptr by index - * \param[in] i The index of the Waitable - * \return a Waitable shared pointer - * \throws std::out_of_range if the argument is higher than the size. - */ - RCLCPP_PUBLIC - rclcpp::Waitable::SharedPtr - get_waitable(size_t i) {return exec_list_.waitable[i];} - -private: - /// Function to reallocate space for entities in the wait set. - /** - * \throws std::runtime_error if wait set couldn't be cleared or resized. - */ - void - prepare_wait_set(); - - void - fill_executable_list(); - - void - fill_memory_strategy(); - - /// Return true if the node belongs to the collector - /** - * \param[in] node_ptr a node base interface shared pointer - * \param[in] weak_groups_to_nodes map to nodes to lookup - * \return boolean whether a node belongs the collector - */ - bool - has_node( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const; - - /// Add all callback groups that can be automatically added by any executor - /// and is not already associated with an executor from nodes - /// that are associated with executor - /** - * \see rclcpp::Executor::add_callback_groups_from_nodes_associated_to_executor() - */ - void - add_callback_groups_from_nodes_associated_to_executor(); - - void - fill_executable_list_from_map(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - - /// Memory strategy: an interface for handling user-defined memory allocation strategies. - rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy_; - - // maps callback groups to nodes. - WeakCallbackGroupsToNodesMap weak_groups_associated_with_executor_to_nodes_; - // maps callback groups to nodes. - WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_; - - typedef std::map> - WeakNodesToGuardConditionsMap; - WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_; - - /// List of weak nodes registered in the static executor - std::list weak_nodes_; - - // Mutex to protect vector of new nodes. - std::mutex new_nodes_mutex_; - std::vector new_nodes_; - - /// Wait set for managing entities that the rmw layer waits on. - rcl_wait_set_t * p_wait_set_ = nullptr; - - /// Executable list: timers, subscribers, clients, services and waitables - rclcpp::experimental::ExecutableList exec_list_; - - /// Bool to check if the entities collector has been initialized - bool initialized_ = false; -}; - -} // namespace executors -} // namespace rclcpp - -#endif // RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_ diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index 5294605eaf..6f22909caf 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -15,24 +15,13 @@ #ifndef RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_ #define RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_ +#include #include -#include -#include #include -#include -#include - -#include "rmw/rmw.h" #include "rclcpp/executor.hpp" -#include "rclcpp/executors/static_executor_entities_collector.hpp" -#include "rclcpp/experimental/executable_list.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/memory_strategies.hpp" -#include "rclcpp/node.hpp" -#include "rclcpp/rate.hpp" -#include "rclcpp/utilities.hpp" -#include "rclcpp/visibility_control.hpp" +#include "rclcpp/executors/executor_entities_collection.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" namespace rclcpp { @@ -65,7 +54,7 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor explicit StaticSingleThreadedExecutor( const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions()); - /// Default destrcutor. + /// Default destructor. RCLCPP_PUBLIC virtual ~StaticSingleThreadedExecutor(); @@ -116,105 +105,31 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor void spin_all(std::chrono::nanoseconds max_duration) override; - /// Add a callback group to an executor. - /** - * \sa rclcpp::Executor::add_callback_group - */ - RCLCPP_PUBLIC - void - add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - bool notify = true) override; - - /// Remove callback group from the executor - /** - * \sa rclcpp::Executor::remove_callback_group - */ - RCLCPP_PUBLIC - void - remove_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - bool notify = true) override; - - /// Add a node to the executor. - /** - * \sa rclcpp::Executor::add_node - */ - RCLCPP_PUBLIC - void - add_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - bool notify = true) override; - - /// Convenience function which takes Node and forwards NodeBaseInterface. - /** - * \sa rclcpp::StaticSingleThreadedExecutor::add_node - */ - RCLCPP_PUBLIC - void - add_node(std::shared_ptr node_ptr, bool notify = true) override; - - /// Remove a node from the executor. - /** - * \sa rclcpp::Executor::remove_node - */ - RCLCPP_PUBLIC - void - remove_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - bool notify = true) override; - - /// Convenience function which takes Node and forwards NodeBaseInterface. - /** - * \sa rclcpp::Executor::remove_node - */ - RCLCPP_PUBLIC - void - remove_node(std::shared_ptr node_ptr, bool notify = true) override; - - RCLCPP_PUBLIC - std::vector - get_all_callback_groups() override; - - /// Get callback groups that belong to executor. - /** - * \sa rclcpp::Executor::get_manually_added_callback_groups() - */ - RCLCPP_PUBLIC - std::vector - get_manually_added_callback_groups() override; - - /// Get callback groups that belong to executor. - /** - * \sa rclcpp::Executor::get_automatically_added_callback_groups_from_nodes() - */ - RCLCPP_PUBLIC - std::vector - get_automatically_added_callback_groups_from_nodes() override; - protected: /** * @brief Executes ready executables from wait set. + * @param collection entities to evaluate for ready executables. + * @param wait_result result to check for ready executables. * @param spin_once if true executes only the first ready executable. * @return true if any executable was ready. */ - RCLCPP_PUBLIC bool - execute_ready_executables(bool spin_once = false); + execute_ready_executables( + const rclcpp::executors::ExecutorEntitiesCollection & collection, + rclcpp::WaitResult & wait_result, + bool spin_once); - RCLCPP_PUBLIC void spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive); - RCLCPP_PUBLIC void spin_once_impl(std::chrono::nanoseconds timeout) override; + std::optional> + collect_and_wait(std::chrono::nanoseconds timeout); + private: RCLCPP_DISABLE_COPY(StaticSingleThreadedExecutor) - - StaticExecutorEntitiesCollector::SharedPtr entities_collector_; }; } // namespace executors diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index f564cbb047..abe5142581 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -30,6 +30,7 @@ #include "rclcpp/experimental/ros_message_intra_process_buffer.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" #include "tracetools/tracetools.h" @@ -99,6 +100,15 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff static_cast(this)); } + void + 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_); + } + bool is_ready(rcl_wait_set_t * wait_set) override { diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp index e879043d04..429eb1dd25 100644 --- a/rclcpp/include/rclcpp/wait_result.hpp +++ b/rclcpp/include/rclcpp/wait_result.hpp @@ -17,13 +17,21 @@ #include #include +#include +#include #include +#include #include "rcl/wait.h" #include "rclcpp/macros.hpp" #include "rclcpp/wait_result_kind.hpp" +#include "rclcpp/client.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/timer.hpp" + namespace rclcpp { @@ -134,6 +142,151 @@ class WaitResult final } } + /// Get the next ready timer and its index in the wait result, but do not clear it. + /** + * The returned timer is not cleared automatically, as it the case with the + * other next_ready_*()-like functions. + * Instead, this function returns the timer and the index that identifies it + * in the wait result, so that it can be cleared (marked as taken or used) + * in a separate step with clear_timer_with_index(). + * This is necessary in some multi-threaded executor implementations. + * + * If the timer is not cleared using the index, subsequent calls to this + * function will return the same timer. + * + * If there is no ready timer, then nullptr will be returned and the index + * will be invalid and should not be used. + * + * \param[in] start_index index at which to start searching for the next ready + * timer in the wait result. If the start_index is out of bounds for the + * list of timers in the wait result, then {nullptr, start_index} will be + * returned. Defaults to 0. + * \return next ready timer pointer and its index in the wait result, or + * {nullptr, start_index} if none was found. + */ + std::pair, size_t> + peek_next_ready_timer(size_t start_index = 0) + { + check_wait_result_dirty(); + auto ret = std::shared_ptr{nullptr}; + size_t ii = start_index; + if (this->kind() == WaitResultKind::Ready) { + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set(); + for (; ii < wait_set.size_of_timers(); ++ii) { + if (rcl_wait_set.timers[ii] != nullptr) { + ret = wait_set.timers(ii); + break; + } + } + } + return {ret, ii}; + } + + /// Clear the timer at the given index. + /** + * Clearing a timer from the wait result prevents it from being returned by + * the peek_next_ready_timer() on subsequent calls. + * + * The index should come from the peek_next_ready_timer() function, and + * should only be used with this function if the timer pointer was valid. + * + * \throws std::out_of_range if the given index is out of range + */ + void + clear_timer_with_index(size_t index) + { + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set(); + if (index >= wait_set.size_of_timers()) { + throw std::out_of_range("given timer index is out of range"); + } + rcl_wait_set.timers[index] = nullptr; + } + + /// Get the next ready subscription, clearing it from the wait result. + std::shared_ptr + next_ready_subscription() + { + check_wait_result_dirty(); + auto ret = std::shared_ptr{nullptr}; + if (this->kind() == WaitResultKind::Ready) { + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set(); + for (size_t ii = 0; ii < wait_set.size_of_subscriptions(); ++ii) { + if (rcl_wait_set.subscriptions[ii] != nullptr) { + ret = wait_set.subscriptions(ii); + rcl_wait_set.subscriptions[ii] = nullptr; + break; + } + } + } + return ret; + } + + /// Get the next ready service, clearing it from the wait result. + std::shared_ptr + next_ready_service() + { + check_wait_result_dirty(); + auto ret = std::shared_ptr{nullptr}; + if (this->kind() == WaitResultKind::Ready) { + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set(); + for (size_t ii = 0; ii < wait_set.size_of_services(); ++ii) { + if (rcl_wait_set.services[ii] != nullptr) { + ret = wait_set.services(ii); + rcl_wait_set.services[ii] = nullptr; + break; + } + } + } + return ret; + } + + /// Get the next ready client, clearing it from the wait result. + std::shared_ptr + next_ready_client() + { + check_wait_result_dirty(); + auto ret = std::shared_ptr{nullptr}; + if (this->kind() == WaitResultKind::Ready) { + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set(); + for (size_t ii = 0; ii < wait_set.size_of_clients(); ++ii) { + if (rcl_wait_set.clients[ii] != nullptr) { + ret = wait_set.clients(ii); + rcl_wait_set.clients[ii] = nullptr; + break; + } + } + } + return ret; + } + + /// Get the next ready waitable, clearing it from the wait result. + std::shared_ptr + next_ready_waitable() + { + check_wait_result_dirty(); + auto waitable = std::shared_ptr{nullptr}; + auto data = std::shared_ptr{nullptr}; + + if (this->kind() == WaitResultKind::Ready) { + auto & wait_set = this->get_wait_set(); + 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)) { + waitable = cur_waitable; + break; + } + } + } + + return waitable; + } + private: RCLCPP_DISABLE_COPY(WaitResult) @@ -151,12 +304,25 @@ class WaitResult final // Should be enforced by the static factory methods on this class. assert(WaitResultKind::Ready == wait_result_kind); // Secure thread-safety (if provided) and shared ownership (if needed). - wait_set_pointer_->wait_result_acquire(); + this->get_wait_set().wait_result_acquire(); } - const WaitResultKind wait_result_kind_; + /// Check if the wait result is invalid because the wait set was modified. + void + check_wait_result_dirty() + { + // In the case that the wait set was modified while the result was out, + // we must mark the wait result as no longer valid + if (wait_set_pointer_ && this->get_wait_set().wait_result_dirty_) { + this->wait_result_kind_ = WaitResultKind::Invalid; + } + } + + WaitResultKind wait_result_kind_; WaitSetT * wait_set_pointer_ = nullptr; + + size_t next_waitable_index_ = 0; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/wait_result_kind.hpp b/rclcpp/include/rclcpp/wait_result_kind.hpp index 3ce65bf4f3..7980d1d127 100644 --- a/rclcpp/include/rclcpp/wait_result_kind.hpp +++ b/rclcpp/include/rclcpp/wait_result_kind.hpp @@ -26,6 +26,7 @@ enum RCLCPP_PUBLIC WaitResultKind Ready, //get_subscription_handle().get(), + subscription_entry.subscription->get_subscription_handle().get(), nullptr); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set"); } } @@ -271,8 +269,7 @@ class StoragePolicyCommon [this](const auto & inner_guard_conditions) { for (const auto & guard_condition : inner_guard_conditions) { - auto guard_condition_ptr_pair = get_raw_pointer_from_smart_pointer(guard_condition); - if (nullptr == guard_condition_ptr_pair.second) { + if (!guard_condition) { // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. if (HasStrongOwnership) { // This will not happen in fixed sized storage, as it holds @@ -285,10 +282,10 @@ class StoragePolicyCommon } rcl_ret_t ret = rcl_wait_set_add_guard_condition( &rcl_wait_set_, - &guard_condition_ptr_pair.second->get_rcl_guard_condition(), + &guard_condition->get_rcl_guard_condition(), nullptr); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set"); } } }; @@ -301,8 +298,7 @@ class StoragePolicyCommon // Add timers. for (const auto & timer : timers) { - auto timer_ptr_pair = get_raw_pointer_from_smart_pointer(timer); - if (nullptr == timer_ptr_pair.second) { + if (!timer) { // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. if (HasStrongOwnership) { // This will not happen in fixed sized storage, as it holds @@ -315,17 +311,16 @@ class StoragePolicyCommon } rcl_ret_t ret = rcl_wait_set_add_timer( &rcl_wait_set_, - timer_ptr_pair.second->get_timer_handle().get(), + timer->get_timer_handle().get(), nullptr); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set"); } } // Add clients. for (const auto & client : clients) { - auto client_ptr_pair = get_raw_pointer_from_smart_pointer(client); - if (nullptr == client_ptr_pair.second) { + if (!client) { // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. if (HasStrongOwnership) { // This will not happen in fixed sized storage, as it holds @@ -338,7 +333,7 @@ class StoragePolicyCommon } rcl_ret_t ret = rcl_wait_set_add_client( &rcl_wait_set_, - client_ptr_pair.second->get_client_handle().get(), + client->get_client_handle().get(), nullptr); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -347,8 +342,7 @@ class StoragePolicyCommon // Add services. for (const auto & service : services) { - auto service_ptr_pair = get_raw_pointer_from_smart_pointer(service); - if (nullptr == service_ptr_pair.second) { + if (!service) { // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. if (HasStrongOwnership) { // This will not happen in fixed sized storage, as it holds @@ -361,17 +355,16 @@ class StoragePolicyCommon } rcl_ret_t ret = rcl_wait_set_add_service( &rcl_wait_set_, - service_ptr_pair.second->get_service_handle().get(), + service->get_service_handle().get(), nullptr); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set"); } } // Add waitables. for (auto & waitable_entry : waitables) { - auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable); - if (nullptr == waitable_ptr_pair.second) { + if (!waitable_entry.waitable) { // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. if (HasStrongOwnership) { // This will not happen in fixed sized storage, as it holds @@ -382,8 +375,7 @@ class StoragePolicyCommon needs_pruning_ = true; continue; } - rclcpp::Waitable & waitable = *waitable_ptr_pair.second; - waitable.add_to_wait_set(&rcl_wait_set_); + waitable_entry.waitable->add_to_wait_set(&rcl_wait_set_); } } @@ -405,6 +397,32 @@ class StoragePolicyCommon needs_resize_ = true; } + size_t size_of_subscriptions() const {return 0;} + size_t size_of_timers() const {return 0;} + size_t size_of_clients() const {return 0;} + size_t size_of_services() const {return 0;} + size_t size_of_waitables() const {return 0;} + + template + typename SubscriptionsIterable::value_type + subscriptions(size_t) const {return nullptr;} + + template + typename TimersIterable::value_type + timers(size_t) const {return nullptr;} + + template + typename ClientsIterable::value_type + clients(size_t) const {return nullptr;} + + template + typename ServicesIterable::value_type + services(size_t) const {return nullptr;} + + template + typename WaitablesIterable::value_type + waitables(size_t) const {return nullptr;} + rcl_wait_set_t rcl_wait_set_; rclcpp::Context::SharedPtr context_; diff --git a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp index 4cec85f39a..8f97596218 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp @@ -204,15 +204,19 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo void storage_rebuild_rcl_wait_set(const ArrayOfExtraGuardConditions & extra_guard_conditions) { + this->storage_acquire_ownerships(); + this->storage_rebuild_rcl_wait_set_with_sets( - subscriptions_, - guard_conditions_, + shared_subscriptions_, + shared_guard_conditions_, extra_guard_conditions, - timers_, - clients_, - services_, - waitables_ + shared_timers_, + shared_clients_, + shared_services_, + shared_waitables_ ); + + this->storage_release_ownerships(); } template @@ -382,6 +386,8 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo return weak_ptr.expired(); }; // remove guard conditions which have been deleted + subscriptions_.erase( + std::remove_if(subscriptions_.begin(), subscriptions_.end(), p), subscriptions_.end()); guard_conditions_.erase( std::remove_if(guard_conditions_.begin(), guard_conditions_.end(), p), guard_conditions_.end()); @@ -407,6 +413,7 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo } }; // Lock all the weak pointers and hold them until released. + lock_all(subscriptions_, shared_subscriptions_); lock_all(guard_conditions_, shared_guard_conditions_); lock_all(timers_, shared_timers_); lock_all(clients_, shared_clients_); @@ -438,6 +445,7 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo shared_ptr.reset(); } }; + reset_all(shared_subscriptions_); reset_all(shared_guard_conditions_); reset_all(shared_timers_); reset_all(shared_clients_); @@ -445,6 +453,61 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo reset_all(shared_waitables_); } + size_t size_of_subscriptions() const + { + return shared_subscriptions_.size(); + } + + size_t size_of_timers() const + { + return shared_timers_.size(); + } + + size_t size_of_clients() const + { + return shared_clients_.size(); + } + + size_t size_of_services() const + { + return shared_services_.size(); + } + + size_t size_of_waitables() const + { + return shared_waitables_.size(); + } + + std::shared_ptr + subscriptions(size_t ii) const + { + return shared_subscriptions_[ii].subscription; + } + + std::shared_ptr + timers(size_t ii) const + { + return shared_timers_[ii]; + } + + std::shared_ptr + clients(size_t ii) const + { + return shared_clients_[ii]; + } + + std::shared_ptr + services(size_t ii) const + { + return shared_services_[ii]; + } + + std::shared_ptr + waitables(size_t ii) const + { + return shared_waitables_[ii].waitable; + } + size_t ownership_reference_counter_ = 0; SequenceOfWeakSubscriptions subscriptions_; diff --git a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp index be2e569c40..4afc2a1b27 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp @@ -290,7 +290,7 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon return create_wait_result(WaitResultKind::Empty); } else { // Some other error case, throw. - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "rcl_wait() failed"); } } while (should_loop()); diff --git a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp index 434947c19f..7f5cad74ad 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp @@ -188,6 +188,61 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom // Explicitly do nothing. } + size_t size_of_subscriptions() const + { + return subscriptions_.size(); + } + + size_t size_of_timers() const + { + return timers_.size(); + } + + size_t size_of_clients() const + { + return clients_.size(); + } + + size_t size_of_services() const + { + return services_.size(); + } + + size_t size_of_waitables() const + { + return waitables_.size(); + } + + typename ArrayOfSubscriptions::value_type + subscriptions(size_t ii) const + { + return subscriptions_[ii]; + } + + typename ArrayOfTimers::value_type + timers(size_t ii) const + { + return timers_[ii]; + } + + typename ArrayOfClients::value_type + clients(size_t ii) const + { + return clients_[ii]; + } + + typename ArrayOfServices::value_type + services(size_t ii) const + { + return services_[ii]; + } + + typename ArrayOfWaitables::value_type + waitables(size_t ii) const + { + return waitables_[ii]; + } + const ArrayOfSubscriptions subscriptions_; const ArrayOfGuardConditions guard_conditions_; const ArrayOfTimers timers_; diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index b3f41f8ed5..ce69da6bf2 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -153,6 +153,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli throw std::runtime_error("subscription already associated with a wait set"); } this->storage_add_subscription(std::move(local_subscription)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } if (mask.include_events) { for (auto key_event_pair : inner_subscription->get_event_handlers()) { @@ -164,6 +165,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli throw std::runtime_error("subscription event already associated with a wait set"); } this->storage_add_waitable(std::move(event), std::move(local_subscription)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } } if (mask.include_intra_process_waitable) { @@ -180,6 +182,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->storage_add_waitable( std::move(inner_subscription->get_intra_process_waitable()), std::move(local_subscription)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } } }); @@ -224,6 +227,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli auto local_subscription = inner_subscription; local_subscription->exchange_in_use_by_wait_set_state(local_subscription.get(), false); this->storage_remove_subscription(std::move(local_subscription)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } if (mask.include_events) { for (auto key_event_pair : inner_subscription->get_event_handlers()) { @@ -231,6 +235,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli auto local_subscription = inner_subscription; local_subscription->exchange_in_use_by_wait_set_state(event.get(), false); this->storage_remove_waitable(std::move(event)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } } if (mask.include_intra_process_waitable) { @@ -239,6 +244,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // This is the case when intra process is enabled for the subscription. inner_subscription->exchange_in_use_by_wait_set_state(local_waitable.get(), false); this->storage_remove_waitable(std::move(local_waitable)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } } }); @@ -289,6 +295,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the guard condition has already been added. this->storage_add_guard_condition(std::move(inner_guard_condition)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -326,6 +333,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the guard condition is not in the wait set. this->storage_remove_guard_condition(std::move(inner_guard_condition)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -357,6 +365,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the timer has already been added. this->storage_add_timer(std::move(inner_timer)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -384,6 +393,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the timer is not in the wait set. this->storage_remove_timer(std::move(inner_timer)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -415,6 +425,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the client has already been added. this->storage_add_client(std::move(inner_client)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -442,6 +453,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the client is not in the wait set. this->storage_remove_client(std::move(inner_client)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -473,6 +485,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the service has already been added. this->storage_add_service(std::move(inner_service)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -500,6 +513,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the service is not in the wait set. this->storage_remove_service(std::move(inner_service)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -551,6 +565,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the waitable has already been added. this->storage_add_waitable(std::move(inner_waitable), std::move(associated_entity)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -578,6 +593,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // fixed sized storage policies. // It will throw if the waitable is not in the wait set. this->storage_remove_waitable(std::move(inner_waitable)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -715,6 +731,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli throw std::runtime_error("wait_result_acquire() called while already holding"); } wait_result_holding_ = true; + wait_result_dirty_ = false; // this method comes from the SynchronizationPolicy this->sync_wait_result_acquire(); // this method comes from the StoragePolicy @@ -734,6 +751,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli throw std::runtime_error("wait_result_release() called while not holding"); } wait_result_holding_ = false; + wait_result_dirty_ = false; // this method comes from the StoragePolicy this->storage_release_ownerships(); // this method comes from the SynchronizationPolicy @@ -741,6 +759,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli } bool wait_result_holding_ = false; + bool wait_result_dirty_ = false; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/any_executable.cpp b/rclcpp/src/rclcpp/any_executable.cpp index 769deacb11..2449cbe1f7 100644 --- a/rclcpp/src/rclcpp/any_executable.cpp +++ b/rclcpp/src/rclcpp/any_executable.cpp @@ -21,6 +21,7 @@ AnyExecutable::AnyExecutable() timer(nullptr), service(nullptr), client(nullptr), + waitable(nullptr), callback_group(nullptr), node_base(nullptr) {} diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index e2bc473d54..bcacaabebe 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -66,6 +66,7 @@ CallbackGroup::size() const timer_ptrs_.size() + waitable_ptrs_.size(); } + void CallbackGroup::collect_all_ptrs( std::function sub_func, std::function service_func, diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 416a82be7c..776e6de4ea 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include +#include +#include #include #include #include @@ -22,13 +24,14 @@ #include "rcl/allocator.h" #include "rcl/error_handling.h" +#include "rclcpp/executors/executor_notify_waitable.hpp" +#include "rclcpp/subscription_wait_set_mask.hpp" #include "rcpputils/scope_exit.hpp" #include "rclcpp/dynamic_typesupport/dynamic_message.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/utilities.hpp" @@ -38,21 +41,29 @@ using namespace std::chrono_literals; -using rclcpp::exceptions::throw_from_rcl_error; using rclcpp::Executor; +/// Mask to indicate to the waitset to only add the subscription. +/// The events and intraprocess waitable are already added via the callback group. +static constexpr rclcpp::SubscriptionWaitSetMask kDefaultSubscriptionMask = {true, false, false}; + class rclcpp::ExecutorImplementation {}; Executor::Executor(const rclcpp::ExecutorOptions & options) : spinning(false), interrupt_guard_condition_(std::make_shared(options.context)), shutdown_guard_condition_(std::make_shared(options.context)), - memory_strategy_(options.memory_strategy), + context_(options.context), + notify_waitable_(std::make_shared( + [this]() { + this->entities_need_rebuild_.store(true); + })), + entities_need_rebuild_(true), + collector_(notify_waitable_), + wait_set_({}, {}, {}, {}, {}, {}, options.context), + current_notify_waitable_(notify_waitable_), impl_(std::make_unique()) { - // Store the context for later use. - context_ = options.context; - shutdown_callback_handle_ = context_->add_on_shutdown_callback( [weak_gc = std::weak_ptr{shutdown_guard_condition_}]() { auto strong_gc = weak_gc.lock(); @@ -61,75 +72,46 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) } }); - // The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond, - // and one for the executor's guard cond (interrupt_guard_condition_) - memory_strategy_->add_guard_condition(*shutdown_guard_condition_.get()); - - // Put the executor's guard condition in - memory_strategy_->add_guard_condition(*interrupt_guard_condition_.get()); - rcl_allocator_t allocator = memory_strategy_->get_allocator(); + notify_waitable_->set_on_ready_callback( + [this](auto, auto) { + this->entities_need_rebuild_.store(true); + }); - rcl_ret_t ret = rcl_wait_set_init( - &wait_set_, - 0, 2, 0, 0, 0, 0, - context_->get_rcl_context().get(), - allocator); - if (RCL_RET_OK != ret) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to create wait set: %s", rcl_get_error_string().str); - rcl_reset_error(); - throw_from_rcl_error(ret, "Failed to create wait set in Executor constructor"); - } + notify_waitable_->add_guard_condition(interrupt_guard_condition_); + notify_waitable_->add_guard_condition(shutdown_guard_condition_); } Executor::~Executor() { - // Disassociate all callback groups. - for (auto & pair : weak_groups_to_nodes_) { - auto group = pair.first.lock(); - if (group) { - std::atomic_bool & has_executor = group->get_associated_with_executor_atomic(); - has_executor.store(false); - } - } - // Disassociate all nodes. - std::for_each( - weak_nodes_.begin(), weak_nodes_.end(), - [](rclcpp::node_interfaces::NodeBaseInterface::WeakPtr weak_node_ptr) - { - auto shared_node_ptr = weak_node_ptr.lock(); - if (shared_node_ptr) { - std::atomic_bool & has_executor = shared_node_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); - } + std::lock_guard guard(mutex_); + + notify_waitable_->remove_guard_condition(interrupt_guard_condition_); + notify_waitable_->remove_guard_condition(shutdown_guard_condition_); + current_collection_.timers.update( + {}, {}, + [this](auto timer) {wait_set_.remove_timer(timer);}); + + current_collection_.subscriptions.update( + {}, {}, + [this](auto subscription) { + wait_set_.remove_subscription(subscription, kDefaultSubscriptionMask); }); - weak_nodes_.clear(); - weak_groups_associated_with_executor_to_nodes_.clear(); - weak_groups_to_nodes_associated_with_executor_.clear(); - weak_groups_to_nodes_.clear(); - for (const auto & pair : weak_groups_to_guard_conditions_) { - auto guard_condition = pair.second; - memory_strategy_->remove_guard_condition(guard_condition); - } - weak_groups_to_guard_conditions_.clear(); - for (const auto & pair : weak_nodes_to_guard_conditions_) { - auto guard_condition = pair.second; - memory_strategy_->remove_guard_condition(guard_condition); - } - weak_nodes_to_guard_conditions_.clear(); + current_collection_.clients.update( + {}, {}, + [this](auto client) {wait_set_.remove_client(client);}); - // Finalize the wait set. - if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy wait set: %s", rcl_get_error_string().str); - rcl_reset_error(); - } - // Remove and release the sigint guard condition - memory_strategy_->remove_guard_condition(shutdown_guard_condition_.get()); - memory_strategy_->remove_guard_condition(interrupt_guard_condition_.get()); + current_collection_.services.update( + {}, {}, + [this](auto service) {wait_set_.remove_service(service);}); + + current_collection_.guard_conditions.update( + {}, {}, + [this](auto guard_condition) {wait_set_.remove_guard_condition(guard_condition);}); + + current_collection_.waitables.update( + {}, {}, + [this](auto waitable) {wait_set_.remove_waitable(waitable);}); // Remove shutdown callback handle registered to Context if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) { @@ -143,95 +125,39 @@ Executor::~Executor() std::vector Executor::get_all_callback_groups() { - std::vector groups; - std::lock_guard guard{mutex_}; - for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) { - groups.push_back(group_node_ptr.first); - } - for (auto const & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) { - groups.push_back(group_node_ptr.first); - } - return groups; + this->collector_.update_collections(); + return this->collector_.get_all_callback_groups(); } std::vector Executor::get_manually_added_callback_groups() { - std::vector groups; - std::lock_guard guard{mutex_}; - for (auto const & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) { - groups.push_back(group_node_ptr.first); - } - return groups; + this->collector_.update_collections(); + return this->collector_.get_manually_added_callback_groups(); } std::vector Executor::get_automatically_added_callback_groups_from_nodes() { - std::vector groups; - std::lock_guard guard{mutex_}; - for (auto const & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) { - groups.push_back(group_node_ptr.first); - } - return groups; + this->collector_.update_collections(); + return this->collector_.get_automatically_added_callback_groups(); } void -Executor::add_callback_groups_from_nodes_associated_to_executor() -{ - for (auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (node) { - node->for_each_callback_group( - [this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr) - { - if ( - shared_group_ptr->automatically_add_to_executor_with_node() && - !shared_group_ptr->get_associated_with_executor_atomic().load()) - { - this->add_callback_group_to_map( - shared_group_ptr, - node, - weak_groups_to_nodes_associated_with_executor_, - true); - } - }); - } - } -} - -void -Executor::add_callback_group_to_map( +Executor::add_callback_group( rclcpp::CallbackGroup::SharedPtr group_ptr, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, bool notify) { - // If the callback_group already has an executor - std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); - if (has_executor.exchange(true)) { - throw std::runtime_error("Callback group has already been added to an executor."); - } + (void) node_ptr; + this->collector_.add_callback_group(group_ptr); - rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr; - auto insert_info = - weak_groups_to_nodes.insert(std::make_pair(weak_group_ptr, node_ptr)); - bool was_inserted = insert_info.second; - if (!was_inserted) { - throw std::runtime_error("Callback group was already added to executor."); - } - // Also add to the map that contains all callback groups - weak_groups_to_nodes_.insert(std::make_pair(weak_group_ptr, node_ptr)); - - if (node_ptr->get_context()->is_valid()) { - auto callback_group_guard_condition = group_ptr->get_notify_guard_condition(); - weak_groups_to_guard_conditions_[weak_group_ptr] = callback_group_guard_condition.get(); - // Add the callback_group's notify condition to the guard condition handles - memory_strategy_->add_guard_condition(*callback_group_guard_condition); + if (!spinning.load()) { + std::lock_guard guard(mutex_); + this->collect_entities(); } if (notify) { - // Interrupt waiting to handle new node try { interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { @@ -242,91 +168,23 @@ Executor::add_callback_group_to_map( } } -void -Executor::add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - bool notify) -{ - std::lock_guard guard{mutex_}; - this->add_callback_group_to_map( - group_ptr, - node_ptr, - weak_groups_associated_with_executor_to_nodes_, - notify); -} - void Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) { - // If the node already has an executor - std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); - if (has_executor.exchange(true)) { - throw std::runtime_error( - std::string("Node '") + node_ptr->get_fully_qualified_name() + - "' has already been added to an executor."); - } - std::lock_guard guard{mutex_}; - node_ptr->for_each_callback_group( - [this, node_ptr, notify](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - if (!group_ptr->get_associated_with_executor_atomic().load() && - group_ptr->automatically_add_to_executor_with_node()) - { - this->add_callback_group_to_map( - group_ptr, - node_ptr, - weak_groups_to_nodes_associated_with_executor_, - notify); - } - }); - - const auto gc = node_ptr->get_shared_notify_guard_condition(); - weak_nodes_to_guard_conditions_[node_ptr] = gc.get(); - // Add the node's notify condition to the guard condition handles - memory_strategy_->add_guard_condition(*gc); - weak_nodes_.push_back(node_ptr); -} + this->collector_.add_node(node_ptr); -void -Executor::remove_callback_group_from_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, - bool notify) -{ - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr; - rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr; - auto iter = weak_groups_to_nodes.find(weak_group_ptr); - if (iter != weak_groups_to_nodes.end()) { - node_ptr = iter->second.lock(); - if (node_ptr == nullptr) { - throw std::runtime_error("Node must not be deleted before its callback group(s)."); - } - weak_groups_to_nodes.erase(iter); - weak_groups_to_nodes_.erase(group_ptr); - std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); - } else { - throw std::runtime_error("Callback group needs to be associated with executor."); + if (!spinning.load()) { + std::lock_guard guard(mutex_); + this->collect_entities(); } - // If the node was matched and removed, interrupt waiting. - if (!has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) && - !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_)) - { - auto iter = weak_groups_to_guard_conditions_.find(weak_group_ptr); - if (iter != weak_groups_to_guard_conditions_.end()) { - memory_strategy_->remove_guard_condition(iter->second); - } - weak_groups_to_guard_conditions_.erase(weak_group_ptr); - - if (notify) { - try { - interrupt_guard_condition_->trigger(); - } catch (const rclcpp::exceptions::RCLError & ex) { - throw std::runtime_error( - std::string( - "Failed to trigger guard condition on callback group remove: ") + ex.what()); - } + + if (notify) { + try { + interrupt_guard_condition_->trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on node add: ") + ex.what()); } } } @@ -336,11 +194,21 @@ Executor::remove_callback_group( rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify) { - std::lock_guard guard{mutex_}; - this->remove_callback_group_from_map( - group_ptr, - weak_groups_associated_with_executor_to_nodes_, - notify); + this->collector_.remove_callback_group(group_ptr); + + if (!spinning.load()) { + std::lock_guard guard(mutex_); + this->collect_entities(); + } + if (notify) { + try { + interrupt_guard_condition_->trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on callback group remove: ") + ex.what()); + } + } } void @@ -352,48 +220,22 @@ Executor::add_node(std::shared_ptr node_ptr, bool notify) void Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) { - if (!node_ptr->get_associated_with_executor_atomic().load()) { - throw std::runtime_error("Node needs to be associated with an executor."); - } + this->collector_.remove_node(node_ptr); - std::lock_guard guard{mutex_}; - bool found_node = false; - auto node_it = weak_nodes_.begin(); - while (node_it != weak_nodes_.end()) { - bool matched = (node_it->lock() == node_ptr); - if (matched) { - found_node = true; - node_it = weak_nodes_.erase(node_it); - } else { - ++node_it; - } - } - if (!found_node) { - throw std::runtime_error("Node needs to be associated with this executor."); + if (!spinning.load()) { + std::lock_guard guard(mutex_); + this->collect_entities(); } - for (auto it = weak_groups_to_nodes_associated_with_executor_.begin(); - it != weak_groups_to_nodes_associated_with_executor_.end(); ) - { - auto weak_node_ptr = it->second; - auto shared_node_ptr = weak_node_ptr.lock(); - auto group_ptr = it->first.lock(); - - // Increment iterator before removing in case it's invalidated - it++; - if (shared_node_ptr == node_ptr) { - remove_callback_group_from_map( - group_ptr, - weak_groups_to_nodes_associated_with_executor_, - notify); + if (notify) { + try { + interrupt_guard_condition_->trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on node remove: ") + ex.what()); } } - - memory_strategy_->remove_guard_condition(node_ptr->get_shared_notify_guard_condition().get()); - weak_nodes_to_guard_conditions_.erase(node_ptr); - - std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); } void @@ -476,20 +318,25 @@ Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive) throw std::runtime_error("spin_some() called while already spinning"); } RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); - bool work_available = false; + while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { - AnyExecutable any_exec; - if (!work_available) { - wait_for_work(std::chrono::milliseconds::zero()); + if (!wait_result_.has_value()) { + wait_for_work(std::chrono::milliseconds(0)); } + + AnyExecutable any_exec; if (get_next_ready_executable(any_exec)) { execute_any_executable(any_exec); - work_available = true; } else { - if (!work_available || !exhaustive) { - break; - } - work_available = false; + // If nothing is ready, reset the result to signal we are + // ready to wait again + wait_result_.reset(); + } + + if (!wait_result_.has_value() && !exhaustive) { + // In the case of spin some, then we can exit + // In the case of spin all, then we will allow ourselves to wait again. + break; } } } @@ -525,22 +372,13 @@ Executor::cancel() } } -void -Executor::set_memory_strategy(rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy) -{ - if (memory_strategy == nullptr) { - throw std::runtime_error("Received NULL memory strategy in executor."); - } - std::lock_guard guard{mutex_}; - memory_strategy_ = memory_strategy; -} - void Executor::execute_any_executable(AnyExecutable & any_exec) { if (!spinning.load()) { return; } + if (any_exec.timer) { TRACETOOLS_TRACEPOINT( rclcpp_executor_execute, @@ -562,16 +400,10 @@ Executor::execute_any_executable(AnyExecutable & any_exec) if (any_exec.waitable) { any_exec.waitable->execute(any_exec.data); } + // Reset the callback_group, regardless of type - any_exec.callback_group->can_be_taken_from().store(true); - // Wake the wait, because it may need to be recalculated or work that - // was previously blocked is now available. - try { - interrupt_guard_condition_->trigger(); - } catch (const rclcpp::exceptions::RCLError & ex) { - throw std::runtime_error( - std::string( - "Failed to trigger guard condition from execute_any_executable: ") + ex.what()); + if (any_exec.callback_group) { + any_exec.callback_group->can_be_taken_from().store(true); } } @@ -711,7 +543,6 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) throw std::runtime_error("Delivered message kind is not supported"); } } - return; } void @@ -733,8 +564,7 @@ Executor::execute_service(rclcpp::ServiceBase::SharedPtr service) } void -Executor::execute_client( - rclcpp::ClientBase::SharedPtr client) +Executor::execute_client(rclcpp::ClientBase::SharedPtr client) { auto request_header = client->create_request_header(); std::shared_ptr response = client->create_response(); @@ -746,227 +576,213 @@ Executor::execute_client( } void -Executor::wait_for_work(std::chrono::nanoseconds timeout) -{ - TRACETOOLS_TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count()); - { - std::lock_guard guard(mutex_); - - // Check weak_nodes_ to find any callback group that is not owned - // by an executor and add it to the list of callbackgroups for - // collect entities. Also exchange to false so it is not - // allowed to add to another executor - add_callback_groups_from_nodes_associated_to_executor(); - - // Collect the subscriptions and timers to be waited on - memory_strategy_->clear_handles(); - bool has_invalid_weak_groups_or_nodes = - memory_strategy_->collect_entities(weak_groups_to_nodes_); - - if (has_invalid_weak_groups_or_nodes) { - std::vector invalid_group_ptrs; - for (auto pair : weak_groups_to_nodes_) { - auto weak_group_ptr = pair.first; - auto weak_node_ptr = pair.second; - if (weak_group_ptr.expired() || weak_node_ptr.expired()) { - invalid_group_ptrs.push_back(weak_group_ptr); - auto node_guard_pair = weak_nodes_to_guard_conditions_.find(weak_node_ptr); - if (node_guard_pair != weak_nodes_to_guard_conditions_.end()) { - auto guard_condition = node_guard_pair->second; - weak_nodes_to_guard_conditions_.erase(weak_node_ptr); - memory_strategy_->remove_guard_condition(guard_condition); - } - } - } - std::for_each( - invalid_group_ptrs.begin(), invalid_group_ptrs.end(), - [this](rclcpp::CallbackGroup::WeakPtr group_ptr) { - if (weak_groups_to_nodes_associated_with_executor_.find(group_ptr) != - weak_groups_to_nodes_associated_with_executor_.end()) - { - weak_groups_to_nodes_associated_with_executor_.erase(group_ptr); - } - if (weak_groups_associated_with_executor_to_nodes_.find(group_ptr) != - weak_groups_associated_with_executor_to_nodes_.end()) - { - weak_groups_associated_with_executor_to_nodes_.erase(group_ptr); - } - auto callback_guard_pair = weak_groups_to_guard_conditions_.find(group_ptr); - if (callback_guard_pair != weak_groups_to_guard_conditions_.end()) { - auto guard_condition = callback_guard_pair->second; - weak_groups_to_guard_conditions_.erase(group_ptr); - memory_strategy_->remove_guard_condition(guard_condition); - } - weak_groups_to_nodes_.erase(group_ptr); - }); - } +Executor::collect_entities() +{ + // Updating the entity collection and waitset expires any active result + this->wait_result_.reset(); + + // Get the current list of available waitables from the collector. + rclcpp::executors::ExecutorEntitiesCollection collection; + this->collector_.update_collections(); + auto callback_groups = this->collector_.get_all_callback_groups(); + rclcpp::executors::build_entities_collection(callback_groups, collection); + + // Make a copy of notify waitable so we can continue to mutate the original + // one outside of the execute loop. + // This prevents the collection of guard conditions in the waitable from changing + // while we are waiting on it. + if (notify_waitable_) { + current_notify_waitable_ = std::make_shared( + *notify_waitable_); + auto notify_waitable = std::static_pointer_cast(current_notify_waitable_); + collection.waitables.insert({notify_waitable.get(), {notify_waitable, {}}}); + } + + // We must remove expired entities here, so that we don't continue to use older entities. + // See https://github.com/ros2/rclcpp/issues/2180 for more information. + current_collection_.remove_expired_entities(); + + // Update each of the groups of entities in the current collection, adding or removing + // from the wait set as necessary. + current_collection_.timers.update( + collection.timers, + [this](auto timer) {wait_set_.add_timer(timer);}, + [this](auto timer) {wait_set_.remove_timer(timer);}); + + current_collection_.subscriptions.update( + collection.subscriptions, + [this](auto subscription) { + wait_set_.add_subscription(subscription, kDefaultSubscriptionMask); + }, + [this](auto subscription) { + wait_set_.remove_subscription(subscription, kDefaultSubscriptionMask); + }); - // clear wait set - rcl_ret_t ret = rcl_wait_set_clear(&wait_set_); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "Couldn't clear wait set"); - } + current_collection_.clients.update( + collection.clients, + [this](auto client) {wait_set_.add_client(client);}, + [this](auto client) {wait_set_.remove_client(client);}); - // The size of waitables are accounted for in size of the other entities - ret = rcl_wait_set_resize( - &wait_set_, memory_strategy_->number_of_ready_subscriptions(), - memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(), - memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(), - memory_strategy_->number_of_ready_events()); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "Couldn't resize the wait set"); - } + current_collection_.services.update( + collection.services, + [this](auto service) {wait_set_.add_service(service);}, + [this](auto service) {wait_set_.remove_service(service);}); - if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) { - throw std::runtime_error("Couldn't fill wait set"); - } - } + current_collection_.guard_conditions.update( + collection.guard_conditions, + [this](auto guard_condition) {wait_set_.add_guard_condition(guard_condition);}, + [this](auto guard_condition) {wait_set_.remove_guard_condition(guard_condition);}); - rcl_ret_t status = - rcl_wait(&wait_set_, std::chrono::duration_cast(timeout).count()); - if (status == RCL_RET_WAIT_SET_EMPTY) { - RCUTILS_LOG_WARN_NAMED( - "rclcpp", - "empty wait set received in rcl_wait(). This should never happen."); - } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) { - using rclcpp::exceptions::throw_from_rcl_error; - throw_from_rcl_error(status, "rcl_wait() failed"); - } + current_collection_.waitables.update( + collection.waitables, + [this](auto waitable) {wait_set_.add_waitable(waitable);}, + [this](auto waitable) {wait_set_.remove_waitable(waitable);}); - // check the null handles in the wait set and remove them from the handles in memory strategy - // for callback-based entities - std::lock_guard guard(mutex_); - memory_strategy_->remove_null_handles(&wait_set_); + // In the case that an entity already has an expired weak pointer + // before being removed from the waitset, additionally prune the waitset. + this->wait_set_.prune_deleted_entities(); + this->entities_need_rebuild_.store(false); } -rclcpp::node_interfaces::NodeBaseInterface::SharedPtr -Executor::get_node_by_group( - const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & - weak_groups_to_nodes, - rclcpp::CallbackGroup::SharedPtr group) +void +Executor::wait_for_work(std::chrono::nanoseconds timeout) { - if (!group) { - return nullptr; - } - rclcpp::CallbackGroup::WeakPtr weak_group_ptr(group); - const auto finder = weak_groups_to_nodes.find(weak_group_ptr); - if (finder != weak_groups_to_nodes.end()) { - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr = finder->second.lock(); - return node_ptr; - } - return nullptr; -} + TRACETOOLS_TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count()); -rclcpp::CallbackGroup::SharedPtr -Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer) -{ - std::lock_guard guard{mutex_}; - for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) { - auto group = pair.first.lock(); - if (!group) { - continue; - } - auto timer_ref = group->find_timer_ptrs_if( - [timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool { - return timer_ptr == timer; - }); - if (timer_ref) { - return group; - } - } + // Clear any previous wait result + this->wait_result_.reset(); - for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) { - auto group = pair.first.lock(); - if (!group) { - continue; - } - auto timer_ref = group->find_timer_ptrs_if( - [timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool { - return timer_ptr == timer; - }); - if (timer_ref) { - return group; + { + std::lock_guard guard(mutex_); + if (current_collection_.empty() || this->entities_need_rebuild_.load()) { + this->collect_entities(); } } - return nullptr; + this->wait_result_.emplace(wait_set_.wait(timeout)); + if (!this->wait_result_ || this->wait_result_->kind() == WaitResultKind::Empty) { + RCUTILS_LOG_WARN_NAMED( + "rclcpp", + "empty wait set received in wait(). This should never happen."); + } } bool Executor::get_next_ready_executable(AnyExecutable & any_executable) -{ - bool success = get_next_ready_executable_from_map(any_executable, weak_groups_to_nodes_); - return success; -} - -bool -Executor::get_next_ready_executable_from_map( - AnyExecutable & any_executable, - const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & - weak_groups_to_nodes) { TRACETOOLS_TRACEPOINT(rclcpp_executor_get_next_ready); - bool success = false; - std::lock_guard guard{mutex_}; - // Check the timers to see if there are any that are ready - memory_strategy_->get_next_timer(any_executable, weak_groups_to_nodes); - if (any_executable.timer) { - success = true; + + bool valid_executable = false; + + if (!wait_result_.has_value() || wait_result_->kind() != rclcpp::WaitResultKind::Ready) { + return false; } - if (!success) { - // Check the subscriptions to see if there are any that are ready - memory_strategy_->get_next_subscription(any_executable, weak_groups_to_nodes); - if (any_executable.subscription) { - success = true; + + if (!valid_executable) { + size_t current_timer_index = 0; + while (true) { + auto [timer, timer_index] = wait_result_->peek_next_ready_timer(current_timer_index); + if (nullptr == timer) { + break; + } + current_timer_index = timer_index; + auto entity_iter = current_collection_.timers.find(timer->get_timer_handle().get()); + if (entity_iter != current_collection_.timers.end()) { + auto callback_group = entity_iter->second.callback_group.lock(); + if (callback_group && !callback_group->can_be_taken_from()) { + continue; + } + // At this point the timer is either ready for execution or was perhaps + // it was canceled, based on the result of call(), but either way it + // should not be checked again from peek_next_ready_timer(), so clear + // it from the wait result. + wait_result_->clear_timer_with_index(current_timer_index); + // Check that the timer should be called still, i.e. it wasn't canceled. + if (!timer->call()) { + continue; + } + any_executable.timer = timer; + any_executable.callback_group = callback_group; + valid_executable = true; + break; + } } } - if (!success) { - // Check the services to see if there are any that are ready - memory_strategy_->get_next_service(any_executable, weak_groups_to_nodes); - if (any_executable.service) { - success = true; + + if (!valid_executable) { + while (auto subscription = wait_result_->next_ready_subscription()) { + auto entity_iter = current_collection_.subscriptions.find( + subscription->get_subscription_handle().get()); + if (entity_iter != current_collection_.subscriptions.end()) { + auto callback_group = entity_iter->second.callback_group.lock(); + if (callback_group && !callback_group->can_be_taken_from()) { + continue; + } + any_executable.subscription = subscription; + any_executable.callback_group = callback_group; + valid_executable = true; + break; + } } } - if (!success) { - // Check the clients to see if there are any that are ready - memory_strategy_->get_next_client(any_executable, weak_groups_to_nodes); - if (any_executable.client) { - success = true; + + if (!valid_executable) { + while (auto service = wait_result_->next_ready_service()) { + auto entity_iter = current_collection_.services.find(service->get_service_handle().get()); + if (entity_iter != current_collection_.services.end()) { + auto callback_group = entity_iter->second.callback_group.lock(); + if (callback_group && !callback_group->can_be_taken_from()) { + continue; + } + any_executable.service = service; + any_executable.callback_group = callback_group; + valid_executable = true; + break; + } } } - if (!success) { - // Check the waitables to see if there are any that are ready - memory_strategy_->get_next_waitable(any_executable, weak_groups_to_nodes); - if (any_executable.waitable) { - any_executable.data = any_executable.waitable->take_data(); - success = true; + + if (!valid_executable) { + while (auto client = wait_result_->next_ready_client()) { + auto entity_iter = current_collection_.clients.find(client->get_client_handle().get()); + if (entity_iter != current_collection_.clients.end()) { + auto callback_group = entity_iter->second.callback_group.lock(); + if (callback_group && !callback_group->can_be_taken_from()) { + continue; + } + any_executable.client = client; + any_executable.callback_group = callback_group; + valid_executable = true; + break; + } } } - // At this point any_executable should be valid with either a valid subscription - // or a valid timer, or it should be a null shared_ptr - if (success) { - rclcpp::CallbackGroup::WeakPtr weak_group_ptr = any_executable.callback_group; - auto iter = weak_groups_to_nodes.find(weak_group_ptr); - if (iter == weak_groups_to_nodes.end()) { - success = false; + + if (!valid_executable) { + while (auto waitable = wait_result_->next_ready_waitable()) { + auto entity_iter = current_collection_.waitables.find(waitable.get()); + if (entity_iter != current_collection_.waitables.end()) { + auto callback_group = entity_iter->second.callback_group.lock(); + if (callback_group && !callback_group->can_be_taken_from()) { + continue; + } + any_executable.waitable = waitable; + any_executable.callback_group = callback_group; + any_executable.data = waitable->take_data(); + valid_executable = true; + break; + } } } - if (success) { - // If it is valid, check to see if the group is mutually exclusive or - // not, then mark it accordingly ..Check if the callback_group belongs to this executor - if (any_executable.callback_group && any_executable.callback_group->type() == \ - CallbackGroupType::MutuallyExclusive) - { - // It should not have been taken otherwise + if (any_executable.callback_group) { + if (any_executable.callback_group->type() == CallbackGroupType::MutuallyExclusive) { assert(any_executable.callback_group->can_be_taken_from().load()); - // Set to false to indicate something is being run from this group - // This is reset to true either when the any_executable is executed or when the - // any_executable is destructued any_executable.callback_group->can_be_taken_from().store(false); } } - // If there is no ready executable, return false - return success; + + + return valid_executable; } bool @@ -989,22 +805,6 @@ Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanos return success; } -// Returns true iff the weak_groups_to_nodes map has node_ptr as the value in any of its entry. -bool -Executor::has_node( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & - weak_groups_to_nodes) const -{ - return std::find_if( - weak_groups_to_nodes.begin(), - weak_groups_to_nodes.end(), - [&](const WeakCallbackGroupsToNodesMap::value_type & other) -> bool { - auto other_ptr = other.second.lock(); - return other_ptr == node_ptr; - }) != weak_groups_to_nodes.end(); -} - bool Executor::is_spinning() { diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index 88a878824a..b6e030d340 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -39,6 +39,30 @@ void ExecutorEntitiesCollection::clear() waitables.clear(); } +size_t ExecutorEntitiesCollection::remove_expired_entities() +{ + auto remove_entities = [](auto & collection) -> size_t { + size_t removed = 0; + for (auto it = collection.begin(); it != collection.end(); ) { + if (it->second.entity.expired()) { + ++removed; + it = collection.erase(it); + } else { + ++it; + } + } + return removed; + }; + + return + remove_entities(subscriptions) + + remove_entities(timers) + + remove_entities(guard_conditions) + + remove_entities(clients) + + remove_entities(services) + + remove_entities(waitables); +} + void build_entities_collection( const std::vector & callback_groups, @@ -203,7 +227,7 @@ ready_executables( } } - for (auto & [handle, entry] : collection.waitables) { + for (const auto & [handle, entry] : collection.waitables) { auto waitable = entry.entity.lock(); if (!waitable) { continue; @@ -218,13 +242,10 @@ ready_executables( rclcpp::AnyExecutable exec; exec.waitable = waitable; exec.callback_group = group_info; - exec.data = waitable->take_data(); executables.push_back(exec); added++; } - return added; } - } // namespace executors } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp index 84ada64925..702716a758 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -61,6 +61,12 @@ ExecutorEntitiesCollector::~ExecutorEntitiesCollector() if (group_ptr) { group_ptr->get_associated_with_executor_atomic().store(false); } + // Disassociate the guard condition from the executor notify waitable + auto guard_condition_it = weak_groups_to_guard_conditions_.find(weak_group_ptr); + if (guard_condition_it != weak_groups_to_guard_conditions_.end()) { + this->notify_waitable_->remove_guard_condition(guard_condition_it->second); + weak_groups_to_guard_conditions_.erase(guard_condition_it); + } } pending_manually_added_groups_.clear(); pending_manually_removed_groups_.clear(); @@ -105,7 +111,8 @@ void ExecutorEntitiesCollector::remove_node( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) { - if (!node_ptr->get_associated_with_executor_atomic().load()) { + std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); + if (!has_executor.exchange(false)) { throw std::runtime_error( std::string("Node '") + node_ptr->get_fully_qualified_name() + "' needs to be associated with an executor."); @@ -143,6 +150,11 @@ ExecutorEntitiesCollector::add_callback_group(rclcpp::CallbackGroup::SharedPtr g } this->pending_manually_added_groups_.insert(group_ptr); + + // Store callback group notify guard condition in map and add it to the notify waitable + auto group_guard_condition = group_ptr->get_notify_guard_condition(); + weak_groups_to_guard_conditions_.insert({group_ptr, group_guard_condition}); + this->notify_waitable_->add_guard_condition(group_guard_condition); } void @@ -161,7 +173,6 @@ ExecutorEntitiesCollector::remove_callback_group(rclcpp::CallbackGroup::SharedPt throw std::runtime_error("Node must not be deleted before its callback group(s)."); } */ - auto weak_group_ptr = rclcpp::CallbackGroup::WeakPtr(group_ptr); std::lock_guard lock(mutex_); bool associated = manually_added_groups_.count(group_ptr) != 0; @@ -314,7 +325,11 @@ ExecutorEntitiesCollector::process_queues() if (node_it != weak_nodes_.end()) { remove_weak_node(node_it); } else { - throw std::runtime_error("Node needs to be associated with this executor."); + // The node may have been destroyed and removed from the colletion before + // we processed the queues. Don't throw if the pointer is already expired. + if (!weak_node_ptr.expired()) { + throw std::runtime_error("Node needs to be associated with this executor."); + } } auto node_ptr = weak_node_ptr.lock(); @@ -337,6 +352,13 @@ ExecutorEntitiesCollector::process_queues() auto group_ptr = weak_group_ptr.lock(); if (group_ptr) { this->add_callback_group_to_collection(group_ptr, manually_added_groups_); + } else { + // Disassociate the guard condition from the executor notify waitable + auto guard_condition_it = weak_groups_to_guard_conditions_.find(weak_group_ptr); + if (guard_condition_it != weak_groups_to_guard_conditions_.end()) { + this->notify_waitable_->remove_guard_condition(guard_condition_it->second); + weak_groups_to_guard_conditions_.erase(guard_condition_it); + } } } pending_manually_added_groups_.clear(); diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index 15a31cd60d..85bedcead1 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -27,15 +27,17 @@ ExecutorNotifyWaitable::ExecutorNotifyWaitable(std::function on_exec { } -ExecutorNotifyWaitable::ExecutorNotifyWaitable(const ExecutorNotifyWaitable & other) -: ExecutorNotifyWaitable(other.execute_callback_) +ExecutorNotifyWaitable::ExecutorNotifyWaitable(ExecutorNotifyWaitable & other) { + std::lock_guard lock(other.guard_condition_mutex_); + this->execute_callback_ = other.execute_callback_; this->notify_guard_conditions_ = other.notify_guard_conditions_; } -ExecutorNotifyWaitable & ExecutorNotifyWaitable::operator=(const ExecutorNotifyWaitable & other) +ExecutorNotifyWaitable & ExecutorNotifyWaitable::operator=(ExecutorNotifyWaitable & other) { if (this != &other) { + std::lock_guard lock(other.guard_condition_mutex_); this->execute_callback_ = other.execute_callback_; this->notify_guard_conditions_ = other.notify_guard_conditions_; } @@ -46,20 +48,17 @@ void 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_) { auto guard_condition = weak_guard_condition.lock(); - if (guard_condition) { - auto rcl_guard_condition = &guard_condition->get_rcl_guard_condition(); + if (!guard_condition) {continue;} - rcl_ret_t ret = rcl_wait_set_add_guard_condition( - wait_set, - rcl_guard_condition, NULL); + rcl_guard_condition_t * cond = &guard_condition->get_rcl_guard_condition(); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "failed to add guard condition to wait set"); - } + 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( + ret, "failed to add guard condition to wait set"); } } } @@ -71,15 +70,16 @@ ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set) bool any_ready = false; for (size_t ii = 0; ii < wait_set->size_of_guard_conditions; ++ii) { - auto rcl_guard_condition = wait_set->guard_conditions[ii]; + const auto * rcl_guard_condition = wait_set->guard_conditions[ii]; if (nullptr == rcl_guard_condition) { continue; } - for (auto weak_guard_condition : this->notify_guard_conditions_) { + for (const auto & weak_guard_condition : this->notify_guard_conditions_) { auto guard_condition = weak_guard_condition.lock(); if (guard_condition && &guard_condition->get_rcl_guard_condition() == rcl_guard_condition) { any_ready = true; + break; } } } diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index 507d47f913..2d5c76e817 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -55,7 +55,7 @@ MultiThreadedExecutor::spin() if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false);); std::vector threads; size_t thread_id = 0; { @@ -99,6 +99,18 @@ MultiThreadedExecutor::run(size_t this_thread_number) execute_any_executable(any_exec); + if (any_exec.callback_group && + any_exec.callback_group->type() == CallbackGroupType::MutuallyExclusive) + { + try { + interrupt_guard_condition_->trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on callback group change: ") + ex.what()); + } + } + // Clear the callback_group to prevent the AnyExecutable destructor from // resetting the callback group `can_be_taken_from` any_exec.callback_group.reset(); diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index e7f311c147..975733b497 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -31,6 +31,11 @@ SingleThreadedExecutor::spin() throw std::runtime_error("spin() called while already spinning"); } RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + + // Clear any previous result and rebuild the waitset + this->wait_result_.reset(); + this->entities_need_rebuild_ = true; + 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_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp deleted file mode 100644 index 73926b12bc..0000000000 --- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp +++ /dev/null @@ -1,527 +0,0 @@ -// Copyright 2020 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. - -#include "rclcpp/executors/static_executor_entities_collector.hpp" - -#include -#include -#include -#include -#include -#include - -#include "rclcpp/memory_strategy.hpp" -#include "rclcpp/executors/static_single_threaded_executor.hpp" -#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" - -using rclcpp::executors::StaticExecutorEntitiesCollector; - -StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector() -{ - // Disassociate all callback groups and thus nodes. - for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) { - auto group = pair.first.lock(); - if (group) { - std::atomic_bool & has_executor = group->get_associated_with_executor_atomic(); - has_executor.store(false); - } - } - for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) { - auto group = pair.first.lock(); - if (group) { - std::atomic_bool & has_executor = group->get_associated_with_executor_atomic(); - has_executor.store(false); - } - } - // Disassociate all nodes - for (const auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (node) { - std::atomic_bool & has_executor = node->get_associated_with_executor_atomic(); - has_executor.store(false); - } - } - weak_groups_associated_with_executor_to_nodes_.clear(); - weak_groups_to_nodes_associated_with_executor_.clear(); - exec_list_.clear(); - weak_nodes_.clear(); - weak_nodes_to_guard_conditions_.clear(); -} - -void -StaticExecutorEntitiesCollector::init( - rcl_wait_set_t * p_wait_set, - rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy) -{ - // Empty initialize executable list - exec_list_ = rclcpp::experimental::ExecutableList(); - // Get executor's wait_set_ pointer - p_wait_set_ = p_wait_set; - // Get executor's memory strategy ptr - if (memory_strategy == nullptr) { - throw std::runtime_error("Received NULL memory strategy in executor waitable."); - } - memory_strategy_ = memory_strategy; - - // Get memory strategy and executable list. Prepare wait_set_ - std::shared_ptr shared_ptr; - execute(shared_ptr); - - // The entities collector is now initialized - initialized_ = true; -} - -void -StaticExecutorEntitiesCollector::fini() -{ - memory_strategy_->clear_handles(); - exec_list_.clear(); -} - -std::shared_ptr -StaticExecutorEntitiesCollector::take_data() -{ - return nullptr; -} - -void -StaticExecutorEntitiesCollector::execute(std::shared_ptr & data) -{ - (void) data; - // Fill memory strategy with entities coming from weak_nodes_ - fill_memory_strategy(); - // Fill exec_list_ with entities coming from weak_nodes_ (same as memory strategy) - fill_executable_list(); - // Resize the wait_set_ based on memory_strategy handles (rcl_wait_set_resize) - prepare_wait_set(); - // Add new nodes guard conditions to map - std::lock_guard guard{new_nodes_mutex_}; - for (const auto & weak_node : new_nodes_) { - if (auto node_ptr = weak_node.lock()) { - weak_nodes_to_guard_conditions_[node_ptr] = - node_ptr->get_shared_notify_guard_condition().get(); - } - } - new_nodes_.clear(); -} - -void -StaticExecutorEntitiesCollector::fill_memory_strategy() -{ - memory_strategy_->clear_handles(); - bool has_invalid_weak_groups_or_nodes = - memory_strategy_->collect_entities(weak_groups_to_nodes_associated_with_executor_); - // Clean up any invalid nodes, if they were detected - if (has_invalid_weak_groups_or_nodes) { - std::vector invalid_group_ptrs; - for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) { - auto & weak_group_ptr = pair.first; - auto & weak_node_ptr = pair.second; - if (weak_group_ptr.expired() || weak_node_ptr.expired()) { - invalid_group_ptrs.push_back(weak_group_ptr); - } - } - std::for_each( - invalid_group_ptrs.begin(), invalid_group_ptrs.end(), - [this](rclcpp::CallbackGroup::WeakPtr group_ptr) { - weak_groups_to_nodes_associated_with_executor_.erase(group_ptr); - }); - } - has_invalid_weak_groups_or_nodes = - memory_strategy_->collect_entities(weak_groups_associated_with_executor_to_nodes_); - // Clean up any invalid nodes, if they were detected - if (has_invalid_weak_groups_or_nodes) { - std::vector invalid_group_ptrs; - for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) { - auto & weak_group_ptr = pair.first; - const auto & weak_node_ptr = pair.second; - if (weak_group_ptr.expired() || weak_node_ptr.expired()) { - invalid_group_ptrs.push_back(weak_group_ptr); - } - } - std::for_each( - invalid_group_ptrs.begin(), invalid_group_ptrs.end(), - [this](rclcpp::CallbackGroup::WeakPtr group_ptr) { - weak_groups_associated_with_executor_to_nodes_.erase(group_ptr); - }); - } - - // Add the static executor waitable to the memory strategy - memory_strategy_->add_waitable_handle(this->shared_from_this()); -} - -void -StaticExecutorEntitiesCollector::fill_executable_list() -{ - exec_list_.clear(); - add_callback_groups_from_nodes_associated_to_executor(); - fill_executable_list_from_map(weak_groups_associated_with_executor_to_nodes_); - fill_executable_list_from_map(weak_groups_to_nodes_associated_with_executor_); - // Add the executor's waitable to the executable list - exec_list_.add_waitable(shared_from_this()); -} -void -StaticExecutorEntitiesCollector::fill_executable_list_from_map( - const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & - weak_groups_to_nodes) -{ - for (const auto & pair : weak_groups_to_nodes) { - auto group = pair.first.lock(); - auto node = pair.second.lock(); - if (!node || !group || !group->can_be_taken_from().load()) { - continue; - } - group->find_timer_ptrs_if( - [this](const rclcpp::TimerBase::SharedPtr & timer) { - if (timer) { - exec_list_.add_timer(timer); - } - return false; - }); - group->find_subscription_ptrs_if( - [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) { - if (subscription) { - exec_list_.add_subscription(subscription); - } - return false; - }); - group->find_service_ptrs_if( - [this](const rclcpp::ServiceBase::SharedPtr & service) { - if (service) { - exec_list_.add_service(service); - } - return false; - }); - group->find_client_ptrs_if( - [this](const rclcpp::ClientBase::SharedPtr & client) { - if (client) { - exec_list_.add_client(client); - } - return false; - }); - group->find_waitable_ptrs_if( - [this](const rclcpp::Waitable::SharedPtr & waitable) { - if (waitable) { - exec_list_.add_waitable(waitable); - } - return false; - }); - } -} - -void -StaticExecutorEntitiesCollector::prepare_wait_set() -{ - // clear wait set - if (rcl_wait_set_clear(p_wait_set_) != RCL_RET_OK) { - throw std::runtime_error("Couldn't clear wait set"); - } - - // The size of waitables are accounted for in size of the other entities - rcl_ret_t ret = rcl_wait_set_resize( - p_wait_set_, memory_strategy_->number_of_ready_subscriptions(), - memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(), - memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(), - memory_strategy_->number_of_ready_events()); - - if (RCL_RET_OK != ret) { - throw std::runtime_error( - std::string("Couldn't resize the wait set: ") + rcl_get_error_string().str); - } -} - -void -StaticExecutorEntitiesCollector::refresh_wait_set(std::chrono::nanoseconds timeout) -{ - // clear wait set (memset to '0' all wait_set_ entities - // but keeps the wait_set_ number of entities) - if (rcl_wait_set_clear(p_wait_set_) != RCL_RET_OK) { - throw std::runtime_error("Couldn't clear wait set"); - } - - if (!memory_strategy_->add_handles_to_wait_set(p_wait_set_)) { - throw std::runtime_error("Couldn't fill wait set"); - } - - rcl_ret_t status = - rcl_wait(p_wait_set_, std::chrono::duration_cast(timeout).count()); - - if (status == RCL_RET_WAIT_SET_EMPTY) { - RCUTILS_LOG_WARN_NAMED( - "rclcpp", - "empty wait set received in rcl_wait(). This should never happen."); - } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) { - using rclcpp::exceptions::throw_from_rcl_error; - throw_from_rcl_error(status, "rcl_wait() failed"); - } -} - -void -StaticExecutorEntitiesCollector::add_to_wait_set(rcl_wait_set_t * wait_set) -{ - // Add waitable guard conditions (one for each registered node) into the wait set. - for (const auto & pair : weak_nodes_to_guard_conditions_) { - auto & gc = pair.second; - detail::add_guard_condition_to_rcl_wait_set(*wait_set, *gc); - } -} - -size_t StaticExecutorEntitiesCollector::get_number_of_ready_guard_conditions() -{ - std::lock_guard guard{new_nodes_mutex_}; - return weak_nodes_to_guard_conditions_.size() + new_nodes_.size(); -} - -bool -StaticExecutorEntitiesCollector::add_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) -{ - bool is_new_node = false; - // If the node already has an executor - std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); - if (has_executor.exchange(true)) { - throw std::runtime_error("Node has already been added to an executor."); - } - node_ptr->for_each_callback_group( - [this, node_ptr, &is_new_node](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - if ( - !group_ptr->get_associated_with_executor_atomic().load() && - group_ptr->automatically_add_to_executor_with_node()) - { - is_new_node = (add_callback_group( - group_ptr, - node_ptr, - weak_groups_to_nodes_associated_with_executor_) || - is_new_node); - } - }); - weak_nodes_.push_back(node_ptr); - return is_new_node; -} - -bool -StaticExecutorEntitiesCollector::add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) -{ - // If the callback_group already has an executor - std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); - if (has_executor.exchange(true)) { - throw std::runtime_error("Callback group has already been added to an executor."); - } - bool is_new_node = !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) && - !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_); - rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr; - auto insert_info = weak_groups_to_nodes.insert( - std::make_pair(weak_group_ptr, node_ptr)); - bool was_inserted = insert_info.second; - if (!was_inserted) { - throw std::runtime_error("Callback group was already added to executor."); - } - if (is_new_node) { - std::lock_guard guard{new_nodes_mutex_}; - new_nodes_.push_back(node_ptr); - return true; - } - return false; -} - -bool -StaticExecutorEntitiesCollector::add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) -{ - return add_callback_group(group_ptr, node_ptr, weak_groups_associated_with_executor_to_nodes_); -} - -bool -StaticExecutorEntitiesCollector::remove_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr) -{ - return this->remove_callback_group_from_map( - group_ptr, - weak_groups_associated_with_executor_to_nodes_); -} - -bool -StaticExecutorEntitiesCollector::remove_callback_group_from_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) -{ - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr; - rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr; - auto iter = weak_groups_to_nodes.find(weak_group_ptr); - if (iter != weak_groups_to_nodes.end()) { - node_ptr = iter->second.lock(); - if (node_ptr == nullptr) { - throw std::runtime_error("Node must not be deleted before its callback group(s)."); - } - weak_groups_to_nodes.erase(iter); - } else { - throw std::runtime_error("Callback group needs to be associated with executor."); - } - // If the node was matched and removed, interrupt waiting. - if (!has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) && - !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_)) - { - rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr); - weak_nodes_to_guard_conditions_.erase(node_weak_ptr); - return true; - } - return false; -} - -bool -StaticExecutorEntitiesCollector::remove_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) -{ - if (!node_ptr->get_associated_with_executor_atomic().load()) { - return false; - } - bool node_found = false; - auto node_it = weak_nodes_.begin(); - while (node_it != weak_nodes_.end()) { - bool matched = (node_it->lock() == node_ptr); - if (matched) { - weak_nodes_.erase(node_it); - node_found = true; - break; - } - ++node_it; - } - if (!node_found) { - return false; - } - std::vector found_group_ptrs; - std::for_each( - weak_groups_to_nodes_associated_with_executor_.begin(), - weak_groups_to_nodes_associated_with_executor_.end(), - [&found_group_ptrs, node_ptr]( - std::pair key_value_pair) - { - auto & weak_node_ptr = key_value_pair.second; - auto shared_node_ptr = weak_node_ptr.lock(); - auto group_ptr = key_value_pair.first.lock(); - if (shared_node_ptr == node_ptr) { - found_group_ptrs.push_back(group_ptr); - } - }); - std::for_each( - found_group_ptrs.begin(), found_group_ptrs.end(), - [this](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - this->remove_callback_group_from_map( - group_ptr, - weak_groups_to_nodes_associated_with_executor_); - }); - std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); - return true; -} - -bool -StaticExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set) -{ - // Check wait_set guard_conditions for added/removed entities to/from a node - for (size_t i = 0; i < p_wait_set->size_of_guard_conditions; ++i) { - if (p_wait_set->guard_conditions[i] != NULL) { - auto found_guard_condition = std::find_if( - weak_nodes_to_guard_conditions_.begin(), weak_nodes_to_guard_conditions_.end(), - [&](std::pair pair) -> bool { - const rcl_guard_condition_t & rcl_gc = pair.second->get_rcl_guard_condition(); - return &rcl_gc == p_wait_set->guard_conditions[i]; - }); - if (found_guard_condition != weak_nodes_to_guard_conditions_.end()) { - return true; - } - } - } - // None of the guard conditions triggered belong to a registered node - return false; -} - -// Returns true iff the weak_groups_to_nodes map has node_ptr as the value in any of its entry. -bool -StaticExecutorEntitiesCollector::has_node( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & - weak_groups_to_nodes) const -{ - return std::find_if( - weak_groups_to_nodes.begin(), - weak_groups_to_nodes.end(), - [&](const WeakCallbackGroupsToNodesMap::value_type & other) -> bool { - auto other_ptr = other.second.lock(); - return other_ptr == node_ptr; - }) != weak_groups_to_nodes.end(); -} - -void -StaticExecutorEntitiesCollector::add_callback_groups_from_nodes_associated_to_executor() -{ - for (const auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (node) { - node->for_each_callback_group( - [this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr) - { - if (shared_group_ptr->automatically_add_to_executor_with_node() && - !shared_group_ptr->get_associated_with_executor_atomic().load()) - { - add_callback_group( - shared_group_ptr, - node, - weak_groups_to_nodes_associated_with_executor_); - } - }); - } - } -} - -std::vector -StaticExecutorEntitiesCollector::get_all_callback_groups() -{ - std::vector groups; - for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) { - groups.push_back(group_node_ptr.first); - } - for (const auto & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) { - groups.push_back(group_node_ptr.first); - } - return groups; -} - -std::vector -StaticExecutorEntitiesCollector::get_manually_added_callback_groups() -{ - std::vector groups; - for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) { - groups.push_back(group_node_ptr.first); - } - return groups; -} - -std::vector -StaticExecutorEntitiesCollector::get_automatically_added_callback_groups_from_nodes() -{ - std::vector groups; - for (const auto & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) { - groups.push_back(group_node_ptr.first); - } - return groups; -} diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 3c14b37b45..3348f422f9 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -12,31 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rclcpp/executors/static_single_threaded_executor.hpp" - -#include -#include -#include -#include - +#include "rclcpp/executors/executor_entities_collection.hpp" #include "rcpputils/scope_exit.hpp" +#include "rclcpp/executors/static_single_threaded_executor.hpp" +#include "rclcpp/any_executable.hpp" + using rclcpp::executors::StaticSingleThreadedExecutor; -using rclcpp::experimental::ExecutableList; -StaticSingleThreadedExecutor::StaticSingleThreadedExecutor( - const rclcpp::ExecutorOptions & options) +StaticSingleThreadedExecutor::StaticSingleThreadedExecutor(const rclcpp::ExecutorOptions & options) : rclcpp::Executor(options) { - entities_collector_ = std::make_shared(); } -StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() -{ - if (entities_collector_->is_init()) { - entities_collector_->fini(); - } -} +StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() {} void StaticSingleThreadedExecutor::spin() @@ -46,14 +35,11 @@ StaticSingleThreadedExecutor::spin() } RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); - // Set memory_strategy_ and exec_list_ based on weak_nodes_ - // Prepare wait_set_ based on memory_strategy_ - entities_collector_->init(&wait_set_, memory_strategy_); - + // This is essentially the contents of the rclcpp::Executor::wait_for_work method, + // except we need to keep the wait result to reproduce the StaticSingleThreadedExecutor + // behavior. while (rclcpp::ok(this->context_) && spinning.load()) { - // Refresh wait set and wait for work - entities_collector_->refresh_wait_set(); - execute_ready_executables(); + this->spin_once_impl(std::chrono::nanoseconds(-1)); } } @@ -64,7 +50,6 @@ StaticSingleThreadedExecutor::spin_some(std::chrono::nanoseconds max_duration) if (std::chrono::nanoseconds(0) == max_duration) { max_duration = std::chrono::nanoseconds::max(); } - return this->spin_some_impl(max_duration, false); } @@ -80,36 +65,32 @@ StaticSingleThreadedExecutor::spin_all(std::chrono::nanoseconds max_duration) void StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive) { - // Make sure the entities collector has been initialized - if (!entities_collector_->is_init()) { - entities_collector_->init(&wait_set_, memory_strategy_); - } - auto start = std::chrono::steady_clock::now(); auto max_duration_not_elapsed = [max_duration, start]() { - if (std::chrono::nanoseconds(0) == max_duration) { - // told to spin forever if need be - return true; - } else if (std::chrono::steady_clock::now() - start < max_duration) { - // told to spin only for some maximum amount of time - return true; - } - // spun too long - return false; + const auto spin_forever = std::chrono::nanoseconds(0) == max_duration; + const auto cur_duration = std::chrono::steady_clock::now() - start; + return spin_forever || (cur_duration < max_duration); }; if (spinning.exchange(true)) { throw std::runtime_error("spin_some() called while already spinning"); } - RCPPUTILS_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 - entities_collector_->refresh_wait_set(std::chrono::milliseconds::zero()); - // Execute ready executables - bool work_available = execute_ready_executables(); - if (!work_available || !exhaustive) { - break; + std::lock_guard guard(mutex_); + + auto wait_result = this->collect_and_wait(std::chrono::nanoseconds(0)); + if (wait_result.has_value()) { + // Execute ready executables + bool work_available = this->execute_ready_executables( + current_collection_, + wait_result.value(), + false); + if (!work_available || !exhaustive) { + break; + } } } } @@ -117,163 +98,95 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati void StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout) { - // Make sure the entities collector has been initialized - if (!entities_collector_->is_init()) { - entities_collector_->init(&wait_set_, memory_strategy_); - } - if (rclcpp::ok(context_) && spinning.load()) { - // Wait until we have a ready entity or timeout expired - entities_collector_->refresh_wait_set(timeout); - // Execute ready executables - execute_ready_executables(true); + std::lock_guard guard(mutex_); + auto wait_result = this->collect_and_wait(timeout); + if (wait_result.has_value()) { + this->execute_ready_executables(current_collection_, wait_result.value(), true); + } } } -void -StaticSingleThreadedExecutor::add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - bool notify) +std::optional> +StaticSingleThreadedExecutor::collect_and_wait(std::chrono::nanoseconds timeout) { - bool is_new_node = entities_collector_->add_callback_group(group_ptr, node_ptr); - if (is_new_node && notify) { - // Interrupt waiting to handle new node - interrupt_guard_condition_->trigger(); + if (current_collection_.empty() || this->entities_need_rebuild_.load()) { + this->collect_entities(); } -} - -void -StaticSingleThreadedExecutor::add_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) -{ - bool is_new_node = entities_collector_->add_node(node_ptr); - if (is_new_node && notify) { - // Interrupt waiting to handle new node - interrupt_guard_condition_->trigger(); + auto wait_result = wait_set_.wait(std::chrono::nanoseconds(timeout)); + if (wait_result.kind() == WaitResultKind::Empty) { + RCUTILS_LOG_WARN_NAMED( + "rclcpp", + "empty wait set received in wait(). This should never happen."); + return {}; } + return wait_result; } -void -StaticSingleThreadedExecutor::add_node(std::shared_ptr node_ptr, bool notify) +// This preserves the "scheduling semantics" of the StaticSingleThreadedExecutor +// from the original implementation. +bool StaticSingleThreadedExecutor::execute_ready_executables( + const rclcpp::executors::ExecutorEntitiesCollection & collection, + rclcpp::WaitResult & wait_result, + bool spin_once) { - this->add_node(node_ptr->get_node_base_interface(), notify); -} - -void -StaticSingleThreadedExecutor::remove_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify) -{ - bool node_removed = entities_collector_->remove_callback_group(group_ptr); - // If the node was matched and removed, interrupt waiting - if (node_removed && notify) { - interrupt_guard_condition_->trigger(); + bool any_ready_executable = false; + if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { + return any_ready_executable; } -} -void -StaticSingleThreadedExecutor::remove_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) -{ - bool node_removed = entities_collector_->remove_node(node_ptr); - if (!node_removed) { - throw std::runtime_error("Node needs to be associated with this executor."); - } - // If the node was matched and removed, interrupt waiting - if (notify) { - interrupt_guard_condition_->trigger(); + while (auto subscription = wait_result.next_ready_subscription()) { + auto entity_iter = collection.subscriptions.find(subscription->get_subscription_handle().get()); + if (entity_iter != collection.subscriptions.end()) { + execute_subscription(subscription); + any_ready_executable = true; + if (spin_once) {return any_ready_executable;} + } } -} - -std::vector -StaticSingleThreadedExecutor::get_all_callback_groups() -{ - return entities_collector_->get_all_callback_groups(); -} - -std::vector -StaticSingleThreadedExecutor::get_manually_added_callback_groups() -{ - return entities_collector_->get_manually_added_callback_groups(); -} -std::vector -StaticSingleThreadedExecutor::get_automatically_added_callback_groups_from_nodes() -{ - return entities_collector_->get_automatically_added_callback_groups_from_nodes(); -} - -void -StaticSingleThreadedExecutor::remove_node(std::shared_ptr node_ptr, bool notify) -{ - this->remove_node(node_ptr->get_node_base_interface(), notify); -} - -bool -StaticSingleThreadedExecutor::execute_ready_executables(bool spin_once) -{ - bool any_ready_executable = false; - - // Execute all the ready subscriptions - for (size_t i = 0; i < wait_set_.size_of_subscriptions; ++i) { - if (i < entities_collector_->get_number_of_subscriptions()) { - if (wait_set_.subscriptions[i]) { - execute_subscription(entities_collector_->get_subscription(i)); - if (spin_once) { - return true; - } - any_ready_executable = true; - } + size_t current_timer_index = 0; + while (true) { + auto [timer, timer_index] = wait_result.peek_next_ready_timer(current_timer_index); + if (nullptr == timer) { + break; } - } - // Execute all the ready timers - for (size_t i = 0; i < wait_set_.size_of_timers; ++i) { - if (i < entities_collector_->get_number_of_timers()) { - if (wait_set_.timers[i] && entities_collector_->get_timer(i)->is_ready()) { - auto timer = entities_collector_->get_timer(i); - timer->call(); - execute_timer(std::move(timer)); - if (spin_once) { - return true; - } + current_timer_index = timer_index; + auto entity_iter = collection.timers.find(timer->get_timer_handle().get()); + if (entity_iter != collection.timers.end()) { + wait_result.clear_timer_with_index(current_timer_index); + if (timer->call()) { + execute_timer(timer); any_ready_executable = true; + if (spin_once) {return any_ready_executable;} } } } - // Execute all the ready services - for (size_t i = 0; i < wait_set_.size_of_services; ++i) { - if (i < entities_collector_->get_number_of_services()) { - if (wait_set_.services[i]) { - execute_service(entities_collector_->get_service(i)); - if (spin_once) { - return true; - } - any_ready_executable = true; - } + + while (auto client = wait_result.next_ready_client()) { + auto entity_iter = collection.clients.find(client->get_client_handle().get()); + if (entity_iter != collection.clients.end()) { + execute_client(client); + any_ready_executable = true; + if (spin_once) {return any_ready_executable;} } } - // Execute all the ready clients - for (size_t i = 0; i < wait_set_.size_of_clients; ++i) { - if (i < entities_collector_->get_number_of_clients()) { - if (wait_set_.clients[i]) { - execute_client(entities_collector_->get_client(i)); - if (spin_once) { - return true; - } - any_ready_executable = true; - } + + while (auto service = wait_result.next_ready_service()) { + auto entity_iter = collection.services.find(service->get_service_handle().get()); + if (entity_iter != collection.services.end()) { + execute_service(service); + any_ready_executable = true; + if (spin_once) {return any_ready_executable;} } } - // Execute all the ready waitables - for (size_t i = 0; i < entities_collector_->get_number_of_waitables(); ++i) { - auto waitable = entities_collector_->get_waitable(i); - if (waitable->is_ready(&wait_set_)) { + + 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(); waitable->execute(data); - if (spin_once) { - return true; - } any_ready_executable = true; + if (spin_once) {return any_ready_executable;} } } return any_ready_executable; diff --git a/rclcpp/test/benchmark/benchmark_executor.cpp b/rclcpp/test/benchmark/benchmark_executor.cpp index 652007b589..65bb3a1007 100644 --- a/rclcpp/test/benchmark/benchmark_executor.cpp +++ b/rclcpp/test/benchmark/benchmark_executor.cpp @@ -362,42 +362,3 @@ BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark } } } - -BENCHMARK_F( - PerformanceTestExecutorSimple, - static_executor_entities_collector_execute)(benchmark::State & st) -{ - rclcpp::executors::StaticExecutorEntitiesCollector::SharedPtr entities_collector_ = - std::make_shared(); - entities_collector_->add_node(node->get_node_base_interface()); - - rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); - rcl_allocator_t allocator = rcl_get_default_allocator(); - auto shared_context = node->get_node_base_interface()->get_context(); - rcl_context_t * context = shared_context->get_rcl_context().get(); - rcl_ret_t ret = rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator); - if (ret != RCL_RET_OK) { - st.SkipWithError(rcutils_get_error_string().str); - } - RCPPUTILS_SCOPE_EXIT( - { - rcl_ret_t ret = rcl_wait_set_fini(&wait_set); - if (ret != RCL_RET_OK) { - st.SkipWithError(rcutils_get_error_string().str); - } - }); - - auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); - rclcpp::GuardCondition guard_condition(shared_context); - - entities_collector_->init(&wait_set, memory_strategy); - RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); - - reset_heap_counters(); - - for (auto _ : st) { - (void)_; - std::shared_ptr data = entities_collector_->take_data(); - entities_collector_->execute(data); - } -} diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 25ca4082e1..c2e6b2bfe4 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -494,12 +494,6 @@ if(TARGET test_multi_threaded_executor) target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME}) endif() -ament_add_gtest(test_static_executor_entities_collector executors/test_static_executor_entities_collector.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) -if(TARGET test_static_executor_entities_collector) - target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME} mimick rcpputils::rcpputils ${test_msgs_TARGETS}) -endif() - ament_add_gtest(test_entities_collector executors/test_entities_collector.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) if(TARGET test_entities_collector) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index da4b087607..1f4409c7fd 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -82,8 +82,6 @@ class TestExecutors : public ::testing::Test int callback_count; }; -// spin_all and spin_some are not implemented correctly in StaticSingleThreadedExecutor, see: -// https://github.com/ros2/rclcpp/issues/1219 for tracking template class TestExecutorsStable : public TestExecutors {}; @@ -106,10 +104,7 @@ TYPED_TEST(TestExecutors, detachOnDestruction) } // Make sure that the executor can automatically remove expired nodes correctly -// Currently fails for StaticSingleThreadedExecutor so it is being skipped, see: -// https://github.com/ros2/rclcpp/issues/1231 -TYPED_TEST(TestExecutorsStable, addTemporaryNode) -{ +TYPED_TEST(TestExecutors, addTemporaryNode) { using ExecutorType = TypeParam; ExecutorType executor; @@ -177,16 +172,19 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { using ExecutorType = TypeParam; ExecutorType executor; - executor.add_node(this->node); - bool timer_completed = false; - auto timer = this->node->create_wall_timer(1ms, [&]() {timer_completed = true;}); + std::atomic_bool timer_completed = false; + auto timer = this->node->create_wall_timer( + 1ms, [&]() { + timer_completed.store(true); + }); + executor.add_node(this->node); std::thread spinner([&]() {executor.spin();}); - // Sleep for a short time to verify executor.spin() is going, and didn't throw. + // Sleep for a short time to verify executor.spin() is going, and didn't throw. auto start = std::chrono::steady_clock::now(); - while (!timer_completed && (std::chrono::steady_clock::now() - start) < 10s) { + while (!timer_completed.load() && (std::chrono::steady_clock::now() - start) < 10s) { std::this_thread::sleep_for(1ms); } @@ -341,11 +339,18 @@ class TestWaitable : public rclcpp::Waitable void 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. + // This is necessary if trigger() results in the wait set waking, but not + // 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_); } void trigger() { + trigger_count_++; gc_.trigger(); } @@ -378,6 +383,7 @@ class TestWaitable : public rclcpp::Waitable execute(std::shared_ptr & data) override { (void) data; + trigger_count_--; count_++; if (nullptr != on_execute_callback_) { on_execute_callback_(); @@ -414,13 +420,14 @@ class TestWaitable : public rclcpp::Waitable get_number_of_ready_guard_conditions() override {return 1;} size_t - get_count() + get_count() const { return count_; } private: - size_t count_ = 0; + std::atomic trigger_count_ = 0; + std::atomic count_ = 0; rclcpp::GuardCondition gc_; std::function on_execute_callback_ = nullptr; }; diff --git a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp index 5ca6c1c25a..1a0f3f88c4 100644 --- a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp @@ -56,7 +56,7 @@ TEST_F(TestStaticSingleThreadedExecutor, add_callback_group_trigger_guard_failed "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( executor.add_callback_group(cb_group, node->get_node_base_interface(), true), - std::runtime_error("error not set")); + std::runtime_error("Failed to trigger guard condition on callback group add: error not set")); } } @@ -69,7 +69,7 @@ TEST_F(TestStaticSingleThreadedExecutor, add_node_trigger_guard_failed) { "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( executor.add_node(node), - std::runtime_error("error not set")); + std::runtime_error("Failed to trigger guard condition on node add: error not set")); } } @@ -86,7 +86,8 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_callback_group_trigger_guard_fai "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( executor.remove_callback_group(cb_group, true), - std::runtime_error("error not set")); + std::runtime_error( + "Failed to trigger guard condition on callback group remove: error not set")); } } @@ -99,7 +100,7 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_node_failed) { "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( executor.remove_node(node, true), - std::runtime_error("Node needs to be associated with this executor.")); + std::runtime_error("Node '/ns/node' needs to be associated with an executor.")); } } @@ -114,7 +115,7 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_node_trigger_guard_failed) { "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( executor.remove_node(node, true), - std::runtime_error("error not set")); + std::runtime_error("Failed to trigger guard condition on node remove: error not set")); } } diff --git a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp index 9d52d14035..fefec6c8fa 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -200,22 +200,31 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t { using ExecutorType = TypeParam; + auto count_callback_groups_in_node = [](auto node) { + size_t num = 0; + node->get_node_base_interface()->for_each_callback_group( + [&num](auto) { + num++; + }); + return num; + }; + ExecutorType executor; auto node = std::make_shared("my_node", "/ns"); executor.add_node(node->get_node_base_interface()); - ASSERT_EQ(executor.get_all_callback_groups().size(), 1u); - std::atomic_int timer_count {0}; + ASSERT_EQ(executor.get_all_callback_groups().size(), count_callback_groups_in_node(node)); + std::atomic_size_t timer_count {0}; auto timer_callback = [&executor, &timer_count]() { - if (timer_count > 0) { - ASSERT_EQ(executor.get_all_callback_groups().size(), 3u); + auto cur_timer_count = timer_count++; + printf("in timer_callback(%zu)\n", cur_timer_count); + if (cur_timer_count > 0) { executor.cancel(); } - timer_count++; }; rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer( - 2s, timer_callback, cb_grp); + 1s, timer_callback, cb_grp); rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); auto timer2_callback = []() {}; @@ -227,6 +236,7 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t rclcpp::TimerBase::SharedPtr timer3_ = node->create_wall_timer( 2s, timer3_callback, cb_grp3); executor.spin(); + ASSERT_GT(timer_count.load(), 0u); } /* diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index 706b80aef1..668ab96797 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -46,23 +46,6 @@ class DummyExecutor : public rclcpp::Executor { spin_node_once_nanoseconds(node, std::chrono::milliseconds(100)); } - - rclcpp::memory_strategy::MemoryStrategy * memory_strategy_ptr() - { - return memory_strategy_.get(); - } - - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr local_get_node_by_group( - rclcpp::CallbackGroup::SharedPtr group) - { - std::lock_guard guard_{mutex_}; // only to make the TSA happy - return get_node_by_group(weak_groups_to_nodes_, group); - } - - rclcpp::CallbackGroup::SharedPtr local_get_group_by_timer(rclcpp::TimerBase::SharedPtr timer) - { - return get_group_by_timer(timer); - } }; class TestExecutor : public ::testing::Test @@ -130,7 +113,7 @@ TEST_F(TestExecutor, constructor_bad_wait_set_init) { auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_init, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( static_cast(std::make_unique()), - std::runtime_error("Failed to create wait set in Executor constructor: error not set")); + std::runtime_error("Failed to create wait set: error not set")); } TEST_F(TestExecutor, add_callback_group_twice) { @@ -142,7 +125,7 @@ TEST_F(TestExecutor, add_callback_group_twice) { cb_group->get_associated_with_executor_atomic().exchange(false); RCLCPP_EXPECT_THROW_EQ( dummy.add_callback_group(cb_group, node->get_node_base_interface(), false), - std::runtime_error("Callback group was already added to executor.")); + std::runtime_error("Callback group has already been added to this executor.")); } TEST_F(TestExecutor, add_callback_group_failed_trigger_guard_condition) { @@ -168,9 +151,15 @@ TEST_F(TestExecutor, remove_callback_group_null_node) { node.reset(); + + /** + * TODO(mjcarroll): Assert this when we are enforcing that nodes must be destroyed + * after their created callback groups. RCLCPP_EXPECT_THROW_EQ( dummy.remove_callback_group(cb_group, false), std::runtime_error("Node must not be deleted before its callback group(s).")); + */ + EXPECT_NO_THROW(dummy.remove_callback_group(cb_group, false)); } TEST_F(TestExecutor, remove_callback_group_failed_trigger_guard_condition) { @@ -197,7 +186,7 @@ TEST_F(TestExecutor, remove_node_not_associated) { RCLCPP_EXPECT_THROW_EQ( dummy.remove_node(node->get_node_base_interface(), false), - std::runtime_error("Node needs to be associated with an executor.")); + std::runtime_error("Node '/ns/node' needs to be associated with an executor.")); } TEST_F(TestExecutor, remove_node_associated_with_different_executor) { @@ -211,7 +200,7 @@ TEST_F(TestExecutor, remove_node_associated_with_different_executor) { RCLCPP_EXPECT_THROW_EQ( dummy2.remove_node(node1->get_node_base_interface(), false), - std::runtime_error("Node needs to be associated with this executor.")); + std::runtime_error("Node '/ns/node1' needs to be associated with this executor.")); } TEST_F(TestExecutor, spin_node_once_nanoseconds) { @@ -328,42 +317,14 @@ TEST_F(TestExecutor, cancel_failed_trigger_guard_condition) { std::runtime_error("Failed to trigger guard condition in cancel: error not set")); } -TEST_F(TestExecutor, set_memory_strategy_nullptr) { - DummyExecutor dummy; - - RCLCPP_EXPECT_THROW_EQ( - dummy.set_memory_strategy(nullptr), - std::runtime_error("Received NULL memory strategy in executor.")); -} - -TEST_F(TestExecutor, set_memory_strategy) { - DummyExecutor dummy; - rclcpp::memory_strategy::MemoryStrategy::SharedPtr strategy = - std::make_shared< - rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>(); - - dummy.set_memory_strategy(strategy); - EXPECT_EQ(dummy.memory_strategy_ptr(), strategy.get()); -} - -TEST_F(TestExecutor, spin_once_failed_trigger_guard_condition) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - auto timer = - node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}); - - dummy.add_node(node); - // Wait for the wall timer to have expired. - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); +TEST_F(TestExecutor, create_executor_fail_wait_set_clear) { + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( - dummy.spin_once(std::chrono::milliseconds(1)), - std::runtime_error( - "Failed to trigger guard condition from execute_any_executable: error not set")); + DummyExecutor dummy, + std::runtime_error("Couldn't clear the wait set: error not set")); } -TEST_F(TestExecutor, spin_some_fail_wait_set_clear) { +TEST_F(TestExecutor, spin_all_fail_wait_set_clear) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); auto timer = @@ -371,9 +332,10 @@ TEST_F(TestExecutor, spin_some_fail_wait_set_clear) { dummy.add_node(node); auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( - dummy.spin_some(std::chrono::milliseconds(1)), - std::runtime_error("Couldn't clear wait set: error not set")); + dummy.spin_all(std::chrono::milliseconds(1)), + std::runtime_error("Couldn't clear the wait set: error not set")); } TEST_F(TestExecutor, spin_some_fail_wait_set_resize) { @@ -401,7 +363,7 @@ TEST_F(TestExecutor, spin_some_fail_add_handles_to_wait_set) { RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( dummy.spin_some(std::chrono::milliseconds(1)), - std::runtime_error("Couldn't fill wait set")); + std::runtime_error("Couldn't fill wait set: error not set")); } TEST_F(TestExecutor, spin_some_fail_wait) { @@ -417,71 +379,6 @@ TEST_F(TestExecutor, spin_some_fail_wait) { std::runtime_error("rcl_wait() failed: error not set")); } -TEST_F(TestExecutor, get_node_by_group_null_group) { - DummyExecutor dummy; - ASSERT_EQ(nullptr, dummy.local_get_node_by_group(nullptr)); -} - -TEST_F(TestExecutor, get_node_by_group) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - dummy.add_callback_group(cb_group, node->get_node_base_interface(), false); - ASSERT_EQ(node->get_node_base_interface().get(), dummy.local_get_node_by_group(cb_group).get()); -} - -TEST_F(TestExecutor, get_node_by_group_not_found) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - ASSERT_EQ(nullptr, dummy.local_get_node_by_group(cb_group).get()); -} - -TEST_F(TestExecutor, get_group_by_timer_nullptr) { - DummyExecutor dummy; - ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(nullptr)); -} - -TEST_F(TestExecutor, get_group_by_timer) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - auto timer = - node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group); - dummy.add_node(node); - - ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get()); -} - -TEST_F(TestExecutor, get_group_by_timer_with_deleted_group) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - auto timer = - node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group); - dummy.add_node(node); - - cb_group.reset(); - - ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(timer).get()); -} - -TEST_F(TestExecutor, get_group_by_timer_add_callback_group) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - auto timer = - node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group); - dummy.add_callback_group(cb_group, node->get_node_base_interface(), false); - - ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get()); -} - TEST_F(TestExecutor, spin_until_future_complete_in_spin_until_future_complete) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); From c5bc4b65284c71a86fc1758cc7eeb22204aa2bdd Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 29 Mar 2024 08:47:15 -0700 Subject: [PATCH 355/455] update rclcpp::Waitable API to use references and const (#2467) Signed-off-by: William Woodall --- 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) From f510db152957fa284685b6a468fa38f7575a61a1 Mon Sep 17 00:00:00 2001 From: h-suzuki-isp <146712054+h-suzuki-isp@users.noreply.github.com> Date: Tue, 2 Apr 2024 02:07:45 +0900 Subject: [PATCH 356/455] Add tracepoint for generic publisher/subscriber (#2448) Signed-off-by: h-suzuki --- .../include/rclcpp/generic_subscription.hpp | 26 ++++++++++++------- rclcpp/src/rclcpp/generic_publisher.cpp | 5 ++++ rclcpp/src/rclcpp/generic_subscription.cpp | 2 +- rclcpp/src/rclcpp/subscription_base.cpp | 3 +++ 4 files changed, 25 insertions(+), 11 deletions(-) diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp index e9bf79deea..dd0e8be94d 100644 --- a/rclcpp/include/rclcpp/generic_subscription.hpp +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -84,13 +84,22 @@ class GenericSubscription : public rclcpp::SubscriptionBase options.event_callbacks, options.use_default_callbacks, DeliveredMessageKind::SERIALIZED_MESSAGE), - callback_([callback]( - std::shared_ptr serialized_message, - const rclcpp::MessageInfo & message_info) mutable { - callback.dispatch(serialized_message, message_info); - }), + any_callback_(callback), ts_lib_(ts_lib) - {} + { + TRACETOOLS_TRACEPOINT( + rclcpp_subscription_init, + static_cast(get_subscription_handle().get()), + static_cast(this)); + TRACETOOLS_TRACEPOINT( + rclcpp_subscription_callback_added, + static_cast(this), + static_cast(&any_callback_)); + +#ifndef TRACETOOLS_DISABLED + any_callback_.register_callback_for_tracing(); +#endif + } RCLCPP_PUBLIC virtual ~GenericSubscription() = default; @@ -153,10 +162,7 @@ class GenericSubscription : public rclcpp::SubscriptionBase private: RCLCPP_DISABLE_COPY(GenericSubscription) - - std::function, - const rclcpp::MessageInfo)> callback_; + AnySubscriptionCallback> any_callback_; // The type support library should stay loaded, so it is stored in the GenericSubscription std::shared_ptr ts_lib_; }; diff --git a/rclcpp/src/rclcpp/generic_publisher.cpp b/rclcpp/src/rclcpp/generic_publisher.cpp index 87ace1eb19..d526a6fac1 100644 --- a/rclcpp/src/rclcpp/generic_publisher.cpp +++ b/rclcpp/src/rclcpp/generic_publisher.cpp @@ -23,6 +23,10 @@ namespace rclcpp void GenericPublisher::publish(const rclcpp::SerializedMessage & message) { + TRACETOOLS_TRACEPOINT( + rclcpp_publish, + nullptr, + static_cast(&message.get_rcl_serialized_message())); auto return_code = rcl_publish_serialized_message( get_publisher_handle().get(), &message.get_rcl_serialized_message(), NULL); @@ -68,6 +72,7 @@ void GenericPublisher::deserialize_message( void GenericPublisher::publish_loaned_message(void * loaned_message) { + TRACETOOLS_TRACEPOINT(rclcpp_publish, nullptr, static_cast(loaned_message)); auto return_code = rcl_publish_loaned_message( get_publisher_handle().get(), loaned_message, NULL); diff --git a/rclcpp/src/rclcpp/generic_subscription.cpp b/rclcpp/src/rclcpp/generic_subscription.cpp index 2d2be61277..ae28354b98 100644 --- a/rclcpp/src/rclcpp/generic_subscription.cpp +++ b/rclcpp/src/rclcpp/generic_subscription.cpp @@ -51,7 +51,7 @@ GenericSubscription::handle_serialized_message( const std::shared_ptr & message, const rclcpp::MessageInfo & message_info) { - callback_(message, message_info); + any_callback_.dispatch(message, message_info); } void diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index e8510b6444..f7c238a910 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -244,6 +244,9 @@ SubscriptionBase::take_serialized( &message_out.get_rcl_serialized_message(), &message_info_out.get_rmw_message_info(), nullptr); + TRACETOOLS_TRACEPOINT( + rclcpp_take, + static_cast(&message_out.get_rcl_serialized_message())); if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { return false; } else if (RCL_RET_OK != ret) { From f9c4894f96ea083fc5acfcea4f1ea83850759e63 Mon Sep 17 00:00:00 2001 From: jmachowinski Date: Tue, 2 Apr 2024 18:11:30 +0200 Subject: [PATCH 357/455] Flaky timer test fix (#2469) * fix(time_source): Fixed possible race condition Signed-off-by: Janosch Machowinski * fix(test_executors_time_cancel_behaviour): Fixed multiple race conditions Signed-off-by: Janosch Machowinski --------- Signed-off-by: Janosch Machowinski Co-authored-by: Janosch Machowinski --- rclcpp/src/rclcpp/time_source.cpp | 13 +++-- .../test_executors_timer_cancel_behavior.cpp | 50 ++++++++++++++++--- 2 files changed, 52 insertions(+), 11 deletions(-) diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 465ceaf5a7..c6e39ace70 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -288,10 +288,10 @@ class TimeSource::NodeState final // Detach the attached node void detachNode() { - std::lock_guard guard(node_base_lock_); // destroy_clock_sub() *must* be first here, to ensure that the executor // can't possibly call any of the callbacks as we are cleaning up. destroy_clock_sub(); + std::lock_guard guard(node_base_lock_); clocks_state_.disable_ros_time(); if (on_set_parameters_callback_) { node_parameters_->remove_on_set_parameters_callback(on_set_parameters_callback_.get()); @@ -409,9 +409,14 @@ class TimeSource::NodeState final "/clock", qos_, [this](std::shared_ptr msg) { - // We are using node_base_ as an indication if there is a node attached. - // Only call the clock_cb if that is the case. - if (node_base_ != nullptr) { + bool execute_cb = false; + { + std::lock_guard guard(node_base_lock_); + // We are using node_base_ as an indication if there is a node attached. + // Only call the clock_cb if that is the case. + execute_cb = node_base_ != nullptr; + } + if (execute_cb) { clock_cb(msg); } }, diff --git a/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp index 7fd1676c5d..f6bbd2ccc8 100644 --- a/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp @@ -54,8 +54,16 @@ class TimerNode : public rclcpp::Node std::bind(&TimerNode::Timer2Callback, this)); } - int GetTimer1Cnt() {return cnt1_;} - int GetTimer2Cnt() {return cnt2_;} + int GetTimer1Cnt() + { + const std::lock_guard lock(mutex_); + return cnt1_; + } + int GetTimer2Cnt() + { + const std::lock_guard lock(mutex_); + return cnt2_; + } void ResetTimer1() { @@ -82,16 +90,24 @@ class TimerNode : public rclcpp::Node private: void Timer1Callback() { - cnt1_++; + { + const std::lock_guard lock(mutex_); + cnt1_++; + } RCLCPP_DEBUG(this->get_logger(), "Timer 1! (%d)", cnt1_); } void Timer2Callback() { - cnt2_++; + { + const std::lock_guard lock(mutex_); + cnt2_++; + } RCLCPP_DEBUG(this->get_logger(), "Timer 2! (%d)", cnt2_); } + std::mutex mutex_; + rclcpp::TimerBase::SharedPtr timer1_; rclcpp::TimerBase::SharedPtr timer2_; int cnt1_ = 0; @@ -130,6 +146,18 @@ class ClockPublisher : public rclcpp::Node } } + bool wait_for_connection(std::chrono::nanoseconds timeout) + { + auto end_time = std::chrono::steady_clock::now() + timeout; + while (clock_publisher_->get_subscription_count() == 0 && + (std::chrono::steady_clock::now() < end_time)) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + + return clock_publisher_->get_subscription_count() != 0; + } + void sleep_for(rclcpp::Duration duration) { rclcpp::Time start_time(0, 0, RCL_ROS_TIME); @@ -148,7 +176,10 @@ class ClockPublisher : public rclcpp::Node return; } std::this_thread::sleep_for(realtime_clock_step_.to_chrono()); - rostime_ += ros_update_duration_; + { + const std::lock_guard lock(mutex_); + rostime_ += ros_update_duration_; + } } } @@ -163,9 +194,11 @@ class ClockPublisher : public rclcpp::Node void PublishClock() { - const std::lock_guard lock(mutex_); auto message = rosgraph_msgs::msg::Clock(); - message.clock = rostime_; + { + const std::lock_guard lock(mutex_); + message.clock = rostime_; + } clock_publisher_->publish(message); } @@ -227,6 +260,9 @@ class TestTimerCancelBehavior : public ::testing::Test [this]() { executor.spin(); }); + + EXPECT_TRUE(this->sim_clock_node->wait_for_connection(50ms)); + EXPECT_EQ(RCL_ROS_TIME, node->get_clock()->ros_time_is_active()); } void TearDown() From f7a7954ae74f1b92164cbc21900b6cf89c5b8e16 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 4 Apr 2024 16:04:54 +0200 Subject: [PATCH 358/455] Removed test_timers_manager clang warning (#2479) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- rclcpp/test/rclcpp/test_timers_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/test/rclcpp/test_timers_manager.cpp b/rclcpp/test/rclcpp/test_timers_manager.cpp index 90d321c188..ef7605aadc 100644 --- a/rclcpp/test/rclcpp/test_timers_manager.cpp +++ b/rclcpp/test/rclcpp/test_timers_manager.cpp @@ -375,7 +375,7 @@ TEST_F(TestTimersManager, check_one_timer_cancel_doesnt_affect_other_timers) // since typical users aren't going to mess around with the timer manager. t1 = TimerT::make_shared( 1ms, - [&t1_runs, &t1, cancel_iter]() { + [ =, &t1_runs, &t1]() { t1_runs++; if (t1_runs == cancel_iter) { t1->cancel(); From 9171835c35cd6b8ba370c78cb49ba5fc29bc66ed Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 4 Apr 2024 19:50:12 +0200 Subject: [PATCH 359/455] Increase the cppcheck timeout to 1200 seconds (#2484) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- rclcpp/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index c95df3e768..33d21e1b05 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -298,7 +298,7 @@ install( if(TEST cppcheck) # must set the property after ament_package() - set_tests_properties(cppcheck PROPERTIES TIMEOUT 600) + set_tests_properties(cppcheck PROPERTIES TIMEOUT 1200) endif() if(TEST cpplint) From 3cdb25934ed261c78bdfbcf5ec9f06e0573be81e Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Thu, 4 Apr 2024 14:56:23 -0700 Subject: [PATCH 360/455] address ambiguous auto variable. (#2481) Signed-off-by: Tomoya Fujita Signed-off-by: Steve Nogar --- rclcpp/include/rclcpp/client.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index f69ab0b28f..fdba5160a8 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -864,7 +864,7 @@ class Client : public ClientBase "Received invalid sequence number. Ignoring..."); return std::nullopt; } - auto value = std::move(it->second.second); + std::optional value = std::move(it->second.second); this->pending_requests_.erase(request_number); return value; } From fa596801c46902078d7c23866788783a4408dde4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 5 Apr 2024 19:29:20 +0200 Subject: [PATCH 361/455] Disable compare-function-pointers in test_utilities (#2489) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- rclcpp/test/rclcpp/test_utilities.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/rclcpp/test/rclcpp/test_utilities.cpp b/rclcpp/test/rclcpp/test_utilities.cpp index c177efb7fc..98874845e3 100644 --- a/rclcpp/test/rclcpp/test_utilities.cpp +++ b/rclcpp/test/rclcpp/test_utilities.cpp @@ -25,7 +25,20 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/utilities.hpp" +#ifdef __clang__ +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wordered-compare-function-pointers" +#endif +// TODO(ahcorde): the function mocking_utils::patch_and_return called with +// rcl_logging_configure_with_output_handler is returning: "Comparison between pointer and integer" +// Disabling this warning is fine for now. +// Related issue https://github.com/ros2/rclcpp/issues/2488 #include "../mocking_utils/patch.hpp" + +#ifdef __clang__ +#pragma clang diagnostic pop +#endif + #include "../utils/rclcpp_gtest_macros.hpp" TEST(TestUtilities, remove_ros_arguments) { From ea4c98c43fa9c3a1274d37afb8326aaadc4ca196 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 7 Apr 2024 00:23:50 +0900 Subject: [PATCH 362/455] Use the same context for the specified node in rclcpp::spin functions (#2433) * Use the same conext for the specified node in rclcpp::spin_xx functions Signed-off-by: GitHub * Add test for spinning with non-default-context Signed-off-by: Kotaro Yoshimoto * Format code Signed-off-by: Kotaro Yoshimoto --------- Signed-off-by: GitHub Signed-off-by: Kotaro Yoshimoto --- rclcpp/include/rclcpp/executors.hpp | 4 ++- rclcpp/src/rclcpp/executors.cpp | 12 ++++++-- .../test/rclcpp/executors/test_executors.cpp | 30 +++++++++++++++++++ 3 files changed, 42 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 55a38709a7..adb9c6d6c6 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -120,7 +120,9 @@ spin_until_future_complete( const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { - rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::ExecutorOptions options; + options.context = node_ptr->get_context(); + rclcpp::executors::SingleThreadedExecutor executor(options); return executors::spin_node_until_future_complete(executor, node_ptr, future, timeout); } diff --git a/rclcpp/src/rclcpp/executors.cpp b/rclcpp/src/rclcpp/executors.cpp index 3ea8d658ad..94137d50bc 100644 --- a/rclcpp/src/rclcpp/executors.cpp +++ b/rclcpp/src/rclcpp/executors.cpp @@ -19,7 +19,9 @@ rclcpp::spin_all( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, std::chrono::nanoseconds max_duration) { - rclcpp::executors::SingleThreadedExecutor exec; + rclcpp::ExecutorOptions options; + options.context = node_ptr->get_context(); + rclcpp::executors::SingleThreadedExecutor exec(options); exec.spin_node_all(node_ptr, max_duration); } @@ -32,7 +34,9 @@ rclcpp::spin_all(rclcpp::Node::SharedPtr node_ptr, std::chrono::nanoseconds max_ void rclcpp::spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) { - rclcpp::executors::SingleThreadedExecutor exec; + rclcpp::ExecutorOptions options; + options.context = node_ptr->get_context(); + rclcpp::executors::SingleThreadedExecutor exec(options); exec.spin_node_some(node_ptr); } @@ -45,7 +49,9 @@ rclcpp::spin_some(rclcpp::Node::SharedPtr node_ptr) void rclcpp::spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) { - rclcpp::executors::SingleThreadedExecutor exec; + rclcpp::ExecutorOptions options; + options.context = node_ptr->get_context(); + rclcpp::executors::SingleThreadedExecutor exec(options); exec.add_node(node_ptr); exec.spin(); exec.remove_node(node_ptr); diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 3434f473c6..1a538eaa30 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -839,3 +839,33 @@ TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) rclcpp::shutdown(); } + +// Check spin functions with non default context +TEST(TestExecutors, testSpinWithNonDefaultContext) +{ + auto non_default_context = std::make_shared(); + non_default_context->init(0, nullptr); + + { + auto node = + std::make_unique("node", rclcpp::NodeOptions().context(non_default_context)); + + EXPECT_NO_THROW(rclcpp::spin_some(node->get_node_base_interface())); + + EXPECT_NO_THROW(rclcpp::spin_all(node->get_node_base_interface(), 1s)); + + auto check_spin_until_future_complete = [&]() { + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(true); + + auto shared_future = future.share(); + auto ret = rclcpp::spin_until_future_complete( + node->get_node_base_interface(), shared_future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); + }; + EXPECT_NO_THROW(check_spin_until_future_complete()); + } + + rclcpp::shutdown(non_default_context); +} From 04ea0bb00293387791522590b7347a2282cda290 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Sat, 6 Apr 2024 16:42:04 -0700 Subject: [PATCH 363/455] =?UTF-8?q?call=20shutdown=20in=20LifecycleNode=20?= =?UTF-8?q?dtor=20to=20avoid=20leaving=20the=20device=20in=20un=E2=80=A6?= =?UTF-8?q?=20(#2450)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * call shutdown in LifecycleNode dtor to avoid leaving the device in unknown state. Signed-off-by: Tomoya Fujita * add test to verify LifecycleNode::shutdown is called on destructor. Signed-off-by: Tomoya Fujita --------- Signed-off-by: Tomoya Fujita --- rclcpp_lifecycle/src/lifecycle_node.cpp | 16 ++ rclcpp_lifecycle/test/test_lifecycle_node.cpp | 140 ++++++++++++++++++ 2 files changed, 156 insertions(+) diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 0c448cf5e6..1ec9498b4a 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -152,6 +152,22 @@ LifecycleNode::LifecycleNode( LifecycleNode::~LifecycleNode() { + // shutdown if necessary to avoid leaving the device in unknown state + if (LifecycleNode::get_current_state().id() != + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + auto ret = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + auto finalized = LifecycleNode::shutdown(ret); + if (finalized.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED || + ret != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) + { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp_lifecycle"), + "Shutdown error in destruction of LifecycleNode: final state(%s)", + finalized.label().c_str()); + } + } + // release sub-interfaces in an order that allows them to consult with node_base during tear-down node_waitables_.reset(); node_time_source_.reset(); diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index fdb9be6153..58ce89897c 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -447,6 +447,146 @@ TEST_F(TestDefaultStateMachine, bad_mood) { EXPECT_EQ(1u, test_node->number_of_callbacks); } + +TEST_F(TestDefaultStateMachine, shutdown_from_each_primary_state) { + auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + + // PRIMARY_STATE_UNCONFIGURED to shutdown + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + } + + // PRIMARY_STATE_INACTIVE to shutdown + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + ret = reset_key; + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + } + + // PRIMARY_STATE_ACTIVE to shutdown + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + ret = reset_key; + auto activated = test_node->activate(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); + ret = reset_key; + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + } + + // PRIMARY_STATE_FINALIZED to shutdown + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + ret = reset_key; + auto activated = test_node->activate(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); + ret = reset_key; + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + ret = reset_key; + auto finalized_again = test_node->shutdown(ret); + EXPECT_EQ(reset_key, ret); + EXPECT_EQ(finalized_again.id(), State::PRIMARY_STATE_FINALIZED); + } +} + +TEST_F(TestDefaultStateMachine, test_shutdown_on_dtor) { + auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + + bool shutdown_cb_called = false; + auto on_shutdown_callback = + [&shutdown_cb_called](const rclcpp_lifecycle::State &) -> + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn { + shutdown_cb_called = true; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + }; + + // PRIMARY_STATE_UNCONFIGURED to shutdown via dtor + shutdown_cb_called = false; + { + auto test_node = std::make_shared("testnode"); + test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1)); + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + EXPECT_FALSE(shutdown_cb_called); + } + EXPECT_TRUE(shutdown_cb_called); + + // PRIMARY_STATE_INACTIVE to shutdown via dtor + shutdown_cb_called = false; + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1)); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + EXPECT_FALSE(shutdown_cb_called); + } + EXPECT_TRUE(shutdown_cb_called); + + // PRIMARY_STATE_ACTIVE to shutdown via dtor + shutdown_cb_called = false; + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1)); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + ret = reset_key; + auto activated = test_node->activate(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); + EXPECT_FALSE(shutdown_cb_called); + } + EXPECT_TRUE(shutdown_cb_called); + + // PRIMARY_STATE_FINALIZED to shutdown via dtor + shutdown_cb_called = false; + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1)); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + ret = reset_key; + auto activated = test_node->activate(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); + ret = reset_key; + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + EXPECT_TRUE(shutdown_cb_called); // should be called already + } + EXPECT_TRUE(shutdown_cb_called); +} + TEST_F(TestDefaultStateMachine, lifecycle_subscriber) { auto test_node = std::make_shared>("testnode"); From 63c2e2ef6f7367b4420e4fd0eab5d78b551cb794 Mon Sep 17 00:00:00 2001 From: Homalozoa X Date: Mon, 8 Apr 2024 21:17:46 +0800 Subject: [PATCH 364/455] fix: init concatenated_vector with begin() & end() (#2492) *. this commit will fix the warning [-Wstringop-overflow=] #2461 Signed-off-by: homalozoa --- rclcpp/include/rclcpp/experimental/intra_process_manager.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index ffd77cb99c..a403c20c8b 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -253,7 +253,8 @@ class IntraProcessManager // So this case is equivalent to all the buffers requiring ownership // Merge the two vector of ids into a unique one - std::vector concatenated_vector(sub_ids.take_shared_subscriptions); + std::vector concatenated_vector( + sub_ids.take_shared_subscriptions.begin(), sub_ids.take_shared_subscriptions.end()); concatenated_vector.insert( concatenated_vector.end(), sub_ids.take_ownership_subscriptions.begin(), From bfa7efac46c257e46592d93730f477b64a3c129a Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 8 Apr 2024 07:35:51 -0700 Subject: [PATCH 365/455] Ensure waitables handle guard condition retriggering (#2483) Signed-off-by: William Woodall Signed-off-by: Michael Carroll Co-authored-by: Michael Carroll --- .../executors/executor_notify_waitable.hpp | 1 - .../subscription_intra_process.hpp | 19 +- .../strategies/allocator_memory_strategy.hpp | 23 ++- rclcpp/src/rclcpp/event_handler.cpp | 1 + .../executors/executor_notify_waitable.cpp | 5 + rclcpp/test/rclcpp/CMakeLists.txt | 5 + .../test_allocator_memory_strategy.cpp | 176 +----------------- rclcpp/test/rclcpp/test_memory_strategy.cpp | 1 + .../wait_set_policies/test_static_storage.cpp | 1 + .../test_storage_policy_common.cpp | 1 + .../test_thread_safe_synchronization.cpp | 1 + .../waitables/test_intra_process_waitable.cpp | 46 +++++ .../waitables/waitable_test_helpers.hpp | 117 ++++++++++++ 13 files changed, 215 insertions(+), 182 deletions(-) create mode 100644 rclcpp/test/rclcpp/waitables/test_intra_process_waitable.cpp create mode 100644 rclcpp/test/rclcpp/waitables/waitable_test_helpers.hpp diff --git a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp index 0de01fd4b0..d389cbf1e0 100644 --- a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp @@ -50,7 +50,6 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable RCLCPP_PUBLIC ExecutorNotifyWaitable(ExecutorNotifyWaitable & other); - RCLCPP_PUBLIC ExecutorNotifyWaitable & operator=(ExecutorNotifyWaitable & other); diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 5c562a61ff..0624f92c62 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -101,6 +101,23 @@ class SubscriptionIntraProcess virtual ~SubscriptionIntraProcess() = default; + void + add_to_wait_set(rcl_wait_set_t & wait_set) override + { + // This block is necessary when the guard condition wakes the wait set, but + // the intra process waitable was not handled before the wait set is waited + // on again. + // Basically we're keeping the guard condition triggered so long as there is + // data in the buffer. + if (this->buffer_->has_data()) { + // If there is data still to be processed, indicate to the + // executor or waitset by triggering the guard condition. + this->trigger_guard_condition(); + } + // Let the parent classes handle the rest of the work: + return SubscriptionIntraProcessBufferT::add_to_wait_set(wait_set); + } + std::shared_ptr take_data() override { @@ -149,7 +166,7 @@ class SubscriptionIntraProcess typename std::enable_if::value, void>::type execute_impl(const std::shared_ptr & data) { - if (!data) { + if (nullptr == data) { return; } diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 28e929c495..1da372abfd 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -17,7 +17,6 @@ #include #include -#include #include "rcl/allocator.h" @@ -121,8 +120,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } for (size_t i = 0; i < waitable_handles_.size(); ++i) { - if (waitable_handles_[i]->is_ready(*wait_set)) { - waitable_triggered_handles_.emplace_back(std::move(waitable_handles_[i])); + if (!waitable_handles_[i]->is_ready(*wait_set)) { + waitable_handles_[i].reset(); } } @@ -146,7 +145,10 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy timer_handles_.end() ); - waitable_handles_.clear(); + waitable_handles_.erase( + std::remove(waitable_handles_.begin(), waitable_handles_.end(), nullptr), + waitable_handles_.end() + ); } bool collect_entities(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override @@ -390,9 +392,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy rclcpp::AnyExecutable & any_exec, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override { - auto & waitable_handles = waitable_triggered_handles_; - auto it = waitable_handles.begin(); - while (it != waitable_handles.end()) { + auto it = waitable_handles_.begin(); + while (it != waitable_handles_.end()) { std::shared_ptr & waitable = *it; if (waitable) { // Find the group for this handle and see if it can be serviced @@ -400,7 +401,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy if (!group) { // Group was not found, meaning the waitable is not valid... // Remove it from the ready list and continue looking - it = waitable_handles.erase(it); + it = waitable_handles_.erase(it); continue; } if (!group->can_be_taken_from().load()) { @@ -413,11 +414,11 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy any_exec.waitable = waitable; any_exec.callback_group = group; any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes); - waitable_handles.erase(it); + waitable_handles_.erase(it); return; } // Else, the waitable is no longer valid, remove it and continue - it = waitable_handles.erase(it); + it = waitable_handles_.erase(it); } } @@ -498,8 +499,6 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy VectorRebind> timer_handles_; VectorRebind> waitable_handles_; - VectorRebind> waitable_triggered_handles_; - std::shared_ptr allocator_; }; diff --git a/rclcpp/src/rclcpp/event_handler.cpp b/rclcpp/src/rclcpp/event_handler.cpp index 6232e5b0d9..630bc26d33 100644 --- a/rclcpp/src/rclcpp/event_handler.cpp +++ b/rclcpp/src/rclcpp/event_handler.cpp @@ -15,6 +15,7 @@ #include #include +#include "rcl/error_handling.h" #include "rcl/event.h" #include "rclcpp/event_handler.hpp" diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index a95bc3797c..09789a13ff 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -48,6 +48,11 @@ void ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t & wait_set) { std::lock_guard lock(guard_condition_mutex_); + + // Note: no guard conditions need to be re-triggered, since the guard + // conditions in this class are not tracking a stateful condition, but instead + // only serve to interrupt the wait set when new information is available to + // consider. for (auto weak_guard_condition : this->notify_guard_conditions_) { auto guard_condition = weak_guard_condition.lock(); if (!guard_condition) {continue;} diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index c2e6b2bfe4..d52c94b1d2 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -566,6 +566,11 @@ if(TARGET test_thread_safe_synchronization) target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() +ament_add_gtest(test_intra_process_waitable waitables/test_intra_process_waitable.cpp) +if(TARGET test_intra_process_waitable) + target_link_libraries(test_intra_process_waitable ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() + ament_add_gtest(test_rosout_qos test_rosout_qos.cpp) if(TARGET test_rosout_qos) target_link_libraries(test_rosout_qos ${PROJECT_NAME} rcl::rcl rmw::rmw) diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index d25b3490c2..452228645b 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -60,57 +60,6 @@ class TestWaitable : public rclcpp::Waitable void execute(const std::shared_ptr &) override {} }; -static bool test_waitable_result2 = false; - -class TestWaitable2 : public rclcpp::Waitable -{ -public: - explicit TestWaitable2(rcl_publisher_t * pub_ptr) - : pub_ptr_(pub_ptr), - pub_event_(rcl_get_zero_initialized_event()) - { - EXPECT_EQ( - rcl_publisher_event_init(&pub_event_, pub_ptr_, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED), - RCL_RET_OK); - } - - ~TestWaitable2() - { - EXPECT_EQ(rcl_event_fini(&pub_event_), RCL_RET_OK); - } - - 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); - } - - bool is_ready(const rcl_wait_set_t &) override - { - return test_waitable_result2; - } - - std::shared_ptr - take_data() override - { - return nullptr; - } - - void execute(const std::shared_ptr & data) override - { - (void) data; - } - - size_t get_number_of_ready_events() override - { - return 1; - } - -private: - rcl_publisher_t * pub_ptr_; - rcl_event_t pub_event_; - size_t wait_set_event_index_; -}; - struct RclWaitSetSizes { size_t size_of_subscriptions = 0; @@ -705,129 +654,20 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_timer) { } TEST_F(TestAllocatorMemoryStrategy, get_next_waitable) { + auto node1 = std::make_shared("waitable_node", "ns"); + auto node2 = std::make_shared("waitable_node2", "ns"); + rclcpp::Waitable::SharedPtr waitable1 = std::make_shared(); + rclcpp::Waitable::SharedPtr waitable2 = std::make_shared(); + node1->get_node_waitables_interface()->add_waitable(waitable1, nullptr); + node2->get_node_waitables_interface()->add_waitable(waitable2, nullptr); + auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { rclcpp::AnyExecutable result; allocator_memory_strategy()->get_next_waitable(result, weak_groups_to_nodes); return result; }; - { - auto node1 = std::make_shared( - "waitable_node", "ns", - rclcpp::NodeOptions() - .start_parameter_event_publisher(false) - .start_parameter_services(false)); - - rclcpp::PublisherOptions pub_options; - pub_options.use_default_callbacks = false; - - auto pub1 = node1->create_publisher( - "test_topic_1", rclcpp::QoS(10), pub_options); - - auto waitable1 = - std::make_shared(pub1->get_publisher_handle().get()); - node1->get_node_waitables_interface()->add_waitable(waitable1, nullptr); - - auto basic_node = create_node_with_disabled_callback_groups("basic_node"); - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - basic_node->for_each_callback_group( - [basic_node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - basic_node->get_node_base_interface())); - }); - node1->for_each_callback_group( - [node1, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node1->get_node_base_interface())); - }); - allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); - - rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); - ASSERT_EQ( - rcl_wait_set_init( - &wait_set, - allocator_memory_strategy()->number_of_ready_subscriptions(), - allocator_memory_strategy()->number_of_guard_conditions(), - allocator_memory_strategy()->number_of_ready_timers(), - allocator_memory_strategy()->number_of_ready_clients(), - allocator_memory_strategy()->number_of_ready_services(), - allocator_memory_strategy()->number_of_ready_events(), - rclcpp::contexts::get_global_default_context()->get_rcl_context().get(), - allocator_memory_strategy()->get_allocator()), - RCL_RET_OK); - - ASSERT_TRUE(allocator_memory_strategy()->add_handles_to_wait_set(&wait_set)); - - ASSERT_EQ( - rcl_wait( - &wait_set, - std::chrono::duration_cast(std::chrono::milliseconds(100)) - .count()), - RCL_RET_OK); - test_waitable_result2 = true; - allocator_memory_strategy()->remove_null_handles(&wait_set); - - rclcpp::AnyExecutable result = get_next_entity(weak_groups_to_nodes); - EXPECT_EQ(result.node_base, node1->get_node_base_interface()); - test_waitable_result2 = false; - - EXPECT_EQ(rcl_wait_set_fini(&wait_set), RCL_RET_OK); - } - - { - auto node2 = std::make_shared( - "waitable_node2", "ns", - rclcpp::NodeOptions() - .start_parameter_services(false) - .start_parameter_event_publisher(false)); - - rclcpp::PublisherOptions pub_options; - pub_options.use_default_callbacks = false; - - auto pub2 = node2->create_publisher( - "test_topic_2", rclcpp::QoS(10), pub_options); - - auto waitable2 = - std::make_shared(pub2->get_publisher_handle().get()); - node2->get_node_waitables_interface()->add_waitable(waitable2, nullptr); - - auto basic_node2 = std::make_shared( - "basic_node2", "ns", - rclcpp::NodeOptions() - .start_parameter_services(false) - .start_parameter_event_publisher(false)); - WeakCallbackGroupsToNodesMap weak_groups_to_uncollected_nodes; - basic_node2->for_each_callback_group( - [basic_node2, &weak_groups_to_uncollected_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_uncollected_nodes.insert( - std::pair( - group_ptr, - basic_node2->get_node_base_interface())); - }); - node2->for_each_callback_group( - [node2, - &weak_groups_to_uncollected_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_uncollected_nodes.insert( - std::pair( - group_ptr, - node2->get_node_base_interface())); - }); - - rclcpp::AnyExecutable failed_result = get_next_entity(weak_groups_to_uncollected_nodes); - EXPECT_EQ(failed_result.node_base, nullptr); - } + EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity)); } TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_mutually_exclusive) { diff --git a/rclcpp/test/rclcpp/test_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp index 348b7f0143..4c166ebb88 100644 --- a/rclcpp/test/rclcpp/test_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/test_memory_strategy.cpp @@ -37,6 +37,7 @@ class TestWaitable : public rclcpp::Waitable public: void add_to_wait_set(rcl_wait_set_t &) override {} bool is_ready(const rcl_wait_set_t &) override {return true;} + std::shared_ptr take_data() override {return nullptr;} void execute(const std::shared_ptr &) override {} }; 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 55573cc11b..f3e94b6aa0 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp @@ -50,6 +50,7 @@ class TestWaitable : public rclcpp::Waitable public: TestWaitable() : is_ready_(false) {} + void add_to_wait_set(rcl_wait_set_t &) override {} bool is_ready(const rcl_wait_set_t &) override {return is_ready_;} 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 cd44918236..eaf3a866d1 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,6 +50,7 @@ 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 { if (!add_to_wait_set_) { 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 7554f56270..e6a034832b 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,6 +50,7 @@ class TestWaitable : public rclcpp::Waitable public: TestWaitable() : is_ready_(false) {} + void add_to_wait_set(rcl_wait_set_t &) override {} bool is_ready(const rcl_wait_set_t &) override {return is_ready_;} diff --git a/rclcpp/test/rclcpp/waitables/test_intra_process_waitable.cpp b/rclcpp/test/rclcpp/waitables/test_intra_process_waitable.cpp new file mode 100644 index 0000000000..723e80513b --- /dev/null +++ b/rclcpp/test/rclcpp/waitables/test_intra_process_waitable.cpp @@ -0,0 +1,46 @@ +// Copyright 2024 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. + +#include + +#include "rclcpp/rclcpp.hpp" +#include "test_msgs/msg/empty.hpp" + +#include "./waitable_test_helpers.hpp" + +class TestIntraProcessWaitable : public ::testing::Test +{ +protected: + static void SetUpTestCase() {rclcpp::init(0, nullptr);} + static void TearDownTestCase() {rclcpp::shutdown();} +}; + +TEST_F(TestIntraProcessWaitable, test_that_waitable_stays_ready_after_second_wait) { + auto node = std::make_shared( + "test_node", + rclcpp::NodeOptions().use_intra_process_comms(true)); + + using test_msgs::msg::Empty; + auto sub = node->create_subscription("test_topic", 10, [](const Empty &) {}); + auto pub = node->create_publisher("test_topic", 10); + + auto make_sub_intra_process_waitable_ready = [pub]() { + pub->publish(Empty()); + }; + + rclcpp::test::waitables::do_test_that_waitable_stays_ready_after_second_wait( + sub->get_intra_process_waitable(), + make_sub_intra_process_waitable_ready, + true /* expected_to_stay_ready */); +} diff --git a/rclcpp/test/rclcpp/waitables/waitable_test_helpers.hpp b/rclcpp/test/rclcpp/waitables/waitable_test_helpers.hpp new file mode 100644 index 0000000000..49e074d5b0 --- /dev/null +++ b/rclcpp/test/rclcpp/waitables/waitable_test_helpers.hpp @@ -0,0 +1,117 @@ +// Copyright 2024 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__WAITABLES__WAITABLE_TEST_HELPERS_HPP_ +#define RCLCPP__WAITABLES__WAITABLE_TEST_HELPERS_HPP_ + +#include + +#include +#include +#include + +#include + +namespace rclcpp +{ +namespace test +{ +namespace waitables +{ + +/// Test that a given waitable is ready after a second wait. +/** + * The purpose of this test is to check that a waitable will remain ready + * on subsequent wait calls, if that is the expected behavior. + * Not all waitables should remain ready after a wait call, which can be + * expressed in the expected_to_stay_ready argument which defaults to true. + * If set to false, it will check that it is not ready after a second wait, as + * well as some other parts of the test. + * + * The given waitable should: + * + * - not be ready initially + * - not be ready after being waited on (and timing out) + * - should become ready after the make_waitable_ready method is called + * - may or may not be ready at this point + * - should be ready after waiting on it, within the wait_timeout + * - should be ready still after a second wait (unless expected_to_stay_ready = false) + * - if expected_to_stay_ready, should become not ready after a take_data/execute + */ +template +void +do_test_that_waitable_stays_ready_after_second_wait( + const std::shared_ptr & waitable, + std::function make_waitable_ready, + bool expected_to_stay_ready = true, + std::chrono::nanoseconds wait_timeout = std::chrono::seconds(5)) +{ + rclcpp::WaitSet wait_set; + wait_set.add_waitable(waitable); + + // not ready initially + EXPECT_FALSE(waitable->is_ready(wait_set.get_rcl_wait_set())) + << "waitable is unexpectedly ready before waiting"; + + // not ready after a wait that timesout + { + auto wait_result = wait_set.wait(std::chrono::seconds(0)); + EXPECT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Timeout) + << "wait set did not timeout as expected"; + EXPECT_FALSE(waitable->is_ready(wait_set.get_rcl_wait_set())) + << "waitable is unexpectedly ready after waiting, but before making ready"; + } + + // make it ready and wait on it + make_waitable_ready(); + { + auto wait_result = wait_set.wait(wait_timeout); + EXPECT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Ready) + << "wait set was not ready after the waitable should have been made ready"; + EXPECT_TRUE(waitable->is_ready(wait_set.get_rcl_wait_set())) + << "waitable is unexpectedly not ready after making it ready and waiting"; + } + + // wait again, and see that it is ready as expected or not expected + { + auto wait_result = wait_set.wait(std::chrono::seconds(0)); + if (expected_to_stay_ready) { + EXPECT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Ready) + << "wait set was not ready on a second wait on the waitable"; + EXPECT_TRUE(waitable->is_ready(wait_set.get_rcl_wait_set())) + << "waitable unexpectedly not ready after second wait"; + } else { + EXPECT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Timeout) + << "wait set did not time out after the waitable should have no longer been ready"; + EXPECT_FALSE(waitable->is_ready(wait_set.get_rcl_wait_set())) + << "waitable was ready after waiting a second time, which was not expected"; + } + } + + // if expected_to_stay_ready, check that take_data/execute makes it not ready + if (expected_to_stay_ready) { + waitable->execute(waitable->take_data()); + auto wait_result = wait_set.wait(std::chrono::seconds(0)); + EXPECT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Timeout) + << "wait set did not time out after the waitable should have no longer been ready"; + EXPECT_FALSE(waitable->is_ready(wait_set.get_rcl_wait_set())) + << "waitable was unexpectedly ready after a take_data and execute"; + } +} + +} // namespace waitables +} // namespace test +} // namespace rclcpp + +#endif // RCLCPP__WAITABLES__WAITABLE_TEST_HELPERS_HPP_ From dd81ef26a0c1def6e9c328b17f86f126cf40ccf3 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 9 Apr 2024 16:45:09 -0700 Subject: [PATCH 366/455] Remake of "fix: Fixed race condition in action server between is_ready and take" (#2495) Some background information: is_ready, take_data and execute data may be called from different threads in any order. The code in the old state expected them to be called in series, without interruption. This lead to multiple race conditions, as the state of the pimpl objects was altered by the three functions in a non thread safe way. Signed-off-by: Janosch Machowinski Signed-off-by: William Woodall Signed-off-by: Janosch Machowinski Co-authored-by: Janosch Machowinski --- .../include/rclcpp_action/server.hpp | 24 +- rclcpp_action/src/client.cpp | 370 ++++++++++++------ rclcpp_action/src/server.cpp | 272 +++++++------ rclcpp_action/test/test_server.cpp | 3 + 4 files changed, 422 insertions(+), 247 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 7461de2867..dc64991c46 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -22,6 +22,7 @@ #include #include +#include "action_msgs/srv/cancel_goal.hpp" #include "rcl/event_callback.h" #include "rcl_action/action_server.h" #include "rosidl_runtime_c/action_type_support_struct.h" @@ -77,6 +78,7 @@ class ServerBase : public rclcpp::Waitable GoalService, ResultService, CancelService, + Expired, }; RCLCPP_ACTION_PUBLIC @@ -279,19 +281,29 @@ class ServerBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC void - execute_goal_request_received(const std::shared_ptr & data); + execute_goal_request_received( + rcl_ret_t ret, + rcl_action_goal_info_t goal_info, + rmw_request_id_t request_header, + std::shared_ptr message); /// Handle a request to cancel goals on the server /// \internal RCLCPP_ACTION_PUBLIC void - execute_cancel_request_received(const std::shared_ptr & data); + execute_cancel_request_received( + rcl_ret_t ret, + std::shared_ptr request, + rmw_request_id_t request_header); /// Handle a request to get the result of an action /// \internal RCLCPP_ACTION_PUBLIC void - execute_result_request_received(const std::shared_ptr & data); + execute_result_request_received( + rcl_ret_t ret, + std::shared_ptr result_request, + rmw_request_id_t request_header); /// Handle a timeout indicating a completed goal should be forgotten by the server /// \internal @@ -345,7 +357,8 @@ class Server : public ServerBase, public std::enable_shared_from_this)>; + const GoalUUID &, + std::shared_ptr)>; /// Signature of a callback that accepts or rejects requests to cancel a goal. using CancelCallback = std::function>)>; /// Signature of a callback that is used to notify when the goal has been accepted. @@ -455,7 +468,8 @@ class Server : public ServerBase, public std::enable_shared_from_this rcl_goal_handle, - GoalUUID uuid, std::shared_ptr goal_request_message) override + GoalUUID uuid, + std::shared_ptr goal_request_message) override { std::shared_ptr> goal_handle; std::weak_ptr> weak_this = this->shared_from_this(); diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index fc5b3eeb90..3ea9b1fb1a 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "rcl_action/action_client.h" #include "rcl_action/wait.h" @@ -31,6 +32,67 @@ namespace rclcpp_action { +struct ClientBaseData +{ + struct FeedbackReadyData + { + FeedbackReadyData(rcl_ret_t retIn, std::shared_ptr msg) + : ret(retIn), feedback_message(msg) {} + rcl_ret_t ret; + std::shared_ptr feedback_message; + }; + struct StatusReadyData + { + StatusReadyData(rcl_ret_t retIn, std::shared_ptr msg) + : ret(retIn), status_message(msg) {} + rcl_ret_t ret; + std::shared_ptr status_message; + }; + struct GoalResponseData + { + GoalResponseData(rcl_ret_t retIn, rmw_request_id_t header, std::shared_ptr response) + : ret(retIn), response_header(header), goal_response(response) {} + rcl_ret_t ret; + rmw_request_id_t response_header; + std::shared_ptr goal_response; + }; + struct CancelResponseData + { + CancelResponseData(rcl_ret_t retIn, rmw_request_id_t header, std::shared_ptr response) + : ret(retIn), response_header(header), cancel_response(response) {} + rcl_ret_t ret; + rmw_request_id_t response_header; + std::shared_ptr cancel_response; + }; + struct ResultResponseData + { + ResultResponseData(rcl_ret_t retIn, rmw_request_id_t header, std::shared_ptr response) + : ret(retIn), response_header(header), result_response(response) {} + rcl_ret_t ret; + rmw_request_id_t response_header; + std::shared_ptr result_response; + }; + + std::variant< + FeedbackReadyData, + StatusReadyData, + GoalResponseData, + CancelResponseData, + ResultResponseData + > data; + + explicit ClientBaseData(FeedbackReadyData && data_in) + : data(std::move(data_in)) {} + explicit ClientBaseData(StatusReadyData && data_in) + : data(std::move(data_in)) {} + explicit ClientBaseData(GoalResponseData && data_in) + : data(std::move(data_in)) {} + explicit ClientBaseData(CancelResponseData && data_in) + : data(std::move(data_in)) {} + explicit ClientBaseData(ResultResponseData && data_in) + : data(std::move(data_in)) {} +}; + class ClientBaseImpl { public: @@ -94,11 +156,11 @@ class ClientBaseImpl size_t num_clients{0u}; size_t num_services{0u}; - bool is_feedback_ready{false}; - bool is_status_ready{false}; - bool is_goal_response_ready{false}; - bool is_cancel_response_ready{false}; - bool is_result_response_ready{false}; + // Lock for action_client_ + std::recursive_mutex action_client_mutex_; + + // next ready event for taking, will be set by is_ready and will be processed by take_data + std::atomic next_ready_event; rclcpp::Context::SharedPtr context_; rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_; @@ -142,6 +204,7 @@ bool ClientBase::action_server_is_ready() const { bool is_ready; + std::lock_guard lock(pimpl_->action_client_mutex_); rcl_ret_t ret = rcl_action_server_is_available( this->pimpl_->node_handle.get(), this->pimpl_->client_handle.get(), @@ -255,6 +318,7 @@ ClientBase::get_number_of_ready_services() void ClientBase::add_to_wait_set(rcl_wait_set_t & wait_set) { + std::lock_guard lock(pimpl_->action_client_mutex_); rcl_ret_t ret = rcl_action_wait_set_add_action_client( &wait_set, pimpl_->client_handle.get(), nullptr, nullptr); if (RCL_RET_OK != ret) { @@ -265,24 +329,56 @@ ClientBase::add_to_wait_set(rcl_wait_set_t & wait_set) bool 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(), - &pimpl_->is_feedback_ready, - &pimpl_->is_status_ready, - &pimpl_->is_goal_response_ready, - &pimpl_->is_cancel_response_ready, - &pimpl_->is_result_response_ready); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "failed to check for any ready entities"); + bool is_feedback_ready{false}; + bool is_status_ready{false}; + bool is_goal_response_ready{false}; + bool is_cancel_response_ready{false}; + bool is_result_response_ready{false}; + + rcl_ret_t ret; + { + std::lock_guard lock(pimpl_->action_client_mutex_); + ret = rcl_action_client_wait_set_get_entities_ready( + &wait_set, pimpl_->client_handle.get(), + &is_feedback_ready, + &is_status_ready, + &is_goal_response_ready, + &is_cancel_response_ready, + &is_result_response_ready); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to check for any ready entities"); + } + } + + pimpl_->next_ready_event = std::numeric_limits::max(); + + if (is_feedback_ready) { + pimpl_->next_ready_event = static_cast(EntityType::FeedbackSubscription); + return true; + } + + if (is_status_ready) { + pimpl_->next_ready_event = static_cast(EntityType::StatusSubscription); + return true; + } + + if (is_goal_response_ready) { + pimpl_->next_ready_event = static_cast(EntityType::GoalClient); + return true; + } + + if (is_result_response_ready) { + pimpl_->next_ready_event = static_cast(EntityType::ResultClient); + return true; + } + + if (is_cancel_response_ready) { + pimpl_->next_ready_event = static_cast(EntityType::CancelClient); + return true; } - return - pimpl_->is_feedback_ready || - pimpl_->is_status_ready || - pimpl_->is_goal_response_ready || - pimpl_->is_cancel_response_ready || - pimpl_->is_result_response_ready; + + return false; } void @@ -433,7 +529,6 @@ ClientBase::set_callback_to_entity( } }; - // 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. @@ -551,140 +646,155 @@ ClientBase::clear_on_ready_callback() std::shared_ptr ClientBase::take_data() { - if (pimpl_->is_feedback_ready) { - std::shared_ptr feedback_message = this->create_feedback_message(); - rcl_ret_t ret = rcl_action_take_feedback( - pimpl_->client_handle.get(), feedback_message.get()); - return std::static_pointer_cast( - std::make_shared>>( - ret, feedback_message)); - } else if (pimpl_->is_status_ready) { - std::shared_ptr status_message = this->create_status_message(); - rcl_ret_t ret = rcl_action_take_status( - pimpl_->client_handle.get(), status_message.get()); - return std::static_pointer_cast( - std::make_shared>>( - ret, status_message)); - } else if (pimpl_->is_goal_response_ready) { - rmw_request_id_t response_header; - std::shared_ptr goal_response = this->create_goal_response(); - rcl_ret_t ret = rcl_action_take_goal_response( - pimpl_->client_handle.get(), &response_header, goal_response.get()); - return std::static_pointer_cast( - std::make_shared>>( - ret, response_header, goal_response)); - } else if (pimpl_->is_result_response_ready) { - rmw_request_id_t response_header; - std::shared_ptr result_response = this->create_result_response(); - rcl_ret_t ret = rcl_action_take_result_response( - pimpl_->client_handle.get(), &response_header, result_response.get()); - return std::static_pointer_cast( - std::make_shared>>( - ret, response_header, result_response)); - } else if (pimpl_->is_cancel_response_ready) { - rmw_request_id_t response_header; - std::shared_ptr cancel_response = this->create_cancel_response(); - rcl_ret_t ret = rcl_action_take_cancel_response( - pimpl_->client_handle.get(), &response_header, cancel_response.get()); - return std::static_pointer_cast( - std::make_shared>>( - ret, response_header, cancel_response)); - } else { + // next_ready_event is an atomic, caching localy + size_t next_ready_event = pimpl_->next_ready_event.exchange(std::numeric_limits::max()); + + if (next_ready_event == std::numeric_limits::max()) { throw std::runtime_error("Taking data from action client but nothing is ready"); } + + return take_data_by_entity_id(next_ready_event); } std::shared_ptr ClientBase::take_data_by_entity_id(size_t id) { + std::shared_ptr data_ptr; + rcl_ret_t ret; + // Mark as ready the entity from which we want to take data switch (static_cast(id)) { case EntityType::GoalClient: - pimpl_->is_goal_response_ready = true; + { + rmw_request_id_t response_header; + std::shared_ptr goal_response; + { + std::lock_guard lock(pimpl_->action_client_mutex_); + + goal_response = this->create_goal_response(); + ret = rcl_action_take_goal_response( + pimpl_->client_handle.get(), &response_header, goal_response.get()); + } + data_ptr = std::make_shared( + ClientBaseData::GoalResponseData( + ret, response_header, goal_response)); + } break; case EntityType::ResultClient: - pimpl_->is_result_response_ready = true; + { + rmw_request_id_t response_header; + std::shared_ptr result_response; + { + std::lock_guard lock(pimpl_->action_client_mutex_); + result_response = this->create_result_response(); + ret = rcl_action_take_result_response( + pimpl_->client_handle.get(), &response_header, result_response.get()); + } + data_ptr = + std::make_shared( + ClientBaseData::ResultResponseData( + ret, response_header, result_response)); + } break; case EntityType::CancelClient: - pimpl_->is_cancel_response_ready = true; + { + rmw_request_id_t response_header; + std::shared_ptr cancel_response; + { + std::lock_guard lock(pimpl_->action_client_mutex_); + cancel_response = this->create_cancel_response(); + ret = rcl_action_take_cancel_response( + pimpl_->client_handle.get(), &response_header, cancel_response.get()); + } + data_ptr = + std::make_shared( + ClientBaseData::CancelResponseData( + ret, response_header, cancel_response)); + } break; case EntityType::FeedbackSubscription: - pimpl_->is_feedback_ready = true; + { + std::shared_ptr feedback_message; + { + std::lock_guard lock(pimpl_->action_client_mutex_); + feedback_message = this->create_feedback_message(); + ret = rcl_action_take_feedback( + pimpl_->client_handle.get(), feedback_message.get()); + } + data_ptr = + std::make_shared( + ClientBaseData::FeedbackReadyData( + ret, feedback_message)); + } break; case EntityType::StatusSubscription: - pimpl_->is_status_ready = true; + { + std::shared_ptr status_message; + { + std::lock_guard lock(pimpl_->action_client_mutex_); + status_message = this->create_status_message(); + ret = rcl_action_take_status( + pimpl_->client_handle.get(), status_message.get()); + } + data_ptr = + std::make_shared( + ClientBaseData::StatusReadyData( + ret, status_message)); + } break; } - return take_data(); + return std::static_pointer_cast(data_ptr); } void -ClientBase::execute(const std::shared_ptr & data) +ClientBase::execute(const std::shared_ptr & data_in) { - if (!data) { - throw std::runtime_error("'data' is empty"); + if (!data_in) { + throw std::invalid_argument("'data_in' is unexpectedly empty"); } - if (pimpl_->is_feedback_ready) { - auto shared_ptr = std::static_pointer_cast>>(data); - auto ret = std::get<0>(*shared_ptr); - pimpl_->is_feedback_ready = false; - if (RCL_RET_OK == ret) { - auto feedback_message = std::get<1>(*shared_ptr); - this->handle_feedback_message(feedback_message); - } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "error taking feedback"); - } - } else if (pimpl_->is_status_ready) { - auto shared_ptr = std::static_pointer_cast>>(data); - auto ret = std::get<0>(*shared_ptr); - pimpl_->is_status_ready = false; - if (RCL_RET_OK == ret) { - auto status_message = std::get<1>(*shared_ptr); - this->handle_status_message(status_message); - } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "error taking status"); - } - } else if (pimpl_->is_goal_response_ready) { - auto shared_ptr = std::static_pointer_cast< - std::tuple>>(data); - auto ret = std::get<0>(*shared_ptr); - pimpl_->is_goal_response_ready = false; - if (RCL_RET_OK == ret) { - auto response_header = std::get<1>(*shared_ptr); - auto goal_response = std::get<2>(*shared_ptr); - this->handle_goal_response(response_header, goal_response); - } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "error taking goal response"); - } - } else if (pimpl_->is_result_response_ready) { - auto shared_ptr = std::static_pointer_cast< - std::tuple>>(data); - auto ret = std::get<0>(*shared_ptr); - pimpl_->is_result_response_ready = false; - if (RCL_RET_OK == ret) { - auto response_header = std::get<1>(*shared_ptr); - auto result_response = std::get<2>(*shared_ptr); - this->handle_result_response(response_header, result_response); - } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "error taking result response"); - } - } else if (pimpl_->is_cancel_response_ready) { - auto shared_ptr = std::static_pointer_cast< - std::tuple>>(data); - auto ret = std::get<0>(*shared_ptr); - pimpl_->is_cancel_response_ready = false; - if (RCL_RET_OK == ret) { - auto response_header = std::get<1>(*shared_ptr); - auto cancel_response = std::get<2>(*shared_ptr); - this->handle_cancel_response(response_header, cancel_response); - } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "error taking cancel response"); - } - } else { - throw std::runtime_error("Executing action client but nothing is ready"); - } + std::shared_ptr data_ptr = std::static_pointer_cast(data_in); + + std::visit( + [&](auto && data) -> void { + using T = std::decay_t; + if constexpr (std::is_same_v) { + if (RCL_RET_OK == data.ret) { + this->handle_feedback_message(data.feedback_message); + } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) { + rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking feedback"); + } + } + if constexpr (std::is_same_v) { + if (RCL_RET_OK == data.ret) { + this->handle_status_message(data.status_message); + } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) { + rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking status"); + } + } + if constexpr (std::is_same_v) { + if (RCL_RET_OK == data.ret) { + this->handle_goal_response(data.response_header, data.goal_response); + } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) { + rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking goal response"); + } + } + if constexpr (std::is_same_v) { + if (RCL_RET_OK == data.ret) { + this->handle_result_response(data.response_header, data.result_response); + } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) { + rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking result response"); + } + } + if constexpr (std::is_same_v) { + if (RCL_RET_OK == data.ret) { + this->handle_cancel_response(data.response_header, data.cancel_response); + } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) { + rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking cancel response"); + } + } + }, data_ptr->data); } } // namespace rclcpp_action diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 565aaa4f3b..fefc02d6ad 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -18,7 +18,9 @@ #include #include #include +#include #include +#include #include "rcl_action/action_server.h" #include "rcl_action/wait.h" @@ -26,15 +28,48 @@ #include "rcpputils/scope_exit.hpp" #include "action_msgs/msg/goal_status_array.hpp" -#include "action_msgs/srv/cancel_goal.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp_action/server.hpp" using rclcpp_action::ServerBase; using rclcpp_action::GoalUUID; +struct ServerBaseData; + namespace rclcpp_action { + +struct ServerBaseData +{ + using GoalRequestData = std::tuple< + rcl_ret_t, + const rcl_action_goal_info_t, + rmw_request_id_t, + std::shared_ptr + >; + + using CancelRequestData = std::tuple< + rcl_ret_t, + std::shared_ptr, + rmw_request_id_t + >; + + using ResultRequestData = std::tuple, rmw_request_id_t>; + + using GoalExpiredData = struct Empty {}; + + std::variant data; + + explicit ServerBaseData(GoalRequestData && data_in) + : data(std::move(data_in)) {} + explicit ServerBaseData(CancelRequestData && data_in) + : data(std::move(data_in)) {} + explicit ServerBaseData(ResultRequestData && data_in) + : data(std::move(data_in)) {} + explicit ServerBaseData(GoalExpiredData && data_in) + : data(std::move(data_in)) {} +}; + class ServerBaseImpl { public: @@ -60,11 +95,6 @@ class ServerBaseImpl size_t num_services_ = 0; size_t num_guard_conditions_ = 0; - std::atomic goal_request_ready_{false}; - std::atomic cancel_request_ready_{false}; - std::atomic result_request_ready_{false}; - std::atomic goal_expired_{false}; - // Lock for unordered_maps std::recursive_mutex unordered_map_mutex_; @@ -75,8 +105,12 @@ class ServerBaseImpl // rcl goal handles are kept so api to send result doesn't try to access freed memory std::unordered_map> goal_handles_; + // next ready event for taking, will be set by is_ready and will be processed by take_data + std::atomic next_ready_event; + rclcpp::Logger logger_; }; + } // namespace rclcpp_action ServerBase::ServerBase( @@ -194,124 +228,153 @@ ServerBase::is_ready(const rcl_wait_set_t & wait_set) &goal_expired); } - pimpl_->goal_request_ready_ = goal_request_ready; - pimpl_->cancel_request_ready_ = cancel_request_ready; - pimpl_->result_request_ready_ = result_request_ready; - pimpl_->goal_expired_ = goal_expired; - if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } - return pimpl_->goal_request_ready_.load() || - pimpl_->cancel_request_ready_.load() || - pimpl_->result_request_ready_.load() || - pimpl_->goal_expired_.load(); -} + pimpl_->next_ready_event = std::numeric_limits::max(); -std::shared_ptr -ServerBase::take_data() -{ - if (pimpl_->goal_request_ready_.load()) { - rcl_ret_t ret; - rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); - rmw_request_id_t request_header; + if (goal_request_ready) { + pimpl_->next_ready_event = static_cast(EntityType::GoalService); + return true; + } - std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + if (cancel_request_ready) { + pimpl_->next_ready_event = static_cast(EntityType::CancelService); + return true; + } - std::shared_ptr message = create_goal_request(); - ret = rcl_action_take_goal_request( - pimpl_->action_server_.get(), - &request_header, - message.get()); - - return std::static_pointer_cast( - std::make_shared - >>( - ret, - goal_info, - request_header, message)); - } else if (pimpl_->cancel_request_ready_.load()) { - rcl_ret_t ret; - rmw_request_id_t request_header; + if (result_request_ready) { + pimpl_->next_ready_event = static_cast(EntityType::ResultService); + return true; + } - // Initialize cancel request - auto request = std::make_shared(); + if (goal_expired) { + pimpl_->next_ready_event = static_cast(EntityType::Expired); + return true; + } - std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); - ret = rcl_action_take_cancel_request( - pimpl_->action_server_.get(), - &request_header, - request.get()); + return false; +} - return std::static_pointer_cast( - std::make_shared - , - rmw_request_id_t>>(ret, request, request_header)); - } else if (pimpl_->result_request_ready_.load()) { - rcl_ret_t ret; - // Get the result request message - rmw_request_id_t request_header; - std::shared_ptr result_request = create_result_request(); - std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); - ret = rcl_action_take_result_request( - pimpl_->action_server_.get(), &request_header, result_request.get()); - - return std::static_pointer_cast( - std::make_shared, rmw_request_id_t>>( - ret, result_request, request_header)); - } else if (pimpl_->goal_expired_.load()) { - return nullptr; - } else { - throw std::runtime_error("Taking data from action server but nothing is ready"); +std::shared_ptr +ServerBase::take_data() +{ + size_t next_ready_event = pimpl_->next_ready_event.exchange(std::numeric_limits::max()); + + if (next_ready_event == std::numeric_limits::max()) { + throw std::runtime_error("ServerBase::take_data() called but no data is ready"); } + + return take_data_by_entity_id(next_ready_event); } std::shared_ptr ServerBase::take_data_by_entity_id(size_t id) { + std::shared_ptr data_ptr; // Mark as ready the entity from which we want to take data switch (static_cast(id)) { case EntityType::GoalService: - pimpl_->goal_request_ready_ = true; + { + rcl_ret_t ret; + rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); + rmw_request_id_t request_header; + + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + + std::shared_ptr message = create_goal_request(); + ret = rcl_action_take_goal_request( + pimpl_->action_server_.get(), + &request_header, + message.get()); + + data_ptr = std::make_shared( + ServerBaseData::GoalRequestData(ret, goal_info, request_header, message)); + } break; case EntityType::ResultService: - pimpl_->result_request_ready_ = true; + { + rcl_ret_t ret; + // Get the result request message + rmw_request_id_t request_header; + std::shared_ptr result_request = create_result_request(); + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + ret = rcl_action_take_result_request( + pimpl_->action_server_.get(), &request_header, result_request.get()); + + data_ptr = + std::make_shared( + ServerBaseData::ResultRequestData(ret, result_request, request_header)); + } break; case EntityType::CancelService: - pimpl_->cancel_request_ready_ = true; + { + rcl_ret_t ret; + rmw_request_id_t request_header; + + // Initialize cancel request + auto request = std::make_shared(); + + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + ret = rcl_action_take_cancel_request( + pimpl_->action_server_.get(), + &request_header, + request.get()); + + data_ptr = + std::make_shared( + ServerBaseData::CancelRequestData(ret, request, request_header)); + } + break; + case EntityType::Expired: + { + data_ptr = + std::make_shared(ServerBaseData::GoalExpiredData()); + } break; } - return take_data(); + return std::static_pointer_cast(data_ptr); } void -ServerBase::execute(const std::shared_ptr & data) +ServerBase::execute(const std::shared_ptr & data_in) { - if (!data && !pimpl_->goal_expired_.load()) { - throw std::runtime_error("'data' is empty"); - } - - if (pimpl_->goal_request_ready_.load()) { - execute_goal_request_received(data); - } else if (pimpl_->cancel_request_ready_.load()) { - execute_cancel_request_received(data); - } else if (pimpl_->result_request_ready_.load()) { - execute_result_request_received(data); - } else if (pimpl_->goal_expired_.load()) { - execute_check_expired_goals(); - } else { - throw std::runtime_error("Executing action server but nothing is ready"); + if (!data_in) { + throw std::runtime_error("ServerBase::execute: give data pointer was null"); } + + std::shared_ptr data_ptr = std::static_pointer_cast(data_in); + + std::visit( + [&](auto && data) -> void { + using T = std::decay_t; + if constexpr (std::is_same_v) { + execute_goal_request_received( + std::get<0>(data), std::get<1>(data), std::get<2>(data), + std::get<3>(data)); + } + if constexpr (std::is_same_v) { + execute_cancel_request_received(std::get<0>(data), std::get<1>(data), std::get<2>(data)); + } + if constexpr (std::is_same_v) { + execute_result_request_received(std::get<0>(data), std::get<1>(data), std::get<2>(data)); + } + if constexpr (std::is_same_v) { + execute_check_expired_goals(); + } + }, + data_ptr->data); } void -ServerBase::execute_goal_request_received(const std::shared_ptr & data) +ServerBase::execute_goal_request_received( + rcl_ret_t ret, + rcl_action_goal_info_t goal_info, + rmw_request_id_t request_header, + const std::shared_ptr message) { - auto shared_ptr = std::static_pointer_cast - >>(data); - rcl_ret_t ret = std::get<0>(*shared_ptr); if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) { // Ignore take failure because connext fails if it receives a sample without valid data. // This happens when a client shuts down and connext receives a sample saying the client is @@ -320,14 +383,6 @@ ServerBase::execute_goal_request_received(const std::shared_ptr & data) } else if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } - rcl_action_goal_info_t goal_info = std::get<1>(*shared_ptr); - rmw_request_id_t request_header = std::get<2>(*shared_ptr); - std::shared_ptr message = std::get<3>(*shared_ptr); - - bool expected = true; - if (!pimpl_->goal_request_ready_.compare_exchange_strong(expected, false)) { - return; - } GoalUUID uuid = get_goal_id_from_goal_request(message.get()); convert(uuid, &goal_info); @@ -408,12 +463,11 @@ ServerBase::execute_goal_request_received(const std::shared_ptr & data) } void -ServerBase::execute_cancel_request_received(const std::shared_ptr & data) +ServerBase::execute_cancel_request_received( + rcl_ret_t ret, + std::shared_ptr request, + rmw_request_id_t request_header) { - auto shared_ptr = std::static_pointer_cast - , - rmw_request_id_t>>(data); - auto ret = std::get<0>(*shared_ptr); if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) { // Ignore take failure because connext fails if it receives a sample without valid data. // This happens when a client shuts down and connext receives a sample saying the client is @@ -422,9 +476,6 @@ ServerBase::execute_cancel_request_received(const std::shared_ptr & data) } else if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } - auto request = std::get<1>(*shared_ptr); - auto request_header = std::get<2>(*shared_ptr); - pimpl_->cancel_request_ready_ = false; // Convert c++ message to C message rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request(); @@ -506,11 +557,11 @@ ServerBase::execute_cancel_request_received(const std::shared_ptr & data) } void -ServerBase::execute_result_request_received(const std::shared_ptr & data) +ServerBase::execute_result_request_received( + rcl_ret_t ret, + std::shared_ptr result_request, + rmw_request_id_t request_header) { - auto shared_ptr = std::static_pointer_cast - , rmw_request_id_t>>(data); - auto ret = std::get<0>(*shared_ptr); if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) { // Ignore take failure because connext fails if it receives a sample without valid data. // This happens when a client shuts down and connext receives a sample saying the client is @@ -519,10 +570,7 @@ ServerBase::execute_result_request_received(const std::shared_ptr & data) } else if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } - auto result_request = std::get<1>(*shared_ptr); - auto request_header = std::get<2>(*shared_ptr); - pimpl_->result_request_ready_ = false; std::shared_ptr result_response; // check if the goal exists diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 9ffa31797f..c63d1dfdfc 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -1110,6 +1110,9 @@ TEST_F(TestGoalRequestServer, is_ready_rcl_error) { EXPECT_NO_THROW(action_server_->add_to_wait_set(wait_set)); EXPECT_TRUE(action_server_->is_ready(wait_set)); + + EXPECT_NO_THROW(action_server_->take_data()); + 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); From ddc8a9c847a169bc9d436cc909e9a6618e63ba0f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 11 Apr 2024 11:55:44 -0400 Subject: [PATCH 367/455] Fix uninitialized memory in test (#2498) When I added in the tests for large messages, I made a mistake and reserved space in the strings, but didn't actually expand it. Thus, we were writing into uninitialized memory. Fix this by just using the correct constructor for string, which will allocate and initialize the memory properly. Signed-off-by: Chris Lalancette --- rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp index 9367a89014..d70b0e382d 100644 --- a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp @@ -379,9 +379,7 @@ TEST_F(TestPublisher, test_large_message_unique) auto pub = node->create_publisher(topic_name, 1); static constexpr size_t length = 10 * 1024 * 1024; - auto message_data = std::make_unique(); - message_data->reserve(length); - std::fill(message_data->begin(), message_data->begin() + length, '#'); + auto message_data = std::make_unique(length, '#'); pub->publish(std::move(message_data)); } @@ -400,8 +398,6 @@ TEST_F(TestPublisher, test_large_message_constref) auto pub = node->create_publisher(topic_name, 1); static constexpr size_t length = 10 * 1024 * 1024; - std::string message_data; - message_data.reserve(length); - std::fill(message_data.begin(), message_data.begin() + length, '#'); + std::string message_data(length, '#'); pub->publish(message_data); } From 634cb873a3106fa03a91bee89755184116abef40 Mon Sep 17 00:00:00 2001 From: jmachowinski Date: Fri, 12 Apr 2024 14:28:59 +0200 Subject: [PATCH 368/455] Added optional TimerInfo to timer callback (#2343) Signed-off-by: Alexis Tsogias Signed-off-by: Janosch Machowinski Co-authored-by: Alexis Tsogias Co-authored-by: Janosch Machowinski --- rclcpp/include/rclcpp/executor.hpp | 2 +- .../events_executor_event_types.hpp | 3 + .../rclcpp/experimental/timers_manager.hpp | 9 ++- .../strategies/allocator_memory_strategy.hpp | 4 +- rclcpp/include/rclcpp/timer.hpp | 57 ++++++++++++++----- rclcpp/src/rclcpp/executor.cpp | 12 ++-- .../static_single_threaded_executor.cpp | 12 ++-- .../events_executor/events_executor.cpp | 16 +++--- .../rclcpp/experimental/timers_manager.cpp | 29 +++++++--- .../rclcpp/executors/test_events_queue.cpp | 1 + .../node_interfaces/test_node_timers.cpp | 4 +- rclcpp/test/rclcpp/test_timer.cpp | 22 +++++++ 12 files changed, 126 insertions(+), 45 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index cfd7ea81fd..132b8150fe 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -487,7 +487,7 @@ class Executor */ RCLCPP_PUBLIC static void - execute_timer(rclcpp::TimerBase::SharedPtr timer); + execute_timer(rclcpp::TimerBase::SharedPtr timer, const std::shared_ptr & dataPtr); /// Run service server executable. /** diff --git a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp index 79c2c5f905..0da641ea6e 100644 --- a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp +++ b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp @@ -15,6 +15,8 @@ #ifndef RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_ #define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_ +#include + namespace rclcpp { namespace experimental @@ -34,6 +36,7 @@ enum ExecutorEventType struct ExecutorEvent { const void * entity_key; + std::shared_ptr data; int waitable_data; ExecutorEventType type; size_t num_events; diff --git a/rclcpp/include/rclcpp/experimental/timers_manager.hpp b/rclcpp/include/rclcpp/experimental/timers_manager.hpp index 197397e8b8..af3337bfd6 100644 --- a/rclcpp/include/rclcpp/experimental/timers_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/timers_manager.hpp @@ -86,7 +86,8 @@ class TimersManager RCLCPP_PUBLIC TimersManager( std::shared_ptr context, - std::function on_ready_callback = nullptr); + std::function &)> on_ready_callback = nullptr); /** * @brief Destruct the TimersManager object making sure to stop thread and release memory. @@ -164,9 +165,10 @@ class TimersManager * the TimersManager on_ready_callback was passed during construction. * * @param timer_id the timer ID of the timer to execute + * @param data internal data of the timer */ RCLCPP_PUBLIC - void execute_ready_timer(const rclcpp::TimerBase * timer_id); + void execute_ready_timer(const rclcpp::TimerBase * timer_id, const std::shared_ptr & data); /** * @brief Get the amount of time before the next timer triggers. @@ -529,7 +531,8 @@ class TimersManager void execute_ready_timers_unsafe(); // Callback to be called when timer is ready - std::function on_ready_callback_; + std::function &)> on_ready_callback_ = nullptr; // Thread used to run the timers execution task std::thread timers_thread_; diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 1da372abfd..28eff94aed 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -370,7 +370,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy ++it; continue; } - if (!timer->call()) { + auto data = timer->call(); + if (!data) { // timer was cancelled, skip it. ++it; continue; @@ -379,6 +380,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy any_exec.timer = timer; any_exec.callback_group = group; any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes); + any_exec.data = data; timer_handles_.erase(it); return; } diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 6060d8bd78..ad9e96ea40 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -43,6 +44,12 @@ namespace rclcpp { +struct TimerInfo +{ + Time expected_call_time; + Time actual_call_time; +}; + class TimerBase { public: @@ -101,16 +108,20 @@ class TimerBase * The multithreaded executor takes advantage of this to avoid scheduling * the callback multiple times. * - * \return `true` if the callback should be executed, `false` if the timer was canceled. + * \return a valid shared_ptr if the callback should be executed, + * an invalid shared_ptr (nullptr) if the timer was canceled. */ RCLCPP_PUBLIC - virtual bool + virtual std::shared_ptr call() = 0; /// Call the callback function when the timer signal is emitted. + /** + * \param[in] data the pointer returned by the call function + */ RCLCPP_PUBLIC virtual void - execute_callback() = 0; + execute_callback(const std::shared_ptr & data) = 0; RCLCPP_PUBLIC std::shared_ptr @@ -198,16 +209,17 @@ class TimerBase set_on_reset_callback(rcl_event_callback_t callback, const void * user_data); }; - using VoidCallbackType = std::function; using TimerCallbackType = std::function; +using TimerInfoCallbackType = std::function; /// Generic timer. Periodically executes a user-specified callback. template< typename FunctorT, typename std::enable_if< rclcpp::function_traits::same_arguments::value || - rclcpp::function_traits::same_arguments::value + rclcpp::function_traits::same_arguments::value || + rclcpp::function_traits::same_arguments::value >::type * = nullptr > class GenericTimer : public TimerBase @@ -256,27 +268,28 @@ class GenericTimer : public TimerBase * \sa rclcpp::TimerBase::call * \throws std::runtime_error if it failed to notify timer that callback will occurr */ - bool + std::shared_ptr call() override { - rcl_ret_t ret = rcl_timer_call(timer_handle_.get()); + auto timer_call_info_ = std::make_shared(); + rcl_ret_t ret = rcl_timer_call_with_info(timer_handle_.get(), timer_call_info_.get()); if (ret == RCL_RET_TIMER_CANCELED) { - return false; + return nullptr; } if (ret != RCL_RET_OK) { throw std::runtime_error("Failed to notify timer that callback occurred"); } - return true; + return timer_call_info_; } /** * \sa rclcpp::TimerBase::execute_callback */ void - execute_callback() override + execute_callback(const std::shared_ptr & data) override { TRACETOOLS_TRACEPOINT(callback_start, reinterpret_cast(&callback_), false); - execute_callback_delegate<>(); + execute_callback_delegate<>(*static_cast(data.get())); TRACETOOLS_TRACEPOINT(callback_end, reinterpret_cast(&callback_)); } @@ -288,7 +301,7 @@ class GenericTimer : public TimerBase >::type * = nullptr > void - execute_callback_delegate() + execute_callback_delegate(const rcl_timer_call_info_t &) { callback_(); } @@ -300,11 +313,26 @@ class GenericTimer : public TimerBase >::type * = nullptr > void - execute_callback_delegate() + execute_callback_delegate(const rcl_timer_call_info_t &) { callback_(*this); } + + template< + typename CallbackT = FunctorT, + typename std::enable_if< + rclcpp::function_traits::same_arguments::value + >::type * = nullptr + > + void + execute_callback_delegate(const rcl_timer_call_info_t & timer_call_info_) + { + const TimerInfo info{Time{timer_call_info_.expected_call_time, clock_->get_clock_type()}, + Time{timer_call_info_.actual_call_time, clock_->get_clock_type()}}; + callback_(info); + } + /// Is the clock steady (i.e. is the time between ticks constant?) /** \return True if the clock used by this timer is steady. */ bool @@ -323,7 +351,8 @@ template< typename FunctorT, typename std::enable_if< rclcpp::function_traits::same_arguments::value || - rclcpp::function_traits::same_arguments::value + rclcpp::function_traits::same_arguments::value || + rclcpp::function_traits::same_arguments::value >::type * = nullptr > class WallTimer : public GenericTimer diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index a54d71e21b..0b78e12912 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -383,7 +383,7 @@ Executor::execute_any_executable(AnyExecutable & any_exec) TRACETOOLS_TRACEPOINT( rclcpp_executor_execute, static_cast(any_exec.timer->get_timer_handle().get())); - execute_timer(any_exec.timer); + execute_timer(any_exec.timer, any_exec.data); } if (any_exec.subscription) { TRACETOOLS_TRACEPOINT( @@ -547,9 +547,9 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) } void -Executor::execute_timer(rclcpp::TimerBase::SharedPtr timer) +Executor::execute_timer(rclcpp::TimerBase::SharedPtr timer, const std::shared_ptr & dataPtr) { - timer->execute_callback(); + timer->execute_callback(dataPtr); } void @@ -690,6 +690,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) if (entity_iter != current_collection_.timers.end()) { auto callback_group = entity_iter->second.callback_group.lock(); if (callback_group && !callback_group->can_be_taken_from()) { + current_timer_index++; continue; } // At this point the timer is either ready for execution or was perhaps @@ -698,7 +699,9 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) // it from the wait result. wait_result_->clear_timer_with_index(current_timer_index); // Check that the timer should be called still, i.e. it wasn't canceled. - if (!timer->call()) { + any_executable.data = timer->call(); + if (!any_executable.data) { + current_timer_index++; continue; } any_executable.timer = timer; @@ -706,6 +709,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) valid_executable = true; break; } + current_timer_index++; } } diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 831076c61c..2d837103e5 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -154,11 +154,15 @@ bool StaticSingleThreadedExecutor::execute_ready_executables( auto entity_iter = collection.timers.find(timer->get_timer_handle().get()); if (entity_iter != collection.timers.end()) { wait_result.clear_timer_with_index(current_timer_index); - if (timer->call()) { - execute_timer(timer); - any_ready_executable = true; - if (spin_once) {return any_ready_executable;} + auto data = timer->call(); + if (!data) { + // someone canceled the timer between is_ready and call + continue; } + + execute_timer(std::move(timer), 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 f7ba6da2df..ce6a103ab2 100644 --- a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp +++ b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp @@ -40,10 +40,12 @@ EventsExecutor::EventsExecutor( // The timers manager can be used either to only track timers (in this case an expired // timer will generate an executor event and then it will be executed by the executor thread) // or it can also take care of executing expired timers in its dedicated thread. - std::function timer_on_ready_cb = nullptr; + std::function &)> timer_on_ready_cb = nullptr; if (!execute_timers_separate_thread) { - timer_on_ready_cb = [this](const rclcpp::TimerBase * timer_id) { - ExecutorEvent event = {timer_id, -1, ExecutorEventType::TIMER_EVENT, 1}; + timer_on_ready_cb = + [this](const rclcpp::TimerBase * timer_id, const std::shared_ptr & data) { + ExecutorEvent event = {timer_id, data, -1, ExecutorEventType::TIMER_EVENT, 1}; this->events_queue_->enqueue(event); }; } @@ -88,7 +90,7 @@ EventsExecutor::EventsExecutor( } ExecutorEvent event = - {notify_waitable_entity_id, waitable_data, ExecutorEventType::WAITABLE_EVENT, 1}; + {notify_waitable_entity_id, nullptr, waitable_data, ExecutorEventType::WAITABLE_EVENT, 1}; this->events_queue_->enqueue(event); }); @@ -325,7 +327,7 @@ EventsExecutor::execute_event(const ExecutorEvent & event) case ExecutorEventType::TIMER_EVENT: { timers_manager_->execute_ready_timer( - static_cast(event.entity_key)); + static_cast(event.entity_key), event.data); break; } case ExecutorEventType::WAITABLE_EVENT: @@ -485,7 +487,7 @@ EventsExecutor::create_entity_callback( { std::function callback = [this, entity_key, event_type](size_t num_events) { - ExecutorEvent event = {entity_key, -1, event_type, num_events}; + ExecutorEvent event = {entity_key, nullptr, -1, event_type, num_events}; this->events_queue_->enqueue(event); }; return callback; @@ -497,7 +499,7 @@ EventsExecutor::create_waitable_callback(const rclcpp::Waitable * entity_key) std::function callback = [this, entity_key](size_t num_events, int waitable_data) { ExecutorEvent event = - {entity_key, waitable_data, ExecutorEventType::WAITABLE_EVENT, num_events}; + {entity_key, nullptr, waitable_data, ExecutorEventType::WAITABLE_EVENT, num_events}; this->events_queue_->enqueue(event); }; return callback; diff --git a/rclcpp/src/rclcpp/experimental/timers_manager.cpp b/rclcpp/src/rclcpp/experimental/timers_manager.cpp index 39924afa56..2caa0a6b15 100644 --- a/rclcpp/src/rclcpp/experimental/timers_manager.cpp +++ b/rclcpp/src/rclcpp/experimental/timers_manager.cpp @@ -27,7 +27,7 @@ using rclcpp::experimental::TimersManager; TimersManager::TimersManager( std::shared_ptr context, - std::function on_ready_callback) + std::function &)> on_ready_callback) : on_ready_callback_(on_ready_callback), context_(context) { @@ -148,8 +148,12 @@ bool TimersManager::execute_head_timer() if (timer_ready) { // NOTE: here we always execute the timer, regardless of whether the // on_ready_callback is set or not. - head_timer->call(); - head_timer->execute_callback(); + auto data = head_timer->call(); + if (!data) { + // someone canceled the timer between is_ready and call + return false; + } + head_timer->execute_callback(data); timers_heap.heapify_root(); weak_timers_heap_.store(timers_heap); } @@ -157,7 +161,9 @@ bool TimersManager::execute_head_timer() return timer_ready; } -void TimersManager::execute_ready_timer(const rclcpp::TimerBase * timer_id) +void TimersManager::execute_ready_timer( + const rclcpp::TimerBase * timer_id, + const std::shared_ptr & data) { TimerPtr ready_timer; { @@ -165,7 +171,7 @@ void TimersManager::execute_ready_timer(const rclcpp::TimerBase * timer_id) ready_timer = weak_timers_heap_.get_timer(timer_id); } if (ready_timer) { - ready_timer->execute_callback(); + ready_timer->execute_callback(data); } } @@ -215,11 +221,16 @@ void TimersManager::execute_ready_timers_unsafe() const size_t number_ready_timers = locked_heap.get_number_ready_timers(); size_t executed_timers = 0; while (executed_timers < number_ready_timers && head_timer->is_ready()) { - head_timer->call(); - if (on_ready_callback_) { - on_ready_callback_(head_timer.get()); + auto data = head_timer->call(); + if (data) { + if (on_ready_callback_) { + on_ready_callback_(head_timer.get(), data); + } else { + head_timer->execute_callback(data); + } } else { - head_timer->execute_callback(); + // someone canceled the timer between is_ready and call + // we don't do anything, as the timer is now 'processed' } executed_timers++; diff --git a/rclcpp/test/rclcpp/executors/test_events_queue.cpp b/rclcpp/test/rclcpp/executors/test_events_queue.cpp index de8242b55b..741e6ad384 100644 --- a/rclcpp/test/rclcpp/executors/test_events_queue.cpp +++ b/rclcpp/test/rclcpp/executors/test_events_queue.cpp @@ -68,6 +68,7 @@ TEST(TestEventsQueue, SimpleQueueTest) // Lets push an event into the queue and get it back rclcpp::experimental::executors::ExecutorEvent push_event = { simple_queue.get(), + nullptr, 99, rclcpp::experimental::executors::ExecutorEventType::SUBSCRIPTION_EVENT, 1}; diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp index dc92dee610..ce6343fb5c 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp @@ -32,8 +32,8 @@ class TestTimer : public rclcpp::TimerBase : TimerBase(node->get_clock(), std::chrono::nanoseconds(1), node->get_node_base_interface()->get_context()) {} - bool call() override {return true;} - void execute_callback() override {} + std::shared_ptr call() override {return nullptr;} + void execute_callback(const std::shared_ptr &) override {} bool is_steady() override {return false;} }; diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index 6d26bc54b8..55c0722ae1 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -252,6 +252,28 @@ TEST_P(TestTimer, callback_with_timer) { EXPECT_FALSE(timer_ptr->is_ready()); } +TEST_P(TestTimer, callback_with_timer_info) { + rclcpp::TimerInfo info; + auto timer_callback = [&info](const rclcpp::TimerInfo & timer_info) { + info = timer_info; + }; + switch (timer_type) { + case TimerType::WALL_TIMER: + timer = test_node->create_wall_timer(1ms, timer_callback); + break; + case TimerType::GENERIC_TIMER: + timer = test_node->create_timer(1ms, timer_callback); + break; + } + auto start = std::chrono::steady_clock::now(); + while (info.actual_call_time.nanoseconds() == 0 && + (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100)) + { + executor->spin_once(std::chrono::milliseconds(10)); + } + EXPECT_GE(info.actual_call_time, info.expected_call_time); +} + TEST_P(TestTimer, callback_with_period_zero) { rclcpp::TimerBase * timer_ptr = nullptr; auto timer_callback = [&timer_ptr](rclcpp::TimerBase & timer) { From dec22a296f7972edeabb610c663c196cd0050f60 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 12 Apr 2024 07:50:22 -0700 Subject: [PATCH 369/455] fixup var names to snake case (#2501) Signed-off-by: William Woodall --- rclcpp/include/rclcpp/executor.hpp | 2 +- rclcpp/include/rclcpp/timer.hpp | 6 +++--- rclcpp/src/rclcpp/executor.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 132b8150fe..d6b78fd133 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -487,7 +487,7 @@ class Executor */ RCLCPP_PUBLIC static void - execute_timer(rclcpp::TimerBase::SharedPtr timer, const std::shared_ptr & dataPtr); + execute_timer(rclcpp::TimerBase::SharedPtr timer, const std::shared_ptr & data_ptr); /// Run service server executable. /** diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index ad9e96ea40..0ed62007d4 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -326,10 +326,10 @@ class GenericTimer : public TimerBase >::type * = nullptr > void - execute_callback_delegate(const rcl_timer_call_info_t & timer_call_info_) + execute_callback_delegate(const rcl_timer_call_info_t & timer_call_info) { - const TimerInfo info{Time{timer_call_info_.expected_call_time, clock_->get_clock_type()}, - Time{timer_call_info_.actual_call_time, clock_->get_clock_type()}}; + const TimerInfo info{Time{timer_call_info.expected_call_time, clock_->get_clock_type()}, + Time{timer_call_info.actual_call_time, clock_->get_clock_type()}}; callback_(info); } diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 0b78e12912..537a88a2f3 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -547,9 +547,9 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) } void -Executor::execute_timer(rclcpp::TimerBase::SharedPtr timer, const std::shared_ptr & dataPtr) +Executor::execute_timer(rclcpp::TimerBase::SharedPtr timer, const std::shared_ptr & data_ptr) { - timer->execute_callback(dataPtr); + timer->execute_callback(data_ptr); } void From 90e451154cc23c4c231ca55dc103358f38cfd999 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 12 Apr 2024 14:57:15 -0700 Subject: [PATCH 370/455] [wjwwood] Updated "Data race fixes" (#2500) * Fix callback group logic in executor Signed-off-by: Janosch Machowinski * fix: Fixed unnecessary copy of wait_set Signed-off-by: Janosch Machowinski * fix(executor): Fixed race conditions with rebuild of wait_sets Before this change, the rebuild of wait set would be triggered after the wait set was waken up. With bad timing, this could lead to the rebuild not happening with multi threaded executor. Signed-off-by: Janosch Machowinski * fix(Executor): Fixed lost of entities rebuild request Signed-off-by: Janosch Machowinski * chore: Added assert for not set callback_group in execute_any_executable Signed-off-by: Janosch Machowinski * Add test for cbg getting reset Signed-off-by: Michael Carroll Co-authored-by: Janosch Machowinski * chore: renamed test cases to snake_case Signed-off-by: Janosch Machowinski * style Signed-off-by: William Woodall * fixup test to avoid polling and short timeouts Signed-off-by: William Woodall * fix: Use correct notify_waitable_ instance Signed-off-by: Janosch Machowinski * fix(StaticSingleThreadedExecutor): Added missing special case handling for current_notify_waitable_ Signed-off-by: Janosch Machowinski * fix(TestCallbackGroup): Fixed test after change to timers Signed-off-by: Janosch Machowinski --------- Signed-off-by: Janosch Machowinski Signed-off-by: Janosch Machowinski Signed-off-by: Michael Carroll Signed-off-by: William Woodall Co-authored-by: Janosch Machowinski Co-authored-by: Michael Carroll Co-authored-by: Janosch Machowinski --- rclcpp/include/rclcpp/executor.hpp | 9 + rclcpp/include/rclcpp/wait_result.hpp | 2 +- rclcpp/src/rclcpp/executor.cpp | 125 +++++++------- .../executor_entities_collection.cpp | 10 +- .../static_single_threaded_executor.cpp | 9 +- rclcpp/test/rclcpp/CMakeLists.txt | 9 + ...test_executors_callback_group_behavior.cpp | 156 ++++++++++++++++++ 7 files changed, 247 insertions(+), 73 deletions(-) create mode 100644 rclcpp/test/rclcpp/executors/test_executors_callback_group_behavior.cpp diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index d6b78fd133..7bdc53d251 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -550,6 +550,15 @@ class Executor AnyExecutable & any_executable, std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); + /// This function triggers a recollect of all entities that are registered to the executor. + /** + * Calling this function is thread safe. + * + * \param[in] notify if true will execute a trigger that will wake up a waiting executor + */ + void + trigger_entity_recollect(bool notify); + /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins. std::atomic_bool spinning; diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp index 74d3d2183c..3384a7846a 100644 --- a/rclcpp/include/rclcpp/wait_result.hpp +++ b/rclcpp/include/rclcpp/wait_result.hpp @@ -274,7 +274,7 @@ class WaitResult final if (this->kind() == WaitResultKind::Ready) { auto & wait_set = this->get_wait_set(); - auto rcl_wait_set = wait_set.get_rcl_wait_set(); + 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)) { diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 537a88a2f3..5df70a9465 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include #include @@ -72,13 +73,10 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) } }); - notify_waitable_->set_on_ready_callback( - [this](auto, auto) { - this->entities_need_rebuild_.store(true); - }); - notify_waitable_->add_guard_condition(interrupt_guard_condition_); notify_waitable_->add_guard_condition(shutdown_guard_condition_); + + wait_set_.add_waitable(notify_waitable_); } Executor::~Executor() @@ -122,6 +120,20 @@ Executor::~Executor() } } +void Executor::trigger_entity_recollect(bool notify) +{ + this->entities_need_rebuild_.store(true); + + if (!spinning.load() && entities_need_rebuild_.exchange(false)) { + std::lock_guard guard(mutex_); + this->collect_entities(); + } + + if (notify) { + interrupt_guard_condition_->trigger(); + } +} + std::vector Executor::get_all_callback_groups() { @@ -152,19 +164,12 @@ Executor::add_callback_group( (void) node_ptr; this->collector_.add_callback_group(group_ptr); - if (!spinning.load()) { - std::lock_guard guard(mutex_); - this->collect_entities(); - } - - if (notify) { - try { - interrupt_guard_condition_->trigger(); - } catch (const rclcpp::exceptions::RCLError & ex) { - throw std::runtime_error( - std::string( - "Failed to trigger guard condition on callback group add: ") + ex.what()); - } + try { + this->trigger_entity_recollect(notify); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on callback group add: ") + ex.what()); } } @@ -173,19 +178,12 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt { this->collector_.add_node(node_ptr); - if (!spinning.load()) { - std::lock_guard guard(mutex_); - this->collect_entities(); - } - - if (notify) { - try { - interrupt_guard_condition_->trigger(); - } catch (const rclcpp::exceptions::RCLError & ex) { - throw std::runtime_error( - std::string( - "Failed to trigger guard condition on node add: ") + ex.what()); - } + try { + this->trigger_entity_recollect(notify); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on node add: ") + ex.what()); } } @@ -196,18 +194,12 @@ Executor::remove_callback_group( { this->collector_.remove_callback_group(group_ptr); - if (!spinning.load()) { - std::lock_guard guard(mutex_); - this->collect_entities(); - } - if (notify) { - try { - interrupt_guard_condition_->trigger(); - } catch (const rclcpp::exceptions::RCLError & ex) { - throw std::runtime_error( - std::string( - "Failed to trigger guard condition on callback group remove: ") + ex.what()); - } + try { + this->trigger_entity_recollect(notify); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on callback group remove: ") + ex.what()); } } @@ -222,19 +214,12 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node { this->collector_.remove_node(node_ptr); - if (!spinning.load()) { - std::lock_guard guard(mutex_); - this->collect_entities(); - } - - if (notify) { - try { - interrupt_guard_condition_->trigger(); - } catch (const rclcpp::exceptions::RCLError & ex) { - throw std::runtime_error( - std::string( - "Failed to trigger guard condition on node remove: ") + ex.what()); - } + try { + this->trigger_entity_recollect(notify); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on node remove: ") + ex.what()); } } @@ -379,6 +364,10 @@ Executor::execute_any_executable(AnyExecutable & any_exec) return; } + assert( + (void("cannot execute an AnyExecutable without a valid callback group"), + any_exec.callback_group)); + if (any_exec.timer) { TRACETOOLS_TRACEPOINT( rclcpp_executor_execute, @@ -403,9 +392,7 @@ Executor::execute_any_executable(AnyExecutable & any_exec) } // Reset the callback_group, regardless of type - if (any_exec.callback_group) { - any_exec.callback_group->can_be_taken_from().store(true); - } + any_exec.callback_group->can_be_taken_from().store(true); } template @@ -642,7 +629,6 @@ Executor::collect_entities() // In the case that an entity already has an expired weak pointer // before being removed from the waitset, additionally prune the waitset. this->wait_set_.prune_deleted_entities(); - this->entities_need_rebuild_.store(false); } void @@ -655,7 +641,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) { std::lock_guard guard(mutex_); - if (current_collection_.empty() || this->entities_need_rebuild_.load()) { + if (this->entities_need_rebuild_.exchange(false) || current_collection_.empty()) { this->collect_entities(); } } @@ -664,6 +650,13 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) RCUTILS_LOG_WARN_NAMED( "rclcpp", "empty wait set received in wait(). This should never happen."); + } else { + if (this->wait_result_->kind() == WaitResultKind::Ready && current_notify_waitable_) { + auto & rcl_wait_set = this->wait_result_->get_wait_set().get_rcl_wait_set(); + if (current_notify_waitable_->is_ready(rcl_wait_set)) { + current_notify_waitable_->execute(current_notify_waitable_->take_data()); + } + } } } @@ -689,7 +682,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) auto entity_iter = current_collection_.timers.find(timer->get_timer_handle().get()); if (entity_iter != current_collection_.timers.end()) { auto callback_group = entity_iter->second.callback_group.lock(); - if (callback_group && !callback_group->can_be_taken_from()) { + if (!callback_group || !callback_group->can_be_taken_from()) { current_timer_index++; continue; } @@ -719,7 +712,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) subscription->get_subscription_handle().get()); if (entity_iter != current_collection_.subscriptions.end()) { auto callback_group = entity_iter->second.callback_group.lock(); - if (callback_group && !callback_group->can_be_taken_from()) { + if (!callback_group || !callback_group->can_be_taken_from()) { continue; } any_executable.subscription = subscription; @@ -735,7 +728,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) auto entity_iter = current_collection_.services.find(service->get_service_handle().get()); if (entity_iter != current_collection_.services.end()) { auto callback_group = entity_iter->second.callback_group.lock(); - if (callback_group && !callback_group->can_be_taken_from()) { + if (!callback_group || !callback_group->can_be_taken_from()) { continue; } any_executable.service = service; @@ -751,7 +744,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) auto entity_iter = current_collection_.clients.find(client->get_client_handle().get()); if (entity_iter != current_collection_.clients.end()) { auto callback_group = entity_iter->second.callback_group.lock(); - if (callback_group && !callback_group->can_be_taken_from()) { + if (!callback_group || !callback_group->can_be_taken_from()) { continue; } any_executable.client = client; @@ -767,7 +760,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable) auto entity_iter = current_collection_.waitables.find(waitable.get()); if (entity_iter != current_collection_.waitables.end()) { auto callback_group = entity_iter->second.callback_group.lock(); - if (callback_group && !callback_group->can_be_taken_from()) { + if (!callback_group || !callback_group->can_be_taken_from()) { continue; } any_executable.waitable = waitable; diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index 765002b2ef..68ac56b656 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -153,7 +153,7 @@ ready_executables( continue; } auto group_info = group_cache(entity_iter->second.callback_group); - if (group_info && !group_info->can_be_taken_from().load()) { + if (!group_info || !group_info->can_be_taken_from().load()) { continue; } if (!entity->call()) { @@ -176,7 +176,7 @@ ready_executables( continue; } auto group_info = group_cache(entity_iter->second.callback_group); - if (group_info && !group_info->can_be_taken_from().load()) { + if (!group_info || !group_info->can_be_taken_from().load()) { continue; } rclcpp::AnyExecutable exec; @@ -196,7 +196,7 @@ ready_executables( continue; } auto group_info = group_cache(entity_iter->second.callback_group); - if (group_info && !group_info->can_be_taken_from().load()) { + if (!group_info || !group_info->can_be_taken_from().load()) { continue; } rclcpp::AnyExecutable exec; @@ -216,7 +216,7 @@ ready_executables( continue; } auto group_info = group_cache(entity_iter->second.callback_group); - if (group_info && !group_info->can_be_taken_from().load()) { + if (!group_info || !group_info->can_be_taken_from().load()) { continue; } rclcpp::AnyExecutable exec; @@ -236,7 +236,7 @@ ready_executables( continue; } auto group_info = group_cache(entry.callback_group); - if (group_info && !group_info->can_be_taken_from().load()) { + if (!group_info || !group_info->can_be_taken_from().load()) { continue; } rclcpp::AnyExecutable exec; diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 2d837103e5..d517ccafd0 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -110,7 +110,7 @@ StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout) std::optional> StaticSingleThreadedExecutor::collect_and_wait(std::chrono::nanoseconds timeout) { - if (current_collection_.empty() || this->entities_need_rebuild_.load()) { + if (this->entities_need_rebuild_.exchange(false) || current_collection_.empty()) { this->collect_entities(); } auto wait_result = wait_set_.wait(std::chrono::nanoseconds(timeout)); @@ -119,6 +119,13 @@ StaticSingleThreadedExecutor::collect_and_wait(std::chrono::nanoseconds timeout) "rclcpp", "empty wait set received in wait(). This should never happen."); return {}; + } else { + if (wait_result.kind() == WaitResultKind::Ready && current_notify_waitable_) { + auto & rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set(); + if (current_notify_waitable_->is_ready(rcl_wait_set)) { + current_notify_waitable_->execute(current_notify_waitable_->take_data()); + } + } } return wait_result; } diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index d52c94b1d2..ea29f7a288 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -473,6 +473,15 @@ if(TARGET test_executors) target_link_libraries(test_executors_timer_cancel_behavior ${PROJECT_NAME} ${rosgraph_msgs_TARGETS}) endif() +ament_add_gtest( + test_executors_callback_group_behavior + executors/test_executors_callback_group_behavior.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) +if(TARGET test_executors) + target_link_libraries(test_executors_callback_group_behavior ${PROJECT_NAME}) +endif() + ament_add_gtest( test_executors_intraprocess executors/test_executors_intraprocess.cpp diff --git a/rclcpp/test/rclcpp/executors/test_executors_callback_group_behavior.cpp b/rclcpp/test/rclcpp/executors/test_executors_callback_group_behavior.cpp new file mode 100644 index 0000000000..49391cd838 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executors_callback_group_behavior.cpp @@ -0,0 +1,156 @@ +// Copyright 2024 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. + +/** + * This test checks that when callback groups go out of scope, that their associated executable + * entities should not be returned as valid executables. + * + * The test makes use of a bit of executor internals, but is meant to prevent regressions of behavior. + * Ref: https://github.com/ros2/rclcpp/issues/2474 + */ + +#include + +#include +#include + +#include +#include +#include + +std::chrono::milliseconds g_timer_period {1}; + +class CustomExecutor : public rclcpp::Executor +{ +public: + explicit CustomExecutor(const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions()) + : rclcpp::Executor(options) + {} + + ~CustomExecutor() override = default; + + void spin() override {} + + void collect() + { + this->collect_entities(); + } + + void wait() + { + this->wait_for_work(g_timer_period * 10); + } + + size_t collected_timers() const + { + return this->current_collection_.timers.size(); + } + + rclcpp::AnyExecutable next() + { + rclcpp::AnyExecutable ret; + this->get_next_ready_executable(ret); + return ret; + } +}; + + +TEST(TestCallbackGroup, valid_callback_group) +{ + rclcpp::init(0, nullptr); + + // Create a timer associated with a callback group + auto node = std::make_shared("node"); + + std::promise promise; + std::future future = promise.get_future(); + auto timer_callback = [&promise]() { + promise.set_value(); + }; + + // Add the callback group to the executor + auto executor = CustomExecutor(); + auto cbg = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, true); + auto timer = node->create_wall_timer(g_timer_period, timer_callback, cbg); + executor.add_callback_group(cbg, node->get_node_base_interface()); + + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + executor.spin_until_future_complete(future, std::chrono::seconds(10))); + + // Collect the entities + executor.collect(); + EXPECT_EQ(1u, executor.collected_timers()); + + executor.wait(); + auto next_executable = executor.next(); + EXPECT_EQ(timer, next_executable.timer); + EXPECT_EQ(cbg, next_executable.callback_group); + EXPECT_NE(nullptr, next_executable.data); + + EXPECT_EQ(nullptr, next_executable.client); + EXPECT_EQ(nullptr, next_executable.service); + EXPECT_EQ(nullptr, next_executable.subscription); + EXPECT_EQ(nullptr, next_executable.waitable); + + rclcpp::shutdown(); +} + +TEST(TestCallbackGroup, invalid_callback_group) +{ + rclcpp::init(0, nullptr); + + // Create a timer associated with a callback group + auto node = std::make_shared("node"); + + std::promise promise; + std::future future = promise.get_future(); + auto timer_callback = [&promise]() { + promise.set_value(); + }; + + // Add the callback group to the executor + auto executor = CustomExecutor(); + auto cbg = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + auto timer = node->create_wall_timer(g_timer_period, timer_callback, cbg); + executor.add_callback_group(cbg, node->get_node_base_interface()); + + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + executor.spin_until_future_complete(future, std::chrono::seconds(10))); + + // Collect the entities + executor.collect(); + EXPECT_EQ(1u, executor.collected_timers()); + + executor.wait(); + + cbg.reset(); + + // Since the callback group has been reset, this should not be allowed to + // be a valid executable (timer and cbg should both be nullptr). + // In the regression, timer == next_executable.timer whil + // next_executable.callback_group == nullptr, which was incorrect. + auto next_executable = executor.next(); + EXPECT_EQ(nullptr, next_executable.timer); + EXPECT_EQ(nullptr, next_executable.callback_group); + + EXPECT_EQ(nullptr, next_executable.client); + EXPECT_EQ(nullptr, next_executable.service); + EXPECT_EQ(nullptr, next_executable.subscription); + EXPECT_EQ(nullptr, next_executable.waitable); + EXPECT_EQ(nullptr, next_executable.data); + + rclcpp::shutdown(); +} From 839348c60174049da974faecce1d83cc0b07ff90 Mon Sep 17 00:00:00 2001 From: jmachowinski Date: Mon, 15 Apr 2024 19:05:17 +0200 Subject: [PATCH 371/455] Callback after cancel (#2281) * feat(Client): Added function to stop callbacks of a goal handle This function allows us to drop the handle in a locked context. If we do not do this within a lock, there will be a race condition between the deletion of the shared_ptr of the handle and the result / feedback callbacks. * fix: make Client goal handle recursive This fixes deadlocks due to release of goal handles in callbacks etc. * fix(ActionGoalClient): Fixed memory leak for nominal case This fixes a memory leak due to a self reference in the ClientGoalHandle. Note, this fix will only work, if the ClientGoalHandle ever receives a result callback. * doc: Updated documentation of rclcpp_action::Client::async_send_goal * docs: Made the async_send_goal documentation more explicit Signed-off-by: Janosch Machowinski Co-authored-by: Janosch Machowinski --- .../include/rclcpp_action/client.hpp | 93 ++++++++++++++++--- .../rclcpp_action/client_goal_handle_impl.hpp | 1 + 2 files changed, 81 insertions(+), 13 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 94cf4aab31..2993f28bc3 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -405,12 +405,22 @@ class Client : public ClientBase /// Send an action goal and asynchronously get the result. /** - * If the goal is accepted by an action server, the returned future is set to a `ClientGoalHandle`. + * If the goal is accepted by an action server, the returned future is set to a `GoalHandle::SharedPtr`. * If the goal is rejected by an action server, then the future is set to a `nullptr`. * - * The returned goal handle is used to monitor the status of the goal and get the final result. - * It is valid as long as you hold a reference to the shared pointer or until the - * rclcpp_action::Client is destroyed at which point the goal status will become UNKNOWN. + * The goal handle in the future is used to monitor the status of the goal and get the final result. + * + * If callbacks were set in @param options, you will receive callbacks, as long as you hold a reference + * to the shared pointer contained in the returned future, or rclcpp_action::Client is destroyed. Dropping + * the shared pointer to the goal handle will not cancel the goal. In order to cancel it, you must explicitly + * call async_cancel_goal. + * + * WARNING this method has inconsistent behaviour and a memory leak bug. + * If you set the result callback in @param options, the handle will be self referencing, and you will receive + * callbacks even though you do not hold a reference to the shared pointer. In this case, the self reference will + * be deleted if the result callback was received. If there is no result callback, there will be a memory leak. + * + * To prevent the memory leak, you may call stop_callbacks() explicit. This will delete the self reference. * * \param[in] goal The goal request. * \param[in] options Options for sending the goal request. Contains references to callbacks for @@ -448,7 +458,7 @@ class Client : public ClientBase std::shared_ptr goal_handle( new GoalHandle(goal_info, options.feedback_callback, options.result_callback)); { - std::lock_guard guard(goal_handles_mutex_); + std::lock_guard guard(goal_handles_mutex_); goal_handles_[goal_handle->get_goal_id()] = goal_handle; } promise->set_value(goal_handle); @@ -466,7 +476,7 @@ class Client : public ClientBase // To prevent the list from growing out of control, forget about any goals // with no more user references { - std::lock_guard guard(goal_handles_mutex_); + std::lock_guard guard(goal_handles_mutex_); auto goal_handle_it = goal_handles_.begin(); while (goal_handle_it != goal_handles_.end()) { if (!goal_handle_it->second.lock()) { @@ -496,7 +506,7 @@ class Client : public ClientBase typename GoalHandle::SharedPtr goal_handle, ResultCallback result_callback = nullptr) { - std::lock_guard lock(goal_handles_mutex_); + std::lock_guard lock(goal_handles_mutex_); if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { throw exceptions::UnknownGoalHandleError(); } @@ -531,7 +541,7 @@ class Client : public ClientBase typename GoalHandle::SharedPtr goal_handle, CancelCallback cancel_callback = nullptr) { - std::lock_guard lock(goal_handles_mutex_); + std::lock_guard lock(goal_handles_mutex_); if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { throw exceptions::UnknownGoalHandleError(); } @@ -562,6 +572,63 @@ class Client : public ClientBase return async_cancel(cancel_request, cancel_callback); } + /// Stops the callbacks for the goal in a thread safe way + /** + * This will NOT cancel the goal, it will only stop the callbacks. + * + * After the call to this function, it is guaranteed that there + * will be no more callbacks from the goal. This is not guaranteed + * if multiple threads are involved, and the goal_handle is just + * dropped. + * + * \param[in] goal_handle The goal were the callbacks shall be stopped + */ + void stop_callbacks(typename GoalHandle::SharedPtr goal_handle) + { + goal_handle->set_feedback_callback(typename GoalHandle::FeedbackCallback()); + goal_handle->set_result_callback(typename GoalHandle::ResultCallback()); + + std::lock_guard guard(goal_handles_mutex_); + const GoalUUID & goal_id = goal_handle->get_goal_id(); + auto it = goal_handles_.find(goal_id); + if (goal_handles_.end() == it) { + // someone else already deleted the entry + // e.g. the result callback + RCLCPP_DEBUG( + this->get_logger(), + "Given goal is unknown. Ignoring..."); + return; + } + goal_handles_.erase(it); + } + + /// Stops the callbacks for the goal in a thread safe way + /** + * For futher information see stop_callbacks(typename GoalHandle::SharedPtr goal_handle) + */ + void stop_callbacks(const GoalUUID & goal_id) + { + typename GoalHandle::SharedPtr goal_handle; + { + std::lock_guard guard(goal_handles_mutex_); + auto it = goal_handles_.find(goal_id); + if (goal_handles_.end() == it) { + // someone else already deleted the entry + // e.g. the result callback + RCLCPP_DEBUG( + this->get_logger(), + "Given goal is unknown. Ignoring..."); + return; + } + + goal_handle = it->lock(); + } + + if (goal_handle) { + stop_callbacks(goal_handle); + } + } + /// Asynchronously request all goals at or before a specified time be canceled. /** * \param[in] stamp The timestamp for the cancel goal request. @@ -590,7 +657,7 @@ class Client : public ClientBase virtual ~Client() { - std::lock_guard guard(goal_handles_mutex_); + std::lock_guard guard(goal_handles_mutex_); auto it = goal_handles_.begin(); while (it != goal_handles_.end()) { typename GoalHandle::SharedPtr goal_handle = it->second.lock(); @@ -637,7 +704,7 @@ class Client : public ClientBase void handle_feedback_message(std::shared_ptr message) override { - std::lock_guard guard(goal_handles_mutex_); + std::lock_guard guard(goal_handles_mutex_); using FeedbackMessage = typename ActionT::Impl::FeedbackMessage; typename FeedbackMessage::SharedPtr feedback_message = std::static_pointer_cast(message); @@ -674,7 +741,7 @@ class Client : public ClientBase void handle_status_message(std::shared_ptr message) override { - std::lock_guard guard(goal_handles_mutex_); + std::lock_guard guard(goal_handles_mutex_); using GoalStatusMessage = typename ActionT::Impl::GoalStatusMessage; auto status_message = std::static_pointer_cast(message); for (const GoalStatus & status : status_message->status_list) { @@ -723,7 +790,7 @@ class Client : public ClientBase wrapped_result.goal_id = goal_handle->get_goal_id(); wrapped_result.code = static_cast(result_response->status); goal_handle->set_result(wrapped_result); - std::lock_guard lock(goal_handles_mutex_); + std::lock_guard lock(goal_handles_mutex_); goal_handles_.erase(goal_handle->get_goal_id()); }); } catch (rclcpp::exceptions::RCLError & ex) { @@ -755,7 +822,7 @@ class Client : public ClientBase } std::map goal_handles_; - std::mutex goal_handles_mutex_; + std::recursive_mutex goal_handles_mutex_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index d12b4fc5b3..58b1d7f248 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -75,6 +75,7 @@ ClientGoalHandle::set_result(const WrappedResult & wrapped_result) result_promise_.set_value(wrapped_result); if (result_callback_) { result_callback_(wrapped_result); + result_callback_ = ResultCallback(); } } From 1351737f345ca333269b4b644091188944e7b473 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 16 Apr 2024 15:42:55 +0200 Subject: [PATCH 372/455] Changelog MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- rclcpp/CHANGELOG.rst | 135 ++++++++++++++++++++++++++++++++ rclcpp_action/CHANGELOG.rst | 27 +++++++ rclcpp_components/CHANGELOG.rst | 3 + rclcpp_lifecycle/CHANGELOG.rst | 8 ++ 4 files changed, 173 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index c02354634b..483add40ad 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,141 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +28.0.1 (2024-04-16) +------------------- +* [wjwwood] Updated "Data race fixes" (`#2500 `_) + * Fix callback group logic in executor + * fix: Fixed unnecessary copy of wait_set + * fix(executor): Fixed race conditions with rebuild of wait_sets + Before this change, the rebuild of wait set would be triggered + after the wait set was waken up. With bad timing, this could + lead to the rebuild not happening with multi threaded executor. + * fix(Executor): Fixed lost of entities rebuild request + * chore: Added assert for not set callback_group in execute_any_executable + * Add test for cbg getting reset + Co-authored-by: Janosch Machowinski + * chore: renamed test cases to snake_case + * style + * fixup test to avoid polling and short timeouts + * fix: Use correct notify_waitable\_ instance + * fix(StaticSingleThreadedExecutor): Added missing special case handling for current_notify_waitable\_ + * fix(TestCallbackGroup): Fixed test after change to timers + --------- + Co-authored-by: Janosch Machowinski + Co-authored-by: Michael Carroll + Co-authored-by: Janosch Machowinski +* fixup var names to snake case (`#2501 `_) +* Added optional TimerInfo to timer callback (`#2343 `_) + Co-authored-by: Alexis Tsogias + Co-authored-by: Janosch Machowinski +* Fix uninitialized memory in test (`#2498 `_) + When I added in the tests for large messages, I made a mistake and reserved space in the strings, but didn't actually expand it. Thus, we were writing into uninitialized memory. Fix this by just using the correct constructor for string, which will allocate and initialize the memory properly. +* Ensure waitables handle guard condition retriggering (`#2483 `_) + Co-authored-by: Michael Carroll +* fix: init concatenated_vector with begin() & end() (`#2492 `_) + *. this commit will fix the warning [-Wstringop-overflow=] `#2461 `_ +* Use the same context for the specified node in rclcpp::spin functions (`#2433 `_) + * Use the same conext for the specified node in rclcpp::spin_xx functions + * Add test for spinning with non-default-context + * Format code + --------- +* Disable compare-function-pointers in test_utilities (`#2489 `_) +* address ambiguous auto variable. (`#2481 `_) +* Increase the cppcheck timeout to 1200 seconds (`#2484 `_) +* Removed test_timers_manager clang warning (`#2479 `_) +* Flaky timer test fix (`#2469 `_) + * fix(time_source): Fixed possible race condition + * fix(test_executors_time_cancel_behaviour): Fixed multiple race conditions + --------- + Co-authored-by: Janosch Machowinski +* Add tracepoint for generic publisher/subscriber (`#2448 `_) +* update rclcpp::Waitable API to use references and const (`#2467 `_) +* Utilize rclcpp::WaitSet as part of the executors (`#2142 `_) + * Deprecate callback_group call taking context + * Add base executor objects that can be used by implementors + * Template common operations + * Address reviewer feedback: + * Add callback to EntitiesCollector constructor + * Make function to check automatically added callback groups take a list + * Lint + * Address reviewer feedback and fix templates + * Lint and docs + * Make executor own the notify waitable + * Add pending queue to collector, remove from waitable + Also change node's get_guard_condition to return shared_ptr + * Change interrupt guard condition to shared_ptr + Check if guard condition is valid before adding it to the waitable + * Lint and docs + * Utilize rclcpp::WaitSet as part of the executors + * Don't exchange atomic twice + * Fix add_node and add more tests + * Make get_notify_guard_condition follow API tick-tock + * Improve callback group tick-tocking + * Don't lock twice + * Address reviewer feedback + * Add thread safety annotations and make locks consistent + * @wip + * Reset callback groups for multithreaded executor + * Avoid many small function calls when building executables + * Re-trigger guard condition if buffer has data + * Address reviewer feedback + * Trace points + * Remove tracepoints + * Reducing diff + * Reduce diff + * Uncrustify + * Restore tests + * Back to weak_ptr and reduce test time + * reduce diff and lint + * Restore static single threaded tests that weren't working before + * Restore more tests + * Fix multithreaded test + * Fix assert + * Fix constructor test + * Change ready_executables signature back + * Don't enforce removing callback groups before nodes + * Remove the "add_valid_node" API + * Only notify if the trigger condition is valid + * Only trigger if valid and needed + * Fix spin_some/spin_all implementation + * Restore single threaded executor + * Picking ABI-incompatible executor changes + * Add PIMPL + * Additional waitset prune + * Fix bad merge + * Expand test timeout + * Introduce method to clear expired entities from a collection + * Make sure to call remove_expired_entities(). + * Prune queued work when callback group is removed + * Prune subscriptions from dynamic storage + * Styles fixes. + * Re-trigger guard conditions + * Condense to just use watiable.take_data + * Lint + * Address reviewer comments (nits) + * Lock mutex when copying + * Refactors to static single threaded based on reviewers + * More small refactoring + * Lint + * Lint + * Add ready executable accessors to WaitResult + * Make use of accessors from wait_set + * Fix tests + * Fix more tests + * Tidy up single threaded executor implementation + * Don't null out timer, rely on call + * change how timers are checked from wait result in executors + * peak -> peek + * fix bug in next_waitable logic + * fix bug in StaticSTE that broke the add callback groups to executor tests + * style + --------- + Co-authored-by: Chris Lalancette + Co-authored-by: William Woodall +* fix flakiness in TestTimersManager unit-test (`#2468 `_) + the previous version of the test was relying on the assumption that a timer with 1ms period gets called at least 6 times if the main thread waits 15ms. this is true most of the times, but it's not guaranteed, especially when running the test on windows CI servers. the new version of the test makes no assumptions on how much time it takes for the timers manager to invoke the timers, but rather focuses on ensuring that they are called the right amount of times, which is what's important for the purpose of the test +* Contributors: Alberto Soragna, Alejandro Hernández Cordero, Chris Lalancette, Homalozoa X, Kotaro Yoshimoto, Michael Carroll, Tomoya Fujita, William Woodall, h-suzuki-isp, jmachowinski + 28.0.0 (2024-03-28) ------------------- * fix spin_some_max_duration unit-test for events-executor (`#2465 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index b7b7a7e4c7..39e8003699 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,33 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +28.0.1 (2024-04-16) +------------------- +* Callback after cancel (`#2281 `_) + * feat(Client): Added function to stop callbacks of a goal handle + This function allows us to drop the handle in a locked context. + If we do not do this within a lock, there will be a race condition between + the deletion of the shared_ptr of the handle and the result / feedback + callbacks. + * fix: make Client goal handle recursive + This fixes deadlocks due to release of goal handles in callbacks etc. + * fix(ActionGoalClient): Fixed memory leak for nominal case + This fixes a memory leak due to a self reference in the ClientGoalHandle. + Note, this fix will only work, if the ClientGoalHandle ever receives + a result callback. + * doc: Updated documentation of rclcpp_action::Client::async_send_goal + * docs: Made the async_send_goal documentation more explicit + Co-authored-by: Janosch Machowinski +* Remake of "fix: Fixed race condition in action server between is_ready and take" (`#2495 `_) + Some background information: is_ready, take_data and execute data + may be called from different threads in any order. The code in the old + state expected them to be called in series, without interruption. + This lead to multiple race conditions, as the state of the pimpl objects + was altered by the three functions in a non thread safe way. + Co-authored-by: Janosch Machowinski +* update rclcpp::Waitable API to use references and const (`#2467 `_) +* Contributors: William Woodall, jmachowinski + 28.0.0 (2024-03-28) ------------------- * Do not generate the exception when action service response timeout. (`#2464 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index ad7db5096e..fd9a5d0302 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +28.0.1 (2024-04-16) +------------------- + 28.0.0 (2024-03-28) ------------------- * Add EXECUTOR docs (`#2440 `_) diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 0e8c604097..64b059593b 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,14 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +28.0.1 (2024-04-16) +------------------- +* call shutdown in LifecycleNode dtor to avoid leaving the device in un… (`#2450 `_) + * call shutdown in LifecycleNode dtor to avoid leaving the device in unknown state. + * add test to verify LifecycleNode::shutdown is called on destructor. + --------- +* Contributors: Tomoya Fujita + 28.0.0 (2024-03-28) ------------------- * Update quality declaration documents (`#2427 `_) From 535d56f690164dcfcca145d2bc3c5fca18234631 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 16 Apr 2024 15:43:00 +0200 Subject: [PATCH 373/455] 28.0.1 --- rclcpp/package.xml | 2 +- rclcpp_action/package.xml | 2 +- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/package.xml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 103b1c1c33..451acd8926 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 28.0.0 + 28.0.1 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 8c1dd30127..4b7b77a59a 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 28.0.0 + 28.0.1 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index e309698b84..a599d1d49a 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 28.0.0 + 28.0.1 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 6a2157c002..62e6800d58 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 28.0.0 + 28.0.1 Package containing a prototype for lifecycle implementation Ivan Paunovic From 6bb9407b9079eeaaef005613cef3a5a1224f1a36 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 16 Apr 2024 08:35:56 -0700 Subject: [PATCH 374/455] Reduce overhead for inheriting from rclcpp::Executor when base functionality is not reused (#2506) * feat(clock): Added function to interrupt sleep This is useful in case a second thread needs to wake up another thread, that is sleeping using a clock. Signed-off-by: Janosch Machowinski * feat: Added support for multi thread wait / shutdown This adds support for multiple threads waiting on the same clock, while an shutdown is invoked. Signed-off-by: Janosch Machowinski * chore: Made Executor fully overloadable This commit makes every public funciton virtual, and adds virtual impl function for the existing template functions. The goal of this commit is to be able to fully control the everything from a derived class. Signed-off-by: Janosch Machowinski * Update rclcpp/include/rclcpp/executor.hpp Co-authored-by: William Woodall Signed-off-by: jmachowinski * Update rclcpp/include/rclcpp/executor.hpp Co-authored-by: William Woodall Signed-off-by: jmachowinski * Update rclcpp/include/rclcpp/executor.hpp Co-authored-by: William Woodall Signed-off-by: jmachowinski * Update rclcpp/include/rclcpp/executor.hpp Co-authored-by: William Woodall Signed-off-by: jmachowinski * docs: added doc string for spin_until_future_complete_impl Signed-off-by: Janosch Machowinski * made is_spinning not virtual Signed-off-by: Janosch Machowinski * feat: Changed interface of spin_until_future_complete_impl This change allows it to use a second thread to wait for the future to become ready. Signed-off-by: Janosch Machowinski * doc fixup Signed-off-by: William Woodall * undo template->implicit conversion change Signed-off-by: William Woodall * style Signed-off-by: William Woodall --------- Signed-off-by: Janosch Machowinski Signed-off-by: Janosch Machowinski Signed-off-by: jmachowinski Signed-off-by: William Woodall Co-authored-by: Janosch Machowinski Co-authored-by: jmachowinski --- rclcpp/include/rclcpp/clock.hpp | 10 ++ rclcpp/include/rclcpp/executor.hpp | 85 +++++------ rclcpp/src/rclcpp/clock.cpp | 71 ++++++--- rclcpp/src/rclcpp/executor.cpp | 64 +++++++- rclcpp/test/rclcpp/CMakeLists.txt | 4 + rclcpp/test/rclcpp/test_clock.cpp | 229 +++++++++++++++++++++++++++++ 6 files changed, 390 insertions(+), 73 deletions(-) create mode 100644 rclcpp/test/rclcpp/test_clock.cpp diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 702b224d53..e73f3849d3 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -193,6 +193,16 @@ class Clock bool ros_time_is_active(); + /** + * Cancels an ongoing or future sleep operation of one thread. + * + * This function can be used by one thread, to wakeup another thread that is + * blocked using any of the sleep_ or wait_ methods of this class. + */ + RCLCPP_PUBLIC + void + cancel_sleep_or_wait(); + /// Return the rcl_clock_t clock handle RCLCPP_PUBLIC rcl_clock_t * diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 7bdc53d251..5e5a054814 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -272,12 +272,12 @@ class Executor * \param[in] node Shared pointer to the node to add. */ RCLCPP_PUBLIC - void + virtual void spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node); /// Convenience function which takes Node and forwards NodeBaseInterface. RCLCPP_PUBLIC - void + virtual void spin_node_some(std::shared_ptr node); /// Collect work once and execute all available work, optionally within a max duration. @@ -307,14 +307,14 @@ class Executor * \param[in] node Shared pointer to the node to add. */ RCLCPP_PUBLIC - void + virtual void spin_node_all( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, std::chrono::nanoseconds max_duration); /// Convenience function which takes Node and forwards NodeBaseInterface. RCLCPP_PUBLIC - void + virtual void spin_node_all(std::shared_ptr node, std::chrono::nanoseconds max_duration); /// Collect and execute work repeatedly within a duration or until no more work is available. @@ -366,52 +366,12 @@ class Executor const FutureT & future, std::chrono::duration timeout = std::chrono::duration(-1)) { - // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete - // inside a callback executed by an executor. - - // Check the future before entering the while loop. - // If the future is already complete, don't try to spin. - std::future_status status = future.wait_for(std::chrono::seconds(0)); - if (status == std::future_status::ready) { - return FutureReturnCode::SUCCESS; - } - - auto end_time = std::chrono::steady_clock::now(); - std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast( - timeout); - if (timeout_ns > std::chrono::nanoseconds::zero()) { - end_time += timeout_ns; - } - std::chrono::nanoseconds timeout_left = timeout_ns; - - if (spinning.exchange(true)) { - throw std::runtime_error("spin_until_future_complete() called while already spinning"); - } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); - while (rclcpp::ok(this->context_) && spinning.load()) { - // Do one item of work. - spin_once_impl(timeout_left); - - // Check if the future is set, return SUCCESS if it is. - status = future.wait_for(std::chrono::seconds(0)); - if (status == std::future_status::ready) { - return FutureReturnCode::SUCCESS; - } - // If the original timeout is < 0, then this is blocking, never TIMEOUT. - if (timeout_ns < std::chrono::nanoseconds::zero()) { - continue; + return spin_until_future_complete_impl( + std::chrono::duration_cast(timeout), + [&future](std::chrono::nanoseconds wait_time) { + return future.wait_for(wait_time); } - // Otherwise check if we still have time to wait, return TIMEOUT if not. - auto now = std::chrono::steady_clock::now(); - if (now >= end_time) { - return FutureReturnCode::TIMEOUT; - } - // Subtract the elapsed time from the original timeout. - timeout_left = std::chrono::duration_cast(end_time - now); - } - - // The future did not complete before ok() returned false, return INTERRUPTED. - return FutureReturnCode::INTERRUPTED; + ); } /// Cancel any running spin* function, causing it to return. @@ -420,7 +380,7 @@ class Executor * \throws std::runtime_error if there is an issue triggering the guard condition */ RCLCPP_PUBLIC - void + virtual void cancel(); /// Returns true if the executor is currently spinning. @@ -433,6 +393,14 @@ class Executor is_spinning(); protected: + /// Constructor that will not initialize any non-trivial members. + /** + * This constructor is intended to be used by any derived executor + * that explicitly does not want to use the default implementation provided + * by this class. + */ + explicit Executor(const std::shared_ptr & context); + /// Add a node to executor, execute the next available unit of work, and remove the node. /** * Implementation of spin_node_once using std::chrono::nanoseconds @@ -447,6 +415,23 @@ class Executor rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, std::chrono::nanoseconds timeout); + /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted. + /** + * \sa spin_until_future_complete() + * The only difference with spin_until_future_complete() is that the future's + * type is obscured through a std::function which lets you wait on it + * reguardless of type. + * + * \param[in] timeout see spin_until_future_complete() for details + * \param[in] wait_for_future function to wait on the future and get the + * status after waiting + */ + RCLCPP_PUBLIC + virtual FutureReturnCode + spin_until_future_complete_impl( + std::chrono::nanoseconds timeout, + const std::function & wait_for_future); + /// Collect work and execute available work, optionally within a duration. /** * Implementation of spin_some and spin_all. diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index f46649a77d..5c13f19d13 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -49,6 +49,10 @@ class Clock::Impl rcl_clock_t rcl_clock_; rcl_allocator_t allocator_; + bool stop_sleeping_ = false; + bool shutdown_ = false; + std::condition_variable cv_; + std::mutex wait_mutex_; std::mutex clock_mutex_; }; @@ -79,8 +83,20 @@ Clock::now() const return now; } +void +Clock::cancel_sleep_or_wait() +{ + { + std::unique_lock lock(impl_->wait_mutex_); + impl_->stop_sleeping_ = true; + } + impl_->cv_.notify_one(); +} + bool -Clock::sleep_until(Time until, Context::SharedPtr context) +Clock::sleep_until( + Time until, + Context::SharedPtr context) { if (!context || !context->is_valid()) { throw std::runtime_error("context cannot be slept with because it's invalid"); @@ -91,12 +107,14 @@ Clock::sleep_until(Time until, Context::SharedPtr context) } bool time_source_changed = false; - std::condition_variable cv; - // Wake this thread if the context is shutdown rclcpp::OnShutdownCallbackHandle shutdown_cb_handle = context->add_on_shutdown_callback( - [&cv]() { - cv.notify_one(); + [this]() { + { + std::unique_lock lock(impl_->wait_mutex_); + impl_->shutdown_ = true; + } + impl_->cv_.notify_one(); }); // No longer need the shutdown callback when this function exits auto callback_remover = rcpputils::scope_exit( @@ -112,22 +130,24 @@ Clock::sleep_until(Time until, Context::SharedPtr context) const std::chrono::steady_clock::time_point chrono_until = chrono_entry + std::chrono::nanoseconds(delta_t.nanoseconds()); - // loop over spurious wakeups but notice shutdown - std::unique_lock lock(impl_->clock_mutex_); - while (now() < until && context->is_valid()) { - cv.wait_until(lock, chrono_until); + // loop over spurious wakeups but notice shutdown or stop of sleep + std::unique_lock lock(impl_->wait_mutex_); + while (now() < until && !impl_->stop_sleeping_ && !impl_->shutdown_ && context->is_valid()) { + impl_->cv_.wait_until(lock, chrono_until); } + impl_->stop_sleeping_ = false; } else if (this_clock_type == RCL_SYSTEM_TIME) { auto system_time = std::chrono::system_clock::time_point( // Cast because system clock resolution is too big for nanoseconds on some systems std::chrono::duration_cast( std::chrono::nanoseconds(until.nanoseconds()))); - // loop over spurious wakeups but notice shutdown - std::unique_lock lock(impl_->clock_mutex_); - while (now() < until && context->is_valid()) { - cv.wait_until(lock, system_time); + // loop over spurious wakeups but notice shutdown or stop of sleep + std::unique_lock lock(impl_->wait_mutex_); + while (now() < until && !impl_->stop_sleeping_ && !impl_->shutdown_ && context->is_valid()) { + impl_->cv_.wait_until(lock, system_time); } + impl_->stop_sleeping_ = false; } else if (this_clock_type == RCL_ROS_TIME) { // Install jump handler for any amount of time change, for two purposes: // - if ROS time is active, check if time reached on each new clock sample @@ -139,11 +159,12 @@ Clock::sleep_until(Time until, Context::SharedPtr context) threshold.min_forward.nanoseconds = 1; auto clock_handler = create_jump_callback( nullptr, - [&cv, &time_source_changed](const rcl_time_jump_t & jump) { + [this, &time_source_changed](const rcl_time_jump_t & jump) { if (jump.clock_change != RCL_ROS_TIME_NO_CHANGE) { + std::lock_guard lk(impl_->wait_mutex_); time_source_changed = true; } - cv.notify_one(); + impl_->cv_.notify_one(); }, threshold); @@ -153,19 +174,25 @@ Clock::sleep_until(Time until, Context::SharedPtr context) std::chrono::duration_cast( std::chrono::nanoseconds(until.nanoseconds()))); - // loop over spurious wakeups but notice shutdown or time source change - std::unique_lock lock(impl_->clock_mutex_); - while (now() < until && context->is_valid() && !time_source_changed) { - cv.wait_until(lock, system_time); + // loop over spurious wakeups but notice shutdown, stop of sleep or time source change + std::unique_lock lock(impl_->wait_mutex_); + while (now() < until && !impl_->stop_sleeping_ && !impl_->shutdown_ && context->is_valid() && + !time_source_changed) + { + impl_->cv_.wait_until(lock, system_time); } + impl_->stop_sleeping_ = false; } else { // RCL_ROS_TIME with ros_time_is_active. // Just wait without "until" because installed // jump callbacks wake the cv on every new sample. - std::unique_lock lock(impl_->clock_mutex_); - while (now() < until && context->is_valid() && !time_source_changed) { - cv.wait(lock); + std::unique_lock lock(impl_->wait_mutex_); + while (now() < until && !impl_->stop_sleeping_ && !impl_->shutdown_ && context->is_valid() && + !time_source_changed) + { + impl_->cv_.wait(lock); } + impl_->stop_sleeping_ = false; } } diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 5df70a9465..42a4d274dc 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -50,6 +50,14 @@ static constexpr rclcpp::SubscriptionWaitSetMask kDefaultSubscriptionMask = {tru class rclcpp::ExecutorImplementation {}; +Executor::Executor(const std::shared_ptr & context) +: spinning(false), + entities_need_rebuild_(true), + collector_(nullptr), + wait_set_({}, {}, {}, {}, {}, {}, context) +{ +} + Executor::Executor(const rclcpp::ExecutorOptions & options) : spinning(false), interrupt_guard_condition_(std::make_shared(options.context)), @@ -120,7 +128,8 @@ Executor::~Executor() } } -void Executor::trigger_entity_recollect(bool notify) +void +Executor::trigger_entity_recollect(bool notify) { this->entities_need_rebuild_.store(true); @@ -240,6 +249,59 @@ Executor::spin_node_once_nanoseconds( this->remove_node(node, false); } +rclcpp::FutureReturnCode +Executor::spin_until_future_complete_impl( + std::chrono::nanoseconds timeout, + const std::function & wait_for_future) +{ + // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete + // inside a callback executed by an executor. + + // Check the future before entering the while loop. + // If the future is already complete, don't try to spin. + std::future_status status = wait_for_future(std::chrono::seconds(0)); + if (status == std::future_status::ready) { + return FutureReturnCode::SUCCESS; + } + + auto end_time = std::chrono::steady_clock::now(); + std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast( + timeout); + if (timeout_ns > std::chrono::nanoseconds::zero()) { + end_time += timeout_ns; + } + std::chrono::nanoseconds timeout_left = timeout_ns; + + if (spinning.exchange(true)) { + throw std::runtime_error("spin_until_future_complete() called while already spinning"); + } + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + while (rclcpp::ok(this->context_) && spinning.load()) { + // Do one item of work. + spin_once_impl(timeout_left); + + // Check if the future is set, return SUCCESS if it is. + status = wait_for_future(std::chrono::seconds(0)); + if (status == std::future_status::ready) { + return FutureReturnCode::SUCCESS; + } + // If the original timeout is < 0, then this is blocking, never TIMEOUT. + if (timeout_ns < std::chrono::nanoseconds::zero()) { + continue; + } + // Otherwise check if we still have time to wait, return TIMEOUT if not. + auto now = std::chrono::steady_clock::now(); + if (now >= end_time) { + return FutureReturnCode::TIMEOUT; + } + // Subtract the elapsed time from the original timeout. + timeout_left = std::chrono::duration_cast(end_time - now); + } + + // The future did not complete before ok() returned false, return INTERRUPTED. + return FutureReturnCode::INTERRUPTED; +} + void Executor::spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node) { diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index ea29f7a288..844ca630ac 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -55,6 +55,10 @@ ament_add_gtest(test_client test_client.cpp) if(TARGET test_client) target_link_libraries(test_client ${PROJECT_NAME} mimick ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS}) endif() +ament_add_gtest(test_clock test_clock.cpp) +if(TARGET test_clock) + target_link_libraries(test_clock ${PROJECT_NAME} mimick ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS}) +endif() ament_add_gtest(test_copy_all_parameter_values test_copy_all_parameter_values.cpp) if(TARGET test_copy_all_parameter_values) target_link_libraries(test_copy_all_parameter_values ${PROJECT_NAME}) diff --git a/rclcpp/test/rclcpp/test_clock.cpp b/rclcpp/test/rclcpp/test_clock.cpp new file mode 100644 index 0000000000..2ddd775707 --- /dev/null +++ b/rclcpp/test/rclcpp/test_clock.cpp @@ -0,0 +1,229 @@ +// Copyright 2024 Cellumation GmbH +// +// 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. + +#include + +#include + +#include "rcl/error_handling.h" +#include "rcl/time.h" +#include "rclcpp/clock.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time_source.hpp" + +#include "../utils/rclcpp_gtest_macros.hpp" + +using namespace std::chrono_literals; + +class TestClockWakeup : public ::testing::TestWithParam +{ +public: + void test_wakeup_before_sleep(const rclcpp::Clock::SharedPtr & clock) + { + std::atomic_bool thread_finished = false; + + std::thread wait_thread = std::thread( + [&clock, &thread_finished]() + { + // make sure the thread starts sleeping late + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + clock->sleep_until(clock->now() + std::chrono::seconds(3)); + thread_finished = true; + }); + + // notify the clock, that the sleep shall be interrupted + clock->cancel_sleep_or_wait(); + + auto start_time = std::chrono::steady_clock::now(); + auto cur_time = start_time; + while (!thread_finished && start_time + std::chrono::seconds(1) > cur_time) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + cur_time = std::chrono::steady_clock::now(); + } + + wait_thread.join(); + + EXPECT_TRUE(thread_finished); + EXPECT_LT(cur_time, start_time + std::chrono::seconds(1)); + } + + void test_wakeup_after_sleep(const rclcpp::Clock::SharedPtr & clock) + { + std::atomic_bool thread_finished = false; + + std::thread wait_thread = std::thread( + [&clock, &thread_finished]() + { + clock->sleep_until(clock->now() + std::chrono::seconds(3)); + thread_finished = true; + }); + + // make sure the thread is already sleeping before we send the cancel + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // notify the clock, that the sleep shall be interrupted + clock->cancel_sleep_or_wait(); + + auto start_time = std::chrono::steady_clock::now(); + auto cur_time = start_time; + while (!thread_finished && start_time + std::chrono::seconds(1) > cur_time) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + cur_time = std::chrono::steady_clock::now(); + } + + wait_thread.join(); + + EXPECT_TRUE(thread_finished); + EXPECT_LT(cur_time, start_time + std::chrono::seconds(1)); + } + +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("my_node"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +INSTANTIATE_TEST_SUITE_P( + Clocks, + TestClockWakeup, + ::testing::Values( + RCL_SYSTEM_TIME, RCL_ROS_TIME, RCL_STEADY_TIME +)); + +TEST_P(TestClockWakeup, wakeup_sleep) { + auto clock = std::make_shared(GetParam()); + test_wakeup_after_sleep(clock); + test_wakeup_before_sleep(clock); +} + +TEST_F(TestClockWakeup, wakeup_sleep_ros_time_active) { + node->set_parameter({"use_sim_time", true}); + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source(node); + time_source.attachClock(clock); + + EXPECT_TRUE(clock->ros_time_is_active()); + + test_wakeup_after_sleep(clock); + test_wakeup_before_sleep(clock); +} + +TEST_F(TestClockWakeup, no_wakeup_on_sim_time) { + node->set_parameter({"use_sim_time", true}); + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source(node); + time_source.attachClock(clock); + + EXPECT_TRUE(clock->ros_time_is_active()); + + std::atomic_bool thread_finished = false; + + std::thread wait_thread = std::thread( + [&clock, &thread_finished]() + { + // make sure the thread starts sleeping late + clock->sleep_until(clock->now() + std::chrono::milliseconds(10)); + thread_finished = true; + }); + + // make sure, that the sim time clock does not wakeup, as no clock is provided + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + EXPECT_FALSE(thread_finished); + + // notify the clock, that the sleep shall be interrupted + clock->cancel_sleep_or_wait(); + + auto start_time = std::chrono::steady_clock::now(); + auto cur_time = start_time; + while (!thread_finished && start_time + std::chrono::seconds(1) > cur_time) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + cur_time = std::chrono::steady_clock::now(); + } + + wait_thread.join(); + + EXPECT_TRUE(thread_finished); + EXPECT_LT(cur_time, start_time + std::chrono::seconds(1)); +} + +TEST_F(TestClockWakeup, multiple_threads_wait_on_one_clock) { + auto clock = std::make_shared(RCL_ROS_TIME); + + std::vector thread_finished(10, false); + + std::vector threads; + + for (size_t nr = 0; nr < thread_finished.size(); nr++) { + threads.push_back( + std::thread( + [&clock, &thread_finished, nr]() + { + // make sure the thread starts sleeping late + clock->sleep_until(clock->now() + std::chrono::seconds(10)); + thread_finished[nr] = true; + })); + } + + // wait a bit so all threads can execute the sleep_until + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + for (const bool & finished : thread_finished) { + EXPECT_FALSE(finished); + } + + rclcpp::shutdown(); + + auto start_time = std::chrono::steady_clock::now(); + auto cur_time = start_time; + bool threads_finished = false; + while (!threads_finished && start_time + std::chrono::seconds(1) > cur_time) { + threads_finished = true; + for (const bool finished : thread_finished) { + if (!finished) { + threads_finished = false; + } + } + + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + cur_time = std::chrono::steady_clock::now(); + } + + for (const bool finished : thread_finished) { + EXPECT_TRUE(finished); + } + + for (auto & thread : threads) { + thread.join(); + } + + EXPECT_LT(cur_time, start_time + std::chrono::seconds(1)); +} From d648a7c926933dca9e6ceae88cf9e4bc57e280c3 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 16 Apr 2024 11:42:38 -0400 Subject: [PATCH 375/455] Remove references to index.ros.org. (#2504) Signed-off-by: Chris Lalancette --- README.md | 2 +- rclcpp/README.md | 2 +- rclcpp_action/README.md | 2 +- rclcpp_components/README.md | 2 +- rclcpp_lifecycle/README.md | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index f4fe5837aa..baf82c8520 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,7 @@ rclcpp provides the standard C++ API for interacting with ROS 2. `#include "rclcpp/rclcpp.hpp"` allows use of the most common elements of the ROS 2 system. -The link to the latest API documentation can be found on the rclcpp package info page, at the [ROS Index](https://index.ros.org/p/rclcpp/). +The link to the latest API documentation can be found on the [rclcpp package info page](https://docs.ros.org/en/rolling/p/rclcpp). ### Examples diff --git a/rclcpp/README.md b/rclcpp/README.md index d3c390b449..8ec47772f1 100644 --- a/rclcpp/README.md +++ b/rclcpp/README.md @@ -2,7 +2,7 @@ The ROS client library in C++. -The link to the latest rclcpp API documentation, which includes a complete list of its main components and features, can be found on the rclcpp package info page, at the [ROS Index](https://index.ros.org/p/rclcpp/). +The link to the latest rclcpp API documentation, which includes a complete list of its main components and features, can be found on the [rclcpp package info page](https://docs.ros.org/en/rolling/p/rclcpp). ## Quality Declaration diff --git a/rclcpp_action/README.md b/rclcpp_action/README.md index dbb91a6ba3..bc3b85e6bd 100644 --- a/rclcpp_action/README.md +++ b/rclcpp_action/README.md @@ -2,7 +2,7 @@ Adds action APIs for C++. -The link to the latest rclcpp_action API documentation, which includes a complete list of its main components and features, can be found on the rclcpp_action package info page, at the [ROS Index](https://index.ros.org/p/rclcpp_action/). +The link to the latest rclcpp_action API documentation, which includes a complete list of its main components and features, can be found on the [rclcpp_action package info page](https://docs.ros.org/en/rolling/p/rclcpp_action). For more information about Actions in ROS 2, see the [design document](http://design.ros2.org/articles/actions.html). ## Quality Declaration diff --git a/rclcpp_components/README.md b/rclcpp_components/README.md index 0b0e0a7aec..fcc4c04ea0 100644 --- a/rclcpp_components/README.md +++ b/rclcpp_components/README.md @@ -2,7 +2,7 @@ Package containing tools for dynamically loadable components. -The link to the latest rclcpp_components API documentation, which includes a complete list of its main components and features, can be found on the rclcpp_components package info page, at the [ROS Index](https://index.ros.org/p/rclcpp_components/). +The link to the latest rclcpp_components API documentation, which includes a complete list of its main components and features, can be found on the [rclcpp_components package info page](https://docs.ros.org/en/rolling/p/rclcpp_components). ## Quality Declaration diff --git a/rclcpp_lifecycle/README.md b/rclcpp_lifecycle/README.md index e182390136..72bb50fbd2 100644 --- a/rclcpp_lifecycle/README.md +++ b/rclcpp_lifecycle/README.md @@ -2,7 +2,7 @@ Package containing a prototype for lifecycle implementation. -The link to the latest rclcpp_lifecycle API documentation, which includes a complete list of its main components and features, can be found on the rclcpp_lifecycle package info page, at the [ROS Index](https://index.ros.org/p/rclcpp_lifecycle/). +The link to the latest rclcpp_lifecycle API documentation, which includes a complete list of its main components and features, can be found on the [rclcpp_lifecycle package info page](https://docs.ros.org/en/rolling/p/rclcpp_lifecycle). For more information about LifeCycle in ROS 2, see the [design document](http://design.ros2.org/articles/node_lifecycle.html). ## Quality Declaration From 3fd0831af28370cf5c325e18b38c19f48a98ff7b Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 16 Apr 2024 17:14:53 +0000 Subject: [PATCH 376/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 8 +++++++- rclcpp_action/CHANGELOG.rst | 5 +++++ rclcpp_components/CHANGELOG.rst | 5 +++++ rclcpp_lifecycle/CHANGELOG.rst | 5 +++++ 4 files changed, 22 insertions(+), 1 deletion(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 483add40ad..4f65a5eb81 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove references to index.ros.org. (`#2504 `_) +* Reduce overhead for inheriting from rclcpp::Executor when base functionality is not reused (`#2506 `_) +* Contributors: Chris Lalancette, William Woodall, jmachowinski + 28.0.1 (2024-04-16) ------------------- * [wjwwood] Updated "Data race fixes" (`#2500 `_) @@ -34,7 +40,7 @@ Changelog for package rclcpp * Ensure waitables handle guard condition retriggering (`#2483 `_) Co-authored-by: Michael Carroll * fix: init concatenated_vector with begin() & end() (`#2492 `_) - *. this commit will fix the warning [-Wstringop-overflow=] `#2461 `_ + * this commit will fix the warning [-Wstringop-overflow=] `#2461 `_ * Use the same context for the specified node in rclcpp::spin functions (`#2433 `_) * Use the same conext for the specified node in rclcpp::spin_xx functions * Add test for spinning with non-default-context diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 39e8003699..c33331f76e 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove references to index.ros.org. (`#2504 `_) +* Contributors: Chris Lalancette + 28.0.1 (2024-04-16) ------------------- * Callback after cancel (`#2281 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index fd9a5d0302..72c82208b5 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove references to index.ros.org. (`#2504 `_) +* Contributors: Chris Lalancette + 28.0.1 (2024-04-16) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 64b059593b..164ed6ea35 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove references to index.ros.org. (`#2504 `_) +* Contributors: Chris Lalancette + 28.0.1 (2024-04-16) ------------------- * call shutdown in LifecycleNode dtor to avoid leaving the device in un… (`#2450 `_) From 3d58f0fb703d86461c2ab2fb39cd2640dc26e219 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 16 Apr 2024 17:16:42 +0000 Subject: [PATCH 377/455] 28.1.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 4f65a5eb81..bf9fbe2753 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.1.0 (2024-04-16) +------------------- * Remove references to index.ros.org. (`#2504 `_) * Reduce overhead for inheriting from rclcpp::Executor when base functionality is not reused (`#2506 `_) * Contributors: Chris Lalancette, William Woodall, jmachowinski diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 451acd8926..70ff81a195 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 28.0.1 + 28.1.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index c33331f76e..774eaeab9d 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.1.0 (2024-04-16) +------------------- * Remove references to index.ros.org. (`#2504 `_) * Contributors: Chris Lalancette diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 4b7b77a59a..84bf090bde 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 28.0.1 + 28.1.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 72c82208b5..81bdc08f86 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.1.0 (2024-04-16) +------------------- * Remove references to index.ros.org. (`#2504 `_) * Contributors: Chris Lalancette diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index a599d1d49a..aa10d42307 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 28.0.1 + 28.1.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 164ed6ea35..bbaf3e3d44 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.1.0 (2024-04-16) +------------------- * Remove references to index.ros.org. (`#2504 `_) * Contributors: Chris Lalancette diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 62e6800d58..f21ff3a11f 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 28.0.1 + 28.1.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 55939613a0cc0cef26505118b487cb5aadd987a3 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Tue, 23 Apr 2024 19:29:46 +0800 Subject: [PATCH 378/455] Revise the description of service configure_introspection() (#2511) Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/service.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 9e08dc235d..925c5f6286 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -499,7 +499,7 @@ class Service } } - /// Configure client introspection. + /// Configure service introspection. /** * \param[in] clock clock to use to generate introspection timestamps * \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher From de666d2bf41806d150744d7c5dcd7a07ad3ffcd0 Mon Sep 17 00:00:00 2001 From: Sharmin Ramli Date: Tue, 23 Apr 2024 22:47:56 +0200 Subject: [PATCH 379/455] Check for negative time in rclcpp::Time(int64_t nanoseconds, ...) constructor (#2510) Signed-off-by: Nursharmin Ramli --- rclcpp/include/rclcpp/time.hpp | 1 + rclcpp/src/rclcpp/time.cpp | 10 ++++ rclcpp/test/rclcpp/test_time.cpp | 82 +++----------------------------- 3 files changed, 17 insertions(+), 76 deletions(-) diff --git a/rclcpp/include/rclcpp/time.hpp b/rclcpp/include/rclcpp/time.hpp index a931f3ac9c..7ebf9f21b8 100644 --- a/rclcpp/include/rclcpp/time.hpp +++ b/rclcpp/include/rclcpp/time.hpp @@ -49,6 +49,7 @@ class Time /** * \param nanoseconds since time epoch * \param clock_type clock type + * \throws std::runtime_error if nanoseconds are negative */ RCLCPP_PUBLIC explicit Time(int64_t nanoseconds = 0, rcl_clock_type_t clock_type = RCL_SYSTEM_TIME); diff --git a/rclcpp/src/rclcpp/time.cpp b/rclcpp/src/rclcpp/time.cpp index 978651516c..e2780c04d8 100644 --- a/rclcpp/src/rclcpp/time.cpp +++ b/rclcpp/src/rclcpp/time.cpp @@ -60,6 +60,10 @@ Time::Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type) Time::Time(int64_t nanoseconds, rcl_clock_type_t clock_type) : rcl_time_(init_time_point(clock_type)) { + if (nanoseconds < 0) { + throw std::runtime_error("cannot store a negative time point in rclcpp::Time"); + } + rcl_time_.nanoseconds = nanoseconds; } @@ -249,6 +253,9 @@ Time::operator+=(const rclcpp::Duration & rhs) } rcl_time_.nanoseconds += rhs.nanoseconds(); + if (rcl_time_.nanoseconds < 0) { + throw std::runtime_error("cannot store a negative time point in rclcpp::Time"); + } return *this; } @@ -264,6 +271,9 @@ Time::operator-=(const rclcpp::Duration & rhs) } rcl_time_.nanoseconds -= rhs.nanoseconds(); + if (rcl_time_.nanoseconds < 0) { + throw std::runtime_error("cannot store a negative time point in rclcpp::Time"); + } return *this; } diff --git a/rclcpp/test/rclcpp/test_time.cpp b/rclcpp/test/rclcpp/test_time.cpp index 7d656fb9c4..8406de4efb 100644 --- a/rclcpp/test/rclcpp/test_time.cpp +++ b/rclcpp/test/rclcpp/test_time.cpp @@ -138,6 +138,8 @@ TEST_F(TestTime, conversions) { EXPECT_ANY_THROW(rclcpp::Time(-1, 1)); + EXPECT_ANY_THROW(rclcpp::Time(-1)); + EXPECT_ANY_THROW( { rclcpp::Time assignment(1, 2); @@ -168,48 +170,6 @@ TEST_F(TestTime, conversions) { EXPECT_EQ(time_msg.nanosec, HALF_SEC_IN_NS); EXPECT_EQ(rclcpp::Time(time_msg).nanoseconds(), ONE_AND_HALF_SEC_IN_NS); } - - { - // Can rclcpp::Time be negative or not? The following constructor works: - rclcpp::Time time(-HALF_SEC_IN_NS); - auto time_msg = static_cast(time); - EXPECT_EQ(time_msg.sec, -1); - EXPECT_EQ(time_msg.nanosec, HALF_SEC_IN_NS); - - // The opposite conversion throws... - EXPECT_ANY_THROW( - { - rclcpp::Time negative_time(time_msg); - }); - } - - { - // Can rclcpp::Time be negative or not? The following constructor works: - rclcpp::Time time(-ONE_SEC_IN_NS); - auto time_msg = static_cast(time); - EXPECT_EQ(time_msg.sec, -1); - EXPECT_EQ(time_msg.nanosec, 0u); - - // The opposite conversion throws... - EXPECT_ANY_THROW( - { - rclcpp::Time negative_time(time_msg); - }); - } - - { - // Can rclcpp::Time be negative or not? The following constructor works: - rclcpp::Time time(-ONE_AND_HALF_SEC_IN_NS); - auto time_msg = static_cast(time); - EXPECT_EQ(time_msg.sec, -2); - EXPECT_EQ(time_msg.nanosec, HALF_SEC_IN_NS); - - // The opposite conversion throws... - EXPECT_ANY_THROW( - { - rclcpp::Time negative_time(time_msg); - }); - } } TEST_F(TestTime, operators) { @@ -326,31 +286,18 @@ TEST_F(TestTime, overflow_detectors) { TEST_F(TestTime, overflows) { rclcpp::Time max_time(std::numeric_limits::max()); - rclcpp::Time min_time(std::numeric_limits::min()); rclcpp::Duration one(1ns); rclcpp::Duration two(2ns); - // Cross min/max + // Cross max EXPECT_THROW(max_time + one, std::overflow_error); - EXPECT_THROW(min_time - one, std::underflow_error); - EXPECT_THROW(max_time - min_time, std::overflow_error); - EXPECT_THROW(min_time - max_time, std::underflow_error); EXPECT_THROW(rclcpp::Time(max_time) += one, std::overflow_error); - EXPECT_THROW(rclcpp::Time(min_time) -= one, std::underflow_error); EXPECT_NO_THROW(max_time - max_time); - EXPECT_NO_THROW(min_time - min_time); - // Cross zero in both directions + // Cross zero rclcpp::Time one_time(1); - EXPECT_NO_THROW(one_time - two); - EXPECT_NO_THROW(rclcpp::Time(one_time) -= two); - - rclcpp::Time minus_one_time(-1); - EXPECT_NO_THROW(minus_one_time + two); - EXPECT_NO_THROW(rclcpp::Time(minus_one_time) += two); - - EXPECT_NO_THROW(one_time - minus_one_time); - EXPECT_NO_THROW(minus_one_time - one_time); + EXPECT_THROW(one_time - two, std::runtime_error); + EXPECT_THROW(rclcpp::Time(one_time) -= two, std::runtime_error); rclcpp::Time two_time(2); EXPECT_NO_THROW(one_time - two_time); @@ -432,41 +379,24 @@ TEST_F(TestTime, test_overflow_underflow_throws) { RCLCPP_EXPECT_THROW_EQ( test_time = rclcpp::Time(INT64_MAX) + rclcpp::Duration(1ns), std::overflow_error("addition leads to int64_t overflow")); - RCLCPP_EXPECT_THROW_EQ( - test_time = rclcpp::Time(INT64_MIN) + rclcpp::Duration(-1ns), - std::underflow_error("addition leads to int64_t underflow")); RCLCPP_EXPECT_THROW_EQ( test_time = rclcpp::Time(INT64_MAX) - rclcpp::Duration(-1ns), std::overflow_error("time subtraction leads to int64_t overflow")); - RCLCPP_EXPECT_THROW_EQ( - test_time = rclcpp::Time(INT64_MIN) - rclcpp::Duration(1ns), - std::underflow_error("time subtraction leads to int64_t underflow")); test_time = rclcpp::Time(INT64_MAX); RCLCPP_EXPECT_THROW_EQ( test_time += rclcpp::Duration(1ns), std::overflow_error("addition leads to int64_t overflow")); - test_time = rclcpp::Time(INT64_MIN); - RCLCPP_EXPECT_THROW_EQ( - test_time += rclcpp::Duration(-1ns), - std::underflow_error("addition leads to int64_t underflow")); test_time = rclcpp::Time(INT64_MAX); RCLCPP_EXPECT_THROW_EQ( test_time -= rclcpp::Duration(-1ns), std::overflow_error("time subtraction leads to int64_t overflow")); - test_time = rclcpp::Time(INT64_MIN); - RCLCPP_EXPECT_THROW_EQ( - test_time -= rclcpp::Duration(1ns), - std::underflow_error("time subtraction leads to int64_t underflow")); RCLCPP_EXPECT_THROW_EQ( test_time = rclcpp::Duration::from_nanoseconds(INT64_MAX) + rclcpp::Time(1), std::overflow_error("addition leads to int64_t overflow")); - RCLCPP_EXPECT_THROW_EQ( - test_time = rclcpp::Duration::from_nanoseconds(INT64_MIN) + rclcpp::Time(-1), - std::underflow_error("addition leads to int64_t underflow")); } class TestClockSleep : public ::testing::Test From cf6141330a1185c49475b6d449c99baea66c8f5b Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Fri, 26 Apr 2024 16:57:52 +0000 Subject: [PATCH 380/455] Changelog. Signed-off-by: Marco A. Gutierrez --- rclcpp/CHANGELOG.rst | 6 ++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 3 +++ 4 files changed, 15 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index bf9fbe2753..a1241465a8 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Check for negative time in rclcpp::Time(int64_t nanoseconds, ...) constructor (`#2510 `_) +* Revise the description of service configure_introspection() (`#2511 `_) +* Contributors: Barry Xu, Sharmin Ramli + 28.1.0 (2024-04-16) ------------------- * Remove references to index.ros.org. (`#2504 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 774eaeab9d..a30c79564c 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 28.1.0 (2024-04-16) ------------------- * Remove references to index.ros.org. (`#2504 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 81bdc08f86..e7939ae21d 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 28.1.0 (2024-04-16) ------------------- * Remove references to index.ros.org. (`#2504 `_) diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index bbaf3e3d44..57b1519ce8 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 28.1.0 (2024-04-16) ------------------- * Remove references to index.ros.org. (`#2504 `_) From 2cd8900dd20713f54a2a82d189a5e3372dca70be Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Fri, 26 Apr 2024 16:57:57 +0000 Subject: [PATCH 381/455] 28.2.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index a1241465a8..4c71d31267 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.2.0 (2024-04-26) +------------------- * Check for negative time in rclcpp::Time(int64_t nanoseconds, ...) constructor (`#2510 `_) * Revise the description of service configure_introspection() (`#2511 `_) * Contributors: Barry Xu, Sharmin Ramli diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 70ff81a195..dacffd678f 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 28.1.0 + 28.2.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index a30c79564c..60ab9976ca 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.2.0 (2024-04-26) +------------------- 28.1.0 (2024-04-16) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 84bf090bde..a1ffdd5800 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 28.1.0 + 28.2.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index e7939ae21d..d43eadc36c 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.2.0 (2024-04-26) +------------------- 28.1.0 (2024-04-16) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index aa10d42307..28e9ad4d20 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 28.1.0 + 28.2.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 57b1519ce8..00728b1934 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.2.0 (2024-04-26) +------------------- 28.1.0 (2024-04-16) ------------------- diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index f21ff3a11f..a621ab2f39 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 28.1.0 + 28.2.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 5f912eb58e0f6f1718592ece0935f9a0115d03ec Mon Sep 17 00:00:00 2001 From: Scott K Logan Date: Mon, 29 Apr 2024 16:09:43 -0500 Subject: [PATCH 382/455] Add 'mimick' label to tests which use Mimick (#2516) Signed-off-by: Scott K Logan --- rclcpp/test/rclcpp/CMakeLists.txt | 31 +++++++++++++++++++++++++++++++ rclcpp_action/CMakeLists.txt | 3 +++ rclcpp_lifecycle/CMakeLists.txt | 5 +++++ 3 files changed, 39 insertions(+) diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 844ca630ac..036a33f527 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -31,6 +31,7 @@ endif() ament_add_gtest( test_exceptions exceptions/test_exceptions.cpp) +ament_add_test_label(test_exceptions mimick) if(TARGET test_exceptions) target_link_libraries(test_exceptions ${PROJECT_NAME} mimick) endif() @@ -52,10 +53,12 @@ if(TARGET test_any_subscription_callback) target_link_libraries(test_any_subscription_callback ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_client test_client.cpp) +ament_add_test_label(test_client mimick) if(TARGET test_client) target_link_libraries(test_client ${PROJECT_NAME} mimick ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS}) endif() ament_add_gtest(test_clock test_clock.cpp) +ament_add_test_label(test_clock mimick) if(TARGET test_clock) target_link_libraries(test_clock ${PROJECT_NAME} mimick ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS}) endif() @@ -69,6 +72,7 @@ if(TARGET test_create_timer) target_include_directories(test_create_timer PRIVATE ./) endif() ament_add_gtest(test_generic_client test_generic_client.cpp) +ament_add_test_label(test_generic_client mimick) if(TARGET test_generic_client) target_link_libraries(test_generic_client ${PROJECT_NAME} mimick @@ -80,6 +84,7 @@ if(TARGET test_generic_client) ) endif() ament_add_gtest(test_client_common test_client_common.cpp) +ament_add_test_label(test_client_common mimick) if(TARGET test_client_common) target_link_libraries(test_client_common ${PROJECT_NAME} mimick @@ -106,6 +111,7 @@ function(test_add_callback_groups_to_executor_for_rmw_implementation) endfunction() call_for_each_rmw_implementation(test_add_callback_groups_to_executor_for_rmw_implementation) ament_add_gtest(test_expand_topic_or_service_name test_expand_topic_or_service_name.cpp) +ament_add_test_label(test_expand_topic_or_service_name mimick) if(TARGET test_expand_topic_or_service_name) target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME} mimick rcl::rcl rmw::rmw) endif() @@ -137,6 +143,7 @@ if(TARGET test_intra_process_buffer) endif() ament_add_gtest(test_loaned_message test_loaned_message.cpp) +ament_add_test_label(test_loaned_message mimick) target_link_libraries(test_loaned_message ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) ament_add_gtest(test_memory_strategy test_memory_strategy.cpp) @@ -146,6 +153,7 @@ ament_add_gtest(test_message_memory_strategy test_message_memory_strategy.cpp) target_link_libraries(test_message_memory_strategy ${PROJECT_NAME} ${test_msgs_TARGETS}) ament_add_gtest(test_node test_node.cpp TIMEOUT 240) +ament_add_test_label(test_node mimick) if(TARGET test_node) target_link_libraries(test_node ${PROJECT_NAME} mimick rcpputils::rcpputils rmw::rmw ${test_msgs_TARGETS}) endif() @@ -157,6 +165,7 @@ if(TARGET test_node_interfaces__get_node_interfaces) endif() ament_add_gtest(test_node_interfaces__node_base node_interfaces/test_node_base.cpp) +ament_add_test_label(test_node_interfaces__node_base mimick) if(TARGET test_node_interfaces__node_base) target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME} mimick rcl::rcl rmw::rmw) endif() @@ -168,6 +177,7 @@ endif() ament_add_gtest(test_node_interfaces__node_graph node_interfaces/test_node_graph.cpp TIMEOUT 120) +ament_add_test_label(test_node_interfaces__node_graph mimick) if(TARGET test_node_interfaces__node_graph) target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME} mimick rcl::rcl ${test_msgs_TARGETS}) endif() @@ -178,21 +188,25 @@ if(TARGET test_node_interfaces__node_interfaces) endif() ament_add_gtest(test_node_interfaces__node_parameters node_interfaces/test_node_parameters.cpp) +ament_add_test_label(test_node_interfaces__node_parameters mimick) if(TARGET test_node_interfaces__node_parameters) target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME} mimick rcpputils::rcpputils) endif() ament_add_gtest(test_node_interfaces__node_services node_interfaces/test_node_services.cpp) +ament_add_test_label(test_node_interfaces__node_services mimick) if(TARGET test_node_interfaces__node_services) target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME} mimick rcl::rcl) endif() ament_add_gtest(test_node_interfaces__node_timers node_interfaces/test_node_timers.cpp) +ament_add_test_label(test_node_interfaces__node_timers mimick) if(TARGET test_node_interfaces__node_timers) target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME} mimick rcl::rcl) endif() ament_add_gtest(test_node_interfaces__node_topics node_interfaces/test_node_topics.cpp) +ament_add_test_label(test_node_interfaces__node_topics mimick) if(TARGET test_node_interfaces__node_topics) target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME} mimick rcl::rcl ${test_msgs_TARGETS}) endif() @@ -203,6 +217,7 @@ if(TARGET test_node_interfaces__node_type_descriptions) endif() ament_add_gtest(test_node_interfaces__node_waitables node_interfaces/test_node_waitables.cpp) +ament_add_test_label(test_node_interfaces__node_waitables mimick) if(TARGET test_node_interfaces__node_waitables) target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME} mimick rcl::rcl) endif() @@ -238,10 +253,12 @@ if(TARGET test_node_global_args) target_link_libraries(test_node_global_args ${PROJECT_NAME}) endif() ament_add_gtest(test_node_options test_node_options.cpp) +ament_add_test_label(test_node_options mimick) if(TARGET test_node_options) target_link_libraries(test_node_options ${PROJECT_NAME} mimick rcl::rcl) endif() ament_add_gtest(test_init_options test_init_options.cpp) +ament_add_test_label(test_init_options mimick) if(TARGET test_init_options) target_link_libraries(test_init_options ${PROJECT_NAME} mimick rcl::rcl) endif() @@ -270,6 +287,7 @@ if(TARGET test_parameter_map) target_link_libraries(test_parameter_map ${PROJECT_NAME} rcl::rcl rcl_yaml_param_parser::rcl_yaml_param_parser rcutils::rcutils) endif() ament_add_gtest(test_publisher test_publisher.cpp TIMEOUT 120) +ament_add_test_label(test_publisher mimick) if(TARGET test_publisher) target_link_libraries(test_publisher ${PROJECT_NAME} mimick rcl::rcl rcutils::rcutils ${test_msgs_TARGETS}) endif() @@ -335,6 +353,7 @@ function(test_qos_event_for_rmw_implementation) ament_add_gmock(test_qos_event${target_suffix} test_qos_event.cpp ENV ${rmw_implementation_env_var} ) + ament_add_test_label(test_qos_event${target_suffix} mimick) if(TARGET test_qos_event${target_suffix}) target_link_libraries(test_qos_event${target_suffix} ${PROJECT_NAME} mimick rcutils::rcutils rmw::rmw ${test_msgs_TARGETS}) endif() @@ -362,15 +381,18 @@ if(TARGET test_serialized_message) target_link_libraries(test_serialized_message ${PROJECT_NAME} rcpputils::rcpputils ${test_msgs_TARGETS}) endif() ament_add_gtest(test_service test_service.cpp) +ament_add_test_label(test_service mimick) if(TARGET test_service) target_link_libraries(test_service ${PROJECT_NAME} mimick ${rcl_interfaces_TARGES} ${test_msgs_TARGETS}) endif() ament_add_gmock(test_service_introspection test_service_introspection.cpp) +ament_add_test_label(test_service_introspection mimick) if(TARGET test_service_introspection) target_link_libraries(test_service_introspection ${PROJECT_NAME} mimick ${service_msgs_TARGETS} ${test_msgs_TARGETS}) endif() # Creating and destroying nodes is slow with Connext, so this needs larger timeout. ament_add_gtest(test_subscription test_subscription.cpp TIMEOUT 120) +ament_add_test_label(test_subscription mimick) if(TARGET test_subscription) target_link_libraries(test_subscription ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) endif() @@ -421,6 +443,7 @@ endif() ament_add_gtest(test_timer test_timer.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") +ament_add_test_label(test_timer mimick) if(TARGET test_timer) target_link_libraries(test_timer ${PROJECT_NAME} mimick rcl::rcl) endif() @@ -439,6 +462,7 @@ endif() ament_add_gtest(test_utilities test_utilities.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") +ament_add_test_label(test_utilities mimick) if(TARGET test_utilities) target_link_libraries(test_utilities ${PROJECT_NAME} mimick rcl::rcl) endif() @@ -497,6 +521,7 @@ endif() ament_add_gtest(test_static_single_threaded_executor executors/test_static_single_threaded_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") +ament_add_test_label(test_static_single_threaded_executor mimick) if(TARGET test_static_single_threaded_executor) target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) endif() @@ -515,6 +540,7 @@ endif() ament_add_gtest(test_executor_notify_waitable executors/test_executor_notify_waitable.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) +ament_add_test_label(test_executor_notify_waitable mimick) if(TARGET test_executor_notify_waitable) target_link_libraries(test_executor_notify_waitable ${PROJECT_NAME} mimick rcpputils::rcpputils) endif() @@ -532,6 +558,7 @@ endif() ament_add_gtest(test_guard_condition test_guard_condition.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") +ament_add_test_label(test_guard_condition mimick) if(TARGET test_guard_condition) target_link_libraries(test_guard_condition ${PROJECT_NAME} mimick) endif() @@ -565,6 +592,7 @@ if(TARGET test_dynamic_storage) endif() ament_add_gtest(test_storage_policy_common wait_set_policies/test_storage_policy_common.cpp) +ament_add_test_label(test_storage_policy_common mimick) if(TARGET test_storage_policy_common) target_link_libraries(test_storage_policy_common ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) endif() @@ -597,11 +625,13 @@ endif() ament_add_gtest(test_executor test_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) +ament_add_test_label(test_executor mimick) if(TARGET test_executor) target_link_libraries(test_executor ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_graph_listener test_graph_listener.cpp) +ament_add_test_label(test_graph_listener mimick) if(TARGET test_graph_listener) target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick) endif() @@ -613,6 +643,7 @@ function(test_subscription_content_filter_for_rmw_implementation) ENV ${rmw_implementation_env_var} TIMEOUT 120 ) + ament_add_test_label(test_subscription_content_filter${target_suffix} mimick) if(TARGET test_subscription_content_filter${target_suffix}) target_link_libraries(test_subscription_content_filter${target_suffix} ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) endif() diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index 647a820969..f5537d4f30 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -80,6 +80,7 @@ if(BUILD_TESTING) add_subdirectory(test/benchmark) ament_add_gtest(test_client test/test_client.cpp TIMEOUT 180) + ament_add_test_label(test_client mimick) if(TARGET test_client) target_link_libraries(test_client ${PROJECT_NAME} @@ -93,6 +94,7 @@ if(BUILD_TESTING) endif() ament_add_gtest(test_server test/test_server.cpp TIMEOUT 180) + ament_add_test_label(test_server mimick) if(TARGET test_server) target_link_libraries(test_server ${PROJECT_NAME} @@ -104,6 +106,7 @@ if(BUILD_TESTING) endif() ament_add_gtest(test_server_goal_handle test/test_server_goal_handle.cpp) + ament_add_test_label(test_server_goal_handle mimick) if(TARGET test_server_goal_handle) target_link_libraries(test_server_goal_handle ${PROJECT_NAME} diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 66b1990ec5..d2bdb791b7 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -95,6 +95,7 @@ if(BUILD_TESTING) endif() ament_add_gtest(test_lifecycle_node test/test_lifecycle_node.cpp TIMEOUT 120) + ament_add_test_label(test_lifecycle_node mimick) if(TARGET test_lifecycle_node) target_link_libraries(test_lifecycle_node ${PROJECT_NAME} mimick rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp rcutils::rcutils) endif() @@ -103,6 +104,7 @@ if(BUILD_TESTING) target_link_libraries(test_lifecycle_publisher ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp ${test_msgs_TARGETS}) endif() ament_add_gtest(test_lifecycle_service_client test/test_lifecycle_service_client.cpp TIMEOUT 120) + ament_add_test_label(test_lifecycle_service_client mimick) if(TARGET test_lifecycle_service_client) target_link_libraries(test_lifecycle_service_client ${PROJECT_NAME} @@ -113,6 +115,7 @@ if(BUILD_TESTING) rcutils::rcutils) endif() ament_add_gtest(test_client test/test_client.cpp TIMEOUT 120) + ament_add_test_label(test_client mimick) if(TARGET test_client) target_link_libraries(test_client ${PROJECT_NAME} @@ -121,6 +124,7 @@ if(BUILD_TESTING) rclcpp::rclcpp) endif() ament_add_gtest(test_service test/test_service.cpp TIMEOUT 120) + ament_add_test_label(test_service mimick) if(TARGET test_service) target_link_libraries(test_service ${PROJECT_NAME} @@ -145,6 +149,7 @@ if(BUILD_TESTING) target_link_libraries(test_state_wrapper ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp) endif() ament_add_gtest(test_transition_wrapper test/test_transition_wrapper.cpp) + ament_add_test_label(test_transition_wrapper mimick) if(TARGET test_transition_wrapper) target_link_libraries(test_transition_wrapper ${PROJECT_NAME} mimick rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp rcutils::rcutils) target_compile_definitions(test_transition_wrapper From f7185dc129c40388e66813c96f2ac5dbb73ffe00 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 2 May 2024 14:06:10 -0700 Subject: [PATCH 383/455] Fixup Executor::spin_all() regression fix (#2517) * test(Executors): Added tests for busy waiting Checks if executors are busy waiting while they should block in spin_some or spin_all. Signed-off-by: Janosch Machowinski * fix: Reworked spinAll test This test was strange. It looked like, it assumed that spin_all did not return instantly. Also it was racy, as the thread could terminate instantly. Signed-off-by: Janosch Machowinski * fix(Executor): Fixed spin_all not returning instantly is no work was available Signed-off-by: Janosch Machowinski * Update rclcpp/test/rclcpp/executors/test_executors.cpp Co-authored-by: Tomoya Fujita Signed-off-by: jmachowinski * test(executors): Added test for busy waiting while calling spin Signed-off-by: Janosch Machowinski * fix(executor): Reset wait_result on every call to spin_some_impl Before, the method would not recollect available work in case of spin_some, spin_all. This would lead to the method behaving differently than to what the documentation states. Signed-off-by: Janosch Machowinski * restore previous test logic for now Signed-off-by: William Woodall * refactor spin_some_impl's logic and improve busy wait tests Signed-off-by: William Woodall * added some more comments about the implementation Signed-off-by: William Woodall --------- Signed-off-by: Janosch Machowinski Signed-off-by: jmachowinski Signed-off-by: William Woodall Co-authored-by: Janosch Machowinski Co-authored-by: jmachowinski Co-authored-by: Tomoya Fujita --- rclcpp/src/rclcpp/executor.cpp | 50 ++++-- .../test/rclcpp/executors/test_executors.cpp | 160 ++++++++++++++++++ 2 files changed, 199 insertions(+), 11 deletions(-) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 42a4d274dc..95b0d7fcc8 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -366,24 +366,52 @@ Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive) } RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + // clear the wait result and wait for work without blocking to collect the work + // for the first time + // both spin_some and spin_all wait for work at the beginning + wait_result_.reset(); + wait_for_work(std::chrono::milliseconds(0)); + bool just_waited = true; + + // The logic of this while loop is as follows: + // + // - while not shutdown, and spinning (not canceled), and not max duration reached... + // - try to get an executable item to execute, and execute it if available + // - otherwise, reset the wait result, and ... + // - if there was no work available just after waiting, break the loop unconditionally + // - this is appropriate for both spin_some and spin_all which use this function + // - else if exhaustive = true, then wait for work again + // - this is only used for spin_all and not spin_some + // - else break + // - this only occurs with spin_some + // + // The logic of this loop is subtle and should be carefully changed if at all. + // See also: + // https://github.com/ros2/rclcpp/issues/2508 + // https://github.com/ros2/rclcpp/pull/2517 while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { - if (!wait_result_.has_value()) { - wait_for_work(std::chrono::milliseconds(0)); - } - AnyExecutable any_exec; if (get_next_ready_executable(any_exec)) { execute_any_executable(any_exec); + just_waited = false; } else { - // If nothing is ready, reset the result to signal we are - // ready to wait again + // if nothing is ready, reset the result to clear it wait_result_.reset(); - } - if (!wait_result_.has_value() && !exhaustive) { - // In the case of spin some, then we can exit - // In the case of spin all, then we will allow ourselves to wait again. - break; + if (just_waited) { + // there was no work after just waiting, always exit in this case + // before the exhaustive condition can be checked + break; + } + + if (exhaustive) { + // if exhaustive, wait for work again + // this only happens for spin_all; spin_some only waits at the start + wait_for_work(std::chrono::milliseconds(0)); + just_waited = true; + } else { + break; + } } } } diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 1a538eaa30..a82b702db5 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -357,6 +357,7 @@ class TestWaitable : public rclcpp::Waitable bool is_ready(const rcl_wait_set_t & wait_set) override { + is_ready_count_++; 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) { @@ -424,8 +425,15 @@ class TestWaitable : public rclcpp::Waitable return count_; } + size_t + get_is_ready_call_count() const + { + return is_ready_count_; + } + private: std::atomic trigger_count_ = 0; + std::atomic is_ready_count_ = 0; std::atomic count_ = 0; rclcpp::GuardCondition gc_; std::function on_execute_callback_ = nullptr; @@ -869,3 +877,155 @@ TEST(TestExecutors, testSpinWithNonDefaultContext) rclcpp::shutdown(non_default_context); } + +template +class TestBusyWaiting : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + + const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); + std::stringstream test_name; + test_name << test_info->test_case_name() << "_" << test_info->name(); + node = std::make_shared("node", test_name.str()); + callback_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + /* automatically_add_to_executor_with_node =*/ false); + + auto waitable_interfaces = node->get_node_waitables_interface(); + waitable = std::make_shared(); + waitable_interfaces->add_waitable(waitable, callback_group); + + executor = std::make_shared(); + executor->add_callback_group(callback_group, node->get_node_base_interface()); + } + + void TearDown() override + { + rclcpp::shutdown(); + } + + void + set_up_and_trigger_waitable(std::function extra_callback = nullptr) + { + this->has_executed = false; + this->waitable->set_on_execute_callback([this, extra_callback]() { + if (!this->has_executed) { + // trigger once to see if the second trigger is handled or not + // this follow up trigger simulates new entities becoming ready while + // the executor is executing something else, e.g. subscription got data + // or a timer expired, etc. + // spin_some would not handle this second trigger, since it collects + // work only once, whereas spin_all should handle it since it + // collects work multiple times + this->waitable->trigger(); + this->has_executed = true; + } + if (nullptr != extra_callback) { + extra_callback(); + } + }); + this->waitable->trigger(); + } + + void + check_for_busy_waits(std::chrono::steady_clock::time_point start_time) + { + // rough time based check, since the work to be done was very small it + // should be safe to check that we didn't use more than half the + // max duration, which itself is much larger than necessary + // however, it could still produce a false-positive + EXPECT_LT( + std::chrono::steady_clock::now() - start_time, + max_duration / 2) + << "executor took a long time to execute when it should have done " + << "nothing and should not have blocked either, but this could be a " + << "false negative if the computer is really slow"; + + // this check is making some assumptions about the implementation of the + // executors, but it should be safe to say that a busy wait may result in + // hundreds or thousands of calls to is_ready(), but "normal" executor + // behavior should be within an order of magnitude of the number of + // times that the waitable was executed + ASSERT_LT(waitable->get_is_ready_call_count(), 10u * this->waitable->get_count()); + } + + static constexpr auto max_duration = 10s; + + rclcpp::Node::SharedPtr node; + rclcpp::CallbackGroup::SharedPtr callback_group; + std::shared_ptr waitable; + std::chrono::steady_clock::time_point start_time; + std::shared_ptr executor; + bool has_executed; +}; + +TYPED_TEST_SUITE(TestBusyWaiting, ExecutorTypes, ExecutorTypeNames); + +TYPED_TEST(TestBusyWaiting, test_spin_all) +{ + this->set_up_and_trigger_waitable(); + + auto start_time = std::chrono::steady_clock::now(); + this->executor->spin_all(this->max_duration); + this->check_for_busy_waits(start_time); + // this should get the initial trigger, and the follow up from in the callback + ASSERT_EQ(this->waitable->get_count(), 2u); +} + +TYPED_TEST(TestBusyWaiting, test_spin_some) +{ + this->set_up_and_trigger_waitable(); + + auto start_time = std::chrono::steady_clock::now(); + this->executor->spin_some(this->max_duration); + this->check_for_busy_waits(start_time); + // this should get the inital trigger, but not the follow up in the callback + ASSERT_EQ(this->waitable->get_count(), 1u); +} + +TYPED_TEST(TestBusyWaiting, test_spin) +{ + std::condition_variable cv; + std::mutex cv_m; + bool first_check_passed = false; + + this->set_up_and_trigger_waitable([&cv, &cv_m, &first_check_passed]() { + cv.notify_one(); + if (!first_check_passed) { + std::unique_lock lk(cv_m); + cv.wait_for(lk, 1s, [&]() {return first_check_passed;}); + } + }); + + auto start_time = std::chrono::steady_clock::now(); + std::thread t([this]() { + this->executor->spin(); + }); + + // wait until thread has started (first execute of waitable) + { + std::unique_lock lk(cv_m); + cv.wait_for(lk, 10s); + } + EXPECT_GT(this->waitable->get_count(), 0u); + + first_check_passed = true; + cv.notify_one(); + + // wait until the executor has finished (second execute of waitable) + { + std::unique_lock lk(cv_m); + cv.wait_for(lk, 10s); + } + EXPECT_EQ(this->waitable->get_count(), 2u); + + this->executor->cancel(); + t.join(); + + this->check_for_busy_waits(start_time); + // this should get the initial trigger, and the follow up from in the callback + ASSERT_EQ(this->waitable->get_count(), 2u); +} From 42b0b5775b4e68718c5949308c9e1a059930ded7 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 9 May 2024 07:34:04 -0400 Subject: [PATCH 384/455] =?UTF-8?q?Revert=20"call=20shutdown=20in=20Lifecy?= =?UTF-8?q?cleNode=20dtor=20to=20avoid=20leaving=20the=20device=20in=20un?= =?UTF-8?q?=E2=80=A6=20(#2450)"=20(#2522)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This reverts commit 04ea0bb00293387791522590b7347a2282cda290. --- rclcpp_lifecycle/src/lifecycle_node.cpp | 16 -- rclcpp_lifecycle/test/test_lifecycle_node.cpp | 140 ------------------ 2 files changed, 156 deletions(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 1ec9498b4a..0c448cf5e6 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -152,22 +152,6 @@ LifecycleNode::LifecycleNode( LifecycleNode::~LifecycleNode() { - // shutdown if necessary to avoid leaving the device in unknown state - if (LifecycleNode::get_current_state().id() != - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) - { - auto ret = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - auto finalized = LifecycleNode::shutdown(ret); - if (finalized.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED || - ret != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) - { - RCLCPP_WARN( - rclcpp::get_logger("rclcpp_lifecycle"), - "Shutdown error in destruction of LifecycleNode: final state(%s)", - finalized.label().c_str()); - } - } - // release sub-interfaces in an order that allows them to consult with node_base during tear-down node_waitables_.reset(); node_time_source_.reset(); diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 58ce89897c..fdb9be6153 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -447,146 +447,6 @@ TEST_F(TestDefaultStateMachine, bad_mood) { EXPECT_EQ(1u, test_node->number_of_callbacks); } - -TEST_F(TestDefaultStateMachine, shutdown_from_each_primary_state) { - auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - - // PRIMARY_STATE_UNCONFIGURED to shutdown - { - auto ret = reset_key; - auto test_node = std::make_shared("testnode"); - auto finalized = test_node->shutdown(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); - } - - // PRIMARY_STATE_INACTIVE to shutdown - { - auto ret = reset_key; - auto test_node = std::make_shared("testnode"); - auto configured = test_node->configure(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); - ret = reset_key; - auto finalized = test_node->shutdown(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); - } - - // PRIMARY_STATE_ACTIVE to shutdown - { - auto ret = reset_key; - auto test_node = std::make_shared("testnode"); - auto configured = test_node->configure(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); - ret = reset_key; - auto activated = test_node->activate(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); - ret = reset_key; - auto finalized = test_node->shutdown(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); - } - - // PRIMARY_STATE_FINALIZED to shutdown - { - auto ret = reset_key; - auto test_node = std::make_shared("testnode"); - auto configured = test_node->configure(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); - ret = reset_key; - auto activated = test_node->activate(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); - ret = reset_key; - auto finalized = test_node->shutdown(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); - ret = reset_key; - auto finalized_again = test_node->shutdown(ret); - EXPECT_EQ(reset_key, ret); - EXPECT_EQ(finalized_again.id(), State::PRIMARY_STATE_FINALIZED); - } -} - -TEST_F(TestDefaultStateMachine, test_shutdown_on_dtor) { - auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - - bool shutdown_cb_called = false; - auto on_shutdown_callback = - [&shutdown_cb_called](const rclcpp_lifecycle::State &) -> - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn { - shutdown_cb_called = true; - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - }; - - // PRIMARY_STATE_UNCONFIGURED to shutdown via dtor - shutdown_cb_called = false; - { - auto test_node = std::make_shared("testnode"); - test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1)); - EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); - EXPECT_FALSE(shutdown_cb_called); - } - EXPECT_TRUE(shutdown_cb_called); - - // PRIMARY_STATE_INACTIVE to shutdown via dtor - shutdown_cb_called = false; - { - auto ret = reset_key; - auto test_node = std::make_shared("testnode"); - test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1)); - auto configured = test_node->configure(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); - EXPECT_FALSE(shutdown_cb_called); - } - EXPECT_TRUE(shutdown_cb_called); - - // PRIMARY_STATE_ACTIVE to shutdown via dtor - shutdown_cb_called = false; - { - auto ret = reset_key; - auto test_node = std::make_shared("testnode"); - test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1)); - auto configured = test_node->configure(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); - ret = reset_key; - auto activated = test_node->activate(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); - EXPECT_FALSE(shutdown_cb_called); - } - EXPECT_TRUE(shutdown_cb_called); - - // PRIMARY_STATE_FINALIZED to shutdown via dtor - shutdown_cb_called = false; - { - auto ret = reset_key; - auto test_node = std::make_shared("testnode"); - test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1)); - auto configured = test_node->configure(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); - ret = reset_key; - auto activated = test_node->activate(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); - ret = reset_key; - auto finalized = test_node->shutdown(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); - EXPECT_TRUE(shutdown_cb_called); // should be called already - } - EXPECT_TRUE(shutdown_cb_called); -} - TEST_F(TestDefaultStateMachine, lifecycle_subscriber) { auto test_node = std::make_shared>("testnode"); From 343b29b617b163ad72b9fe3f6441dd4ed3d3af09 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 9 May 2024 13:32:44 -0700 Subject: [PATCH 385/455] add impl pointer for ExecutorOptions (#2523) * add impl pointer for ExecutorOptions Signed-off-by: William Woodall --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/executor_options.hpp | 24 ++++++++-- rclcpp/src/rclcpp/executor_options.cpp | 55 ++++++++++++++++++++++ 3 files changed, 75 insertions(+), 5 deletions(-) create mode 100644 rclcpp/src/rclcpp/executor_options.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 33d21e1b05..b1dbfec227 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -63,6 +63,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/exceptions/exceptions.cpp src/rclcpp/executable_list.cpp src/rclcpp/executor.cpp + src/rclcpp/executor_options.cpp src/rclcpp/executors.cpp src/rclcpp/executors/executor_entities_collection.cpp src/rclcpp/executors/executor_entities_collector.cpp diff --git a/rclcpp/include/rclcpp/executor_options.hpp b/rclcpp/include/rclcpp/executor_options.hpp index 066f29eb4e..a8fd5b73bf 100644 --- a/rclcpp/include/rclcpp/executor_options.hpp +++ b/rclcpp/include/rclcpp/executor_options.hpp @@ -15,6 +15,8 @@ #ifndef RCLCPP__EXECUTOR_OPTIONS_HPP_ #define RCLCPP__EXECUTOR_OPTIONS_HPP_ +#include + #include "rclcpp/context.hpp" #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/memory_strategies.hpp" @@ -24,18 +26,30 @@ namespace rclcpp { +class ExecutorOptionsImplementation; + /// Options to be passed to the executor constructor. struct ExecutorOptions { - ExecutorOptions() - : memory_strategy(rclcpp::memory_strategies::create_default_strategy()), - context(rclcpp::contexts::get_global_default_context()), - max_conditions(0) - {} + RCLCPP_PUBLIC + ExecutorOptions(); + + RCLCPP_PUBLIC + virtual ~ExecutorOptions(); + + RCLCPP_PUBLIC + ExecutorOptions(const ExecutorOptions &); + + RCLCPP_PUBLIC + ExecutorOptions & operator=(const ExecutorOptions &); rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy; rclcpp::Context::SharedPtr context; size_t max_conditions; + +private: + /// Pointer to implementation + std::unique_ptr impl_; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/executor_options.cpp b/rclcpp/src/rclcpp/executor_options.cpp new file mode 100644 index 0000000000..018d1261ae --- /dev/null +++ b/rclcpp/src/rclcpp/executor_options.cpp @@ -0,0 +1,55 @@ +// Copyright 2024 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. + +#include "rclcpp/executor_options.hpp" + +using rclcpp::ExecutorOptions; + +namespace rclcpp +{ + +class ExecutorOptionsImplementation {}; + +} // namespace rclcpp + +ExecutorOptions::ExecutorOptions() +: memory_strategy(rclcpp::memory_strategies::create_default_strategy()), + context(rclcpp::contexts::get_global_default_context()), + max_conditions(0), + impl_(nullptr) +{} + +ExecutorOptions::~ExecutorOptions() +{} + +ExecutorOptions::ExecutorOptions(const ExecutorOptions & other) +{ + *this = other; +} + +ExecutorOptions & ExecutorOptions::operator=(const ExecutorOptions & other) +{ + if (this == &other) { + return *this; + } + + this->memory_strategy = other.memory_strategy; + this->context = other.context; + this->max_conditions = other.max_conditions; + if (nullptr != other.impl_) { + this->impl_ = std::make_unique(*other.impl_); + } + + return *this; +} From 22df1d593a3e77917db8a7d7b20f57aee80d7e55 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Thu, 23 May 2024 17:53:35 -0700 Subject: [PATCH 386/455] rclcpp::shutdown should not be called before LifecycleNode dtor. (#2527) Signed-off-by: Tomoya Fujita --- .../test/test_lifecycle_publisher.cpp | 82 +++++++++---------- 1 file changed, 38 insertions(+), 44 deletions(-) diff --git a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp index ac93bd86a7..b98de77238 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp @@ -55,12 +55,6 @@ class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode explicit EmptyLifecycleNode(const std::string & node_name, const TimerType & timer_type) : rclcpp_lifecycle::LifecycleNode(node_name) { - rclcpp::PublisherOptionsWithAllocator> options; - publisher_ = - std::make_shared>( - get_node_base_interface().get(), std::string("topic"), rclcpp::QoS(10), options); - add_managed_entity(publisher_); - // For coverage this is being added here switch (timer_type) { case TimerType::WALL_TIMER: @@ -77,14 +71,6 @@ class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode } } } - - std::shared_ptr> publisher() - { - return publisher_; - } - -private: - std::shared_ptr> publisher_; }; class TestLifecyclePublisher : public ::testing::TestWithParam @@ -93,95 +79,103 @@ class TestLifecyclePublisher : public ::testing::TestWithParam void SetUp() { rclcpp::init(0, nullptr); - node_ = std::make_shared("node", GetParam()); } void TearDown() { rclcpp::shutdown(); } - -protected: - std::shared_ptr node_; }; TEST_P(TestLifecyclePublisher, publish_managed_by_node) { + auto node = std::make_shared("node", GetParam()); + + rclcpp::PublisherOptionsWithAllocator> options; + std::shared_ptr> publisher = + node->create_publisher(std::string("topic"), rclcpp::QoS(10), options); + // transition via LifecycleNode auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; auto ret = reset_key; - EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, node_->get_current_state().id()); - node_->trigger_transition( + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, node->get_current_state().id()); + node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE), ret); ASSERT_EQ(success, ret); ret = reset_key; - node_->trigger_transition( + node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE), ret); ASSERT_EQ(success, ret); ret = reset_key; - EXPECT_TRUE(node_->publisher()->is_activated()); + EXPECT_TRUE(publisher->is_activated()); { auto msg_ptr = std::make_unique(); - EXPECT_NO_THROW(node_->publisher()->publish(*msg_ptr)); + EXPECT_NO_THROW(publisher->publish(*msg_ptr)); } { auto msg_ptr = std::make_unique(); - EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr))); + EXPECT_NO_THROW(publisher->publish(std::move(msg_ptr))); } { - auto loaned_msg = node_->publisher()->borrow_loaned_message(); - EXPECT_NO_THROW(node_->publisher()->publish(std::move(loaned_msg))); + auto loaned_msg = publisher->borrow_loaned_message(); + EXPECT_NO_THROW(publisher->publish(std::move(loaned_msg))); } - node_->trigger_transition( + node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE), ret); ASSERT_EQ(success, ret); ret = reset_key; (void)ret; // Just to make clang happy - EXPECT_FALSE(node_->publisher()->is_activated()); + EXPECT_FALSE(publisher->is_activated()); { auto msg_ptr = std::make_unique(); - EXPECT_NO_THROW(node_->publisher()->publish(*msg_ptr)); + EXPECT_NO_THROW(publisher->publish(*msg_ptr)); } { auto msg_ptr = std::make_unique(); - EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr))); + EXPECT_NO_THROW(publisher->publish(std::move(msg_ptr))); } { - auto loaned_msg = node_->publisher()->borrow_loaned_message(); - EXPECT_NO_THROW(node_->publisher()->publish(std::move(loaned_msg))); + auto loaned_msg = publisher->borrow_loaned_message(); + EXPECT_NO_THROW(publisher->publish(std::move(loaned_msg))); } } TEST_P(TestLifecyclePublisher, publish) { + auto node = std::make_shared("node", GetParam()); + + rclcpp::PublisherOptionsWithAllocator> options; + std::shared_ptr> publisher = + node->create_publisher(std::string("topic"), rclcpp::QoS(10), options); + // transition via LifecyclePublisher - node_->publisher()->on_deactivate(); - EXPECT_FALSE(node_->publisher()->is_activated()); + publisher->on_deactivate(); + EXPECT_FALSE(publisher->is_activated()); { auto msg_ptr = std::make_unique(); - EXPECT_NO_THROW(node_->publisher()->publish(*msg_ptr)); + EXPECT_NO_THROW(publisher->publish(*msg_ptr)); } { auto msg_ptr = std::make_unique(); - EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr))); + EXPECT_NO_THROW(publisher->publish(std::move(msg_ptr))); } { - auto loaned_msg = node_->publisher()->borrow_loaned_message(); - EXPECT_NO_THROW(node_->publisher()->publish(std::move(loaned_msg))); + auto loaned_msg = publisher->borrow_loaned_message(); + EXPECT_NO_THROW(publisher->publish(std::move(loaned_msg))); } - node_->publisher()->on_activate(); - EXPECT_TRUE(node_->publisher()->is_activated()); + publisher->on_activate(); + EXPECT_TRUE(publisher->is_activated()); { auto msg_ptr = std::make_unique(); - EXPECT_NO_THROW(node_->publisher()->publish(*msg_ptr)); + EXPECT_NO_THROW(publisher->publish(*msg_ptr)); } { auto msg_ptr = std::make_unique(); - EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr))); + EXPECT_NO_THROW(publisher->publish(std::move(msg_ptr))); } { - auto loaned_msg = node_->publisher()->borrow_loaned_message(); - EXPECT_NO_THROW(node_->publisher()->publish(std::move(loaned_msg))); + auto loaned_msg = publisher->borrow_loaned_message(); + EXPECT_NO_THROW(publisher->publish(std::move(loaned_msg))); } } From 3bc364a10bd1bfe710307147174b37ef62562abd Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Wed, 29 May 2024 09:40:49 -0700 Subject: [PATCH 387/455] call shutdown in LifecycleNode dtor to avoid leaving the device in unknown state (2nd) (#2528) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Revert "Revert "call shutdown in LifecycleNode dtor to avoid leaving the device in un… (#2450)" (#2522)" This reverts commit 42b0b5775b4e68718c5949308c9e1a059930ded7. Signed-off-by: Tomoya Fujita * lifecycle node dtor shutdown should be called only in primary state. Signed-off-by: Tomoya Fujita * adjust warning message if the node is still in transition state in dtor. Signed-off-by: Tomoya Fujita --------- Signed-off-by: Tomoya Fujita --- rclcpp_lifecycle/src/lifecycle_node.cpp | 20 +++ rclcpp_lifecycle/test/test_lifecycle_node.cpp | 140 ++++++++++++++++++ 2 files changed, 160 insertions(+) diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 0c448cf5e6..ccbc388fd8 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -152,6 +152,26 @@ LifecycleNode::LifecycleNode( LifecycleNode::~LifecycleNode() { + auto current_state = LifecycleNode::get_current_state().id(); + // shutdown if necessary to avoid leaving the device in any other primary state + if (current_state < lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { + auto ret = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + auto finalized = LifecycleNode::shutdown(ret); + if (finalized.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED || + ret != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) + { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp_lifecycle"), + "Shutdown error in destruction of LifecycleNode: final state(%s)", + finalized.label().c_str()); + } + } else if (current_state > lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp_lifecycle"), + "Shutdown error in destruction of LifecycleNode: Node still in transition state(%u)", + current_state); + } + // release sub-interfaces in an order that allows them to consult with node_base during tear-down node_waitables_.reset(); node_time_source_.reset(); diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index fdb9be6153..58ce89897c 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -447,6 +447,146 @@ TEST_F(TestDefaultStateMachine, bad_mood) { EXPECT_EQ(1u, test_node->number_of_callbacks); } + +TEST_F(TestDefaultStateMachine, shutdown_from_each_primary_state) { + auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + + // PRIMARY_STATE_UNCONFIGURED to shutdown + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + } + + // PRIMARY_STATE_INACTIVE to shutdown + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + ret = reset_key; + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + } + + // PRIMARY_STATE_ACTIVE to shutdown + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + ret = reset_key; + auto activated = test_node->activate(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); + ret = reset_key; + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + } + + // PRIMARY_STATE_FINALIZED to shutdown + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + ret = reset_key; + auto activated = test_node->activate(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); + ret = reset_key; + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + ret = reset_key; + auto finalized_again = test_node->shutdown(ret); + EXPECT_EQ(reset_key, ret); + EXPECT_EQ(finalized_again.id(), State::PRIMARY_STATE_FINALIZED); + } +} + +TEST_F(TestDefaultStateMachine, test_shutdown_on_dtor) { + auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + + bool shutdown_cb_called = false; + auto on_shutdown_callback = + [&shutdown_cb_called](const rclcpp_lifecycle::State &) -> + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn { + shutdown_cb_called = true; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + }; + + // PRIMARY_STATE_UNCONFIGURED to shutdown via dtor + shutdown_cb_called = false; + { + auto test_node = std::make_shared("testnode"); + test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1)); + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + EXPECT_FALSE(shutdown_cb_called); + } + EXPECT_TRUE(shutdown_cb_called); + + // PRIMARY_STATE_INACTIVE to shutdown via dtor + shutdown_cb_called = false; + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1)); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + EXPECT_FALSE(shutdown_cb_called); + } + EXPECT_TRUE(shutdown_cb_called); + + // PRIMARY_STATE_ACTIVE to shutdown via dtor + shutdown_cb_called = false; + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1)); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + ret = reset_key; + auto activated = test_node->activate(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); + EXPECT_FALSE(shutdown_cb_called); + } + EXPECT_TRUE(shutdown_cb_called); + + // PRIMARY_STATE_FINALIZED to shutdown via dtor + shutdown_cb_called = false; + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1)); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + ret = reset_key; + auto activated = test_node->activate(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); + ret = reset_key; + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + EXPECT_TRUE(shutdown_cb_called); // should be called already + } + EXPECT_TRUE(shutdown_cb_called); +} + TEST_F(TestDefaultStateMachine, lifecycle_subscriber) { auto test_node = std::make_shared>("testnode"); From d8d83a0ee67b7b6427defc802005c382b218459d Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Thu, 30 May 2024 14:57:17 -0700 Subject: [PATCH 388/455] LifecycleNode shutdown on dtor only with valid context. (#2545) Signed-off-by: Tomoya Fujita --- rclcpp_lifecycle/src/lifecycle_node.cpp | 26 +++++++++++++++++-------- 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index ccbc388fd8..d484e9670b 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -155,15 +155,23 @@ LifecycleNode::~LifecycleNode() auto current_state = LifecycleNode::get_current_state().id(); // shutdown if necessary to avoid leaving the device in any other primary state if (current_state < lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { - auto ret = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - auto finalized = LifecycleNode::shutdown(ret); - if (finalized.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED || - ret != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) - { - RCLCPP_WARN( + if (node_base_->get_context()->is_valid()) { + auto ret = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + auto finalized = LifecycleNode::shutdown(ret); + if (finalized.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED || + ret != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) + { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp_lifecycle"), + "Shutdown error in destruction of LifecycleNode: final state(%s)", + finalized.label().c_str()); + } + } else { + // TODO(fujitatomoya): consider when context is gracefully shutdown before. + RCLCPP_DEBUG( rclcpp::get_logger("rclcpp_lifecycle"), - "Shutdown error in destruction of LifecycleNode: final state(%s)", - finalized.label().c_str()); + "Context invalid error in destruction of LifecycleNode: Node still in transition state(%u)", + current_state); } } else if (current_state > lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { RCLCPP_WARN( @@ -174,6 +182,7 @@ LifecycleNode::~LifecycleNode() // release sub-interfaces in an order that allows them to consult with node_base during tear-down node_waitables_.reset(); + node_type_descriptions_.reset(); node_time_source_.reset(); node_parameters_.reset(); node_clock_.reset(); @@ -182,6 +191,7 @@ LifecycleNode::~LifecycleNode() node_timers_.reset(); node_logging_.reset(); node_graph_.reset(); + node_base_.reset(); } const char * From 7c096888caf92aa7557e1d3efc5448b56d8ce81c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 5 Jun 2024 01:22:38 +0200 Subject: [PATCH 389/455] Add test creating two content filter topics with the same topic name (#2546) (#2549) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Mario-DL Co-authored-by: Mario Domínguez López <116071334+Mario-DL@users.noreply.github.com> --- .../test_subscription_content_filter.cpp | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp index 942aa1274e..372af03611 100644 --- a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp +++ b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp @@ -310,3 +310,25 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_fil } } } + +TEST_F( + CLASSNAME( + TestContentFilterSubscription, + RMW_IMPLEMENTATION), create_two_content_filters_with_same_topic_name_and_destroy) { + + // Create another content filter + auto options = rclcpp::SubscriptionOptions(); + + std::string filter_expression = "int32_value > %0"; + std::vector expression_parameters = {"4"}; + + options.content_filter_options.filter_expression = filter_expression; + options.content_filter_options.expression_parameters = expression_parameters; + + auto callback = [](std::shared_ptr) {}; + auto sub_2 = node->create_subscription( + "content_filter_topic", qos, callback, options); + + EXPECT_NE(nullptr, sub_2); + sub_2.reset(); +} From f4923c6f43afaf983f95158584d37687f2679925 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Tue, 11 Jun 2024 07:41:09 -0700 Subject: [PATCH 390/455] revert call shutdown in LifecycleNode destructor (#2557) * Revert "LifecycleNode shutdown on dtor only with valid context. (#2545)" This reverts commit d8d83a0ee67b7b6427defc802005c382b218459d. Signed-off-by: Tomoya Fujita * Revert "call shutdown in LifecycleNode dtor to avoid leaving the device in unknown state (2nd) (#2528)" This reverts commit 3bc364a10bd1bfe710307147174b37ef62562abd. Signed-off-by: Tomoya Fujita --------- Signed-off-by: Tomoya Fujita --- rclcpp_lifecycle/src/lifecycle_node.cpp | 30 ---- rclcpp_lifecycle/test/test_lifecycle_node.cpp | 140 ------------------ 2 files changed, 170 deletions(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index d484e9670b..0c448cf5e6 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -152,37 +152,8 @@ LifecycleNode::LifecycleNode( LifecycleNode::~LifecycleNode() { - auto current_state = LifecycleNode::get_current_state().id(); - // shutdown if necessary to avoid leaving the device in any other primary state - if (current_state < lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { - if (node_base_->get_context()->is_valid()) { - auto ret = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - auto finalized = LifecycleNode::shutdown(ret); - if (finalized.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED || - ret != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) - { - RCLCPP_WARN( - rclcpp::get_logger("rclcpp_lifecycle"), - "Shutdown error in destruction of LifecycleNode: final state(%s)", - finalized.label().c_str()); - } - } else { - // TODO(fujitatomoya): consider when context is gracefully shutdown before. - RCLCPP_DEBUG( - rclcpp::get_logger("rclcpp_lifecycle"), - "Context invalid error in destruction of LifecycleNode: Node still in transition state(%u)", - current_state); - } - } else if (current_state > lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { - RCLCPP_WARN( - rclcpp::get_logger("rclcpp_lifecycle"), - "Shutdown error in destruction of LifecycleNode: Node still in transition state(%u)", - current_state); - } - // release sub-interfaces in an order that allows them to consult with node_base during tear-down node_waitables_.reset(); - node_type_descriptions_.reset(); node_time_source_.reset(); node_parameters_.reset(); node_clock_.reset(); @@ -191,7 +162,6 @@ LifecycleNode::~LifecycleNode() node_timers_.reset(); node_logging_.reset(); node_graph_.reset(); - node_base_.reset(); } const char * diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 58ce89897c..fdb9be6153 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -447,146 +447,6 @@ TEST_F(TestDefaultStateMachine, bad_mood) { EXPECT_EQ(1u, test_node->number_of_callbacks); } - -TEST_F(TestDefaultStateMachine, shutdown_from_each_primary_state) { - auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - - // PRIMARY_STATE_UNCONFIGURED to shutdown - { - auto ret = reset_key; - auto test_node = std::make_shared("testnode"); - auto finalized = test_node->shutdown(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); - } - - // PRIMARY_STATE_INACTIVE to shutdown - { - auto ret = reset_key; - auto test_node = std::make_shared("testnode"); - auto configured = test_node->configure(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); - ret = reset_key; - auto finalized = test_node->shutdown(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); - } - - // PRIMARY_STATE_ACTIVE to shutdown - { - auto ret = reset_key; - auto test_node = std::make_shared("testnode"); - auto configured = test_node->configure(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); - ret = reset_key; - auto activated = test_node->activate(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); - ret = reset_key; - auto finalized = test_node->shutdown(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); - } - - // PRIMARY_STATE_FINALIZED to shutdown - { - auto ret = reset_key; - auto test_node = std::make_shared("testnode"); - auto configured = test_node->configure(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); - ret = reset_key; - auto activated = test_node->activate(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); - ret = reset_key; - auto finalized = test_node->shutdown(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); - ret = reset_key; - auto finalized_again = test_node->shutdown(ret); - EXPECT_EQ(reset_key, ret); - EXPECT_EQ(finalized_again.id(), State::PRIMARY_STATE_FINALIZED); - } -} - -TEST_F(TestDefaultStateMachine, test_shutdown_on_dtor) { - auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - - bool shutdown_cb_called = false; - auto on_shutdown_callback = - [&shutdown_cb_called](const rclcpp_lifecycle::State &) -> - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn { - shutdown_cb_called = true; - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - }; - - // PRIMARY_STATE_UNCONFIGURED to shutdown via dtor - shutdown_cb_called = false; - { - auto test_node = std::make_shared("testnode"); - test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1)); - EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); - EXPECT_FALSE(shutdown_cb_called); - } - EXPECT_TRUE(shutdown_cb_called); - - // PRIMARY_STATE_INACTIVE to shutdown via dtor - shutdown_cb_called = false; - { - auto ret = reset_key; - auto test_node = std::make_shared("testnode"); - test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1)); - auto configured = test_node->configure(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); - EXPECT_FALSE(shutdown_cb_called); - } - EXPECT_TRUE(shutdown_cb_called); - - // PRIMARY_STATE_ACTIVE to shutdown via dtor - shutdown_cb_called = false; - { - auto ret = reset_key; - auto test_node = std::make_shared("testnode"); - test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1)); - auto configured = test_node->configure(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); - ret = reset_key; - auto activated = test_node->activate(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); - EXPECT_FALSE(shutdown_cb_called); - } - EXPECT_TRUE(shutdown_cb_called); - - // PRIMARY_STATE_FINALIZED to shutdown via dtor - shutdown_cb_called = false; - { - auto ret = reset_key; - auto test_node = std::make_shared("testnode"); - test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1)); - auto configured = test_node->configure(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); - ret = reset_key; - auto activated = test_node->activate(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); - ret = reset_key; - auto finalized = test_node->shutdown(ret); - EXPECT_EQ(success, ret); - EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); - EXPECT_TRUE(shutdown_cb_called); // should be called already - } - EXPECT_TRUE(shutdown_cb_called); -} - TEST_F(TestDefaultStateMachine, lifecycle_subscriber) { auto test_node = std::make_shared>("testnode"); From 16795dd8bf4be86bd6ebb3691499b5129dbad796 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 17 Jun 2024 14:22:56 +0000 Subject: [PATCH 391/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 8 ++++++++ rclcpp_action/CHANGELOG.rst | 5 +++++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 10 ++++++++++ 4 files changed, 26 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 4c71d31267..3e356d8c18 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add test creating two content filter topics with the same topic name (`#2546 `_) (`#2549 `_) +* add impl pointer for ExecutorOptions (`#2523 `_) +* Fixup Executor::spin_all() regression fix (`#2517 `_) +* Add 'mimick' label to tests which use Mimick (`#2516 `_) +* Contributors: Alejandro Hernández Cordero, Scott K Logan, William Woodall + 28.2.0 (2024-04-26) ------------------- * Check for negative time in rclcpp::Time(int64_t nanoseconds, ...) constructor (`#2510 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 60ab9976ca..987dc95c01 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add 'mimick' label to tests which use Mimick (`#2516 `_) +* Contributors: Scott K Logan + 28.2.0 (2024-04-26) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index d43eadc36c..bd78775f75 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 28.2.0 (2024-04-26) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 00728b1934..04501944d7 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,16 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* revert call shutdown in LifecycleNode destructor (`#2557 `_) +* LifecycleNode shutdown on dtor only with valid context. (`#2545 `_) +* call shutdown in LifecycleNode dtor to avoid leaving the device in unknown state (2nd) (`#2528 `_) +* rclcpp::shutdown should not be called before LifecycleNode dtor. (`#2527 `_) +* Revert "call shutdown in LifecycleNode dtor to avoid leaving the device in un… (`#2450 `_)" (`#2522 `_) +* Add 'mimick' label to tests which use Mimick (`#2516 `_) +* Contributors: Chris Lalancette, Scott K Logan, Tomoya Fujita + 28.2.0 (2024-04-26) ------------------- From 5149a095c16ef4660c08eedf39a49ca08ec811de Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 17 Jun 2024 14:23:03 +0000 Subject: [PATCH 392/455] 28.3.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 3e356d8c18..8445283f00 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.3.0 (2024-06-17) +------------------- * Add test creating two content filter topics with the same topic name (`#2546 `_) (`#2549 `_) * add impl pointer for ExecutorOptions (`#2523 `_) * Fixup Executor::spin_all() regression fix (`#2517 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index dacffd678f..949f2b9bef 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 28.2.0 + 28.3.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 987dc95c01..73689a94df 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.3.0 (2024-06-17) +------------------- * Add 'mimick' label to tests which use Mimick (`#2516 `_) * Contributors: Scott K Logan diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index a1ffdd5800..319ac74fee 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 28.2.0 + 28.3.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index bd78775f75..70dc592da5 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.3.0 (2024-06-17) +------------------- 28.2.0 (2024-04-26) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 28e9ad4d20..93ec5d39ca 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 28.2.0 + 28.3.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 04501944d7..413fb39ff7 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.3.0 (2024-06-17) +------------------- * revert call shutdown in LifecycleNode destructor (`#2557 `_) * LifecycleNode shutdown on dtor only with valid context. (`#2545 `_) * call shutdown in LifecycleNode dtor to avoid leaving the device in unknown state (2nd) (`#2528 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index a621ab2f39..77d4b5e571 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 28.2.0 + 28.3.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 9f1de85079996ad567adbccfb4d9f73f28cf15e1 Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Wed, 19 Jun 2024 15:07:14 -0400 Subject: [PATCH 393/455] Fix typo in function doc (#2563) Signed-off-by: Christophe Bedard --- rclcpp/include/rclcpp/clock.hpp | 6 +++--- rclcpp_action/include/rclcpp_action/create_server.hpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index e73f3849d3..2050bf53e6 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -217,13 +217,13 @@ class Clock std::mutex & get_clock_mutex() noexcept; - // Add a callback to invoke if the jump threshold is exceeded. + /// Add a callback to invoke if the jump threshold is exceeded. /** * These callback functions must remain valid as long as the * returned shared pointer is valid. * * Function will register callbacks to the callback queue. On time jump all - * callbacks will be executed whose threshold is greater then the time jump; + * callbacks will be executed whose threshold is greater than the time jump; * The logic will first call selected pre_callbacks and then all selected * post_callbacks. * @@ -232,7 +232,7 @@ class Clock * \param pre_callback Must be non-throwing * \param post_callback Must be non-throwing. * \param threshold Callbacks will be triggered if the time jump is greater - * then the threshold. + * than the threshold. * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. * \throws std::bad_alloc if the allocation of the JumpHandler fails. * \warning the instance of the clock must remain valid as long as any created diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index 3aa0e4dea7..c333a60581 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -116,7 +116,7 @@ create_server( * * \sa Server::Server() for more information. * - * \param[in] node] The action server will be added to this node. + * \param[in] node The action server will be added to this node. * \param[in] name The action name. * \param[in] handle_goal A callback that decides if a goal should be accepted or rejected. * \param[in] handle_cancel A callback that decides if a goal should be attempted to be canceled. From a13dc0f1575d905505cce343532f3501f7bbc179 Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Wed, 19 Jun 2024 19:10:21 -0400 Subject: [PATCH 394/455] Fix copy-paste errors in function docs (#2565) Signed-off-by: Christophe Bedard --- rclcpp/include/rclcpp/client.hpp | 2 +- rclcpp/include/rclcpp/publisher.hpp | 4 ++-- rclcpp/include/rclcpp/service.hpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index fdba5160a8..fb7547b424 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -490,7 +490,7 @@ class Client : public ClientBase * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup. * \param[in] node_graph The node graph interface of the corresponding node. * \param[in] service_name Name of the topic to publish to. - * \param[in] client_options options for the subscription. + * \param[in] client_options options for the client. */ Client( rclcpp::node_interfaces::NodeBaseInterface * node_base, diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index c6f16643c6..3f02be1e55 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -128,8 +128,8 @@ class Publisher : public PublisherBase * * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup. * \param[in] topic Name of the topic to publish to. - * \param[in] qos QoS profile for Subcription. - * \param[in] options Options for the subscription. + * \param[in] qos QoS profile for the publisher. + * \param[in] options Options for the publisher. */ Publisher( rclcpp::node_interfaces::NodeBaseInterface * node_base, diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 925c5f6286..8296cb5962 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -307,7 +307,7 @@ class Service * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup. * \param[in] service_name Name of the topic to publish to. * \param[in] any_callback User defined callback to call when a client request is received. - * \param[in] service_options options for the subscription. + * \param[in] service_options options for the service. */ Service( std::shared_ptr node_handle, From eeaa5222a1d11d8e83d4e1b9092b11ee7bc5f39d Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Thu, 20 Jun 2024 20:54:31 -0400 Subject: [PATCH 395/455] Remove unnecessary msg includes in tests (#2566) Signed-off-by: Christophe Bedard --- rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp | 1 - rclcpp/test/rclcpp/test_any_service_callback.cpp | 1 - rclcpp/test/rclcpp/test_any_subscription_callback.cpp | 1 - rclcpp/test/rclcpp/test_create_subscription.cpp | 1 - 4 files changed, 4 deletions(-) diff --git a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp index fefec6c8fa..70389df1a5 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -23,7 +23,6 @@ #include "rclcpp/node.hpp" #include "test_msgs/msg/empty.hpp" -#include "test_msgs/msg/empty.h" #include "rclcpp/exceptions.hpp" #include "rclcpp/executors.hpp" diff --git a/rclcpp/test/rclcpp/test_any_service_callback.cpp b/rclcpp/test/rclcpp/test_any_service_callback.cpp index b6e49097c3..ef46155d99 100644 --- a/rclcpp/test/rclcpp/test_any_service_callback.cpp +++ b/rclcpp/test/rclcpp/test_any_service_callback.cpp @@ -24,7 +24,6 @@ #include "rclcpp/any_service_callback.hpp" #include "rclcpp/service.hpp" #include "test_msgs/srv/empty.hpp" -#include "test_msgs/srv/empty.h" class TestAnyServiceCallback : public ::testing::Test { diff --git a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp index a98271e931..58a9211937 100644 --- a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp +++ b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp @@ -21,7 +21,6 @@ #include "rclcpp/any_subscription_callback.hpp" #include "test_msgs/msg/empty.hpp" -#include "test_msgs/msg/empty.h" // Type adapter to be used in tests. struct MyEmpty {}; diff --git a/rclcpp/test/rclcpp/test_create_subscription.cpp b/rclcpp/test/rclcpp/test_create_subscription.cpp index fd947485e2..06536116a1 100644 --- a/rclcpp/test/rclcpp/test_create_subscription.cpp +++ b/rclcpp/test/rclcpp/test_create_subscription.cpp @@ -20,7 +20,6 @@ #include "rclcpp/create_subscription.hpp" #include "rclcpp/node.hpp" #include "test_msgs/msg/empty.hpp" -#include "test_msgs/msg/empty.h" using namespace std::chrono_literals; From 7d68b9096ff76f7b21b8e655ba1518db9b0273f3 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 25 Jun 2024 17:45:33 +0000 Subject: [PATCH 396/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 7 +++++++ rclcpp_action/CHANGELOG.rst | 5 +++++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 3 +++ 4 files changed, 18 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 8445283f00..9f99bfeadd 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove unnecessary msg includes in tests (`#2566 `_) +* Fix copy-paste errors in function docs (`#2565 `_) +* Fix typo in function doc (`#2563 `_) +* Contributors: Christophe Bedard + 28.3.0 (2024-06-17) ------------------- * Add test creating two content filter topics with the same topic name (`#2546 `_) (`#2549 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 73689a94df..410c80c417 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix typo in function doc (`#2563 `_) +* Contributors: Christophe Bedard + 28.3.0 (2024-06-17) ------------------- * Add 'mimick' label to tests which use Mimick (`#2516 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 70dc592da5..29f0509dde 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 28.3.0 (2024-06-17) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 413fb39ff7..6464b6f02b 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 28.3.0 (2024-06-17) ------------------- * revert call shutdown in LifecycleNode destructor (`#2557 `_) From 8230d15ef7ffa9e824d7e0f9c623537ae331dc73 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 25 Jun 2024 17:45:39 +0000 Subject: [PATCH 397/455] 28.3.1 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 9f99bfeadd..fb5ce43dc3 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.3.1 (2024-06-25) +------------------- * Remove unnecessary msg includes in tests (`#2566 `_) * Fix copy-paste errors in function docs (`#2565 `_) * Fix typo in function doc (`#2563 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 949f2b9bef..95e917b220 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 28.3.0 + 28.3.1 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 410c80c417..5db060d317 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.3.1 (2024-06-25) +------------------- * Fix typo in function doc (`#2563 `_) * Contributors: Christophe Bedard diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 319ac74fee..9fc070e41e 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 28.3.0 + 28.3.1 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 29f0509dde..3ee4cd5d1a 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.3.1 (2024-06-25) +------------------- 28.3.0 (2024-06-17) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 93ec5d39ca..08ebc6d049 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 28.3.0 + 28.3.1 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 6464b6f02b..0696dd8893 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.3.1 (2024-06-25) +------------------- 28.3.0 (2024-06-17) ------------------- diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 77d4b5e571..71d8599a00 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 28.3.0 + 28.3.1 Package containing a prototype for lifecycle implementation Ivan Paunovic From 8de4b90512469d74802871e6c88db2e3135720e7 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Sat, 29 Jun 2024 14:55:28 -0700 Subject: [PATCH 398/455] avoid adding notify waitable twice to events-executor collection (#2564) * avoid adding notify waitable twice to events-executor entities collection Signed-off-by: Alberto Soragna * remove redundant mutex lock Signed-off-by: Alberto Soragna --------- Signed-off-by: Alberto Soragna --- .../events_executor/events_executor.cpp | 9 +-- .../test/rclcpp/executors/test_executors.cpp | 61 +++++++++++++++++++ 2 files changed, 64 insertions(+), 6 deletions(-) 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 ce6a103ab2..c7d6be7e44 100644 --- a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp +++ b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp @@ -415,14 +415,11 @@ EventsExecutor::refresh_current_collection_from_callback_groups() // We could explicitly check for the notify waitable ID when we receive a waitable event // but I think that it's better if the waitable was in the collection and it could be // retrieved in the "standard" way. - // To do it, we need to add the notify waitable as an entry in both the new and - // current collections such that it's neither added or removed. + // To do it, we need to add the notify waitable as an entry in the new collection + // such that it's neither added or removed (it should have already been added + // to the current collection in the constructor) this->add_notify_waitable_to_collection(new_collection.waitables); - // Acquire lock before modifying the current collection - std::lock_guard lock(collection_mutex_); - this->add_notify_waitable_to_collection(current_entities_collection_->waitables); - this->refresh_current_collection(new_collection); } diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index a82b702db5..dfbdbb8f4c 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -807,6 +807,67 @@ TYPED_TEST(TestExecutors, testRaceConditionAddNode) } } +// Check that executors are correctly notified while they are spinning +// we notify twice to ensure that the notify waitable is still working +// after the first notification +TYPED_TEST(TestExecutors, notifyTwiceWhileSpinning) +{ + using ExecutorType = TypeParam; + + // Create executor, add the node and start spinning + ExecutorType executor; + executor.add_node(this->node); + std::thread spinner([&]() {executor.spin();}); + + // Wait for executor to be spinning + while (!executor.is_spinning()) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + + // Create the first subscription while the executor is already spinning + std::atomic sub1_msg_count {0}; + auto sub1 = this->node->template create_subscription( + this->publisher->get_topic_name(), + rclcpp::QoS(10), + [&sub1_msg_count](test_msgs::msg::Empty::ConstSharedPtr) { + sub1_msg_count++; + }); + + // Publish a message and verify it's received + this->publisher->publish(test_msgs::msg::Empty()); + auto start = std::chrono::steady_clock::now(); + while (sub1_msg_count == 0 && (std::chrono::steady_clock::now() - start) < 10s) { + std::this_thread::sleep_for(1ms); + } + EXPECT_EQ(sub1_msg_count, 1u); + + // Create a second subscription while the executor is already spinning + std::atomic sub2_msg_count {0}; + auto sub2 = this->node->template create_subscription( + this->publisher->get_topic_name(), + rclcpp::QoS(10), + [&sub2_msg_count](test_msgs::msg::Empty::ConstSharedPtr) { + sub2_msg_count++; + }); + + // Publish a message and verify it's received by both subscriptions + this->publisher->publish(test_msgs::msg::Empty()); + start = std::chrono::steady_clock::now(); + while ( + sub1_msg_count == 1 && + sub2_msg_count == 0 && + (std::chrono::steady_clock::now() - start) < 10s) + { + std::this_thread::sleep_for(1ms); + } + EXPECT_EQ(sub1_msg_count, 2u); + EXPECT_EQ(sub2_msg_count, 1u); + + // Cancel needs to be called before join, so that executor.spin() returns. + executor.cancel(); + spinner.join(); +} + // Check spin_until_future_complete with node base pointer (instantiates its own executor) TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) { From c743c173e68d92af872cf163e10721a8dbe51dd0 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 2 Jul 2024 07:55:11 -0400 Subject: [PATCH 399/455] Split test_executors.cpp even further. (#2572) That's because it is too large for Windows Debug to compile, so split into smaller bits. Even with this split, the file is too big; that's likely because we are using TYPED_TEST here, which generates multiple symbols per test case. To deal with this, without further breaking up the file, also add in the /bigobj flag when compiling on Windows Debug. Signed-off-by: Chris Lalancette --- rclcpp/test/rclcpp/CMakeLists.txt | 14 + .../test/rclcpp/executors/test_executors.cpp | 261 +----------------- .../executors/test_executors_busy_waiting.cpp | 181 ++++++++++++ .../test/rclcpp/executors/test_waitable.cpp | 125 +++++++++ .../test/rclcpp/executors/test_waitable.hpp | 75 +++++ 5 files changed, 396 insertions(+), 260 deletions(-) create mode 100644 rclcpp/test/rclcpp/executors/test_executors_busy_waiting.cpp create mode 100644 rclcpp/test/rclcpp/executors/test_waitable.cpp create mode 100644 rclcpp/test/rclcpp/executors/test_waitable.hpp diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 036a33f527..b950bd149b 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -486,8 +486,12 @@ endif() ament_add_gtest( test_executors executors/test_executors.cpp + executors/test_waitable.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 180) +if(CMAKE_BUILD_TYPE STREQUAL "Debug" AND MSVC) + target_compile_options(test_executors PRIVATE "/bigobj") +endif() if(TARGET test_executors) target_link_libraries(test_executors ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) endif() @@ -519,6 +523,16 @@ if(TARGET test_executors) target_link_libraries(test_executors_intraprocess ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() +ament_add_gtest( + test_executors_busy_waiting + executors/test_executors_busy_waiting.cpp + executors/test_waitable.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) +if(TARGET test_executors) + target_link_libraries(test_executors_busy_waiting ${PROJECT_NAME}) +endif() + ament_add_gtest(test_static_single_threaded_executor executors/test_static_single_threaded_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") ament_add_test_label(test_static_single_threaded_executor mimick) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index dfbdbb8f4c..5aae03c5bc 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -41,6 +41,7 @@ #include "test_msgs/msg/empty.hpp" #include "./executor_types.hpp" +#include "./test_waitable.hpp" using namespace std::chrono_literals; @@ -331,114 +332,6 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) spinner.join(); } -class TestWaitable : public rclcpp::Waitable -{ -public: - TestWaitable() = default; - - void - 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. - // This is necessary if trigger() results in the wait set waking, but not - // 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_); - } - - void trigger() - { - trigger_count_++; - gc_.trigger(); - } - - bool - is_ready(const rcl_wait_set_t & wait_set) override - { - is_ready_count_++; - 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; - } - } - return false; - } - - std::shared_ptr - take_data() override - { - return nullptr; - } - - std::shared_ptr - take_data_by_entity_id(size_t id) override - { - (void) id; - return nullptr; - } - - void - execute(const std::shared_ptr &) override - { - trigger_count_--; - count_++; - if (nullptr != on_execute_callback_) { - on_execute_callback_(); - } else { - // TODO(wjwwood): I don't know why this was here, but probably it should - // not be there, or test cases where that is important should use the - // on_execute_callback? - std::this_thread::sleep_for(3ms); - } - } - - void - set_on_execute_callback(std::function on_execute_callback) - { - on_execute_callback_ = on_execute_callback; - } - - void - set_on_ready_callback(std::function callback) override - { - auto gc_callback = [callback](size_t count) { - callback(count, 0); - }; - gc_.set_on_trigger_callback(gc_callback); - } - - void - clear_on_ready_callback() override - { - gc_.set_on_trigger_callback(nullptr); - } - - size_t - get_number_of_ready_guard_conditions() override {return 1;} - - size_t - get_count() const - { - return count_; - } - - size_t - get_is_ready_call_count() const - { - return is_ready_count_; - } - -private: - std::atomic trigger_count_ = 0; - std::atomic is_ready_count_ = 0; - std::atomic count_ = 0; - rclcpp::GuardCondition gc_; - std::function on_execute_callback_ = nullptr; -}; - TYPED_TEST(TestExecutors, spinAll) { using ExecutorType = TypeParam; @@ -938,155 +831,3 @@ TEST(TestExecutors, testSpinWithNonDefaultContext) rclcpp::shutdown(non_default_context); } - -template -class TestBusyWaiting : public ::testing::Test -{ -public: - void SetUp() override - { - rclcpp::init(0, nullptr); - - const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); - std::stringstream test_name; - test_name << test_info->test_case_name() << "_" << test_info->name(); - node = std::make_shared("node", test_name.str()); - callback_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive, - /* automatically_add_to_executor_with_node =*/ false); - - auto waitable_interfaces = node->get_node_waitables_interface(); - waitable = std::make_shared(); - waitable_interfaces->add_waitable(waitable, callback_group); - - executor = std::make_shared(); - executor->add_callback_group(callback_group, node->get_node_base_interface()); - } - - void TearDown() override - { - rclcpp::shutdown(); - } - - void - set_up_and_trigger_waitable(std::function extra_callback = nullptr) - { - this->has_executed = false; - this->waitable->set_on_execute_callback([this, extra_callback]() { - if (!this->has_executed) { - // trigger once to see if the second trigger is handled or not - // this follow up trigger simulates new entities becoming ready while - // the executor is executing something else, e.g. subscription got data - // or a timer expired, etc. - // spin_some would not handle this second trigger, since it collects - // work only once, whereas spin_all should handle it since it - // collects work multiple times - this->waitable->trigger(); - this->has_executed = true; - } - if (nullptr != extra_callback) { - extra_callback(); - } - }); - this->waitable->trigger(); - } - - void - check_for_busy_waits(std::chrono::steady_clock::time_point start_time) - { - // rough time based check, since the work to be done was very small it - // should be safe to check that we didn't use more than half the - // max duration, which itself is much larger than necessary - // however, it could still produce a false-positive - EXPECT_LT( - std::chrono::steady_clock::now() - start_time, - max_duration / 2) - << "executor took a long time to execute when it should have done " - << "nothing and should not have blocked either, but this could be a " - << "false negative if the computer is really slow"; - - // this check is making some assumptions about the implementation of the - // executors, but it should be safe to say that a busy wait may result in - // hundreds or thousands of calls to is_ready(), but "normal" executor - // behavior should be within an order of magnitude of the number of - // times that the waitable was executed - ASSERT_LT(waitable->get_is_ready_call_count(), 10u * this->waitable->get_count()); - } - - static constexpr auto max_duration = 10s; - - rclcpp::Node::SharedPtr node; - rclcpp::CallbackGroup::SharedPtr callback_group; - std::shared_ptr waitable; - std::chrono::steady_clock::time_point start_time; - std::shared_ptr executor; - bool has_executed; -}; - -TYPED_TEST_SUITE(TestBusyWaiting, ExecutorTypes, ExecutorTypeNames); - -TYPED_TEST(TestBusyWaiting, test_spin_all) -{ - this->set_up_and_trigger_waitable(); - - auto start_time = std::chrono::steady_clock::now(); - this->executor->spin_all(this->max_duration); - this->check_for_busy_waits(start_time); - // this should get the initial trigger, and the follow up from in the callback - ASSERT_EQ(this->waitable->get_count(), 2u); -} - -TYPED_TEST(TestBusyWaiting, test_spin_some) -{ - this->set_up_and_trigger_waitable(); - - auto start_time = std::chrono::steady_clock::now(); - this->executor->spin_some(this->max_duration); - this->check_for_busy_waits(start_time); - // this should get the inital trigger, but not the follow up in the callback - ASSERT_EQ(this->waitable->get_count(), 1u); -} - -TYPED_TEST(TestBusyWaiting, test_spin) -{ - std::condition_variable cv; - std::mutex cv_m; - bool first_check_passed = false; - - this->set_up_and_trigger_waitable([&cv, &cv_m, &first_check_passed]() { - cv.notify_one(); - if (!first_check_passed) { - std::unique_lock lk(cv_m); - cv.wait_for(lk, 1s, [&]() {return first_check_passed;}); - } - }); - - auto start_time = std::chrono::steady_clock::now(); - std::thread t([this]() { - this->executor->spin(); - }); - - // wait until thread has started (first execute of waitable) - { - std::unique_lock lk(cv_m); - cv.wait_for(lk, 10s); - } - EXPECT_GT(this->waitable->get_count(), 0u); - - first_check_passed = true; - cv.notify_one(); - - // wait until the executor has finished (second execute of waitable) - { - std::unique_lock lk(cv_m); - cv.wait_for(lk, 10s); - } - EXPECT_EQ(this->waitable->get_count(), 2u); - - this->executor->cancel(); - t.join(); - - this->check_for_busy_waits(start_time); - // this should get the initial trigger, and the follow up from in the callback - ASSERT_EQ(this->waitable->get_count(), 2u); -} diff --git a/rclcpp/test/rclcpp/executors/test_executors_busy_waiting.cpp b/rclcpp/test/rclcpp/executors/test_executors_busy_waiting.cpp new file mode 100644 index 0000000000..bc3ce4b62d --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executors_busy_waiting.cpp @@ -0,0 +1,181 @@ +// Copyright 2024 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. + +#include + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "./executor_types.hpp" +#include "./test_waitable.hpp" + +using namespace std::chrono_literals; + +template +class TestBusyWaiting : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + + const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); + std::stringstream test_name; + test_name << test_info->test_case_name() << "_" << test_info->name(); + node = std::make_shared("node", test_name.str()); + callback_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + /* automatically_add_to_executor_with_node =*/ false); + + auto waitable_interfaces = node->get_node_waitables_interface(); + waitable = std::make_shared(); + waitable_interfaces->add_waitable(waitable, callback_group); + + executor = std::make_shared(); + executor->add_callback_group(callback_group, node->get_node_base_interface()); + } + + void TearDown() override + { + rclcpp::shutdown(); + } + + void + set_up_and_trigger_waitable(std::function extra_callback = nullptr) + { + this->has_executed = false; + this->waitable->set_on_execute_callback([this, extra_callback]() { + if (!this->has_executed) { + // trigger once to see if the second trigger is handled or not + // this follow up trigger simulates new entities becoming ready while + // the executor is executing something else, e.g. subscription got data + // or a timer expired, etc. + // spin_some would not handle this second trigger, since it collects + // work only once, whereas spin_all should handle it since it + // collects work multiple times + this->waitable->trigger(); + this->has_executed = true; + } + if (nullptr != extra_callback) { + extra_callback(); + } + }); + this->waitable->trigger(); + } + + void + check_for_busy_waits(std::chrono::steady_clock::time_point start_time) + { + // rough time based check, since the work to be done was very small it + // should be safe to check that we didn't use more than half the + // max duration, which itself is much larger than necessary + // however, it could still produce a false-positive + EXPECT_LT( + std::chrono::steady_clock::now() - start_time, + max_duration / 2) + << "executor took a long time to execute when it should have done " + << "nothing and should not have blocked either, but this could be a " + << "false negative if the computer is really slow"; + + // this check is making some assumptions about the implementation of the + // executors, but it should be safe to say that a busy wait may result in + // hundreds or thousands of calls to is_ready(), but "normal" executor + // behavior should be within an order of magnitude of the number of + // times that the waitable was executed + ASSERT_LT(waitable->get_is_ready_call_count(), 10u * this->waitable->get_count()); + } + + static constexpr auto max_duration = 10s; + + rclcpp::Node::SharedPtr node; + rclcpp::CallbackGroup::SharedPtr callback_group; + std::shared_ptr waitable; + std::chrono::steady_clock::time_point start_time; + std::shared_ptr executor; + bool has_executed; +}; + +TYPED_TEST_SUITE(TestBusyWaiting, ExecutorTypes, ExecutorTypeNames); + +TYPED_TEST(TestBusyWaiting, test_spin_all) +{ + this->set_up_and_trigger_waitable(); + + auto start_time = std::chrono::steady_clock::now(); + this->executor->spin_all(this->max_duration); + this->check_for_busy_waits(start_time); + // this should get the initial trigger, and the follow up from in the callback + ASSERT_EQ(this->waitable->get_count(), 2u); +} + +TYPED_TEST(TestBusyWaiting, test_spin_some) +{ + this->set_up_and_trigger_waitable(); + + auto start_time = std::chrono::steady_clock::now(); + this->executor->spin_some(this->max_duration); + this->check_for_busy_waits(start_time); + // this should get the inital trigger, but not the follow up in the callback + ASSERT_EQ(this->waitable->get_count(), 1u); +} + +TYPED_TEST(TestBusyWaiting, test_spin) +{ + std::condition_variable cv; + std::mutex cv_m; + bool first_check_passed = false; + + this->set_up_and_trigger_waitable([&cv, &cv_m, &first_check_passed]() { + cv.notify_one(); + if (!first_check_passed) { + std::unique_lock lk(cv_m); + cv.wait_for(lk, 1s, [&]() {return first_check_passed;}); + } + }); + + auto start_time = std::chrono::steady_clock::now(); + std::thread t([this]() { + this->executor->spin(); + }); + + // wait until thread has started (first execute of waitable) + { + std::unique_lock lk(cv_m); + cv.wait_for(lk, 10s); + } + EXPECT_GT(this->waitable->get_count(), 0u); + + first_check_passed = true; + cv.notify_one(); + + // wait until the executor has finished (second execute of waitable) + { + std::unique_lock lk(cv_m); + cv.wait_for(lk, 10s); + } + EXPECT_EQ(this->waitable->get_count(), 2u); + + this->executor->cancel(); + t.join(); + + this->check_for_busy_waits(start_time); + // this should get the initial trigger, and the follow up from in the callback + ASSERT_EQ(this->waitable->get_count(), 2u); +} diff --git a/rclcpp/test/rclcpp/executors/test_waitable.cpp b/rclcpp/test/rclcpp/executors/test_waitable.cpp new file mode 100644 index 0000000000..dfae485f30 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_waitable.cpp @@ -0,0 +1,125 @@ +// Copyright 2024 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. + +#include +#include +#include +#include + +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" +#include "rclcpp/waitable.hpp" + +#include "rcl/wait.h" + +#include "test_waitable.hpp" + +using namespace std::chrono_literals; + +void +TestWaitable::add_to_wait_set(rcl_wait_set_t & wait_set) +{ + if (trigger_count_ > 0) { + // Keep the gc triggered until the trigger count is reduced back to zero. + // This is necessary if trigger() results in the wait set waking, but not + // 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_); +} + +void TestWaitable::trigger() +{ + trigger_count_++; + gc_.trigger(); +} + +bool +TestWaitable::is_ready(const rcl_wait_set_t & wait_set) +{ + is_ready_count_++; + 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; + } + } + return false; +} + +std::shared_ptr +TestWaitable::take_data() +{ + return nullptr; +} + +std::shared_ptr +TestWaitable::take_data_by_entity_id(size_t id) +{ + (void) id; + return nullptr; +} + +void +TestWaitable::execute(const std::shared_ptr &) +{ + trigger_count_--; + count_++; + if (nullptr != on_execute_callback_) { + on_execute_callback_(); + } else { + // TODO(wjwwood): I don't know why this was here, but probably it should + // not be there, or test cases where that is important should use the + // on_execute_callback? + std::this_thread::sleep_for(3ms); + } +} + +void +TestWaitable::set_on_execute_callback(std::function on_execute_callback) +{ + on_execute_callback_ = on_execute_callback; +} + +void +TestWaitable::set_on_ready_callback(std::function callback) +{ + auto gc_callback = [callback](size_t count) { + callback(count, 0); + }; + gc_.set_on_trigger_callback(gc_callback); +} + +void +TestWaitable::clear_on_ready_callback() +{ + gc_.set_on_trigger_callback(nullptr); +} + +size_t +TestWaitable::get_number_of_ready_guard_conditions() +{ + return 1; +} + +size_t +TestWaitable::get_count() const +{ + return count_; +} + +size_t +TestWaitable::get_is_ready_call_count() const +{ + return is_ready_count_; +} diff --git a/rclcpp/test/rclcpp/executors/test_waitable.hpp b/rclcpp/test/rclcpp/executors/test_waitable.hpp new file mode 100644 index 0000000000..6c6df18578 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_waitable.hpp @@ -0,0 +1,75 @@ +// Copyright 2024 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__EXECUTORS__TEST_WAITABLE_HPP_ +#define RCLCPP__EXECUTORS__TEST_WAITABLE_HPP_ + +#include +#include +#include + +#include "rclcpp/waitable.hpp" +#include "rclcpp/guard_condition.hpp" + +#include "rcl/wait.h" + +class TestWaitable : public rclcpp::Waitable +{ +public: + TestWaitable() = default; + + void + add_to_wait_set(rcl_wait_set_t & wait_set) override; + + void trigger(); + + bool + is_ready(const rcl_wait_set_t & wait_set) override; + + std::shared_ptr + take_data() override; + + std::shared_ptr + take_data_by_entity_id(size_t id) override; + + void + execute(const std::shared_ptr &) override; + + void + set_on_execute_callback(std::function on_execute_callback); + + void + set_on_ready_callback(std::function callback) override; + + void + clear_on_ready_callback() override; + + size_t + get_number_of_ready_guard_conditions() override; + + size_t + get_count() const; + + size_t + get_is_ready_call_count() const; + +private: + std::atomic trigger_count_ = 0; + std::atomic is_ready_count_ = 0; + std::atomic count_ = 0; + rclcpp::GuardCondition gc_; + std::function on_execute_callback_ = nullptr; +}; + +#endif // RCLCPP__EXECUTORS__TEST_WAITABLE_HPP_ From 069a0018935b33a14632a1cdf4074984a1cf80fe Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Wed, 10 Jul 2024 10:06:43 +0800 Subject: [PATCH 400/455] Release ownership of entities after spinning cancelled (#2556) * Release ownership of entities after spinning cancelled Signed-off-by: Barry Xu * Move release action to every exit point in different spin functions Signed-off-by: Barry Xu * Move wait_result_.reset() before setting spinning to false Signed-off-by: Barry Xu * Update test code Signed-off-by: Barry Xu * Move test code to test_executors.cpp Signed-off-by: Barry Xu --------- Signed-off-by: Barry Xu --- rclcpp/src/rclcpp/executor.cpp | 6 ++--- .../executors/multi_threaded_executor.cpp | 2 +- .../executors/single_threaded_executor.cpp | 2 +- rclcpp/test/rclcpp/CMakeLists.txt | 2 +- .../test/rclcpp/executors/test_executors.cpp | 25 +++++++++++++++++++ 5 files changed, 31 insertions(+), 6 deletions(-) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 95b0d7fcc8..181f0c9a6a 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -275,7 +275,7 @@ Executor::spin_until_future_complete_impl( if (spinning.exchange(true)) { throw std::runtime_error("spin_until_future_complete() called while already spinning"); } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false);); while (rclcpp::ok(this->context_) && spinning.load()) { // Do one item of work. spin_once_impl(timeout_left); @@ -364,7 +364,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"); } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false);); // clear the wait result and wait for work without blocking to collect the work // for the first time @@ -431,7 +431,7 @@ Executor::spin_once(std::chrono::nanoseconds timeout) if (spinning.exchange(true)) { throw std::runtime_error("spin_once() called while already spinning"); } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(wait_result_.reset();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 2d5c76e817..130b4d953f 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -55,7 +55,7 @@ MultiThreadedExecutor::spin() if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false);); + RCPPUTILS_SCOPE_EXIT(wait_result_.reset();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 975733b497..689bbae398 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -30,7 +30,7 @@ SingleThreadedExecutor::spin() if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false);); // Clear any previous result and rebuild the waitset this->wait_result_.reset(); diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index b950bd149b..b171b215d9 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -641,7 +641,7 @@ ament_add_gtest(test_executor test_executor.cpp TIMEOUT 120) ament_add_test_label(test_executor mimick) if(TARGET test_executor) - target_link_libraries(test_executor ${PROJECT_NAME} mimick) + target_link_libraries(test_executor ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) endif() ament_add_gtest(test_graph_listener test_graph_listener.cpp) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 5aae03c5bc..a7129d4488 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -39,6 +39,7 @@ #include "rclcpp/time_source.hpp" #include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" #include "./executor_types.hpp" #include "./test_waitable.hpp" @@ -831,3 +832,27 @@ TEST(TestExecutors, testSpinWithNonDefaultContext) rclcpp::shutdown(non_default_context); } + +TYPED_TEST(TestExecutors, release_ownership_entity_after_spinning_cancel) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + + auto future = std::async(std::launch::async, [&executor] {executor.spin();}); + + auto node = std::make_shared("test_node"); + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) { + }; + auto server = node->create_service("test_service", callback); + while (!executor.is_spinning()) { + std::this_thread::sleep_for(50ms); + } + executor.add_node(node); + std::this_thread::sleep_for(50ms); + executor.cancel(); + std::future_status future_status = future.wait_for(1s); + EXPECT_EQ(future_status, std::future_status::ready); + + EXPECT_EQ(server.use_count(), 1); +} From 304b51c3a17b90aca24e131a2ebade9f8a7a5bec Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 12 Jul 2024 09:44:50 -0400 Subject: [PATCH 401/455] Fix the lifecycle tests on RHEL-9. (#2583) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fix the lifecycle tests on RHEL-9. The full explanation is in the comment, but basically since RHEL doesn't support mocking_utils::inject_on_return, we have to split out certain tests to make sure resources within a process don't collide. Signed-off-by: Chris Lalancette Co-authored-by: Alejandro Hernández Cordero --- rclcpp_lifecycle/CMakeLists.txt | 7 ++ rclcpp_lifecycle/test/test_lifecycle_node.cpp | 16 ----- .../test/test_lifecycle_node_errors.cpp | 69 +++++++++++++++++++ 3 files changed, 76 insertions(+), 16 deletions(-) create mode 100644 rclcpp_lifecycle/test/test_lifecycle_node_errors.cpp diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index d2bdb791b7..9a7335b1bf 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -99,6 +99,13 @@ if(BUILD_TESTING) if(TARGET test_lifecycle_node) target_link_libraries(test_lifecycle_node ${PROJECT_NAME} mimick rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp rcutils::rcutils) endif() + + ament_add_gtest(test_lifecycle_node_errors test/test_lifecycle_node_errors.cpp) + ament_add_test_label(test_lifecycle_node_errors mimick) + if(TARGET test_lifecycle_node_errors) + target_link_libraries(test_lifecycle_node_errors ${PROJECT_NAME} mimick rcl_lifecycle::rcl_lifecycle) + endif() + ament_add_gtest(test_lifecycle_publisher test/test_lifecycle_publisher.cpp) if(TARGET test_lifecycle_publisher) target_link_libraries(test_lifecycle_publisher ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp ${test_msgs_TARGETS}) diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index fdb9be6153..8e0286086e 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -237,22 +237,6 @@ TEST_F(TestDefaultStateMachine, empty_initializer) { EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); } -TEST_F(TestDefaultStateMachine, empty_initializer_rcl_errors) { - { - auto patch = mocking_utils::inject_on_return( - "lib:rclcpp_lifecycle", rcl_lifecycle_state_machine_init, RCL_RET_ERROR); - EXPECT_THROW( - std::make_shared("testnode").reset(), - std::runtime_error); - } - { - auto test_node = std::make_shared("testnode"); - auto patch = mocking_utils::inject_on_return( - "lib:rclcpp_lifecycle", rcl_lifecycle_state_machine_fini, RCL_RET_ERROR); - EXPECT_NO_THROW(test_node.reset()); - } -} - TEST_F(TestDefaultStateMachine, check_logger_services_exist) { // Logger level services are disabled { diff --git a/rclcpp_lifecycle/test/test_lifecycle_node_errors.cpp b/rclcpp_lifecycle/test/test_lifecycle_node_errors.cpp new file mode 100644 index 0000000000..0653b813b9 --- /dev/null +++ b/rclcpp_lifecycle/test/test_lifecycle_node_errors.cpp @@ -0,0 +1,69 @@ +// Copyright 2024 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. + +#include + +#include +#include +#include + +#include "rcl_lifecycle/rcl_lifecycle.h" + +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "./mocking_utils/patch.hpp" + +class TestDefaultStateMachine : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + explicit EmptyLifecycleNode(const std::string & node_name) + : rclcpp_lifecycle::LifecycleNode(node_name) + {} +}; + +// This test is split out of test_lifecycle_node.cpp for an esoteric reason. When running on +// RedHat-based distributions (like Fedora or RHEL), the way that glibc is compiled does not +// allow mocking_utils::inject_on_return to work. Thus the test has to patch_and_return(). +// Unfortunately, this means that the resources are not actually cleaned up, and thus other tests +// may return incorrect results. By having it in a separate process we ensure that the resources +// will at least be cleaned up by the process dying. +TEST_F(TestDefaultStateMachine, empty_initializer_rcl_errors) +{ + { + auto patch = mocking_utils::patch_and_return( + "lib:rclcpp_lifecycle", rcl_lifecycle_state_machine_init, RCL_RET_ERROR); + EXPECT_THROW( + std::make_shared("testnode").reset(), + std::runtime_error); + } + { + auto test_node = std::make_shared("testnode"); + auto patch = mocking_utils::patch_and_return( + "lib:rclcpp_lifecycle", rcl_lifecycle_state_machine_fini, RCL_RET_ERROR); + EXPECT_NO_THROW(test_node.reset()); + } +} From 004db2b3939ec833290fb30cb1876d83176ea7df Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Tue, 16 Jul 2024 06:48:59 -0700 Subject: [PATCH 402/455] remove deprecated APIs from component_manager.hpp (#2585) Signed-off-by: Alberto Soragna --- .../rclcpp_components/component_manager.hpp | 42 ------------------- 1 file changed, 42 deletions(-) diff --git a/rclcpp_components/include/rclcpp_components/component_manager.hpp b/rclcpp_components/include/rclcpp_components/component_manager.hpp index 6238284278..9d98bd89d4 100644 --- a/rclcpp_components/include/rclcpp_components/component_manager.hpp +++ b/rclcpp_components/include/rclcpp_components/component_manager.hpp @@ -185,20 +185,6 @@ class ComponentManager : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); - /** - * \deprecated Use on_load_node() instead - */ - [[deprecated("Use on_load_node() instead")]] - RCLCPP_COMPONENTS_PUBLIC - virtual void - OnLoadNode( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) - { - on_load_node(request_header, request, response); - } - /// Service callback to unload a node in the component /** * \param request_header unused @@ -213,20 +199,6 @@ class ComponentManager : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); - /** - * \deprecated Use on_unload_node() instead - */ - [[deprecated("Use on_unload_node() instead")]] - RCLCPP_COMPONENTS_PUBLIC - virtual void - OnUnloadNode( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) - { - on_unload_node(request_header, request, response); - } - /// Service callback to get the list of nodes in the component /** * Return a two list: one with the unique identifiers and other with full name of the nodes. @@ -242,20 +214,6 @@ class ComponentManager : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); - /** - * \deprecated Use on_list_nodes() instead - */ - [[deprecated("Use on_list_nodes() instead")]] - RCLCPP_COMPONENTS_PUBLIC - virtual void - OnListNodes( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) - { - on_list_nodes(request_header, request, response); - } - protected: std::weak_ptr executor_; From bdf1f8f78a95bb59c4549465300fd0a11867f137 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 17 Jul 2024 11:48:53 +0200 Subject: [PATCH 403/455] Removed deprecated methods and classes (#2575) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- rclcpp/include/rclcpp/callback_group.hpp | 40 ---- rclcpp/include/rclcpp/client.hpp | 17 -- rclcpp/include/rclcpp/create_client.hpp | 22 +- rclcpp/include/rclcpp/event_handler.hpp | 7 - rclcpp/include/rclcpp/loaned_message.hpp | 30 --- rclcpp/include/rclcpp/node.hpp | 36 ---- rclcpp/include/rclcpp/node_impl.hpp | 33 --- .../node_parameters_interface.hpp | 2 - rclcpp/include/rclcpp/parameter_client.hpp | 163 --------------- rclcpp/include/rclcpp/parameter_service.hpp | 14 -- rclcpp/include/rclcpp/publisher.hpp | 23 --- rclcpp/include/rclcpp/rate.hpp | 85 -------- rclcpp/include/rclcpp/subscription.hpp | 12 -- rclcpp/include/rclcpp/typesupport_helpers.hpp | 19 -- rclcpp/include/rclcpp/waitable.hpp | 51 ----- rclcpp/src/rclcpp/rate.cpp | 6 - rclcpp/src/rclcpp/waitable.cpp | 66 +----- rclcpp/test/rclcpp/test_client.cpp | 42 ---- rclcpp/test/rclcpp/test_loaned_message.cpp | 47 ----- rclcpp/test/rclcpp/test_rate.cpp | 195 ------------------ .../rclcpp_lifecycle/lifecycle_node.hpp | 29 --- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 25 --- rclcpp_lifecycle/test/test_client.cpp | 43 ---- rclcpp_lifecycle/test/test_service.cpp | 25 --- 24 files changed, 4 insertions(+), 1028 deletions(-) diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index d8c529d56f..b4a4d4e9a6 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -59,46 +59,6 @@ class CallbackGroup public: RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup) - /// Constructor for CallbackGroup. - /** - * Callback Groups have a type, either 'Mutually Exclusive' or 'Reentrant' - * and when creating one the type must be specified. - * - * Callbacks in Reentrant Callback Groups must be able to: - * - run at the same time as themselves (reentrant) - * - run at the same time as other callbacks in their group - * - run at the same time as other callbacks in other groups - * - * Callbacks in Mutually Exclusive Callback Groups: - * - will not be run multiple times simultaneously (non-reentrant) - * - will not be run at the same time as other callbacks in their group - * - but must run at the same time as callbacks in other groups - * - * Additionally, callback groups have a property which determines whether or - * not they are added to an executor with their associated node automatically. - * When creating a callback group the automatically_add_to_executor_with_node - * argument determines this behavior, and if true it will cause the newly - * created callback group to be added to an executor with the node when the - * Executor::add_node method is used. - * If false, this callback group will not be added automatically and would - * have to be added to an executor manually using the - * Executor::add_callback_group method. - * - * Whether the node was added to the executor before creating the callback - * group, or after, is irrelevant; the callback group will be automatically - * added to the executor in either case. - * - * \param[in] group_type The type of the callback group. - * \param[in] automatically_add_to_executor_with_node A boolean that - * determines whether a callback group is automatically added to an executor - * with the node with which it is associated. - */ - [[deprecated("Use CallbackGroup constructor with context function argument")]] - RCLCPP_PUBLIC - explicit CallbackGroup( - CallbackGroupType group_type, - bool automatically_add_to_executor_with_node = true); - /// Constructor for CallbackGroup. /** * Callback Groups have a type, either 'Mutually Exclusive' or 'Reentrant' diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index fb7547b424..ff90dcb346 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -70,14 +70,6 @@ struct FutureAndRequestId /// Allow implicit conversions to `std::future` by reference. operator FutureT &() {return this->future;} - /// Deprecated, use the `future` member variable instead. - /** - * Allow implicit conversions to `std::future` by value. - * \deprecated - */ - [[deprecated("FutureAndRequestId: use .future instead of an implicit conversion")]] - operator FutureT() {return this->future;} - // delegate future like methods in the std::future impl_ /// See std::future::get(). @@ -436,15 +428,6 @@ class Client : public ClientBase { using detail::FutureAndRequestId>::FutureAndRequestId; - /// Deprecated, use `.future.share()` instead. - /** - * Allow implicit conversions to `std::shared_future` by value. - * \deprecated - */ - [[deprecated( - "FutureAndRequestId: use .future.share() instead of an implicit conversion")]] - operator SharedFuture() {return this->future.share();} - // delegate future like methods in the std::future impl_ /// See std::future::share(). diff --git a/rclcpp/include/rclcpp/create_client.hpp b/rclcpp/include/rclcpp/create_client.hpp index 00e6ff3c0e..8cc73c7500 100644 --- a/rclcpp/include/rclcpp/create_client.hpp +++ b/rclcpp/include/rclcpp/create_client.hpp @@ -47,28 +47,9 @@ create_client( const std::string & service_name, const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr) -{ - return create_client( - node_base, node_graph, node_services, - service_name, - qos.get_rmw_qos_profile(), - group); -} - -/// Create a service client with a given type. -/// \internal -template -typename rclcpp::Client::SharedPtr -create_client( - std::shared_ptr node_base, - std::shared_ptr node_graph, - std::shared_ptr node_services, - const std::string & service_name, - const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group) { rcl_client_options_t options = rcl_client_get_default_options(); - options.qos = qos_profile; + options.qos = qos.get_rmw_qos_profile(); auto cli = rclcpp::Client::make_shared( node_base.get(), @@ -80,7 +61,6 @@ create_client( node_services->add_client(cli_base_ptr, group); return cli; } - } // namespace rclcpp #endif // RCLCPP__CREATE_CLIENT_HPP_ diff --git a/rclcpp/include/rclcpp/event_handler.hpp b/rclcpp/include/rclcpp/event_handler.hpp index 887c571d16..61f198700c 100644 --- a/rclcpp/include/rclcpp/event_handler.hpp +++ b/rclcpp/include/rclcpp/event_handler.hpp @@ -233,8 +233,6 @@ class EventHandlerBase : public Waitable size_t wait_set_event_index_; }; -using QOSEventHandlerBase [[deprecated("Use rclcpp::EventHandlerBase")]] = EventHandlerBase; - template class EventHandler : public EventHandlerBase { @@ -311,11 +309,6 @@ class EventHandler : public EventHandlerBase ParentHandleT parent_handle_; EventCallbackT event_callback_; }; - -template -using QOSEventHandler [[deprecated("Use rclcpp::EventHandler")]] = EventHandler; - } // namespace rclcpp #endif // RCLCPP__EVENT_HANDLER_HPP_ diff --git a/rclcpp/include/rclcpp/loaned_message.hpp b/rclcpp/include/rclcpp/loaned_message.hpp index 6b30e271e0..282fb6c801 100644 --- a/rclcpp/include/rclcpp/loaned_message.hpp +++ b/rclcpp/include/rclcpp/loaned_message.hpp @@ -82,36 +82,6 @@ class LoanedMessage } } - /// Constructor of the LoanedMessage class. - /** - * The constructor of this class allocates memory for a given message type - * and associates this with a given publisher. - * - * Given the publisher instance, a case differentiation is being performaned - * which decides whether the underlying middleware is able to allocate the appropriate - * memory for this message type or not. - * In the case that the middleware can not loan messages, the passed in allocator instance - * is being used to allocate the message within the scope of this class. - * Otherwise, the allocator is being ignored and the allocation is solely performaned - * in the underlying middleware with its appropriate allocation strategy. - * The need for this arises as the user code can be written explicitly targeting a middleware - * capable of loaning messages. - * However, this user code is ought to be usable even when dynamically linked against - * a middleware which doesn't support message loaning in which case the allocator will be used. - * - * \param[in] pub rclcpp::Publisher instance to which the memory belongs - * \param[in] allocator Allocator instance in case middleware can not allocate messages - * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. - */ - [[ - deprecated("used the LoanedMessage constructor that does not use a shared_ptr to the allocator") - ]] - LoanedMessage( - const rclcpp::PublisherBase * pub, - std::shared_ptr> allocator) - : LoanedMessage(*pub, *allocator) - {} - /// Move semantic for RVO LoanedMessage(LoanedMessage && other) : pub_(std::move(other.pub_)), diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 35863abba4..f395bf8ea3 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -257,22 +257,6 @@ class Node : public std::enable_shared_from_this CallbackT callback, rclcpp::CallbackGroup::SharedPtr group = nullptr); - /// Create and return a Client. - /** - * \param[in] service_name The topic to service on. - * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client. - * \param[in] group Callback group to call the service. - * \return Shared pointer to the created client. - * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t - */ - template - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - typename rclcpp::Client::SharedPtr - create_client( - const std::string & service_name, - const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group = nullptr); - /// Create and return a Client. /** * \param[in] service_name The name on which the service is accessible. @@ -287,24 +271,6 @@ class Node : public std::enable_shared_from_this const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); - /// Create and return a Service. - /** - * \param[in] service_name The topic to service on. - * \param[in] callback User-defined callback function. - * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client. - * \param[in] group Callback group to call the service. - * \return Shared pointer to the created service. - * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t - */ - template - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - typename rclcpp::Service::SharedPtr - create_service( - const std::string & service_name, - CallbackT && callback, - const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group = nullptr); - /// Create and return a Service. /** * \param[in] service_name The topic to service on. @@ -1015,8 +981,6 @@ class Node : public std::enable_shared_from_this rclcpp::node_interfaces::OnSetParametersCallbackHandle; using OnSetParametersCallbackType = rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType; - using OnParametersSetCallbackType [[deprecated("use OnSetParametersCallbackType instead")]] = - OnSetParametersCallbackType; using PostSetParametersCallbackHandle = rclcpp::node_interfaces::PostSetParametersCallbackHandle; diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index d55a23f9c1..9ca2c42c2d 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -154,22 +154,6 @@ Node::create_client( group); } -template -typename Client::SharedPtr -Node::create_client( - const std::string & service_name, - const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group) -{ - return rclcpp::create_client( - node_base_, - node_graph_, - node_services_, - extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), - qos_profile, - group); -} - template typename rclcpp::Service::SharedPtr Node::create_service( @@ -187,23 +171,6 @@ Node::create_service( group); } -template -typename rclcpp::Service::SharedPtr -Node::create_service( - const std::string & service_name, - CallbackT && callback, - const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group) -{ - return rclcpp::create_service( - node_base_, - node_services_, - extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), - std::forward(callback), - qos_profile, - group); -} - template std::shared_ptr Node::create_generic_publisher( diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index 301c1e3214..1cf10c1a97 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -52,8 +52,6 @@ struct OnSetParametersCallbackHandle std::function< rcl_interfaces::msg::SetParametersResult( const std::vector &)>; - using OnParametersSetCallbackType [[deprecated("use OnSetParametersCallbackType instead")]] = - OnSetParametersCallbackType; OnSetParametersCallbackType callback; }; diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 7dff6e9ad6..67fbae5054 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -52,37 +52,6 @@ class AsyncParametersClient public: RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient) - /// Create an async parameters client. - /** - * \param[in] node_base_interface The node base interface of the corresponding node. - * \param[in] node_topics_interface Node topic base interface. - * \param[in] node_graph_interface The node graph interface of the corresponding node. - * \param[in] node_services_interface Node service interface. - * \param[in] remote_node_name Name of the remote node - * \param[in] qos_profile The rmw qos profile to use to subscribe - * \param[in] group (optional) The async parameter client will be added to this callback group. - * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t - */ - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - RCLCPP_PUBLIC - AsyncParametersClient( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, - const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, - const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, - const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, - const std::string & remote_node_name, - const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group = nullptr) - : AsyncParametersClient( - node_base_interface, - node_topics_interface, - node_graph_interface, - node_services_interface, - remote_node_name, - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)), - group) - {} - /// Create an async parameters client. /** * \param[in] node_base_interface The node base interface of the corresponding node. @@ -103,31 +72,6 @@ class AsyncParametersClient const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); - /// Constructor - /** - * \param[in] node The async parameters client will be added to this node. - * \param[in] remote_node_name name of the remote node - * \param[in] qos_profile The rmw qos profile to use to subscribe - * \param[in] group (optional) The async parameter client will be added to this callback group. - * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t - */ - template - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - AsyncParametersClient( - const std::shared_ptr node, - const std::string & remote_node_name, - const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group = nullptr) - : AsyncParametersClient( - node->get_node_base_interface(), - node->get_node_topics_interface(), - node->get_node_graph_interface(), - node->get_node_services_interface(), - remote_node_name, - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)), - group) - {} - /** * \param[in] node The async parameters client will be added to this node. * \param[in] remote_node_name (optional) name of the remote node @@ -150,31 +94,6 @@ class AsyncParametersClient group) {} - /// Constructor - /** - * \param[in] node The async parameters client will be added to this node. - * \param[in] remote_node_name Name of the remote node - * \param[in] qos_profile The rmw qos profile to use to subscribe - * \param[in] group (optional) The async parameter client will be added to this callback group. - * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t - */ - template - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - AsyncParametersClient( - NodeT * node, - const std::string & remote_node_name, - const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group = nullptr) - : AsyncParametersClient( - node->get_node_base_interface(), - node->get_node_topics_interface(), - node->get_node_graph_interface(), - node->get_node_services_interface(), - remote_node_name, - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)), - group) - {} - /** * \param[in] node The async parameters client will be added to this node. * \param[in] remote_node_name (optional) name of the remote node @@ -383,19 +302,6 @@ class SyncParametersClient public: RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient) - template - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - SyncParametersClient( - std::shared_ptr node, - const std::string & remote_node_name, - const rmw_qos_profile_t & qos_profile) - : SyncParametersClient( - std::make_shared(), - node, - remote_node_name, - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile))) - {} - template explicit SyncParametersClient( std::shared_ptr node, @@ -408,23 +314,6 @@ class SyncParametersClient qos_profile) {} - template - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - SyncParametersClient( - rclcpp::Executor::SharedPtr executor, - std::shared_ptr node, - const std::string & remote_node_name, - const rmw_qos_profile_t & qos_profile) - : SyncParametersClient( - executor, - node->get_node_base_interface(), - node->get_node_topics_interface(), - node->get_node_graph_interface(), - node->get_node_services_interface(), - remote_node_name, - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile))) - {} - template SyncParametersClient( rclcpp::Executor::SharedPtr executor, @@ -441,19 +330,6 @@ class SyncParametersClient qos_profile) {} - template - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - SyncParametersClient( - NodeT * node, - const std::string & remote_node_name, - const rmw_qos_profile_t & qos_profile) - : SyncParametersClient( - std::make_shared(), - node, - remote_node_name, - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile))) - {} - template explicit SyncParametersClient( NodeT * node, @@ -466,23 +342,6 @@ class SyncParametersClient qos_profile) {} - template - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - SyncParametersClient( - rclcpp::Executor::SharedPtr executor, - NodeT * node, - const std::string & remote_node_name, - const rmw_qos_profile_t & qos_profile) - : SyncParametersClient( - executor, - node->get_node_base_interface(), - node->get_node_topics_interface(), - node->get_node_graph_interface(), - node->get_node_services_interface(), - remote_node_name, - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile))) - {} - template SyncParametersClient( rclcpp::Executor::SharedPtr executor, @@ -499,28 +358,6 @@ class SyncParametersClient qos_profile) {} - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - RCLCPP_PUBLIC - SyncParametersClient( - rclcpp::Executor::SharedPtr executor, - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, - const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, - const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, - const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, - const std::string & remote_node_name, - const rmw_qos_profile_t & qos_profile) - : executor_(executor), node_base_interface_(node_base_interface) - { - async_parameters_client_ = - std::make_shared( - node_base_interface, - node_topics_interface, - node_graph_interface, - node_services_interface, - remote_node_name, - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile))); - } - RCLCPP_PUBLIC SyncParametersClient( rclcpp::Executor::SharedPtr executor, diff --git a/rclcpp/include/rclcpp/parameter_service.hpp b/rclcpp/include/rclcpp/parameter_service.hpp index a952939aca..54353e9579 100644 --- a/rclcpp/include/rclcpp/parameter_service.hpp +++ b/rclcpp/include/rclcpp/parameter_service.hpp @@ -40,20 +40,6 @@ class ParameterService public: RCLCPP_SMART_PTR_DEFINITIONS(ParameterService) - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - RCLCPP_PUBLIC - ParameterService( - const std::shared_ptr node_base, - const std::shared_ptr node_services, - rclcpp::node_interfaces::NodeParametersInterface * node_params, - const rmw_qos_profile_t & qos_profile) - : ParameterService( - node_base, - node_services, - node_params, - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile))) - {} - RCLCPP_PUBLIC ParameterService( const std::shared_ptr node_base, diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 3f02be1e55..4861cd0096 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -96,22 +96,6 @@ class Publisher : public PublisherBase using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; using ROSMessageTypeDeleter = allocator::Deleter; - using MessageAllocatorTraits - [[deprecated("use PublishedTypeAllocatorTraits")]] = - PublishedTypeAllocatorTraits; - using MessageAllocator - [[deprecated("use PublishedTypeAllocator")]] = - PublishedTypeAllocator; - using MessageDeleter - [[deprecated("use PublishedTypeDeleter")]] = - PublishedTypeDeleter; - using MessageUniquePtr - [[deprecated("use std::unique_ptr")]] = - std::unique_ptr; - using MessageSharedPtr - [[deprecated("use std::shared_ptr")]] = - std::shared_ptr; - using BufferSharedPtr = typename rclcpp::experimental::buffers::IntraProcessBuffer< ROSMessageType, ROSMessageTypeAllocator, @@ -423,13 +407,6 @@ class Publisher : public PublisherBase } } - [[deprecated("use get_published_type_allocator() or get_ros_message_type_allocator() instead")]] - std::shared_ptr - get_allocator() const - { - return std::make_shared(published_type_allocator_); - } - PublishedTypeAllocator get_published_type_allocator() const { diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 5b04b72b81..281ddf9de3 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -39,10 +39,6 @@ class RateBase RCLCPP_PUBLIC virtual bool sleep() = 0; - [[deprecated("use get_type() instead")]] - RCLCPP_PUBLIC - virtual bool is_steady() const = 0; - RCLCPP_PUBLIC virtual rcl_clock_type_t get_type() const = 0; @@ -54,82 +50,6 @@ using std::chrono::duration; using std::chrono::duration_cast; using std::chrono::nanoseconds; -template -class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRate : public RateBase -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(GenericRate) - - explicit GenericRate(double rate) - : period_(duration_cast(duration(1.0 / rate))), last_interval_(Clock::now()) - {} - explicit GenericRate(std::chrono::nanoseconds period) - : period_(period), last_interval_(Clock::now()) - {} - - virtual bool - sleep() - { - // Time coming into sleep - auto now = Clock::now(); - // Time of next interval - auto next_interval = last_interval_ + period_; - // Detect backwards time flow - if (now < last_interval_) { - // Best thing to do is to set the next_interval to now + period - next_interval = now + period_; - } - // Calculate the time to sleep - auto time_to_sleep = next_interval - now; - // Update the interval - last_interval_ += period_; - // If the time_to_sleep is negative or zero, don't sleep - if (time_to_sleep <= std::chrono::seconds(0)) { - // If an entire cycle was missed then reset next interval. - // This might happen if the loop took more than a cycle. - // Or if time jumps forward. - if (now > next_interval + period_) { - last_interval_ = now + period_; - } - // Either way do not sleep and return false - return false; - } - // Sleep (will get interrupted by ctrl-c, may not sleep full time) - rclcpp::sleep_for(time_to_sleep); - return true; - } - - [[deprecated("use get_type() instead")]] - virtual bool - is_steady() const - { - return Clock::is_steady; - } - - virtual rcl_clock_type_t get_type() const - { - return Clock::is_steady ? RCL_STEADY_TIME : RCL_SYSTEM_TIME; - } - - virtual void - reset() - { - last_interval_ = Clock::now(); - } - - std::chrono::nanoseconds period() const - { - return period_; - } - -private: - RCLCPP_DISABLE_COPY(GenericRate) - - std::chrono::nanoseconds period_; - using ClockDurationNano = std::chrono::duration; - std::chrono::time_point last_interval_; -}; - class Rate : public RateBase { public: @@ -149,11 +69,6 @@ class Rate : public RateBase virtual bool sleep(); - [[deprecated("use get_type() instead")]] - RCLCPP_PUBLIC - virtual bool - is_steady() const; - RCLCPP_PUBLIC virtual rcl_clock_type_t get_type() const; diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index f8a10a5c27..4e552eb1df 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -90,18 +90,6 @@ class Subscription : public SubscriptionBase using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; using ROSMessageTypeDeleter = allocator::Deleter; - using MessageAllocatorTraits [[deprecated("use ROSMessageTypeAllocatorTraits")]] = - ROSMessageTypeAllocatorTraits; - using MessageAllocator [[deprecated("use ROSMessageTypeAllocator")]] = - ROSMessageTypeAllocator; - using MessageDeleter [[deprecated("use ROSMessageTypeDeleter")]] = - ROSMessageTypeDeleter; - - using ConstMessageSharedPtr [[deprecated]] = std::shared_ptr; - using MessageUniquePtr - [[deprecated("use std::unique_ptr instead")]] = - std::unique_ptr; - private: using SubscriptionTopicStatisticsSharedPtr = std::shared_ptr; diff --git a/rclcpp/include/rclcpp/typesupport_helpers.hpp b/rclcpp/include/rclcpp/typesupport_helpers.hpp index c93b318440..7f75a8052b 100644 --- a/rclcpp/include/rclcpp/typesupport_helpers.hpp +++ b/rclcpp/include/rclcpp/typesupport_helpers.hpp @@ -38,25 +38,6 @@ RCLCPP_PUBLIC std::shared_ptr get_typesupport_library(const std::string & type, const std::string & typesupport_identifier); -/// Extract the type support handle from the library. -/** - * The library needs to match the topic type. The shared library must stay loaded for the lifetime of the result. - * - * \deprecated Use get_message_typesupport_handle() instead - * - * \param[in] type The topic type, e.g. "std_msgs/msg/String" - * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp" - * \param[in] library The shared type support library - * \return A type support handle - */ -[[deprecated("Use `get_message_typesupport_handle` instead")]] -RCLCPP_PUBLIC -const rosidl_message_type_support_t * -get_typesupport_handle( - const std::string & type, - const std::string & typesupport_identifier, - rcpputils::SharedLibrary & library); - /// Extract the message type support handle from the library. /** * The library needs to match the topic type. The shared library must stay loaded for the lifetime of the result. diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index e834765a08..280d79c39d 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -101,23 +101,6 @@ 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. @@ -128,23 +111,6 @@ class Waitable void 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. /** * The input wait set should be the same that was used in a previously call to @@ -212,23 +178,6 @@ 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, diff --git a/rclcpp/src/rclcpp/rate.cpp b/rclcpp/src/rclcpp/rate.cpp index 7c98d6d4fe..083bd223c5 100644 --- a/rclcpp/src/rclcpp/rate.cpp +++ b/rclcpp/src/rclcpp/rate.cpp @@ -71,12 +71,6 @@ Rate::sleep() return true; } -bool -Rate::is_steady() const -{ - return clock_->get_clock_type() == RCL_STEADY_TIME; -} - rcl_clock_type_t Rate::get_type() const { diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index 43b562481c..1ee4f9073f 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -87,81 +87,21 @@ Waitable::clear_on_ready_callback() "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 + return this->is_ready(wait_set); } void -Waitable::execute(std::shared_ptr & data) +Waitable::add_to_wait_set(rcl_wait_set_t & wait_set) { - const std::shared_ptr & const_data = data; - this->execute(const_data); + this->add_to_wait_set(wait_set); } 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/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp index 5c7e5046ab..506d981dd6 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -89,31 +89,10 @@ TEST_F(TestClient, construction_and_destruction) { { auto client = node->create_client("service"); } - { - // suppress deprecated function warning - #if !defined(_WIN32) - # pragma GCC diagnostic push - # pragma GCC diagnostic ignored "-Wdeprecated-declarations" - #else // !defined(_WIN32) - # pragma warning(push) - # pragma warning(disable: 4996) - #endif - - auto client = node->create_client( - "service", rmw_qos_profile_services_default); - - // remove warning suppression - #if !defined(_WIN32) - # pragma GCC diagnostic pop - #else // !defined(_WIN32) - # pragma warning(pop) - #endif - } { auto client = node->create_client( "service", rclcpp::ServicesQoS()); } - { ASSERT_THROW( { @@ -123,27 +102,6 @@ TEST_F(TestClient, construction_and_destruction) { } TEST_F(TestClient, construction_with_free_function) { - { - auto client = rclcpp::create_client( - node->get_node_base_interface(), - node->get_node_graph_interface(), - node->get_node_services_interface(), - "service", - rmw_qos_profile_services_default, - nullptr); - } - { - ASSERT_THROW( - { - auto client = rclcpp::create_client( - node->get_node_base_interface(), - node->get_node_graph_interface(), - node->get_node_services_interface(), - "invalid_?service", - rmw_qos_profile_services_default, - nullptr); - }, rclcpp::exceptions::InvalidServiceNameError); - } { auto client = rclcpp::create_client( node->get_node_base_interface(), diff --git a/rclcpp/test/rclcpp/test_loaned_message.cpp b/rclcpp/test/rclcpp/test_loaned_message.cpp index 085f5ef3fd..0623210079 100644 --- a/rclcpp/test/rclcpp/test_loaned_message.cpp +++ b/rclcpp/test/rclcpp/test_loaned_message.cpp @@ -40,53 +40,6 @@ class TestLoanedMessage : public ::testing::Test } }; -TEST_F(TestLoanedMessage, initialize) { - auto node = std::make_shared("loaned_message_test_node"); - auto pub = node->create_publisher("loaned_message_test_topic", 1); - -// suppress deprecated function warning -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else // !defined(_WIN32) -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - - auto pub_allocator = pub->get_allocator(); - -// remove warning suppression -#if !defined(_WIN32) -# pragma GCC diagnostic pop -#else // !defined(_WIN32) -# pragma warning(pop) -#endif - -// suppress deprecated function warning -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else // !defined(_WIN32) -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - - auto loaned_msg = rclcpp::LoanedMessage(pub.get(), pub_allocator); - -// remove warning suppression -#if !defined(_WIN32) -# pragma GCC diagnostic pop -#else // !defined(_WIN32) -# pragma warning(pop) -#endif - - ASSERT_TRUE(loaned_msg.is_valid()); - loaned_msg.get().float32_value = 42.0f; - ASSERT_EQ(42.0f, loaned_msg.get().float32_value); - - SUCCEED(); -} - TEST_F(TestLoanedMessage, loan_from_pub) { auto node = std::make_shared("loaned_message_test_node"); auto pub = node->create_publisher("loaned_message_test_topic", 1); diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp index b80789c853..86d22cbd94 100644 --- a/rclcpp/test/rclcpp/test_rate.cpp +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -47,21 +47,6 @@ TEST_F(TestRate, rate_basics) { auto start = std::chrono::system_clock::now(); rclcpp::Rate r(period); EXPECT_EQ(rclcpp::Duration(period), r.period()); -// suppress deprecated warnings -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else // !defined(_WIN32) -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - ASSERT_FALSE(r.is_steady()); -// remove warning suppression -#if !defined(_WIN32) -# pragma GCC diagnostic pop -#else // !defined(_WIN32) -# pragma warning(pop) -#endif ASSERT_EQ(RCL_SYSTEM_TIME, r.get_type()); ASSERT_TRUE(r.sleep()); auto one = std::chrono::system_clock::now(); @@ -102,23 +87,6 @@ TEST_F(TestRate, wall_rate_basics) { auto start = std::chrono::steady_clock::now(); rclcpp::WallRate r(period); EXPECT_EQ(rclcpp::Duration(period), r.period()); -// suppress deprecated warnings -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else // !defined(_WIN32) -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - ASSERT_TRUE(r.is_steady()); -// suppress deprecated warnings -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else // !defined(_WIN32) -# pragma warning(push) -# pragma warning(disable: 4996) -#endif ASSERT_EQ(RCL_STEADY_TIME, r.get_type()); ASSERT_TRUE(r.sleep()); auto one = std::chrono::steady_clock::now(); @@ -150,124 +118,6 @@ TEST_F(TestRate, wall_rate_basics) { EXPECT_GT(epsilon, delta); } -/* - Basic test for the deprecated GenericRate class. - */ -TEST_F(TestRate, generic_rate) { - auto period = std::chrono::milliseconds(100); - auto offset = std::chrono::milliseconds(50); - auto epsilon = std::chrono::milliseconds(1); - double overrun_ratio = 1.5; - - { - auto start = std::chrono::system_clock::now(); - -// suppress deprecated warnings -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else // !defined(_WIN32) -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - - rclcpp::GenericRate gr(period); - ASSERT_FALSE(gr.is_steady()); - -// remove warning suppression -#if !defined(_WIN32) -# pragma GCC diagnostic pop -#else // !defined(_WIN32) -# pragma warning(pop) -#endif - - ASSERT_EQ(gr.get_type(), RCL_SYSTEM_TIME); - EXPECT_EQ(period, gr.period()); - ASSERT_TRUE(gr.sleep()); - auto one = std::chrono::system_clock::now(); - auto delta = one - start; - EXPECT_LT(period, delta + epsilon); - EXPECT_GT(period * overrun_ratio, delta); - - rclcpp::sleep_for(offset); - ASSERT_TRUE(gr.sleep()); - auto two = std::chrono::system_clock::now(); - delta = two - start; - EXPECT_LT(2 * period, delta); - EXPECT_GT(2 * period * overrun_ratio, delta); - - rclcpp::sleep_for(offset); - auto two_offset = std::chrono::system_clock::now(); - gr.reset(); - ASSERT_TRUE(gr.sleep()); - auto three = std::chrono::system_clock::now(); - delta = three - two_offset; - EXPECT_LT(period, delta + epsilon); - EXPECT_GT(period * overrun_ratio, delta); - - rclcpp::sleep_for(offset + period); - auto four = std::chrono::system_clock::now(); - ASSERT_FALSE(gr.sleep()); - auto five = std::chrono::system_clock::now(); - delta = five - four; - ASSERT_TRUE(epsilon > delta); - } - - { - auto start = std::chrono::steady_clock::now(); - -// suppress deprecated warnings -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else // !defined(_WIN32) -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - - rclcpp::GenericRate gr(period); - ASSERT_TRUE(gr.is_steady()); - -// remove warning suppression -#if !defined(_WIN32) -# pragma GCC diagnostic pop -#else // !defined(_WIN32) -# pragma warning(pop) -#endif - - ASSERT_EQ(gr.get_type(), RCL_STEADY_TIME); - EXPECT_EQ(period, gr.period()); - ASSERT_TRUE(gr.sleep()); - auto one = std::chrono::steady_clock::now(); - auto delta = one - start; - EXPECT_LT(period, delta); - EXPECT_GT(period * overrun_ratio, delta); - - rclcpp::sleep_for(offset); - ASSERT_TRUE(gr.sleep()); - auto two = std::chrono::steady_clock::now(); - delta = two - start; - EXPECT_LT(2 * period, delta + epsilon); - EXPECT_GT(2 * period * overrun_ratio, delta); - - rclcpp::sleep_for(offset); - auto two_offset = std::chrono::steady_clock::now(); - gr.reset(); - ASSERT_TRUE(gr.sleep()); - auto three = std::chrono::steady_clock::now(); - delta = three - two_offset; - EXPECT_LT(period, delta); - EXPECT_GT(period * overrun_ratio, delta); - - rclcpp::sleep_for(offset + period); - auto four = std::chrono::steady_clock::now(); - ASSERT_FALSE(gr.sleep()); - auto five = std::chrono::steady_clock::now(); - delta = five - four; - EXPECT_GT(epsilon, delta); - } -} - TEST_F(TestRate, from_double) { { rclcpp::Rate rate(1.0); @@ -290,59 +140,14 @@ TEST_F(TestRate, from_double) { TEST_F(TestRate, clock_types) { { rclcpp::Rate rate(1.0, std::make_shared(RCL_SYSTEM_TIME)); -// suppress deprecated warnings -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else // !defined(_WIN32) -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - EXPECT_FALSE(rate.is_steady()); -// remove warning suppression -#if !defined(_WIN32) -# pragma GCC diagnostic pop -#else // !defined(_WIN32) -# pragma warning(pop) -#endif EXPECT_EQ(RCL_SYSTEM_TIME, rate.get_type()); } { rclcpp::Rate rate(1.0, std::make_shared(RCL_STEADY_TIME)); -// suppress deprecated warnings -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else // !defined(_WIN32) -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - EXPECT_TRUE(rate.is_steady()); -// remove warning suppression -#if !defined(_WIN32) -# pragma GCC diagnostic pop -#else // !defined(_WIN32) -# pragma warning(pop) -#endif EXPECT_EQ(RCL_STEADY_TIME, rate.get_type()); } { rclcpp::Rate rate(1.0, std::make_shared(RCL_ROS_TIME)); -// suppress deprecated warnings -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else // !defined(_WIN32) -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - EXPECT_FALSE(rate.is_steady()); -// remove warning suppression -#if !defined(_WIN32) -# pragma GCC diagnostic pop -#else // !defined(_WIN32) -# pragma warning(pop) -#endif EXPECT_EQ(RCL_ROS_TIME, rate.get_type()); } } diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 657ddddc34..d63cc726f9 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -280,19 +280,6 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group = nullptr); - /// Create and return a Client. - /** - * \sa rclcpp::Node::create_client - * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t - */ - template - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - typename rclcpp::Client::SharedPtr - create_client( - const std::string & service_name, - const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group = nullptr); - /// Create and return a Client. /** * \param[in] service_name The name on which the service is accessible. @@ -307,20 +294,6 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); - /// Create and return a Service. - /** - * \sa rclcpp::Node::create_service - * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t - */ - template - [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] - typename rclcpp::Service::SharedPtr - create_service( - const std::string & service_name, - CallbackT && callback, - const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group = nullptr); - /// Create and return a Service. /** * \sa rclcpp::Node::create_service @@ -572,8 +545,6 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rclcpp::node_interfaces::OnSetParametersCallbackHandle; using OnSetParametersCallbackType = rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType; - using OnParametersSetCallbackType [[deprecated("use OnSetParametersCallbackType instead")]] = - OnSetParametersCallbackType; using PostSetParametersCallbackHandle = rclcpp::node_interfaces::PostSetParametersCallbackHandle; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 5ffc8a5f1b..b1e73b6c64 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -120,18 +120,6 @@ LifecycleNode::create_timer( this->node_timers_.get()); } -template -typename rclcpp::Client::SharedPtr -LifecycleNode::create_client( - const std::string & service_name, - const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group) -{ - return rclcpp::create_client( - node_base_, node_graph_, node_services_, - service_name, qos_profile, group); -} - template typename rclcpp::Client::SharedPtr LifecycleNode::create_client( @@ -144,19 +132,6 @@ LifecycleNode::create_client( service_name, qos, group); } -template -typename rclcpp::Service::SharedPtr -LifecycleNode::create_service( - const std::string & service_name, - CallbackT && callback, - const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group) -{ - return rclcpp::create_service( - node_base_, node_services_, - service_name, std::forward(callback), qos_profile, group); -} - template typename rclcpp::Service::SharedPtr LifecycleNode::create_service( diff --git a/rclcpp_lifecycle/test/test_client.cpp b/rclcpp_lifecycle/test/test_client.cpp index 7ded8e346e..f34d71ebfc 100644 --- a/rclcpp_lifecycle/test/test_client.cpp +++ b/rclcpp_lifecycle/test/test_client.cpp @@ -61,27 +61,6 @@ TEST_F(TestClient, construction_and_destruction) { auto client = node_->create_client("service"); EXPECT_TRUE(client); } - { - // suppress deprecated function warning - #if !defined(_WIN32) - # pragma GCC diagnostic push - # pragma GCC diagnostic ignored "-Wdeprecated-declarations" - #else // !defined(_WIN32) - # pragma warning(push) - # pragma warning(disable: 4996) - #endif - - auto client = node_->create_client( - "service", rmw_qos_profile_services_default); - EXPECT_TRUE(client); - - // remove warning suppression - #if !defined(_WIN32) - # pragma GCC diagnostic pop - #else // !defined(_WIN32) - # pragma warning(pop) - #endif - } { auto client = node_->create_client( "service", rclcpp::ServicesQoS()); @@ -97,28 +76,6 @@ TEST_F(TestClient, construction_and_destruction) { } TEST_F(TestClient, construction_with_free_function) { - { - auto client = rclcpp::create_client( - node_->get_node_base_interface(), - node_->get_node_graph_interface(), - node_->get_node_services_interface(), - "service", - rmw_qos_profile_services_default, - nullptr); - EXPECT_TRUE(client); - } - { - ASSERT_THROW( - { - auto client = rclcpp::create_client( - node_->get_node_base_interface(), - node_->get_node_graph_interface(), - node_->get_node_services_interface(), - "invalid_?service", - rmw_qos_profile_services_default, - nullptr); - }, rclcpp::exceptions::InvalidServiceNameError); - } { auto client = rclcpp::create_client( node_->get_node_base_interface(), diff --git a/rclcpp_lifecycle/test/test_service.cpp b/rclcpp_lifecycle/test/test_service.cpp index b3108a9042..13bc25761d 100644 --- a/rclcpp_lifecycle/test/test_service.cpp +++ b/rclcpp_lifecycle/test/test_service.cpp @@ -68,31 +68,6 @@ TEST_F(TestService, construction_and_destruction) { const rclcpp::ServiceBase * const_service_base = service.get(); EXPECT_NE(nullptr, const_service_base->get_service_handle()); } - { - // suppress deprecated function warning - #if !defined(_WIN32) - # pragma GCC diagnostic push - # pragma GCC diagnostic ignored "-Wdeprecated-declarations" - #else // !defined(_WIN32) - # pragma warning(push) - # pragma warning(disable: 4996) - #endif - - auto service = node->create_service( - "service", callback, rmw_qos_profile_services_default); - - // remove warning suppression - #if !defined(_WIN32) - # pragma GCC diagnostic pop - #else // !defined(_WIN32) - # pragma warning(pop) - #endif - - EXPECT_NE(nullptr, service->get_service_handle()); - const rclcpp::ServiceBase * const_service_base = service.get(); - EXPECT_NE(nullptr, const_service_base->get_service_handle()); - } - { ASSERT_THROW( { From 54b8f9cc9736a0e0a160452553a21a2a86acfd43 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Mon, 22 Jul 2024 06:43:16 -0700 Subject: [PATCH 404/455] Have the EventsExecutor use more common code (#2570) * move notify waitable setup to its own function * move mutex lock to retrieve_entity utility * use entities_need_rebuild_ atomic bool in events-executors * remove duplicated set_on_ready_callback for notify_waitable * use mutex from base class rather than a new recursive mutex * use current_collection_ member in events-executor * delay adding notify waitable to collection * postpone clearing the current collection * commonize notify waitable and collection * commonize add/remove node/cbg methods * fix linter errors --------- Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/executor.hpp | 5 +- .../executors/executor_notify_waitable.hpp | 11 ++ .../events_executor/events_executor.hpp | 109 ++---------- rclcpp/src/rclcpp/executor.cpp | 18 +- .../executors/executor_notify_waitable.cpp | 14 +- .../events_executor/events_executor.cpp | 163 ++++-------------- .../test/rclcpp/executors/test_executors.cpp | 2 +- .../test_static_single_threaded_executor.cpp | 8 +- rclcpp/test/rclcpp/test_executor.cpp | 4 +- 9 files changed, 90 insertions(+), 244 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 5e5a054814..ae2087bbc5 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -541,8 +541,9 @@ class Executor * * \param[in] notify if true will execute a trigger that will wake up a waiting executor */ - void - trigger_entity_recollect(bool notify); + RCLCPP_PUBLIC + virtual void + handle_updated_entities(bool notify); /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins. std::atomic_bool spinning; diff --git a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp index d389cbf1e0..7ae1c3b875 100644 --- a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp @@ -122,6 +122,14 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable void clear_on_ready_callback() override; + /// Set a new callback to be called whenever this waitable is executed. + /** + * \param[in] on_execute_callback The new callback + */ + RCLCPP_PUBLIC + void + set_execute_callback(std::function on_execute_callback); + /// Remove a guard condition from being waited on. /** * \param[in] weak_guard_condition The guard condition to remove. @@ -142,7 +150,10 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable /// Callback to run when waitable executes std::function execute_callback_; + /// Mutex to procetect the guard conditions std::mutex guard_condition_mutex_; + /// Mutex to protect the execute callback + std::mutex execute_mutex_; std::function on_ready_callback_; diff --git a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp index dd5b1ebe63..163d8d2367 100644 --- a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp +++ b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp @@ -59,8 +59,6 @@ namespace executors */ class EventsExecutor : public rclcpp::Executor { - friend class EventsExecutorEntitiesCollector; - public: RCLCPP_SMART_PTR_DEFINITIONS(EventsExecutor) @@ -72,7 +70,7 @@ class EventsExecutor : public rclcpp::Executor * \param[in] options Options used to configure the executor. */ RCLCPP_PUBLIC - explicit EventsExecutor( + EventsExecutor( rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue = std::make_unique< rclcpp::experimental::executors::SimpleEventsQueue>(), bool execute_timers_separate_thread = false, @@ -128,87 +126,6 @@ class EventsExecutor : public rclcpp::Executor void spin_all(std::chrono::nanoseconds max_duration) override; - /// Add a node to the executor. - /** - * \sa rclcpp::Executor::add_node - */ - RCLCPP_PUBLIC - void - add_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - bool notify = true) override; - - /// Convenience function which takes Node and forwards NodeBaseInterface. - /** - * \sa rclcpp::EventsExecutor::add_node - */ - RCLCPP_PUBLIC - void - add_node(std::shared_ptr node_ptr, bool notify = true) override; - - /// Remove a node from the executor. - /** - * \sa rclcpp::Executor::remove_node - */ - RCLCPP_PUBLIC - void - remove_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - bool notify = true) override; - - /// Convenience function which takes Node and forwards NodeBaseInterface. - /** - * \sa rclcpp::Executor::remove_node - */ - RCLCPP_PUBLIC - void - remove_node(std::shared_ptr node_ptr, bool notify = true) override; - - /// Add a callback group to an executor. - /** - * \sa rclcpp::Executor::add_callback_group - */ - RCLCPP_PUBLIC - void - add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - bool notify = true) override; - - /// Remove callback group from the executor - /** - * \sa rclcpp::Executor::remove_callback_group - */ - RCLCPP_PUBLIC - void - remove_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - bool notify = true) override; - - /// Get callback groups that belong to executor. - /** - * \sa rclcpp::Executor::get_all_callback_groups() - */ - RCLCPP_PUBLIC - std::vector - get_all_callback_groups() override; - - /// Get callback groups that belong to executor. - /** - * \sa rclcpp::Executor::get_manually_added_callback_groups() - */ - RCLCPP_PUBLIC - std::vector - get_manually_added_callback_groups() override; - - /// Get callback groups that belong to executor. - /** - * \sa rclcpp::Executor::get_automatically_added_callback_groups_from_nodes() - */ - RCLCPP_PUBLIC - std::vector - get_automatically_added_callback_groups_from_nodes() override; - protected: /// Internal implementation of spin_once RCLCPP_PUBLIC @@ -220,6 +137,11 @@ class EventsExecutor : public rclcpp::Executor void spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive); + /// Collect entities from callback groups and refresh the current collection with them + RCLCPP_PUBLIC + void + handle_updated_entities(bool notify) override; + private: RCLCPP_DISABLE_COPY(EventsExecutor) @@ -227,9 +149,9 @@ class EventsExecutor : public rclcpp::Executor void execute_event(const ExecutorEvent & event); - /// Collect entities from callback groups and refresh the current collection with them + /// Rebuilds the executor's notify waitable, as we can't use the one built in the base class void - refresh_current_collection_from_callback_groups(); + setup_notify_waitable(); /// Refresh the current collection using the provided new_collection void @@ -253,6 +175,11 @@ class EventsExecutor : public rclcpp::Executor typename CollectionType::EntitySharedPtr retrieve_entity(typename CollectionType::Key entity_id, CollectionType & collection) { + // Note: we lock the mutex because we assume that you are trying to get an element from the + // current collection... If there will be a use-case to retrieve elements also from other + // collections, we can move the mutex back to the calling codes. + std::lock_guard guard(mutex_); + // Check if the entity_id is in the collection auto it = collection.find(entity_id); if (it == collection.end()) { @@ -273,16 +200,6 @@ class EventsExecutor : public rclcpp::Executor /// Queue where entities can push events rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue_; - std::shared_ptr entities_collector_; - std::shared_ptr notify_waitable_; - - /// Mutex to protect the current_entities_collection_ - std::recursive_mutex collection_mutex_; - std::shared_ptr current_entities_collection_; - - /// Flag used to reduce the number of unnecessary waitable events - std::atomic notify_waitable_event_pushed_ {false}; - /// Timers manager used to track and/or execute associated timers std::shared_ptr timers_manager_; }; diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 181f0c9a6a..69131cc111 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -129,7 +129,7 @@ Executor::~Executor() } void -Executor::trigger_entity_recollect(bool notify) +Executor::handle_updated_entities(bool notify) { this->entities_need_rebuild_.store(true); @@ -174,11 +174,11 @@ Executor::add_callback_group( this->collector_.add_callback_group(group_ptr); try { - this->trigger_entity_recollect(notify); + this->handle_updated_entities(notify); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string( - "Failed to trigger guard condition on callback group add: ") + ex.what()); + "Failed to handle entities update on callback group add: ") + ex.what()); } } @@ -188,11 +188,11 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt this->collector_.add_node(node_ptr); try { - this->trigger_entity_recollect(notify); + this->handle_updated_entities(notify); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string( - "Failed to trigger guard condition on node add: ") + ex.what()); + "Failed to handle entities update on node add: ") + ex.what()); } } @@ -204,11 +204,11 @@ Executor::remove_callback_group( this->collector_.remove_callback_group(group_ptr); try { - this->trigger_entity_recollect(notify); + this->handle_updated_entities(notify); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string( - "Failed to trigger guard condition on callback group remove: ") + ex.what()); + "Failed to handle entities update on callback group remove: ") + ex.what()); } } @@ -224,11 +224,11 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node this->collector_.remove_node(node_ptr); try { - this->trigger_entity_recollect(notify); + this->handle_updated_entities(notify); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string( - "Failed to trigger guard condition on node remove: ") + ex.what()); + "Failed to handle entities update on node remove: ") + ex.what()); } } diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index 09789a13ff..2e62f9dd1a 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include "rclcpp/exceptions.hpp" #include "rclcpp/executors/executor_notify_waitable.hpp" @@ -91,9 +89,9 @@ ExecutorNotifyWaitable::is_ready(const rcl_wait_set_t & wait_set) } void -ExecutorNotifyWaitable::execute(const std::shared_ptr & data) +ExecutorNotifyWaitable::execute(const std::shared_ptr & /*data*/) { - (void) data; + std::lock_guard lock(execute_mutex_); this->execute_callback_(); } @@ -149,6 +147,14 @@ ExecutorNotifyWaitable::clear_on_ready_callback() } } +RCLCPP_PUBLIC +void +ExecutorNotifyWaitable::set_execute_callback(std::function on_execute_callback) +{ + std::lock_guard lock(execute_mutex_); + execute_callback_ = on_execute_callback; +} + void ExecutorNotifyWaitable::add_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition) { 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 c7d6be7e44..b7dd1487f9 100644 --- a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp +++ b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp @@ -52,30 +52,37 @@ EventsExecutor::EventsExecutor( timers_manager_ = std::make_shared(context_, timer_on_ready_cb); - this->current_entities_collection_ = - std::make_shared(); + entities_need_rebuild_ = false; - notify_waitable_ = std::make_shared( + this->setup_notify_waitable(); + + // Ensure that the entities collection is empty (the base class may have added elements + // that we are not interested in) + this->current_collection_.clear(); + + // Make sure that the notify waitable is immediately added to the collection + // to avoid missing events + this->add_notify_waitable_to_collection(current_collection_.waitables); +} + +void +EventsExecutor::setup_notify_waitable() +{ + // The base class already created this object but the events-executor + // needs different callbacks. + assert(notify_waitable_ && "The notify waitable should have already been constructed"); + + notify_waitable_->set_execute_callback( [this]() { // This callback is invoked when: // - the interrupt or shutdown guard condition is triggered: // ---> we need to wake up the executor so that it can terminate // - a node or callback group guard condition is triggered: // ---> the entities collection is changed, we need to update callbacks - notify_waitable_event_pushed_ = false; - this->refresh_current_collection_from_callback_groups(); + entities_need_rebuild_ = false; + this->handle_updated_entities(false); }); - // Make sure that the notify waitable is immediately added to the collection - // to avoid missing events - this->add_notify_waitable_to_collection(current_entities_collection_->waitables); - - notify_waitable_->add_guard_condition(interrupt_guard_condition_); - notify_waitable_->add_guard_condition(shutdown_guard_condition_); - - notify_waitable_->set_on_ready_callback( - this->create_waitable_callback(notify_waitable_.get())); - auto notify_waitable_entity_id = notify_waitable_.get(); notify_waitable_->set_on_ready_callback( [this, notify_waitable_entity_id](size_t num_events, int waitable_data) { @@ -85,7 +92,7 @@ EventsExecutor::EventsExecutor( // For the same reason, if an event of this type has already been pushed but it has not been // processed yet, we avoid pushing additional events. (void)num_events; - if (notify_waitable_event_pushed_.exchange(true)) { + if (entities_need_rebuild_.exchange(true)) { return; } @@ -93,9 +100,6 @@ EventsExecutor::EventsExecutor( {notify_waitable_entity_id, nullptr, waitable_data, ExecutorEventType::WAITABLE_EVENT, 1}; this->events_queue_->enqueue(event); }); - - this->entities_collector_ = - std::make_shared(notify_waitable_); } EventsExecutor::~EventsExecutor() @@ -229,46 +233,6 @@ EventsExecutor::spin_once_impl(std::chrono::nanoseconds timeout) } } -void -EventsExecutor::add_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) -{ - // This field is unused because we don't have to wake up the executor when a node is added. - (void) notify; - - // Add node to entities collector - this->entities_collector_->add_node(node_ptr); - - this->refresh_current_collection_from_callback_groups(); -} - -void -EventsExecutor::add_node(std::shared_ptr node_ptr, bool notify) -{ - this->add_node(node_ptr->get_node_base_interface(), notify); -} - -void -EventsExecutor::remove_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) -{ - // This field is unused because we don't have to wake up the executor when a node is removed. - (void)notify; - - // Remove node from entities collector. - // This will result in un-setting all the event callbacks from its entities. - // After this function returns, this executor will not receive any more events associated - // to these entities. - this->entities_collector_->remove_node(node_ptr); - - this->refresh_current_collection_from_callback_groups(); -} - -void -EventsExecutor::remove_node(std::shared_ptr node_ptr, bool notify) -{ - this->remove_node(node_ptr->get_node_base_interface(), notify); -} void EventsExecutor::execute_event(const ExecutorEvent & event) @@ -278,10 +242,9 @@ EventsExecutor::execute_event(const ExecutorEvent & event) { rclcpp::ClientBase::SharedPtr client; { - std::lock_guard lock(collection_mutex_); client = this->retrieve_entity( static_cast(event.entity_key), - current_entities_collection_->clients); + current_collection_.clients); } if (client) { for (size_t i = 0; i < event.num_events; i++) { @@ -295,10 +258,9 @@ EventsExecutor::execute_event(const ExecutorEvent & event) { rclcpp::SubscriptionBase::SharedPtr subscription; { - std::lock_guard lock(collection_mutex_); subscription = this->retrieve_entity( static_cast(event.entity_key), - current_entities_collection_->subscriptions); + current_collection_.subscriptions); } if (subscription) { for (size_t i = 0; i < event.num_events; i++) { @@ -311,10 +273,9 @@ EventsExecutor::execute_event(const ExecutorEvent & event) { rclcpp::ServiceBase::SharedPtr service; { - std::lock_guard lock(collection_mutex_); service = this->retrieve_entity( static_cast(event.entity_key), - current_entities_collection_->services); + current_collection_.services); } if (service) { for (size_t i = 0; i < event.num_events; i++) { @@ -334,10 +295,9 @@ EventsExecutor::execute_event(const ExecutorEvent & event) { rclcpp::Waitable::SharedPtr waitable; { - std::lock_guard lock(collection_mutex_); waitable = this->retrieve_entity( static_cast(event.entity_key), - current_entities_collection_->waitables); + current_collection_.waitables); } if (waitable) { for (size_t i = 0; i < event.num_events; i++) { @@ -351,61 +311,12 @@ EventsExecutor::execute_event(const ExecutorEvent & event) } void -EventsExecutor::add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - bool notify) +EventsExecutor::handle_updated_entities(bool notify) { - // This field is unused because we don't have to wake up - // the executor when a callback group is added. (void)notify; - (void)node_ptr; - - this->entities_collector_->add_callback_group(group_ptr); - - this->refresh_current_collection_from_callback_groups(); -} - -void -EventsExecutor::remove_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify) -{ - // This field is unused because we don't have to wake up - // the executor when a callback group is removed. - (void)notify; - - this->entities_collector_->remove_callback_group(group_ptr); - - this->refresh_current_collection_from_callback_groups(); -} - -std::vector -EventsExecutor::get_all_callback_groups() -{ - this->entities_collector_->update_collections(); - return this->entities_collector_->get_all_callback_groups(); -} - -std::vector -EventsExecutor::get_manually_added_callback_groups() -{ - this->entities_collector_->update_collections(); - return this->entities_collector_->get_manually_added_callback_groups(); -} - -std::vector -EventsExecutor::get_automatically_added_callback_groups_from_nodes() -{ - this->entities_collector_->update_collections(); - return this->entities_collector_->get_automatically_added_callback_groups(); -} - -void -EventsExecutor::refresh_current_collection_from_callback_groups() -{ // Build the new collection - this->entities_collector_->update_collections(); - auto callback_groups = this->entities_collector_->get_all_callback_groups(); + this->collector_.update_collections(); + auto callback_groups = this->collector_.get_all_callback_groups(); rclcpp::executors::ExecutorEntitiesCollection new_collection; rclcpp::executors::build_entities_collection(callback_groups, new_collection); @@ -428,14 +339,14 @@ EventsExecutor::refresh_current_collection( const rclcpp::executors::ExecutorEntitiesCollection & new_collection) { // Acquire lock before modifying the current collection - std::lock_guard lock(collection_mutex_); + std::lock_guard guard(mutex_); - current_entities_collection_->timers.update( + current_collection_.timers.update( new_collection.timers, [this](rclcpp::TimerBase::SharedPtr timer) {timers_manager_->add_timer(timer);}, [this](rclcpp::TimerBase::SharedPtr timer) {timers_manager_->remove_timer(timer);}); - current_entities_collection_->subscriptions.update( + current_collection_.subscriptions.update( new_collection.subscriptions, [this](auto subscription) { subscription->set_on_new_message_callback( @@ -444,7 +355,7 @@ EventsExecutor::refresh_current_collection( }, [](auto subscription) {subscription->clear_on_new_message_callback();}); - current_entities_collection_->clients.update( + current_collection_.clients.update( new_collection.clients, [this](auto client) { client->set_on_new_response_callback( @@ -453,7 +364,7 @@ EventsExecutor::refresh_current_collection( }, [](auto client) {client->clear_on_new_response_callback();}); - current_entities_collection_->services.update( + current_collection_.services.update( new_collection.services, [this](auto service) { service->set_on_new_request_callback( @@ -464,12 +375,12 @@ EventsExecutor::refresh_current_collection( // DO WE NEED THIS? WE ARE NOT DOING ANYTHING WITH GUARD CONDITIONS /* - current_entities_collection_->guard_conditions.update(new_collection.guard_conditions, + current_collection_.guard_conditions.update(new_collection.guard_conditions, [](auto guard_condition) {(void)guard_condition;}, [](auto guard_condition) {guard_condition->set_on_trigger_callback(nullptr);}); */ - current_entities_collection_->waitables.update( + current_collection_.waitables.update( new_collection.waitables, [this](auto waitable) { waitable->set_on_ready_callback( diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index a7129d4488..72afee8dda 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -638,7 +638,7 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) // and b) refreshing the executor collections. // The inconsistent state would happen if the event was processed before the collections were // finished to be refreshed: the executor would pick up the event but be unable to process it. -// This would leave the `notify_waitable_event_pushed_` flag to true, preventing additional +// This would leave the `entities_need_rebuild_` flag to true, preventing additional // notify waitable events to be pushed. // The behavior is observable only under heavy load, so this test spawns several worker // threads. Due to the nature of the bug, this test may still succeed even if the diff --git a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp index 1a0f3f88c4..05d0b23152 100644 --- a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp @@ -56,7 +56,7 @@ TEST_F(TestStaticSingleThreadedExecutor, add_callback_group_trigger_guard_failed "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( executor.add_callback_group(cb_group, node->get_node_base_interface(), true), - std::runtime_error("Failed to trigger guard condition on callback group add: error not set")); + std::runtime_error("Failed to handle entities update on callback group add: error not set")); } } @@ -69,7 +69,7 @@ TEST_F(TestStaticSingleThreadedExecutor, add_node_trigger_guard_failed) { "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( executor.add_node(node), - std::runtime_error("Failed to trigger guard condition on node add: error not set")); + std::runtime_error("Failed to handle entities update on node add: error not set")); } } @@ -87,7 +87,7 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_callback_group_trigger_guard_fai RCLCPP_EXPECT_THROW_EQ( executor.remove_callback_group(cb_group, true), std::runtime_error( - "Failed to trigger guard condition on callback group remove: error not set")); + "Failed to handle entities update on callback group remove: error not set")); } } @@ -115,7 +115,7 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_node_trigger_guard_failed) { "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( executor.remove_node(node, true), - std::runtime_error("Failed to trigger guard condition on node remove: error not set")); + std::runtime_error("Failed to handle entities update on node remove: error not set")); } } diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index 668ab96797..21a19cee18 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -138,7 +138,7 @@ TEST_F(TestExecutor, add_callback_group_failed_trigger_guard_condition) { "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( dummy.add_callback_group(cb_group, node->get_node_base_interface(), true), - std::runtime_error("Failed to trigger guard condition on callback group add: error not set")); + std::runtime_error("Failed to handle entities update on callback group add: error not set")); } TEST_F(TestExecutor, remove_callback_group_null_node) { @@ -175,7 +175,7 @@ TEST_F(TestExecutor, remove_callback_group_failed_trigger_guard_condition) { RCLCPP_EXPECT_THROW_EQ( dummy.remove_callback_group(cb_group, true), std::runtime_error( - "Failed to trigger guard condition on callback group remove: error not set")); + "Failed to handle entities update on callback group remove: error not set")); } TEST_F(TestExecutor, remove_node_not_associated) { From 647bd65e289c1ed4fbf89f1a151fd21040b940c4 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 22 Jul 2024 14:20:50 -0400 Subject: [PATCH 405/455] Make the subscriber_triggered_to_receive_message test more reliable. (#2584) * Make the subscriber_triggered_to_receive_message test more reliable. In the current code, inside of the timer we create the subscription and the publisher, publish immediately, and expect the subscription to get it immediately. But it may be the case that discovery hasn't even happened between the publisher and the subscription by the time the publish call happens. To make this more reliable, create the subscription and publish *before* we ever create and spin on the timer. This at least gives 100 milliseconds for discovery to happen. That may not be quite enough to make this reliable on all platforms, but in my local testing this helps a lot. Prior to this change I can make this fail one out of 10 times, and after the change I've run 100 times with no failures. Signed-off-by: Chris Lalancette --- .../test_add_callback_groups_to_executor.cpp | 35 +++++++++---------- 1 file changed, 16 insertions(+), 19 deletions(-) diff --git a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp index 70389df1a5..6edcc2cb28 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -215,7 +215,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t std::atomic_size_t timer_count {0}; auto timer_callback = [&executor, &timer_count]() { auto cur_timer_count = timer_count++; - printf("in timer_callback(%zu)\n", cur_timer_count); if (cur_timer_count > 0) { executor.cancel(); } @@ -344,32 +343,30 @@ TYPED_TEST(TestAddCallbackGroupsToExecutorStable, subscriber_triggered_to_receiv received_message_promise.set_value(true); }; - rclcpp::Subscription::SharedPtr subscription; - rclcpp::Publisher::SharedPtr publisher; - // to create a timer with a callback run on another executor - rclcpp::TimerBase::SharedPtr timer = nullptr; std::promise timer_promise; + // create a subscription using the 'cb_grp' callback group + rclcpp::QoS qos = rclcpp::QoS(1).reliable(); + auto options = rclcpp::SubscriptionOptions(); + options.callback_group = cb_grp; + rclcpp::Subscription::SharedPtr subscription = + node->create_subscription("topic_name", qos, sub_callback, options); + // create a publisher to send data + rclcpp::Publisher::SharedPtr publisher = + node->create_publisher("topic_name", qos); auto timer_callback = - [&subscription, &publisher, &timer, &cb_grp, &node, &sub_callback, &timer_promise]() { - if (timer) { - timer.reset(); + [&publisher, &timer_promise]() { + if (publisher->get_subscription_count() == 0) { + // If discovery hasn't happened yet, get out. + return; } - - // create a subscription using the `cb_grp` callback group - rclcpp::QoS qos = rclcpp::QoS(1).reliable(); - auto options = rclcpp::SubscriptionOptions(); - options.callback_group = cb_grp; - subscription = - node->create_subscription("topic_name", qos, sub_callback, options); - // create a publisher to send data - publisher = - node->create_publisher("topic_name", qos); publisher->publish(test_msgs::msg::Empty()); timer_promise.set_value(); }; + // Another executor to run the timer with a callback ExecutorType timer_executor; - timer = node->create_wall_timer(100ms, timer_callback); + + rclcpp::TimerBase::SharedPtr timer = node->create_wall_timer(100ms, timer_callback); timer_executor.add_node(node); auto future = timer_promise.get_future(); timer_executor.spin_until_future_complete(future); From b1fdb18f1e1fc11f9993b9a1172b819266889166 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 24 Jul 2024 10:14:45 +0200 Subject: [PATCH 406/455] Updated rcpputils path API (#2579) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- rclcpp/include/rclcpp/logger.hpp | 29 +++++++++++++++++++ rclcpp/src/rclcpp/logger.cpp | 29 +++++++++++++++++++ .../node_interfaces/test_node_parameters.cpp | 3 +- rclcpp/test/rclcpp/test_logger.cpp | 15 +++------- rclcpp/test/rclcpp/test_node.cpp | 3 +- rclcpp/test/rclcpp/test_parameter_client.cpp | 9 +++--- rclcpp_components/src/component_manager.cpp | 9 +++--- .../test/test_component_manager.cpp | 3 +- 8 files changed, 78 insertions(+), 22 deletions(-) diff --git a/rclcpp/include/rclcpp/logger.hpp b/rclcpp/include/rclcpp/logger.hpp index 3b8e8a1625..ee244fd988 100644 --- a/rclcpp/include/rclcpp/logger.hpp +++ b/rclcpp/include/rclcpp/logger.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__LOGGER_HPP_ #define RCLCPP__LOGGER_HPP_ +#include #include #include #include @@ -77,6 +78,14 @@ RCLCPP_PUBLIC Logger get_node_logger(const rcl_node_t * node); +// TODO(ahcorde): Remove deprecated class on the next release (in Rolling after Kilted). +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif /// Get the current logging directory. /** * For more details of how the logging directory is determined, @@ -85,10 +94,30 @@ get_node_logger(const rcl_node_t * node); * \returns the logging directory being used. * \throws rclcpp::exceptions::RCLError if an unexpected error occurs. */ +[[deprecated("use rclcpp::get_log_directory instead of rclcpp::get_logging_directory")]] RCLCPP_PUBLIC rcpputils::fs::path get_logging_directory(); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + +/// Get the current logging directory. +/** + * For more details of how the logging directory is determined, + * see rcl_logging_get_logging_directory(). + * + * \returns the logging directory being used. + * \throws rclcpp::exceptions::RCLError if an unexpected error occurs. + */ +RCLCPP_PUBLIC +std::filesystem::path +get_log_directory(); + class Logger { public: diff --git a/rclcpp/src/rclcpp/logger.cpp b/rclcpp/src/rclcpp/logger.cpp index 2e83c07013..788ae31d31 100644 --- a/rclcpp/src/rclcpp/logger.cpp +++ b/rclcpp/src/rclcpp/logger.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -54,6 +55,14 @@ get_node_logger(const rcl_node_t * node) return rclcpp::get_logger(logger_name); } +// TODO(ahcorde): Remove deprecated class on the next release (in Rolling after Kilted). +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif rcpputils::fs::path get_logging_directory() { @@ -67,6 +76,26 @@ get_logging_directory() allocator.deallocate(log_dir, allocator.state); return path; } +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + +std::filesystem::path +get_log_directory() +{ + char * log_dir = NULL; + auto allocator = rcutils_get_default_allocator(); + rcl_logging_ret_t ret = rcl_logging_get_logging_directory(allocator, &log_dir); + if (RCL_LOGGING_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + std::string path{log_dir}; + allocator.deallocate(log_dir, allocator.state); + return path; +} Logger Logger::get_child(const std::string & suffix) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp index d262c67a9a..0cc06792a8 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -61,7 +62,7 @@ class TestNodeParameters : public ::testing::Test std::shared_ptr node; rclcpp::node_interfaces::NodeParameters * node_parameters; - rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY}; + std::filesystem::path test_resources_path{TEST_RESOURCES_DIRECTORY}; }; TEST_F(TestNodeParameters, construct_destruct_rcl_errors) { diff --git a/rclcpp/test/rclcpp/test_logger.cpp b/rclcpp/test/rclcpp/test_logger.cpp index 83d42f0ce8..35118cf2df 100644 --- a/rclcpp/test/rclcpp/test_logger.cpp +++ b/rclcpp/test/rclcpp/test_logger.cpp @@ -14,6 +14,7 @@ #include +#include #include #include @@ -210,15 +211,7 @@ TEST(TestLogger, get_logging_directory) { ASSERT_EQ(true, rcutils_set_env("ROS_LOG_DIR", nullptr)); ASSERT_EQ(true, rcutils_set_env("ROS_HOME", nullptr)); - auto path = rclcpp::get_logging_directory(); - auto expected_path = rcpputils::fs::path{"/fake_home_dir"} / ".ros" / "log"; - - // TODO(ivanpauno): Add operator== to rcpputils::fs::path - auto it = path.cbegin(); - auto eit = expected_path.cbegin(); - for (; it != path.cend() && eit != expected_path.cend(); ++it, ++eit) { - EXPECT_EQ(*eit, *it); - } - EXPECT_EQ(it, path.cend()); - EXPECT_EQ(eit, expected_path.cend()); + auto path = rclcpp::get_log_directory(); + auto expected_path = std::filesystem::path{"/fake_home_dir"} / ".ros" / "log"; + EXPECT_EQ(path, expected_path); } diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index ad73aadc2a..c956b273be 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include #include @@ -56,7 +57,7 @@ class TestNode : public ::testing::Test test_resources_path /= "test_node"; } - rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY}; + std::filesystem::path test_resources_path{TEST_RESOURCES_DIRECTORY}; }; /* diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index afc48a652b..caefa0d6c8 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include #include @@ -935,7 +936,7 @@ TEST_F(TestParameterClient, async_parameter_load_parameters) { auto asynchronous_client = std::make_shared(load_node, "/namespace/load_node"); // load parameters - rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY}; + std::filesystem::path test_resources_path{TEST_RESOURCES_DIRECTORY}; const std::string parameters_filepath = ( test_resources_path / "test_node" / "load_parameters.yaml").string(); auto load_future = asynchronous_client->load_parameters(parameters_filepath); @@ -961,7 +962,7 @@ TEST_F(TestParameterClient, sync_parameter_load_parameters) { auto synchronous_client = std::make_shared(load_node); // load parameters - rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY}; + std::filesystem::path test_resources_path{TEST_RESOURCES_DIRECTORY}; const std::string parameters_filepath = ( test_resources_path / "test_node" / "load_parameters.yaml").string(); auto load_future = synchronous_client->load_parameters(parameters_filepath); @@ -983,7 +984,7 @@ TEST_F(TestParameterClient, async_parameter_load_parameters_complicated_regex) { auto asynchronous_client = std::make_shared(load_node, "/namespace/load_node"); // load parameters - rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY}; + std::filesystem::path test_resources_path{TEST_RESOURCES_DIRECTORY}; const std::string parameters_filepath = ( test_resources_path / "test_node" / "load_complicated_parameters.yaml").string(); auto load_future = asynchronous_client->load_parameters(parameters_filepath); @@ -1013,7 +1014,7 @@ TEST_F(TestParameterClient, async_parameter_load_no_valid_parameter) { auto asynchronous_client = std::make_shared(load_node, "/namespace/load_node"); // load parameters - rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY}; + std::filesystem::path test_resources_path{TEST_RESOURCES_DIRECTORY}; const std::string parameters_filepath = ( test_resources_path / "test_node" / "no_valid_parameters.yaml").string(); EXPECT_THROW( diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index 7b77af9c92..f73cd8954a 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -14,6 +14,7 @@ #include "rclcpp_components/component_manager.hpp" +#include #include #include #include @@ -95,11 +96,11 @@ ComponentManager::get_component_resources( throw ComponentManagerException("Invalid resource entry"); } - std::string library_path = parts[1]; - if (!rcpputils::fs::path(library_path).is_absolute()) { - library_path = base_path + "/" + library_path; + std::filesystem::path library_path = parts[1]; + if (!library_path.is_absolute()) { + library_path = (base_path / library_path); } - resources.push_back({parts[0], library_path}); + resources.push_back({parts[0], library_path.string()}); } return resources; } diff --git a/rclcpp_components/test/test_component_manager.cpp b/rclcpp_components/test/test_component_manager.cpp index ebc86ff6f0..d4df8e7d08 100644 --- a/rclcpp_components/test/test_component_manager.cpp +++ b/rclcpp_components/test/test_component_manager.cpp @@ -14,6 +14,7 @@ #include +#include #include #include "rclcpp_components/component_manager.hpp" @@ -51,7 +52,7 @@ TEST_F(TestComponentManager, get_component_resources_valid) EXPECT_EQ("test_rclcpp_components::TestComponentBar", resources[1].first); EXPECT_EQ("test_rclcpp_components::TestComponentNoNode", resources[2].first); - namespace fs = rcpputils::fs; + namespace fs = std::filesystem; EXPECT_TRUE(fs::exists(fs::path(resources[0].second))); EXPECT_TRUE(fs::exists(fs::path(resources[1].second))); EXPECT_TRUE(fs::exists(fs::path(resources[2].second))); From fcc0261c4965108015f7a86ea640d4b082f32a18 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 24 Jul 2024 10:18:37 +0200 Subject: [PATCH 407/455] Changelog MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- rclcpp/CHANGELOG.rst | 51 +++++++++++++++++++++++++++++++++ rclcpp_action/CHANGELOG.rst | 3 ++ rclcpp_components/CHANGELOG.rst | 6 ++++ rclcpp_lifecycle/CHANGELOG.rst | 12 ++++++++ 4 files changed, 72 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index fb5ce43dc3..babe0d839f 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,57 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +28.3.2 (2024-07-24) +------------------- +* Updated rcpputils path API (`#2579 `_) +* Make the subscriber_triggered_to_receive_message test more reliable. (`#2584 `_) + * Make the subscriber_triggered_to_receive_message test more reliable. + In the current code, inside of the timer we create the subscription + and the publisher, publish immediately, and expect the subscription + to get it immediately. But it may be the case that discovery + hasn't even happened between the publisher and the subscription + by the time the publish call happens. + To make this more reliable, create the subscription and publish *before* + we ever create and spin on the timer. This at least gives 100 + milliseconds for discovery to happen. That may not be quite enough + to make this reliable on all platforms, but in my local testing this + helps a lot. Prior to this change I can make this fail one out of 10 + times, and after the change I've run 100 times with no failures. +* Have the EventsExecutor use more common code (`#2570 `_) + * move notify waitable setup to its own function + * move mutex lock to retrieve_entity utility + * use entities_need_rebuild\_ atomic bool in events-executors + * remove duplicated set_on_ready_callback for notify_waitable + * use mutex from base class rather than a new recursive mutex + * use current_collection\_ member in events-executor + * delay adding notify waitable to collection + * postpone clearing the current collection + * commonize notify waitable and collection + * commonize add/remove node/cbg methods + * fix linter errors + --------- +* Removed deprecated methods and classes (`#2575 `_) +* Release ownership of entities after spinning cancelled (`#2556 `_) + * Release ownership of entities after spinning cancelled + * Move release action to every exit point in different spin functions + * Move wait_result\_.reset() before setting spinning to false + * Update test code + * Move test code to test_executors.cpp + --------- +* Split test_executors.cpp even further. (`#2572 `_) + That's because it is too large for Windows Debug to compile, + so split into smaller bits. + Even with this split, the file is too big; that's likely + because we are using TYPED_TEST here, which generates multiple + symbols per test case. To deal with this, without further + breaking up the file, also add in the /bigobj flag when + compiling on Windows Debug. +* avoid adding notify waitable twice to events-executor collection (`#2564 `_) + * avoid adding notify waitable twice to events-executor entities collection + * remove redundant mutex lock + --------- +* Contributors: Alberto Soragna, Alejandro Hernández Cordero, Barry Xu, Chris Lalancette + 28.3.1 (2024-06-25) ------------------- * Remove unnecessary msg includes in tests (`#2566 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 5db060d317..cc08fc039b 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +28.3.2 (2024-07-24) +------------------- + 28.3.1 (2024-06-25) ------------------- * Fix typo in function doc (`#2563 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 3ee4cd5d1a..ecaeef0570 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +28.3.2 (2024-07-24) +------------------- +* Updated rcpputils path API (`#2579 `_) +* remove deprecated APIs from component_manager.hpp (`#2585 `_) +* Contributors: Alberto Soragna, Alejandro Hernández Cordero + 28.3.1 (2024-06-25) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 0696dd8893..aef2e6c9a6 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,18 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +28.3.2 (2024-07-24) +------------------- +* Removed deprecated methods and classes (`#2575 `_) +* Fix the lifecycle tests on RHEL-9. (`#2583 `_) + * Fix the lifecycle tests on RHEL-9. + The full explanation is in the comment, but basically since + RHEL doesn't support mocking_utils::inject_on_return, we have + to split out certain tests to make sure resources within a + process don't collide. + Co-authored-by: Alejandro Hernández Cordero +* Contributors: Alejandro Hernández Cordero, Chris Lalancette + 28.3.1 (2024-06-25) ------------------- From a4d7210b9cef081707ecec8aea7a7988ffd87b7f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 24 Jul 2024 10:18:40 +0200 Subject: [PATCH 408/455] 28.3.2 --- rclcpp/package.xml | 2 +- rclcpp_action/package.xml | 2 +- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/package.xml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 95e917b220..101d0cc585 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 28.3.1 + 28.3.2 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 9fc070e41e..d64ce7fc52 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 28.3.1 + 28.3.2 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 08ebc6d049..2b968b5108 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 28.3.1 + 28.3.2 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 71d8599a00..6fbda32820 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 28.3.1 + 28.3.2 Package containing a prototype for lifecycle implementation Ivan Paunovic From 9b1e6c9d520f09338b789058571c63d283a5c128 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 26 Jul 2024 12:37:34 -0400 Subject: [PATCH 409/455] Only compile the tests once. (#2590) Even when we want to run them on multiple RMWs, we can do that by compiling once, then setting the environment variable appropriately. Signed-off-by: Chris Lalancette --- rclcpp/test/rclcpp/CMakeLists.txt | 82 ++++++++++--------- .../test_subscription_content_filter.cpp | 40 +++++---- 2 files changed, 61 insertions(+), 61 deletions(-) diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index b171b215d9..756e1c67c6 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -99,17 +99,6 @@ ament_add_gtest(test_create_subscription test_create_subscription.cpp) if(TARGET test_create_subscription) target_link_libraries(test_create_subscription ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() -function(test_add_callback_groups_to_executor_for_rmw_implementation) - set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) - ament_add_gmock(test_add_callback_groups_to_executor${target_suffix} test_add_callback_groups_to_executor.cpp - ENV ${rmw_implementation_env_var} - TIMEOUT 120 - ) - if(TARGET test_add_callback_groups_to_executor${target_suffix}) - target_link_libraries(test_add_callback_groups_to_executor${target_suffix} ${PROJECT_NAME} ${test_msgs_TARGETS}) - endif() -endfunction() -call_for_each_rmw_implementation(test_add_callback_groups_to_executor_for_rmw_implementation) ament_add_gtest(test_expand_topic_or_service_name test_expand_topic_or_service_name.cpp) ament_add_test_label(test_expand_topic_or_service_name mimick) if(TARGET test_expand_topic_or_service_name) @@ -337,28 +326,6 @@ if(TARGET test_qos) rmw::rmw ) endif() -function(test_generic_pubsub_for_rmw_implementation) - set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) - ament_add_gmock(test_generic_pubsub${target_suffix} test_generic_pubsub.cpp - ENV ${rmw_implementation_env_var} - ) - if(TARGET test_generic_pubsub${target_suffix}) - target_link_libraries(test_generic_pubsub${target_suffix} ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) - endif() -endfunction() -call_for_each_rmw_implementation(test_generic_pubsub_for_rmw_implementation) - -function(test_qos_event_for_rmw_implementation) - set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) - ament_add_gmock(test_qos_event${target_suffix} test_qos_event.cpp - ENV ${rmw_implementation_env_var} - ) - ament_add_test_label(test_qos_event${target_suffix} mimick) - if(TARGET test_qos_event${target_suffix}) - target_link_libraries(test_qos_event${target_suffix} ${PROJECT_NAME} mimick rcutils::rcutils rmw::rmw ${test_msgs_TARGETS}) - endif() -endfunction() -call_for_each_rmw_implementation(test_qos_event_for_rmw_implementation) ament_add_gmock(test_qos_overriding_options test_qos_overriding_options.cpp) if(TARGET test_qos_overriding_options) @@ -650,16 +617,51 @@ if(TARGET test_graph_listener) target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick) endif() -function(test_subscription_content_filter_for_rmw_implementation) +ament_add_gmock_executable(test_qos_event test_qos_event.cpp) +if(TARGET test_qos_event) + target_link_libraries(test_qos_event ${PROJECT_NAME} mimick rcutils::rcutils rmw::rmw ${test_msgs_TARGETS}) +endif() + +ament_add_gmock_executable(test_generic_pubsub test_generic_pubsub.cpp) +if(TARGET test_generic_pubsub) + target_link_libraries(test_generic_pubsub ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) +endif() + +ament_add_gmock_executable(test_add_callback_groups_to_executor test_add_callback_groups_to_executor.cpp) +if(TARGET test_add_callback_groups_to_executor) + target_link_libraries(test_add_callback_groups_to_executor ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() + +ament_add_gmock_executable(test_subscription_content_filter test_subscription_content_filter.cpp) +if(TARGET test_subscription_content_filter) + target_link_libraries(test_subscription_content_filter ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) +endif() + +function(test_on_all_rmws) set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) - ament_add_gmock(test_subscription_content_filter${target_suffix} - test_subscription_content_filter.cpp + + ament_add_gmock_test(test_qos_event + TEST_NAME test_qos_event${target_suffix} + ENV ${rmw_implementation_env_var} + ) + ament_add_test_label(test_qos_event${target_suffix} mimick) + + ament_add_gmock_test(test_generic_pubsub + TEST_NAME test_generic_pubsub${target_suffix} + ENV ${rmw_implementation_env_var} + ) + + ament_add_gmock_test(test_add_callback_groups_to_executor + TEST_NAME test_add_callback_groups_to_executor${target_suffix} + ENV ${rmw_implementation_env_var} + TIMEOUT 120 + ) + + ament_add_gmock_test(test_subscription_content_filter + TEST_NAME test_subscription_content_filter${target_suffix} ENV ${rmw_implementation_env_var} TIMEOUT 120 ) ament_add_test_label(test_subscription_content_filter${target_suffix} mimick) - if(TARGET test_subscription_content_filter${target_suffix}) - target_link_libraries(test_subscription_content_filter${target_suffix} ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) - endif() endfunction() -call_for_each_rmw_implementation(test_subscription_content_filter_for_rmw_implementation) +call_for_each_rmw_implementation(test_on_all_rmws) diff --git a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp index 372af03611..4b8baf4a24 100644 --- a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp +++ b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp @@ -30,14 +30,7 @@ #include "test_msgs/msg/basic_types.hpp" -#ifdef RMW_IMPLEMENTATION -# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX -# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) -#else -# define CLASSNAME(NAME, SUFFIX) NAME -#endif - -class CLASSNAME (TestContentFilterSubscription, RMW_IMPLEMENTATION) : public ::testing::Test +class TestContentFilterSubscription : public ::testing::Test { public: static void SetUpTestCase() @@ -113,7 +106,8 @@ bool operator==(const test_msgs::msg::BasicTypes & m1, const test_msgs::msg::Bas m1.uint64_value == m2.uint64_value; } -TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), is_cft_enabled) { +TEST_F(TestContentFilterSubscription, is_cft_enabled) +{ { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_subscription_is_cft_enabled, false); @@ -127,7 +121,8 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), is_cft_enab } } -TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), get_content_filter_error) { +TEST_F(TestContentFilterSubscription, get_content_filter_error) +{ auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_subscription_get_content_filter, RCL_RET_ERROR); @@ -137,7 +132,8 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), get_content rclcpp::exceptions::RCLError); } -TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), set_content_filter_error) { +TEST_F(TestContentFilterSubscription, set_content_filter_error) +{ auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_subscription_set_content_filter, RCL_RET_ERROR); @@ -148,7 +144,8 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), set_content rclcpp::exceptions::RCLError); } -TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), get_content_filter) { +TEST_F(TestContentFilterSubscription, get_content_filter) +{ rclcpp::ContentFilterOptions options; if (sub->is_cft_enabled()) { @@ -164,7 +161,8 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), get_content } } -TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), set_content_filter) { +TEST_F(TestContentFilterSubscription, set_content_filter) +{ if (sub->is_cft_enabled()) { EXPECT_NO_THROW( sub->set_content_filter(filter_expression_init, expression_parameters_2)); @@ -175,7 +173,8 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), set_content } } -TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_get_begin) { +TEST_F(TestContentFilterSubscription, content_filter_get_begin) +{ using namespace std::chrono_literals; { test_msgs::msg::BasicTypes msg; @@ -217,7 +216,8 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_fil } } -TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_get_later) { +TEST_F(TestContentFilterSubscription, content_filter_get_later) +{ using namespace std::chrono_literals; { test_msgs::msg::BasicTypes msg; @@ -264,7 +264,8 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_fil } } -TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_reset) { +TEST_F(TestContentFilterSubscription, content_filter_reset) +{ using namespace std::chrono_literals; { test_msgs::msg::BasicTypes msg; @@ -311,11 +312,8 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_fil } } -TEST_F( - CLASSNAME( - TestContentFilterSubscription, - RMW_IMPLEMENTATION), create_two_content_filters_with_same_topic_name_and_destroy) { - +TEST_F(TestContentFilterSubscription, create_two_content_filters_with_same_topic_name_and_destroy) +{ // Create another content filter auto options = rclcpp::SubscriptionOptions(); From 6f5c6f45d73133f8f89ef9a3227b03e6f051db87 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 29 Jul 2024 14:52:23 +0000 Subject: [PATCH 410/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 5 +++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 3 +++ 4 files changed, 14 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index babe0d839f..b03cb7f03d 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Only compile the tests once. (`#2590 `_) +* Contributors: Chris Lalancette + 28.3.2 (2024-07-24) ------------------- * Updated rcpputils path API (`#2579 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index cc08fc039b..a1c202b00f 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 28.3.2 (2024-07-24) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index ecaeef0570..761812efdc 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 28.3.2 (2024-07-24) ------------------- * Updated rcpputils path API (`#2579 `_) diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index aef2e6c9a6..b76c433ffd 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 28.3.2 (2024-07-24) ------------------- * Removed deprecated methods and classes (`#2575 `_) From 45adf6565f99042a5fdcad7b8830e69f5efb03e5 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 29 Jul 2024 14:52:31 +0000 Subject: [PATCH 411/455] 28.3.3 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index b03cb7f03d..185a1de7ff 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.3.3 (2024-07-29) +------------------- * Only compile the tests once. (`#2590 `_) * Contributors: Chris Lalancette diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 101d0cc585..b260140f43 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 28.3.2 + 28.3.3 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index a1c202b00f..ae8bccb987 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.3.3 (2024-07-29) +------------------- 28.3.2 (2024-07-24) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index d64ce7fc52..993091c7e4 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 28.3.2 + 28.3.3 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 761812efdc..157a9868bd 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.3.3 (2024-07-29) +------------------- 28.3.2 (2024-07-24) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 2b968b5108..e2d94ae31f 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 28.3.2 + 28.3.3 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index b76c433ffd..bc0c86ccd0 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +28.3.3 (2024-07-29) +------------------- 28.3.2 (2024-07-24) ------------------- diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 6fbda32820..74b66e93f3 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 28.3.2 + 28.3.3 Package containing a prototype for lifecycle implementation Ivan Paunovic From c46896731c3a02b39140bea9d31ae36c99846344 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 2 Aug 2024 08:01:25 -0400 Subject: [PATCH 412/455] Make more of the Waitable API abstract (#2593) * Make Waitable::{is_ready,add_to_wait_set,execute} abstract APIs. I initially started looking at this because clang was complaining that "all paths through this function will call itself". And it is correct; if an implementation does not override these methods, and they are every called, they will go into an infinite loop calling themselves. However, while looking at it it seemed to me that these were really part of the API that a concrete implementation of Waitable needed to implement. It is fine if an implementation doesn't want to do anything (like the tests), but all of the "real" users in the codebase override these. Thus, remove the implementations here and make these pure virtual functions that all subclasses must implement. * Remove some more "empty" implementations. In particular, these are implementations in the Waitable class that only throw exceptions. Rather than do this, make these APIs pure virtual so all downstream classes have to implement them. All of the current users (except for tests) do anyway, and it makes the API much cleaner. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/waitable.hpp | 12 ++--- rclcpp/src/rclcpp/waitable.cpp | 46 ------------------- .../node_interfaces/test_node_waitables.cpp | 12 ++--- .../test_allocator_memory_strategy.cpp | 12 ++--- rclcpp/test/rclcpp/test_memory_strategy.cpp | 5 ++ .../test_dynamic_storage.cpp | 10 ++-- .../wait_set_policies/test_static_storage.cpp | 10 ++-- .../test_storage_policy_common.cpp | 9 ++-- .../test_thread_safe_synchronization.cpp | 10 ++-- 9 files changed, 47 insertions(+), 79 deletions(-) diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 280d79c39d..c803629dcd 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -109,7 +109,7 @@ class Waitable RCLCPP_PUBLIC virtual void - add_to_wait_set(rcl_wait_set_t & wait_set); + add_to_wait_set(rcl_wait_set_t & wait_set) = 0; /// Check if the Waitable is ready. /** @@ -124,7 +124,7 @@ class Waitable RCLCPP_PUBLIC virtual bool - is_ready(const rcl_wait_set_t & wait_set); + is_ready(const rcl_wait_set_t & wait_set) = 0; /// Take the data so that it can be consumed with `execute`. /** @@ -176,7 +176,7 @@ class Waitable RCLCPP_PUBLIC virtual std::shared_ptr - take_data_by_entity_id(size_t id); + take_data_by_entity_id(size_t id) = 0; /// Execute data that is passed in. /** @@ -203,7 +203,7 @@ class Waitable RCLCPP_PUBLIC virtual void - execute(const std::shared_ptr & data); + execute(const std::shared_ptr & data) = 0; /// Exchange the "in use by wait set" state for this timer. /** @@ -246,7 +246,7 @@ class Waitable RCLCPP_PUBLIC virtual void - set_on_ready_callback(std::function callback); + set_on_ready_callback(std::function callback) = 0; /// Unset any callback registered via set_on_ready_callback. /** @@ -256,7 +256,7 @@ class Waitable RCLCPP_PUBLIC virtual void - clear_on_ready_callback(); + clear_on_ready_callback() = 0; private: std::atomic in_use_by_wait_set_{false}; diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index 1ee4f9073f..ef3a50a8d9 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -54,54 +54,8 @@ Waitable::get_number_of_ready_guard_conditions() return 0u; } -std::shared_ptr -Waitable::take_data_by_entity_id(size_t id) -{ - (void)id; - throw std::runtime_error( - "Custom waitables should override take_data_by_entity_id " - "if they want to use it."); -} - bool 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 and make sure to call it on the waitable destructor."); -} - -bool -Waitable::is_ready(const rcl_wait_set_t & wait_set) -{ - return this->is_ready(wait_set); -} - -void -Waitable::add_to_wait_set(rcl_wait_set_t & wait_set) -{ - this->add_to_wait_set(wait_set); -} - -void -Waitable::execute(const std::shared_ptr & data) -{ - // note this const cast is only required to support a deprecated function - this->execute(const_cast &>(data)); -} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp index aa34a71af4..2b6b1092be 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp @@ -31,13 +31,13 @@ class TestWaitable : public rclcpp::Waitable 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 - { - return nullptr; - } - + std::shared_ptr take_data() override {return nullptr;} void execute(const std::shared_ptr &) override {} + + void set_on_ready_callback(std::function) override {} + void clear_on_ready_callback() override {} + + std::shared_ptr take_data_by_entity_id(size_t) override {return nullptr;} }; 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 452228645b..27c2711916 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -51,13 +51,13 @@ class TestWaitable : public rclcpp::Waitable return test_waitable_result; } - std::shared_ptr - take_data() override - { - return nullptr; - } - + std::shared_ptr take_data() override {return nullptr;} void execute(const std::shared_ptr &) override {} + + void set_on_ready_callback(std::function) override {} + void clear_on_ready_callback() override {} + + std::shared_ptr take_data_by_entity_id(size_t) override {return nullptr;} }; struct RclWaitSetSizes diff --git a/rclcpp/test/rclcpp/test_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp index 4c166ebb88..7ceb4e7be4 100644 --- a/rclcpp/test/rclcpp/test_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/test_memory_strategy.cpp @@ -40,6 +40,11 @@ class TestWaitable : public rclcpp::Waitable std::shared_ptr take_data() override {return nullptr;} void execute(const std::shared_ptr &) override {} + + void set_on_ready_callback(std::function) override {} + void clear_on_ready_callback() override {} + + std::shared_ptr take_data_by_entity_id(size_t) override {return nullptr;} }; class TestMemoryStrategy : public ::testing::Test 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 12bd2f884e..e69d3480d9 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp @@ -52,16 +52,18 @@ class TestWaitable : public rclcpp::Waitable : is_ready_(false) {} void add_to_wait_set(rcl_wait_set_t &) override {} - bool is_ready(const rcl_wait_set_t &) override {return is_ready_;} std::shared_ptr take_data() override {return nullptr;} - - void - execute(const std::shared_ptr &) override {} + void execute(const std::shared_ptr &) override {} void set_is_ready(bool value) {is_ready_ = value;} + void set_on_ready_callback(std::function) override {} + void clear_on_ready_callback() override {} + + std::shared_ptr take_data_by_entity_id(size_t) override {return nullptr;} + private: bool is_ready_; }; 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 f3e94b6aa0..ed3336e123 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp @@ -52,16 +52,18 @@ class TestWaitable : public rclcpp::Waitable : is_ready_(false) {} void add_to_wait_set(rcl_wait_set_t &) override {} - bool is_ready(const rcl_wait_set_t &) override {return is_ready_;} std::shared_ptr take_data() override {return nullptr;} - - void - execute(const std::shared_ptr &) override {} + void execute(const std::shared_ptr &) override {} void set_is_ready(bool value) {is_ready_ = value;} + void set_on_ready_callback(std::function) override {} + void clear_on_ready_callback() override {} + + std::shared_ptr take_data_by_entity_id(size_t) override {return nullptr;} + private: bool is_ready_; }; 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 eaf3a866d1..f7ae12c2f3 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 @@ -61,14 +61,17 @@ class TestWaitable : public rclcpp::Waitable bool is_ready(const rcl_wait_set_t &) override {return is_ready_;} std::shared_ptr take_data() override {return nullptr;} - - void - execute(const std::shared_ptr & data) override {(void)data;} + void execute(const std::shared_ptr &) override {} void set_is_ready(bool value) {is_ready_ = value;} void set_add_to_wait_set(bool value) {add_to_wait_set_ = value;} + void set_on_ready_callback(std::function) override {} + void clear_on_ready_callback() override {} + + std::shared_ptr take_data_by_entity_id(size_t) override {return nullptr;} + private: bool is_ready_; bool add_to_wait_set_; 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 e6a034832b..12c8838188 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 @@ -52,16 +52,18 @@ class TestWaitable : public rclcpp::Waitable : is_ready_(false) {} void add_to_wait_set(rcl_wait_set_t &) override {} - bool is_ready(const rcl_wait_set_t &) override {return is_ready_;} std::shared_ptr take_data() override {return nullptr;} - - void - execute(const std::shared_ptr &) override {} + void execute(const std::shared_ptr &) override {} void set_is_ready(bool value) {is_ready_ = value;} + void set_on_ready_callback(std::function) override {} + void clear_on_ready_callback() override {} + + std::shared_ptr take_data_by_entity_id(size_t) override {return nullptr;} + private: bool is_ready_; }; From 4e4f0cf43be417ba4ab86fa0c25120c852206b43 Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Mon, 5 Aug 2024 11:12:33 -0400 Subject: [PATCH 413/455] Shut down context during init if logging config fails (#2594) We already do this clean-up if some other tasks below fail. Before: [ RUN ] TestUtilities.test_context_init_shutdown_fails [ERROR] [1722555370.075637014] [rclcpp]: rcl context unexpectedly not shutdown during cleanup [WARN] [1722555370.077175569] [rclcpp]: logging was initialized more than once [ OK ] TestUtilities.test_context_init_shutdown_fails (3 ms) After: [ RUN ] TestUtilities.test_context_init_shutdown_fails [WARN] [1722555108.693207861] [rclcpp]: logging was initialized more than once [ OK ] TestUtilities.test_context_init_shutdown_fails (3 ms) Also, remove an unnecessary line in `test_utilities`, and expect context to not be valid if init fails. Signed-off-by: Christophe Bedard --- rclcpp/src/rclcpp/context.cpp | 37 +++++++++++++-------------- rclcpp/test/rclcpp/test_utilities.cpp | 3 ++- 2 files changed, 20 insertions(+), 20 deletions(-) diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index bbc1d29d0f..a9b40b733e 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -212,28 +212,27 @@ Context::init( } rcl_context_.reset(context, __delete_context); - if (init_options.auto_initialize_logging()) { - logging_mutex_ = get_global_logging_mutex(); - std::lock_guard guard(*logging_mutex_); - size_t & count = get_logging_reference_count(); - if (0u == count) { - ret = rcl_logging_configure_with_output_handler( - &rcl_context_->global_arguments, - rcl_init_options_get_allocator(init_options.get_rcl_init_options()), - rclcpp_logging_output_handler); - if (RCL_RET_OK != ret) { - rcl_context_.reset(); - rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure logging"); + try { + if (init_options.auto_initialize_logging()) { + logging_mutex_ = get_global_logging_mutex(); + std::lock_guard guard(*logging_mutex_); + size_t & count = get_logging_reference_count(); + if (0u == count) { + ret = rcl_logging_configure_with_output_handler( + &rcl_context_->global_arguments, + rcl_init_options_get_allocator(init_options.get_rcl_init_options()), + rclcpp_logging_output_handler); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure logging"); + } + } else { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "logging was initialized more than once"); } - } else { - RCLCPP_WARN( - rclcpp::get_logger("rclcpp"), - "logging was initialized more than once"); + ++count; } - ++count; - } - try { std::vector unparsed_ros_arguments = detail::get_unparsed_ros_arguments( argc, argv, &(rcl_context_->global_arguments), rcl_get_default_allocator()); if (!unparsed_ros_arguments.empty()) { diff --git a/rclcpp/test/rclcpp/test_utilities.cpp b/rclcpp/test/rclcpp/test_utilities.cpp index 98874845e3..b64b0a31c6 100644 --- a/rclcpp/test/rclcpp/test_utilities.cpp +++ b/rclcpp/test/rclcpp/test_utilities.cpp @@ -178,11 +178,11 @@ MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <) TEST(TestUtilities, test_context_init_shutdown_fails) { { - auto context = std::make_shared(); auto context_fail_init = std::make_shared(); auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_init, RCL_RET_ERROR); EXPECT_THROW(context_fail_init->init(0, nullptr), rclcpp::exceptions::RCLError); + EXPECT_FALSE(context_fail_init->is_valid()); } { @@ -190,6 +190,7 @@ TEST(TestUtilities, test_context_init_shutdown_fails) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_logging_configure_with_output_handler, RCL_RET_ERROR); EXPECT_THROW(context_fail_init->init(0, nullptr), rclcpp::exceptions::RCLError); + EXPECT_FALSE(context_fail_init->is_valid()); } { From 2739327ee9af0256c7edcbccb810513af0018851 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Mon, 5 Aug 2024 12:07:07 -0700 Subject: [PATCH 414/455] fix rclcpp/test/rclcpp/CMakeLists.txt to check for the correct targets existance (#2596) Signed-off-by: Alberto Soragna --- rclcpp/test/rclcpp/CMakeLists.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 756e1c67c6..217e816e63 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -468,7 +468,7 @@ ament_add_gtest( executors/test_executors_timer_cancel_behavior.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 180) -if(TARGET test_executors) +if(TARGET test_executors_timer_cancel_behavior) target_link_libraries(test_executors_timer_cancel_behavior ${PROJECT_NAME} ${rosgraph_msgs_TARGETS}) endif() @@ -477,7 +477,7 @@ ament_add_gtest( executors/test_executors_callback_group_behavior.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 180) -if(TARGET test_executors) +if(TARGET test_executors_callback_group_behavior) target_link_libraries(test_executors_callback_group_behavior ${PROJECT_NAME}) endif() @@ -486,7 +486,7 @@ ament_add_gtest( executors/test_executors_intraprocess.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 180) -if(TARGET test_executors) +if(TARGET test_executors_intraprocess) target_link_libraries(test_executors_intraprocess ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() @@ -496,7 +496,7 @@ ament_add_gtest( executors/test_waitable.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 180) -if(TARGET test_executors) +if(TARGET test_executors_busy_waiting) target_link_libraries(test_executors_busy_waiting ${PROJECT_NAME}) endif() From 496c45549bf5793482d554d32b76fb114667869c Mon Sep 17 00:00:00 2001 From: Alexis Pojomovsky Date: Wed, 7 Aug 2024 21:15:08 -0400 Subject: [PATCH 415/455] Fix bug in timers lifecycle for events executor (#2586) * Remove expired timers before updating the collection Signed-off-by: Alexis Pojomovsky * Add regression test for reinitialized timers bug Signed-off-by: Alexis Pojomovsky * Add missing includes Signed-off-by: Alexis Pojomovsky * Relocate test under the executors directory Signed-off-by: Alexis Pojomovsky * Extend test to run with all supported executors Signed-off-by: Alexis Pojomovsky * Adjust comment in fix to make it more generic Signed-off-by: Alexis Pojomovsky * Apply ament clang format to test Signed-off-by: Alexis Pojomovsky * Fix uncrustify findings Signed-off-by: Alexis Pojomovsky --------- Signed-off-by: Alexis Pojomovsky Co-authored-by: Alexis Pojomovsky --- .../events_executor/events_executor.cpp | 5 + rclcpp/test/rclcpp/CMakeLists.txt | 7 ++ .../executors/test_reinitialized_timers.cpp | 92 +++++++++++++++++++ 3 files changed, 104 insertions(+) create mode 100644 rclcpp/test/rclcpp/executors/test_reinitialized_timers.cpp 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 b7dd1487f9..5c1a8e8c5c 100644 --- a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp +++ b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp @@ -341,6 +341,11 @@ EventsExecutor::refresh_current_collection( // Acquire lock before modifying the current collection std::lock_guard guard(mutex_); + // Remove expired entities to ensure re-initialized objects + // are updated. This fixes issues with stale state entities. + // See: https://github.com/ros2/rclcpp/pull/2586 + current_collection_.remove_expired_entities(); + current_collection_.timers.update( new_collection.timers, [this](rclcpp::TimerBase::SharedPtr timer) {timers_manager_->add_timer(timer);}, diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 217e816e63..dd4fcc1faa 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -450,6 +450,13 @@ if(TARGET test_interface_traits) target_link_libraries(test_interface_traits ${PROJECT_NAME}) endif() +ament_add_gtest(test_reinitialized_timers + executors/test_reinitialized_timers.cpp + TIMEOUT 30) +if(TARGET test_reinitialized_timers) + target_link_libraries(test_reinitialized_timers ${PROJECT_NAME}) +endif() + ament_add_gtest( test_executors executors/test_executors.cpp diff --git a/rclcpp/test/rclcpp/executors/test_reinitialized_timers.cpp b/rclcpp/test/rclcpp/executors/test_reinitialized_timers.cpp new file mode 100644 index 0000000000..55a46a78e2 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_reinitialized_timers.cpp @@ -0,0 +1,92 @@ +// Copyright 2024 iRobot Corporation. +// +// 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. + +#include + +#include +#include +#include +#include + +#include "rclcpp/executors/multi_threaded_executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/executors/static_single_threaded_executor.hpp" +#include "rclcpp/experimental/executors/events_executor/events_executor.hpp" +#include "rclcpp/rclcpp.hpp" + +template +class TestTimersLifecycle : public testing::Test +{ +public: + void SetUp() override {rclcpp::init(0, nullptr);} + + void TearDown() override {rclcpp::shutdown();} +}; + +using ExecutorTypes = ::testing::Types< + rclcpp::executors::SingleThreadedExecutor, rclcpp::executors::MultiThreadedExecutor, + rclcpp::executors::StaticSingleThreadedExecutor, rclcpp::experimental::executors::EventsExecutor>; + +TYPED_TEST_SUITE(TestTimersLifecycle, ExecutorTypes); + +TYPED_TEST(TestTimersLifecycle, timers_lifecycle_reinitialized_object) +{ + auto timers_period = std::chrono::milliseconds(50); + auto node = std::make_shared("test_node"); + auto executor = std::make_unique(); + + executor->add_node(node); + + size_t count_1 = 0; + auto timer_1 = rclcpp::create_timer( + node, node->get_clock(), rclcpp::Duration(timers_period), [&count_1]() {count_1++;}); + + size_t count_2 = 0; + auto timer_2 = rclcpp::create_timer( + node, node->get_clock(), rclcpp::Duration(timers_period), [&count_2]() {count_2++;}); + + { + std::thread executor_thread([&executor]() {executor->spin();}); + + while (count_2 < 10u) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + executor->cancel(); + executor_thread.join(); + + EXPECT_GE(count_2, 10u); + EXPECT_LE(count_2 - count_1, 1u); + } + + count_1 = 0; + timer_1 = rclcpp::create_timer( + node, node->get_clock(), rclcpp::Duration(timers_period), [&count_1]() {count_1++;}); + + count_2 = 0; + timer_2 = rclcpp::create_timer( + node, node->get_clock(), rclcpp::Duration(timers_period), [&count_2]() {count_2++;}); + + { + std::thread executor_thread([&executor]() {executor->spin();}); + + while (count_2 < 10u) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + executor->cancel(); + executor_thread.join(); + + EXPECT_GE(count_2, 10u); + EXPECT_LE(count_2 - count_1, 1u); + } +} From ab7cf878c2b9bf8df9cda0740ba235c73d8aff25 Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Thu, 8 Aug 2024 16:01:15 -0400 Subject: [PATCH 416/455] Properly test get_service_names_and_types_by_node in rclcpp_lifecycle (#2599) The current `TestLifecycleServiceClient.get_service_names_and_types_by_node` test was using `LifecycleServiceClient`, which is just a normal `rclcpp::Node` with some `rclcpp::Client`s, to test `NodeGraph::get_service_names_and_types_by_node()`. However, `NodeGraph::get_service_names_and_types_by_node()` is for service servers, not clients. The test just ended up checking the built-in services of an `rclcpp::Node`, since it wasn't actually checking the names of the services, and not checking the clients of the `LifecycleServiceClient` or the built-in services of a `rclcpp_lifecycle::LifecycleNode`. I believe this was probably not the intention, since this is an `rclcpp_lifecycle` test. Instead, use an actual `rclcpp_lifecycle::LifecycleNode` and check its service servers by calling `NodeGraph::get_service_names_and_types_by_node()` through `LifecycleNode::get_service_names_and_types_by_node()`. Signed-off-by: Christophe Bedard --- .../test/test_lifecycle_service_client.cpp | 39 ++++++++++++------- 1 file changed, 25 insertions(+), 14 deletions(-) diff --git a/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp index 3698c807f6..fc3f86e573 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp @@ -18,11 +18,14 @@ */ #include +#include +#include #include #include #include #include #include +#include #include #include "lifecycle_msgs/msg/state.hpp" @@ -368,26 +371,34 @@ TEST_F(TestLifecycleServiceClient, lifecycle_transitions) { TEST_F(TestLifecycleServiceClient, get_service_names_and_types_by_node) { - auto node1 = std::make_shared("client1"); - auto node2 = std::make_shared("client2"); - - auto node_graph = node1->get_node_graph_interface(); - ASSERT_NE(nullptr, node_graph); - EXPECT_THROW( - node_graph->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"), + lifecycle_node()->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"), std::runtime_error); - auto service_names_and_types1 = node_graph->get_service_names_and_types_by_node("client1", "/"); - auto service_names_and_types2 = node_graph->get_service_names_and_types_by_node("client2", "/"); + auto service_names_and_types = + lifecycle_node()->get_service_names_and_types_by_node(lifecycle_node_name, "/"); auto start = std::chrono::steady_clock::now(); - while (0 == service_names_and_types1.size() || - service_names_and_types1.size() != service_names_and_types2.size() || + while (0 == service_names_and_types.size() || (std::chrono::steady_clock::now() - start) < std::chrono::seconds(1)) { - service_names_and_types1 = node_graph->get_service_names_and_types_by_node("client1", "/"); - service_names_and_types2 = node_graph->get_service_names_and_types_by_node("client2", "/"); + service_names_and_types = + lifecycle_node()->get_service_names_and_types_by_node(lifecycle_node_name, "/"); + } + const std::array services = { + std::make_pair(node_get_state_topic, "lifecycle_msgs/srv/GetState"), + std::make_pair(node_change_state_topic, "lifecycle_msgs/srv/ChangeState"), + std::make_pair(node_get_available_states_topic, "lifecycle_msgs/srv/GetAvailableStates"), + std::make_pair( + node_get_available_transitions_topic, "lifecycle_msgs/srv/GetAvailableTransitions"), + std::make_pair(node_get_transition_graph_topic, "lifecycle_msgs/srv/GetAvailableTransitions"), + }; + for (const auto & [service_name, service_type] : services) { + ASSERT_TRUE(service_names_and_types.find(service_name) != service_names_and_types.end()) + << service_name; + const auto service_types = service_names_and_types.at(service_name); + EXPECT_TRUE( + std::find(service_types.cbegin(), service_types.cend(), service_type) != service_types.cend()) + << service_name; } - EXPECT_EQ(service_names_and_types1.size(), service_names_and_types2.size()); } TEST_F(TestLifecycleServiceClient, declare_parameter_with_no_initial_values) From 6da83635828fa5b41fe328acc53107e06ca57303 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Thu, 8 Aug 2024 17:05:34 -0700 Subject: [PATCH 417/455] subscriber_statistics_collectors_ should be protected by mutex. (#2592) * subscriber_statistics_collectors_ should be protected by mutex. Signed-off-by: Tomoya Fujita * reduce mutex lock scope. Signed-off-by: Tomoya Fujita --------- Signed-off-by: Tomoya Fujita --- .../rclcpp/topic_statistics/subscription_topic_statistics.hpp | 3 +-- .../topic_statistics/test_subscription_topic_statistics.cpp | 1 - 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp index 781e2c86fc..7a6db13502 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp @@ -172,12 +172,11 @@ class SubscriptionTopicStatistics { auto received_message_age = std::make_unique(); received_message_age->Start(); - subscriber_statistics_collectors_.emplace_back(std::move(received_message_age)); - auto received_message_period = std::make_unique(); received_message_period->Start(); { std::lock_guard lock(mutex_); + subscriber_statistics_collectors_.emplace_back(std::move(received_message_age)); subscriber_statistics_collectors_.emplace_back(std::move(received_message_period)); } diff --git a/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp index 9166272207..a6fbbcbc74 100644 --- a/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp @@ -343,7 +343,6 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no uint64_t message_age_count{0}; uint64_t message_period_count{0}; - std::set received_metrics; for (const auto & msg : received_messages) { if (msg.metrics_source == kMessageAgeSourceLabel) { message_age_count++; From f961ca78557d570a0ef1943d072cad9c5156aa1d Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Mon, 12 Aug 2024 12:39:49 -0400 Subject: [PATCH 418/455] Fix typo in rclcpp_components benchmark_components (#2602) Signed-off-by: Christophe Bedard --- rclcpp_components/test/benchmark/benchmark_components.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp_components/test/benchmark/benchmark_components.cpp b/rclcpp_components/test/benchmark/benchmark_components.cpp index 9b9c8cf4c8..26d607f5fb 100644 --- a/rclcpp_components/test/benchmark/benchmark_components.cpp +++ b/rclcpp_components/test/benchmark/benchmark_components.cpp @@ -109,7 +109,7 @@ BENCHMARK_F(ComponentTest, create_node_instance)(benchmark::State & state) } // Choosing resource 0 - the other two test components were shown empirically to yield - // the same performance charactarisitics, so they shouldn't need their own benchmarks. + // the same performance characteristics, so they shouldn't need their own benchmarks. const std::shared_ptr factory = manager->create_component_factory(resources[0]); From e6b6faf152ae3c758f73a35ad1385bfd987f346f Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Sun, 18 Aug 2024 12:29:52 -0400 Subject: [PATCH 419/455] Fix name of ParameterEventHandler class in doc (#2604) Signed-off-by: Christophe Bedard --- rclcpp/include/rclcpp/parameter_event_handler.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter_event_handler.hpp b/rclcpp/include/rclcpp/parameter_event_handler.hpp index 93b8177fb2..25fbf725ff 100644 --- a/rclcpp/include/rclcpp/parameter_event_handler.hpp +++ b/rclcpp/include/rclcpp/parameter_event_handler.hpp @@ -126,7 +126,7 @@ struct ParameterEventCallbackHandle * // Now that we know the event matches the regular expression we scanned for, we can * // use 'get_parameter_from_event' to get a specific parameter name that we're looking for * rclcpp::Parameter p; - * if (rclcpp::ParameterEventsSubscriber::get_parameter_from_event( + * if (rclcpp::ParameterEventHandler::get_parameter_from_event( * event, p, remote_param_name, fqn)) * { * RCLCPP_INFO( @@ -139,7 +139,7 @@ struct ParameterEventCallbackHandle * * // You can also use 'get_parameter*s*_from_event' to enumerate all changes that came * // in on this event - * auto params = rclcpp::ParameterEventsSubscriber::get_parameters_from_event(event); + * auto params = rclcpp::ParameterEventHandler::get_parameters_from_event(event); * for (auto & p : params) { * RCLCPP_INFO( * node->get_logger(), From dfaaf4739a5bd549d1183ebd02fef024cfc8e2f9 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Mon, 19 Aug 2024 20:13:21 -0700 Subject: [PATCH 420/455] deprecate the static single threaded executor (#2598) * deprecate the static single threded executor Signed-off-by: Alberto Soragna * suppress deprecation warning for static executor and remove benchmark tests Signed-off-by: Alberto Soragna * fix uncrustify linter errors Signed-off-by: Alberto Soragna * fix windows deprecation warnings i created an alias to give the deprecated executor a new name; this works when the class is directly used but it doesn't work in combination with templates (like std::make_shared) because the compiler needs to resolved the type behind the alias triggering the warning Signed-off-by: Alberto Soragna * update test_reinitialized_timers.cpp to use the executor test utilities Signed-off-by: Alberto Soragna * do not use executor pointer Signed-off-by: Alberto Soragna --------- Signed-off-by: Alberto Soragna Co-authored-by: Alberto Soragna --- .../static_single_threaded_executor.hpp | 11 ++- rclcpp/test/benchmark/benchmark_executor.cpp | 89 ------------------- .../test/rclcpp/executors/executor_types.hpp | 22 ++++- .../test/rclcpp/executors/test_executors.cpp | 14 +-- .../executors/test_executors_busy_waiting.cpp | 32 +++++-- .../test_executors_timer_cancel_behavior.cpp | 2 +- .../executors/test_reinitialized_timers.cpp | 24 +++-- .../test_static_single_threaded_executor.cpp | 15 ++-- .../test_add_callback_groups_to_executor.cpp | 42 +-------- 9 files changed, 80 insertions(+), 171 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index 6f22909caf..33674465a8 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -31,9 +31,15 @@ namespace executors /// Static executor implementation /** * This executor is a static version of the original single threaded executor. - * It's static because it doesn't reconstruct the executable list for every iteration. + * It contains some performance optimization to avoid unnecessary reconstructions of + * the executable list for every iteration. * All nodes, callbackgroups, timers, subscriptions etc. are created before * spin() is called, and modified only when an entity is added/removed to/from a node. + * This executor is deprecated because these performance improvements have now been + * applied to all other executors. + * This executor is also considered unstable due to known bugs. + * See the unit-tests that are only applied to `StandardExecutors` for information + * on the known limitations. * * To run this executor instead of SingleThreadedExecutor replace: * rclcpp::executors::SingleThreadedExecutor exec; @@ -44,7 +50,8 @@ namespace executors * exec.spin(); * exec.remove_node(node); */ -class StaticSingleThreadedExecutor : public rclcpp::Executor +class [[deprecated("Use rclcpp::executors::SingleThreadedExecutor")]] StaticSingleThreadedExecutor + : public rclcpp::Executor { public: RCLCPP_SMART_PTR_DEFINITIONS(StaticSingleThreadedExecutor) diff --git a/rclcpp/test/benchmark/benchmark_executor.cpp b/rclcpp/test/benchmark/benchmark_executor.cpp index 65bb3a1007..ea7afa8696 100644 --- a/rclcpp/test/benchmark/benchmark_executor.cpp +++ b/rclcpp/test/benchmark/benchmark_executor.cpp @@ -189,71 +189,6 @@ BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_remove_node)(be } } -BENCHMARK_F( - PerformanceTestExecutorSimple, - static_single_thread_executor_add_node)(benchmark::State & st) -{ - rclcpp::executors::StaticSingleThreadedExecutor executor; - for (auto _ : st) { - (void)_; - executor.add_node(node); - st.PauseTiming(); - executor.remove_node(node); - st.ResumeTiming(); - } -} - -BENCHMARK_F( - PerformanceTestExecutorSimple, - static_single_thread_executor_remove_node)(benchmark::State & st) -{ - rclcpp::executors::StaticSingleThreadedExecutor executor; - for (auto _ : st) { - (void)_; - st.PauseTiming(); - executor.add_node(node); - st.ResumeTiming(); - executor.remove_node(node); - } -} - -BENCHMARK_F( - PerformanceTestExecutorSimple, - static_single_thread_executor_spin_until_future_complete)(benchmark::State & st) -{ - rclcpp::executors::StaticSingleThreadedExecutor executor; - // test success of an immediately finishing future - std::promise promise; - std::future future = promise.get_future(); - promise.set_value(true); - auto shared_future = future.share(); - - auto ret = executor.spin_until_future_complete(shared_future, 100ms); - if (ret != rclcpp::FutureReturnCode::SUCCESS) { - st.SkipWithError(rcutils_get_error_string().str); - } - - reset_heap_counters(); - - for (auto _ : st) { - (void)_; - // static_single_thread_executor has a special design. We need to add/remove the node each - // time you call spin - st.PauseTiming(); - executor.add_node(node); - st.ResumeTiming(); - - ret = executor.spin_until_future_complete(shared_future, 100ms); - if (ret != rclcpp::FutureReturnCode::SUCCESS) { - st.SkipWithError(rcutils_get_error_string().str); - break; - } - st.PauseTiming(); - executor.remove_node(node); - st.ResumeTiming(); - } -} - BENCHMARK_F( PerformanceTestExecutorSimple, single_thread_executor_spin_node_until_future_complete)(benchmark::State & st) @@ -314,30 +249,6 @@ BENCHMARK_F( } } -BENCHMARK_F( - PerformanceTestExecutorSimple, - static_single_thread_executor_spin_node_until_future_complete)(benchmark::State & st) -{ - rclcpp::executors::StaticSingleThreadedExecutor executor; - // test success of an immediately finishing future - std::promise promise; - std::future future = promise.get_future(); - promise.set_value(true); - auto shared_future = future.share(); - - reset_heap_counters(); - - for (auto _ : st) { - (void)_; - auto ret = rclcpp::executors::spin_node_until_future_complete( - executor, node, shared_future, 1s); - if (ret != rclcpp::FutureReturnCode::SUCCESS) { - st.SkipWithError(rcutils_get_error_string().str); - break; - } - } -} - BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark::State & st) { // test success of an immediately finishing future diff --git a/rclcpp/test/rclcpp/executors/executor_types.hpp b/rclcpp/test/rclcpp/executors/executor_types.hpp index 0218a9b547..02462faf5f 100644 --- a/rclcpp/test/rclcpp/executors/executor_types.hpp +++ b/rclcpp/test/rclcpp/executors/executor_types.hpp @@ -25,11 +25,29 @@ #include "rclcpp/executors/static_single_threaded_executor.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" +// suppress deprecated StaticSingleThreadedExecutor warning +// we define an alias that explicitly indicates that this class is deprecated, while avoiding +// polluting a lot of files the gcc pragmas +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif +using DeprecatedStaticSingleThreadedExecutor = rclcpp::executors::StaticSingleThreadedExecutor; +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + using ExecutorTypes = ::testing::Types< rclcpp::executors::SingleThreadedExecutor, rclcpp::executors::MultiThreadedExecutor, - rclcpp::executors::StaticSingleThreadedExecutor, + DeprecatedStaticSingleThreadedExecutor, rclcpp::experimental::executors::EventsExecutor>; class ExecutorTypeNames @@ -47,7 +65,7 @@ class ExecutorTypeNames return "MultiThreadedExecutor"; } - if (std::is_same()) { + if (std::is_same()) { return "StaticSingleThreadedExecutor"; } diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 72afee8dda..b79de72971 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -489,7 +489,7 @@ TYPED_TEST(TestExecutors, spin_some_max_duration) // for them in the meantime. // see: https://github.com/ros2/rclcpp/issues/2462 if ( - std::is_same()) + std::is_same()) { GTEST_SKIP(); } @@ -674,20 +674,20 @@ TYPED_TEST(TestExecutors, testRaceConditionAddNode) } // Create an executor - auto executor = std::make_shared(); + ExecutorType executor; // Start spinning auto executor_thread = std::thread( - [executor]() { - executor->spin(); + [&executor]() { + executor.spin(); }); // Add a node to the executor - executor->add_node(this->node); + executor.add_node(this->node); // Cancel the executor (make sure that it's already spinning first) - while (!executor->is_spinning() && rclcpp::ok()) { + while (!executor.is_spinning() && rclcpp::ok()) { continue; } - executor->cancel(); + executor.cancel(); // Try to join the thread after cancelling the executor // This is the "test". We want to make sure that we can still cancel the executor diff --git a/rclcpp/test/rclcpp/executors/test_executors_busy_waiting.cpp b/rclcpp/test/rclcpp/executors/test_executors_busy_waiting.cpp index bc3ce4b62d..d481638afb 100644 --- a/rclcpp/test/rclcpp/executors/test_executors_busy_waiting.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors_busy_waiting.cpp @@ -47,9 +47,6 @@ class TestBusyWaiting : public ::testing::Test auto waitable_interfaces = node->get_node_waitables_interface(); waitable = std::make_shared(); waitable_interfaces->add_waitable(waitable, callback_group); - - executor = std::make_shared(); - executor->add_callback_group(callback_group, node->get_node_base_interface()); } void TearDown() override @@ -108,7 +105,6 @@ class TestBusyWaiting : public ::testing::Test rclcpp::CallbackGroup::SharedPtr callback_group; std::shared_ptr waitable; std::chrono::steady_clock::time_point start_time; - std::shared_ptr executor; bool has_executed; }; @@ -116,10 +112,16 @@ TYPED_TEST_SUITE(TestBusyWaiting, ExecutorTypes, ExecutorTypeNames); TYPED_TEST(TestBusyWaiting, test_spin_all) { + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_callback_group( + this->callback_group, + this->node->get_node_base_interface()); + this->set_up_and_trigger_waitable(); auto start_time = std::chrono::steady_clock::now(); - this->executor->spin_all(this->max_duration); + executor.spin_all(this->max_duration); this->check_for_busy_waits(start_time); // this should get the initial trigger, and the follow up from in the callback ASSERT_EQ(this->waitable->get_count(), 2u); @@ -127,10 +129,16 @@ TYPED_TEST(TestBusyWaiting, test_spin_all) TYPED_TEST(TestBusyWaiting, test_spin_some) { + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_callback_group( + this->callback_group, + this->node->get_node_base_interface()); + this->set_up_and_trigger_waitable(); auto start_time = std::chrono::steady_clock::now(); - this->executor->spin_some(this->max_duration); + executor.spin_some(this->max_duration); this->check_for_busy_waits(start_time); // this should get the inital trigger, but not the follow up in the callback ASSERT_EQ(this->waitable->get_count(), 1u); @@ -138,6 +146,12 @@ TYPED_TEST(TestBusyWaiting, test_spin_some) TYPED_TEST(TestBusyWaiting, test_spin) { + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_callback_group( + this->callback_group, + this->node->get_node_base_interface()); + std::condition_variable cv; std::mutex cv_m; bool first_check_passed = false; @@ -151,8 +165,8 @@ TYPED_TEST(TestBusyWaiting, test_spin) }); auto start_time = std::chrono::steady_clock::now(); - std::thread t([this]() { - this->executor->spin(); + std::thread t([&executor]() { + executor.spin(); }); // wait until thread has started (first execute of waitable) @@ -172,7 +186,7 @@ TYPED_TEST(TestBusyWaiting, test_spin) } EXPECT_EQ(this->waitable->get_count(), 2u); - this->executor->cancel(); + executor.cancel(); t.join(); this->check_for_busy_waits(start_time); diff --git a/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp index f6bbd2ccc8..72bbceecb2 100644 --- a/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp @@ -286,7 +286,7 @@ using MainExecutorTypes = ::testing::Types< rclcpp::executors::SingleThreadedExecutor, rclcpp::executors::MultiThreadedExecutor, - rclcpp::executors::StaticSingleThreadedExecutor>; + DeprecatedStaticSingleThreadedExecutor>; // TODO(@fujitatomoya): this test excludes EventExecutor because it does not // support simulation time used for this test to relax the racy condition. diff --git a/rclcpp/test/rclcpp/executors/test_reinitialized_timers.cpp b/rclcpp/test/rclcpp/executors/test_reinitialized_timers.cpp index 55a46a78e2..d1ac90b95b 100644 --- a/rclcpp/test/rclcpp/executors/test_reinitialized_timers.cpp +++ b/rclcpp/test/rclcpp/executors/test_reinitialized_timers.cpp @@ -19,12 +19,10 @@ #include #include -#include "rclcpp/executors/multi_threaded_executor.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/executors/static_single_threaded_executor.hpp" -#include "rclcpp/experimental/executors/events_executor/events_executor.hpp" #include "rclcpp/rclcpp.hpp" +#include "./executor_types.hpp" + template class TestTimersLifecycle : public testing::Test { @@ -34,19 +32,17 @@ class TestTimersLifecycle : public testing::Test void TearDown() override {rclcpp::shutdown();} }; -using ExecutorTypes = ::testing::Types< - rclcpp::executors::SingleThreadedExecutor, rclcpp::executors::MultiThreadedExecutor, - rclcpp::executors::StaticSingleThreadedExecutor, rclcpp::experimental::executors::EventsExecutor>; - TYPED_TEST_SUITE(TestTimersLifecycle, ExecutorTypes); TYPED_TEST(TestTimersLifecycle, timers_lifecycle_reinitialized_object) { + using ExecutorType = TypeParam; + ExecutorType executor; + auto timers_period = std::chrono::milliseconds(50); auto node = std::make_shared("test_node"); - auto executor = std::make_unique(); - executor->add_node(node); + executor.add_node(node); size_t count_1 = 0; auto timer_1 = rclcpp::create_timer( @@ -57,12 +53,12 @@ TYPED_TEST(TestTimersLifecycle, timers_lifecycle_reinitialized_object) node, node->get_clock(), rclcpp::Duration(timers_period), [&count_2]() {count_2++;}); { - std::thread executor_thread([&executor]() {executor->spin();}); + std::thread executor_thread([&executor]() {executor.spin();}); while (count_2 < 10u) { std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - executor->cancel(); + executor.cancel(); executor_thread.join(); EXPECT_GE(count_2, 10u); @@ -78,12 +74,12 @@ TYPED_TEST(TestTimersLifecycle, timers_lifecycle_reinitialized_object) node, node->get_clock(), rclcpp::Duration(timers_period), [&count_2]() {count_2++;}); { - std::thread executor_thread([&executor]() {executor->spin();}); + std::thread executor_thread([&executor]() {executor.spin();}); while (count_2 < 10u) { std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - executor->cancel(); + executor.cancel(); executor_thread.join(); EXPECT_GE(count_2, 10u); diff --git a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp index 05d0b23152..a834c7b2be 100644 --- a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp @@ -20,12 +20,13 @@ #include #include "rclcpp/exceptions.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/node.hpp" #include "rclcpp/rclcpp.hpp" -#include "rclcpp/executors.hpp" #include "test_msgs/srv/empty.hpp" +#include "./executor_types.hpp" #include "../../mocking_utils/patch.hpp" #include "../../utils/rclcpp_gtest_macros.hpp" @@ -46,7 +47,7 @@ class TestStaticSingleThreadedExecutor : public ::testing::Test }; TEST_F(TestStaticSingleThreadedExecutor, add_callback_group_trigger_guard_failed) { - rclcpp::executors::StaticSingleThreadedExecutor executor; + DeprecatedStaticSingleThreadedExecutor executor; auto node = std::make_shared("node", "ns"); rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); @@ -61,7 +62,7 @@ TEST_F(TestStaticSingleThreadedExecutor, add_callback_group_trigger_guard_failed } TEST_F(TestStaticSingleThreadedExecutor, add_node_trigger_guard_failed) { - rclcpp::executors::StaticSingleThreadedExecutor executor; + DeprecatedStaticSingleThreadedExecutor executor; auto node = std::make_shared("node", "ns"); { @@ -74,7 +75,7 @@ TEST_F(TestStaticSingleThreadedExecutor, add_node_trigger_guard_failed) { } TEST_F(TestStaticSingleThreadedExecutor, remove_callback_group_trigger_guard_failed) { - rclcpp::executors::StaticSingleThreadedExecutor executor; + DeprecatedStaticSingleThreadedExecutor executor; auto node = std::make_shared("node", "ns"); rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); @@ -92,7 +93,7 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_callback_group_trigger_guard_fai } TEST_F(TestStaticSingleThreadedExecutor, remove_node_failed) { - rclcpp::executors::StaticSingleThreadedExecutor executor; + DeprecatedStaticSingleThreadedExecutor executor; auto node = std::make_shared("node", "ns"); { @@ -105,7 +106,7 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_node_failed) { } TEST_F(TestStaticSingleThreadedExecutor, remove_node_trigger_guard_failed) { - rclcpp::executors::StaticSingleThreadedExecutor executor; + DeprecatedStaticSingleThreadedExecutor executor; auto node = std::make_shared("node", "ns"); executor.add_node(node); @@ -120,7 +121,7 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_node_trigger_guard_failed) { } TEST_F(TestStaticSingleThreadedExecutor, execute_service) { - rclcpp::executors::StaticSingleThreadedExecutor executor; + DeprecatedStaticSingleThreadedExecutor executor; auto node = std::make_shared("node", "ns"); executor.add_node(node); diff --git a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp index 6edcc2cb28..2c628e5cf8 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -29,6 +29,8 @@ #include "rclcpp/executor.hpp" #include "rclcpp/rclcpp.hpp" +#include "./executors/executor_types.hpp" + using namespace std::chrono_literals; template @@ -48,48 +50,8 @@ class TestAddCallbackGroupsToExecutor : public ::testing::Test template class TestAddCallbackGroupsToExecutorStable : public TestAddCallbackGroupsToExecutor {}; -using ExecutorTypes = - ::testing::Types< - rclcpp::executors::SingleThreadedExecutor, - rclcpp::executors::MultiThreadedExecutor, - rclcpp::executors::StaticSingleThreadedExecutor, - rclcpp::experimental::executors::EventsExecutor>; - -class ExecutorTypeNames -{ -public: - template - static std::string GetName(int idx) - { - (void)idx; - if (std::is_same()) { - return "SingleThreadedExecutor"; - } - - if (std::is_same()) { - return "MultiThreadedExecutor"; - } - - if (std::is_same()) { - return "StaticSingleThreadedExecutor"; - } - - if (std::is_same()) { - return "EventsExecutor"; - } - - return ""; - } -}; - TYPED_TEST_SUITE(TestAddCallbackGroupsToExecutor, ExecutorTypes, ExecutorTypeNames); -// StaticSingleThreadedExecutor is not included in these tests for now -using StandardExecutors = - ::testing::Types< - rclcpp::executors::SingleThreadedExecutor, - rclcpp::executors::MultiThreadedExecutor, - rclcpp::experimental::executors::EventsExecutor>; TYPED_TEST_SUITE(TestAddCallbackGroupsToExecutorStable, StandardExecutors, ExecutorTypeNames); /* From 4b1c125cacbb718a32aabfa27b19e9b12669fff6 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 21 Aug 2024 17:58:34 -0400 Subject: [PATCH 421/455] Fix a couple of issues in the documentation. (#2608) These should make it render more nicely. Signed-off-by: Chris Lalancette --- .../rclcpp/parameter_event_handler.hpp | 120 ++++++++++-------- rclcpp/include/rclcpp/publisher.hpp | 2 +- 2 files changed, 67 insertions(+), 55 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter_event_handler.hpp b/rclcpp/include/rclcpp/parameter_event_handler.hpp index 25fbf725ff..589fb1144c 100644 --- a/rclcpp/include/rclcpp/parameter_event_handler.hpp +++ b/rclcpp/include/rclcpp/parameter_event_handler.hpp @@ -67,19 +67,23 @@ struct ParameterEventCallbackHandle * The first step is to instantiate a ParameterEventHandler, providing a ROS node to use * to create any required subscriptions: * - * auto param_handler = std::make_shared(node); + * ```cpp + * auto param_handler = std::make_shared(node); + * ``` * * Next, you can supply a callback to the add_parameter_callback method, as follows: * - * auto cb1 = [&node](const rclcpp::Parameter & p) { - * RCLCPP_INFO( - * node->get_logger(), - * "cb1: Received an update to parameter \"%s\" of type %s: \"%ld\"", - * p.get_name().c_str(), - * p.get_type_name().c_str(), - * p.as_int()); - * }; - * auto handle1 = param_handler->add_parameter_callback("an_int_param", cb1); + * ```cpp + * auto cb1 = [&node](const rclcpp::Parameter & p) { + * RCLCPP_INFO( + * node->get_logger(), + * "cb1: Received an update to parameter \"%s\" of type %s: \"%ld\"", + * p.get_name().c_str(), + * p.get_type_name().c_str(), + * p.as_int()); + * }; + * auto handle1 = param_handler->add_parameter_callback("an_int_param", cb1); + * ``` * * In this case, we didn't supply a node name (the third, optional, parameter) so the * default will be to monitor for changes to the "an_int_param" parameter associated with @@ -92,16 +96,18 @@ struct ParameterEventCallbackHandle * You may also monitor for changes to parameters in other nodes by supplying the node * name to add_parameter_callback: * - * auto cb2 = [&node](const rclcpp::Parameter & p) { - * RCLCPP_INFO( - * node->get_logger(), - * "cb2: Received an update to parameter \"%s\" of type: %s: \"%s\"", - * p.get_name().c_str(), - * p.get_type_name().c_str(), - * p.as_string().c_str()); - * }; - * auto handle2 = param_handler->add_parameter_callback( - * "some_remote_param_name", cb2, "some_remote_node_name"); + * ```cpp + * auto cb2 = [&node](const rclcpp::Parameter & p) { + * RCLCPP_INFO( + * node->get_logger(), + * "cb2: Received an update to parameter \"%s\" of type: %s: \"%s\"", + * p.get_name().c_str(), + * p.get_type_name().c_str(), + * p.as_string().c_str()); + * }; + * auto handle2 = param_handler->add_parameter_callback( + * "some_remote_param_name", cb2, "some_remote_node_name"); + * ``` * * In this case, the callback will be invoked whenever "some_remote_param_name" changes * on remote node "some_remote_node_name". @@ -109,7 +115,9 @@ struct ParameterEventCallbackHandle * To remove a parameter callback, reset the callback handle smart pointer or call * remove_parameter_callback, passing the handle returned from add_parameter_callback: * - * param_handler->remove_parameter_callback(handle2); + * ```cpp + * param_handler->remove_parameter_callback(handle2); + * ``` * * You can also monitor for *all* parameter changes, using add_parameter_event_callback. * In this case, the callback will be invoked whenever any parameter changes in the system. @@ -117,40 +125,42 @@ struct ParameterEventCallbackHandle * is convenient to use a regular expression on the node names or namespaces of interest. * For example: * - * auto cb3 = - * [fqn, remote_param_name, &node](const rcl_interfaces::msg::ParameterEvent & event) { - * // Look for any updates to parameters in "/a_namespace" as well as any parameter changes - * // to our own node ("this_node") - * std::regex re("(/a_namespace/.*)|(/this_node)"); - * if (regex_match(event.node, re)) { - * // Now that we know the event matches the regular expression we scanned for, we can - * // use 'get_parameter_from_event' to get a specific parameter name that we're looking for - * rclcpp::Parameter p; - * if (rclcpp::ParameterEventHandler::get_parameter_from_event( - * event, p, remote_param_name, fqn)) - * { - * RCLCPP_INFO( - * node->get_logger(), - * "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"", - * p.get_name().c_str(), - * p.get_type_name().c_str(), - * p.as_string().c_str()); - * } + * ```cpp + * auto cb3 = + * [fqn, remote_param_name, &node](const rcl_interfaces::msg::ParameterEvent & event) { + * // Look for any updates to parameters in "/a_namespace" as well as any parameter changes + * // to our own node ("this_node") + * std::regex re("(/a_namespace/.*)|(/this_node)"); + * if (regex_match(event.node, re)) { + * // Now that we know the event matches the regular expression we scanned for, we can + * // use 'get_parameter_from_event' to get a specific parameter name that we're looking for + * rclcpp::Parameter p; + * if (rclcpp::ParameterEventHandler::get_parameter_from_event( + * event, p, remote_param_name, fqn)) + * { + * RCLCPP_INFO( + * node->get_logger(), + * "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"", + * p.get_name().c_str(), + * p.get_type_name().c_str(), + * p.as_string().c_str()); + * } * - * // You can also use 'get_parameter*s*_from_event' to enumerate all changes that came - * // in on this event - * auto params = rclcpp::ParameterEventHandler::get_parameters_from_event(event); - * for (auto & p : params) { - * RCLCPP_INFO( - * node->get_logger(), - * "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"", - * p.get_name().c_str(), - * p.get_type_name().c_str(), - * p.value_to_string().c_str()); - * } + * // You can also use 'get_parameter*s*_from_event' to enumerate all changes that came + * // in on this event + * auto params = rclcpp::ParameterEventHandler::get_parameters_from_event(event); + * for (auto & p : params) { + * RCLCPP_INFO( + * node->get_logger(), + * "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"", + * p.get_name().c_str(), + * p.get_type_name().c_str(), + * p.value_to_string().c_str()); * } - * }; - * auto handle3 = param_handler->add_parameter_event_callback(cb3); + * } + * }; + * auto handle3 = param_handler->add_parameter_event_callback(cb3); + * ``` * * For both parameter callbacks and parameter event callbacks, when multiple callbacks are added, * the callbacks are invoked last-in, first-called order (LIFO). @@ -160,7 +170,9 @@ struct ParameterEventCallbackHandle * * To remove a parameter event callback, reset the callback smart pointer or use: * - * param_handler->remove_event_parameter_callback(handle3); + * ```cpp + * param_handler->remove_event_parameter_callback(handle3); + * ``` */ class ParameterEventHandler { diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 4861cd0096..22825daf89 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -187,7 +187,7 @@ class Publisher : public PublisherBase * the loaned message will be directly allocated in the middleware. * If not, the message allocator of this rclcpp::Publisher instance is being used. * - * With a call to \sa `publish` the LoanedMessage instance is being returned to the middleware + * With a call to `publish` the LoanedMessage instance is being returned to the middleware * or free'd accordingly to the allocator. * If the message is not being published but processed differently, the destructor of this * class will either return the message to the middleware or deallocate it via the internal From b7c789328ad3c2fed0b2e72f42cf51190052760c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 22 Aug 2024 08:45:51 +0200 Subject: [PATCH 422/455] Removed clang warnings (#2605) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../test/rclcpp/executors/executor_types.hpp | 15 +++++- .../test/rclcpp/executors/test_executors.cpp | 8 ++++ .../test_executors_timer_cancel_behavior.cpp | 10 +++- .../executors/test_reinitialized_timers.cpp | 2 +- .../test_static_single_threaded_executor.cpp | 48 +++++++++++++++++++ 5 files changed, 80 insertions(+), 3 deletions(-) diff --git a/rclcpp/test/rclcpp/executors/executor_types.hpp b/rclcpp/test/rclcpp/executors/executor_types.hpp index 02462faf5f..baa13be9d5 100644 --- a/rclcpp/test/rclcpp/executors/executor_types.hpp +++ b/rclcpp/test/rclcpp/executors/executor_types.hpp @@ -43,12 +43,19 @@ using DeprecatedStaticSingleThreadedExecutor = rclcpp::executors::StaticSingleTh # pragma warning(pop) #endif +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wdeprecated-declarations" +#endif using ExecutorTypes = ::testing::Types< rclcpp::executors::SingleThreadedExecutor, rclcpp::executors::MultiThreadedExecutor, DeprecatedStaticSingleThreadedExecutor, rclcpp::experimental::executors::EventsExecutor>; +#ifdef __clang__ +# pragma clang diagnostic pop +#endif class ExecutorTypeNames { @@ -64,10 +71,16 @@ class ExecutorTypeNames if (std::is_same()) { return "MultiThreadedExecutor"; } - +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wdeprecated-declarations" +#endif if (std::is_same()) { return "StaticSingleThreadedExecutor"; } +#ifdef __clang__ +# pragma clang diagnostic pop +#endif if (std::is_same()) { return "EventsExecutor"; diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index b79de72971..8e0b0c875f 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -488,11 +488,19 @@ TYPED_TEST(TestExecutors, spin_some_max_duration) // do not properly implement max_duration (it seems), so disable this test // for them in the meantime. // see: https://github.com/ros2/rclcpp/issues/2462 +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wdeprecated-declarations" +#endif + if ( std::is_same()) { GTEST_SKIP(); } +#ifdef __clang__ +# pragma clang diagnostic pop +#endif // Use an isolated callback group to avoid interference from any housekeeping // items that may be in the default callback group of the node. diff --git a/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp index 72bbceecb2..fbf38146fb 100644 --- a/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp @@ -282,12 +282,20 @@ class TestTimerCancelBehavior : public ::testing::Test T executor; }; +#if !defined(_WIN32) +# ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wdeprecated-declarations" +# endif +#endif using MainExecutorTypes = ::testing::Types< rclcpp::executors::SingleThreadedExecutor, rclcpp::executors::MultiThreadedExecutor, DeprecatedStaticSingleThreadedExecutor>; - +#ifdef __clang__ +# pragma clang diagnostic pop +#endif // TODO(@fujitatomoya): this test excludes EventExecutor because it does not // support simulation time used for this test to relax the racy condition. // See more details for https://github.com/ros2/rclcpp/issues/2457. diff --git a/rclcpp/test/rclcpp/executors/test_reinitialized_timers.cpp b/rclcpp/test/rclcpp/executors/test_reinitialized_timers.cpp index d1ac90b95b..99725cb95e 100644 --- a/rclcpp/test/rclcpp/executors/test_reinitialized_timers.cpp +++ b/rclcpp/test/rclcpp/executors/test_reinitialized_timers.cpp @@ -32,7 +32,7 @@ class TestTimersLifecycle : public testing::Test void TearDown() override {rclcpp::shutdown();} }; -TYPED_TEST_SUITE(TestTimersLifecycle, ExecutorTypes); +TYPED_TEST_SUITE(TestTimersLifecycle, ExecutorTypes, ExecutorTypeNames); TYPED_TEST(TestTimersLifecycle, timers_lifecycle_reinitialized_object) { diff --git a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp index a834c7b2be..8e330cbebc 100644 --- a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp @@ -47,7 +47,15 @@ class TestStaticSingleThreadedExecutor : public ::testing::Test }; TEST_F(TestStaticSingleThreadedExecutor, add_callback_group_trigger_guard_failed) { +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wdeprecated-declarations" +#endif + DeprecatedStaticSingleThreadedExecutor executor; +#ifdef __clang__ +# pragma clang diagnostic pop +#endif auto node = std::make_shared("node", "ns"); rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); @@ -62,7 +70,15 @@ TEST_F(TestStaticSingleThreadedExecutor, add_callback_group_trigger_guard_failed } TEST_F(TestStaticSingleThreadedExecutor, add_node_trigger_guard_failed) { +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wdeprecated-declarations" +#endif + DeprecatedStaticSingleThreadedExecutor executor; +#ifdef __clang__ +# pragma clang diagnostic pop +#endif auto node = std::make_shared("node", "ns"); { @@ -75,7 +91,15 @@ TEST_F(TestStaticSingleThreadedExecutor, add_node_trigger_guard_failed) { } TEST_F(TestStaticSingleThreadedExecutor, remove_callback_group_trigger_guard_failed) { +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wdeprecated-declarations" +#endif + DeprecatedStaticSingleThreadedExecutor executor; +#ifdef __clang__ +# pragma clang diagnostic pop +#endif auto node = std::make_shared("node", "ns"); rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); @@ -93,7 +117,15 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_callback_group_trigger_guard_fai } TEST_F(TestStaticSingleThreadedExecutor, remove_node_failed) { +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wdeprecated-declarations" +#endif + DeprecatedStaticSingleThreadedExecutor executor; +#ifdef __clang__ +# pragma clang diagnostic pop +#endif auto node = std::make_shared("node", "ns"); { @@ -106,7 +138,15 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_node_failed) { } TEST_F(TestStaticSingleThreadedExecutor, remove_node_trigger_guard_failed) { +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wdeprecated-declarations" +#endif + DeprecatedStaticSingleThreadedExecutor executor; +#ifdef __clang__ +# pragma clang diagnostic pop +#endif auto node = std::make_shared("node", "ns"); executor.add_node(node); @@ -121,7 +161,15 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_node_trigger_guard_failed) { } TEST_F(TestStaticSingleThreadedExecutor, execute_service) { +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wdeprecated-declarations" +#endif + DeprecatedStaticSingleThreadedExecutor executor; +#ifdef __clang__ +# pragma clang diagnostic pop +#endif auto node = std::make_shared("node", "ns"); executor.add_node(node); From ee94bc63e4ce47a502891480a2796b53d54fcdfc Mon Sep 17 00:00:00 2001 From: "Kang, Hsin-Yi" Date: Thu, 22 Aug 2024 21:46:50 +0800 Subject: [PATCH 423/455] Minor naming fixes for ParameterValue to_string() function (#2609) More appropriate function argument naming. Refer to: https://github.com/ros2/rclcpp/blob/rolling/rclcpp/src/rclcpp/parameter_value.cpp#L83 Signed-off-by: Kang, Hsin-Yi --- rclcpp/include/rclcpp/parameter_value.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/parameter_value.hpp b/rclcpp/include/rclcpp/parameter_value.hpp index fac896ea46..549429aa85 100644 --- a/rclcpp/include/rclcpp/parameter_value.hpp +++ b/rclcpp/include/rclcpp/parameter_value.hpp @@ -359,7 +359,7 @@ class ParameterValue /// Return the value of a parameter as a string RCLCPP_PUBLIC std::string -to_string(const ParameterValue & type); +to_string(const ParameterValue & value); } // namespace rclcpp From e846f56224a39b93f1c609e7ee03fff0662b7453 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Mon, 2 Sep 2024 15:42:48 +0800 Subject: [PATCH 424/455] Correct node name in service test code (#2615) Signed-off-by: Barry Xu --- rclcpp/test/rclcpp/test_service.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index 1a00ceb527..c6b3d3ace1 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -123,7 +123,7 @@ TEST_F(TestServiceSub, construction_and_destruction) { { ASSERT_THROW( { - auto service = node->create_service( + auto service = subnode->create_service( "invalid_service?", callback); }, rclcpp::exceptions::InvalidServiceNameError); } From 2f71d6e249f626772da3f8a1bb7c8d141d9d0d52 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Fri, 6 Sep 2024 14:18:03 +0200 Subject: [PATCH 425/455] remove unnecessary gtest-skip in test_executors (#2600) Signed-off-by: Alberto Soragna --- .../test/rclcpp/executors/test_executors.cpp | 31 +++---------------- 1 file changed, 5 insertions(+), 26 deletions(-) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 8e0b0c875f..cea0900a39 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -480,28 +480,14 @@ TYPED_TEST(TestExecutors, spin_some) // The purpose of this test is to check that the ExecutorT.spin_some() method: // - does not continue executing after max_duration has elapsed -TYPED_TEST(TestExecutors, spin_some_max_duration) +// TODO(wjwwood): The `StaticSingleThreadedExecutor` +// do not properly implement max_duration (it seems), so disable this test +// for them in the meantime. +// see: https://github.com/ros2/rclcpp/issues/2462 +TYPED_TEST(TestExecutorsStable, spin_some_max_duration) { using ExecutorType = TypeParam; - // TODO(wjwwood): The `StaticSingleThreadedExecutor` - // do not properly implement max_duration (it seems), so disable this test - // for them in the meantime. - // see: https://github.com/ros2/rclcpp/issues/2462 -#ifdef __clang__ -# pragma clang diagnostic push -# pragma clang diagnostic ignored "-Wdeprecated-declarations" -#endif - - if ( - std::is_same()) - { - GTEST_SKIP(); - } -#ifdef __clang__ -# pragma clang diagnostic pop -#endif - // Use an isolated callback group to avoid interference from any housekeeping // items that may be in the default callback group of the node. constexpr bool automatically_add_to_executor_with_node = false; @@ -655,13 +641,6 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) TYPED_TEST(TestExecutors, testRaceConditionAddNode) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } // Spawn some threads to do some heavy work std::atomic should_cancel = false; From f7056c0d86cd3b66c31ca02eb17d2dc6e789a3c4 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Wed, 11 Sep 2024 09:08:37 +0200 Subject: [PATCH 426/455] fix events-executor warm-up bug and add unit-tests (#2591) * add unit-test to verify that spin-all doesn't need warm-up Signed-off-by: Alberto Soragna * improve comment and add callback group test Signed-off-by: Alberto Soragna * move executor tests to a new file Signed-off-by: Alberto Soragna * do not require warm up iteration with events executor spin_some Signed-off-by: Alberto Soragna * add spin_some tests and cleanup Signed-off-by: Alberto Soragna * add missing include directives Signed-off-by: Alberto Soragna --------- Signed-off-by: Alberto Soragna Co-authored-by: mergify[bot] <37929162+mergify[bot]@users.noreply.github.com> --- .../events_executor/events_executor.cpp | 20 +- rclcpp/test/rclcpp/CMakeLists.txt | 10 + .../executors/test_executors_warmup.cpp | 244 ++++++++++++++++++ 3 files changed, 273 insertions(+), 1 deletion(-) create mode 100644 rclcpp/test/rclcpp/executors/test_executors_warmup.cpp 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 5c1a8e8c5c..e8a6e1889c 100644 --- a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp +++ b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp @@ -79,7 +79,6 @@ EventsExecutor::setup_notify_waitable() // ---> we need to wake up the executor so that it can terminate // - a node or callback group guard condition is triggered: // ---> the entities collection is changed, we need to update callbacks - entities_need_rebuild_ = false; this->handle_updated_entities(false); }); @@ -168,6 +167,14 @@ EventsExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhau return false; }; + // If this spin is not exhaustive (e.g. spin_some), we need to explicitly check + // if entities need to be rebuilt here rather than letting the notify waitable event do it. + // A non-exhaustive spin would not check for work a second time, thus delaying the execution + // of some entities to the next invocation of spin. + if (!exhaustive) { + this->handle_updated_entities(false); + } + // Get the number of events and timers ready at start const size_t ready_events_at_start = events_queue_->size(); size_t executed_events = 0; @@ -314,6 +321,17 @@ void EventsExecutor::handle_updated_entities(bool notify) { (void)notify; + + // Do not rebuild if we don't need to. + // A rebuild event could be generated, but then + // this function could end up being called from somewhere else + // before that event gets processed, for example if + // a node or callback group is manually added to the executor. + const bool notify_waitable_triggered = entities_need_rebuild_.exchange(false); + if (!notify_waitable_triggered && !this->collector_.has_pending()) { + return; + } + // Build the new collection this->collector_.update_collections(); auto callback_groups = this->collector_.get_all_callback_groups(); diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index dd4fcc1faa..1c6fafe94a 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -507,6 +507,16 @@ if(TARGET test_executors_busy_waiting) target_link_libraries(test_executors_busy_waiting ${PROJECT_NAME}) endif() +ament_add_gtest( + test_executors_warmup + executors/test_executors_warmup.cpp + executors/test_waitable.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) +if(TARGET test_executors_warmup) + target_link_libraries(test_executors_warmup ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() + ament_add_gtest(test_static_single_threaded_executor executors/test_static_single_threaded_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") ament_add_test_label(test_static_single_threaded_executor mimick) diff --git a/rclcpp/test/rclcpp/executors/test_executors_warmup.cpp b/rclcpp/test/rclcpp/executors/test_executors_warmup.cpp new file mode 100644 index 0000000000..7ab26a9da9 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executors_warmup.cpp @@ -0,0 +1,244 @@ +// Copyright 2024 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. + +/** + * This test checks all implementations of rclcpp::executor to check they pass they basic API + * tests. Anything specific to any executor in particular should go in a separate test file. + */ + +#include + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "test_msgs/msg/empty.hpp" + +#include "./executor_types.hpp" + +using namespace std::chrono_literals; + +template +class TestExecutorsWarmup : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TYPED_TEST_SUITE(TestExecutorsWarmup, ExecutorTypes, ExecutorTypeNames); + +// This test verifies that spin_all is correctly collecting work multiple times +// even when one of the items of work is a notifier waitable event and thus results in +// rebuilding the entities collection. +// When spin_all goes back to collect more work, it should see the ready items from +// the new added entities +TYPED_TEST(TestExecutorsWarmup, spin_all_doesnt_require_warmup) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + + // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event + auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true); + auto node = std::make_shared("test_node", node_options); + + // Add node to the executor before creating the entities + executor.add_node(node); + + // Create entities, this will produce a notifier waitable event, telling the executor to refresh + // the entities collection + auto publisher = node->create_publisher("test_topic", rclcpp::QoS(10)); + size_t callback_count = 0; + auto callback = [&callback_count](test_msgs::msg::Empty::ConstSharedPtr) {callback_count++;}; + auto subscription = + node->create_subscription( + "test_topic", rclcpp::QoS(10), std::move(callback)); + + ASSERT_EQ(callback_count, 0u); + + // Publish a message so that the new entities (i.e. the subscriber) already have work to do + publisher->publish(test_msgs::msg::Empty()); + + // We need to select a duration that is greater than + // the time taken to refresh the entities collection and rebuild the waitset. + // spin-all is expected to process the notifier waitable event, rebuild the collection, + // and then collect more work, finding the subscription message event. + // This duration has been selected empirically. + executor.spin_all(std::chrono::milliseconds(500)); + + // Verify that the callback is called as part of the spin above + EXPECT_EQ(callback_count, 1u); +} + +// Same test as `spin_all_doesnt_require_warmup`, but uses a callback group +// This test reproduces the bug reported by https://github.com/ros2/rclcpp/issues/2589 +TYPED_TEST(TestExecutorsWarmup, spin_all_doesnt_require_warmup_with_cbgroup) +{ + using ExecutorType = TypeParam; + + // TODO(alsora): Enable when https://github.com/ros2/rclcpp/pull/2595 gets merged + if ( + std::is_same() || + std::is_same()) + { + GTEST_SKIP(); + } + + ExecutorType executor; + + // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event + auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true); + auto node = std::make_shared("test_node", node_options); + + auto callback_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + + // Add callback group to the executor before creating the entities + executor.add_callback_group(callback_group, node->get_node_base_interface()); + + // Create entities, this will produce a notifier waitable event, telling the executor to refresh + // the entities collection + auto publisher = node->create_publisher("test_topic", rclcpp::QoS(10)); + size_t callback_count = 0; + auto callback = [&callback_count](test_msgs::msg::Empty::ConstSharedPtr) {callback_count++;}; + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = callback_group; + auto subscription = + node->create_subscription( + "test_topic", rclcpp::QoS(10), std::move(callback), sub_options); + + ASSERT_EQ(callback_count, 0u); + + // Publish a message so that the new entities (i.e. the subscriber) already have work to do + publisher->publish(test_msgs::msg::Empty()); + + // We need to select a duration that is greater than + // the time taken to refresh the entities collection and rebuild the waitset. + // spin-all is expected to process the notifier waitable event, rebuild the collection, + // and then collect more work, finding the subscription message event. + // This duration has been selected empirically. + executor.spin_all(std::chrono::milliseconds(500)); + + // Verify that the callback is called as part of the spin above + EXPECT_EQ(callback_count, 1u); +} + +TYPED_TEST(TestExecutorsWarmup, spin_some_doesnt_require_warmup) +{ + using ExecutorType = TypeParam; + + // TODO(alsora): currently only the events-executor passes this test. + // Enable single-threaded and multi-threaded executors + // when https://github.com/ros2/rclcpp/pull/2595 gets merged + if ( + !std::is_same()) + { + GTEST_SKIP(); + } + + ExecutorType executor; + + // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event + auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true); + auto node = std::make_shared("test_node", node_options); + + // Add node to the executor before creating the entities + executor.add_node(node); + + // Create entities, this will produce a notifier waitable event, telling the executor to refresh + // the entities collection + auto publisher = node->create_publisher("test_topic", rclcpp::QoS(10)); + size_t callback_count = 0; + auto callback = [&callback_count](test_msgs::msg::Empty::ConstSharedPtr) {callback_count++;}; + auto subscription = + node->create_subscription( + "test_topic", rclcpp::QoS(10), std::move(callback)); + + ASSERT_EQ(callback_count, 0u); + + // Publish a message so that the new entities (i.e. the subscriber) already have work to do + publisher->publish(test_msgs::msg::Empty()); + + // NOTE: intra-process communication is enabled, so the subscription will immediately see + // the new message, no risk of race conditions where spin_some gets called before the + // message has been delivered. + executor.spin_some(); + + // Verify that the callback is called as part of the spin above + EXPECT_EQ(callback_count, 1u); +} + +TYPED_TEST(TestExecutorsWarmup, spin_some_doesnt_require_warmup_with_cbgroup) +{ + using ExecutorType = TypeParam; + + // TODO(alsora): currently only the events-executor passes this test. + // Enable single-threaded and multi-threaded executors + // when https://github.com/ros2/rclcpp/pull/2595 gets merged + if ( + !std::is_same()) + { + GTEST_SKIP(); + } + + ExecutorType executor; + + // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event + auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true); + auto node = std::make_shared("test_node", node_options); + + auto callback_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + + // Add callback group to the executor before creating the entities + executor.add_callback_group(callback_group, node->get_node_base_interface()); + + // Create entities, this will produce a notifier waitable event, telling the executor to refresh + // the entities collection + auto publisher = node->create_publisher("test_topic", rclcpp::QoS(10)); + size_t callback_count = 0; + auto callback = [&callback_count](test_msgs::msg::Empty::ConstSharedPtr) {callback_count++;}; + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = callback_group; + auto subscription = + node->create_subscription( + "test_topic", rclcpp::QoS(10), std::move(callback), sub_options); + + ASSERT_EQ(callback_count, 0u); + + // Publish a message so that the new entities (i.e. the subscriber) already have work to do + publisher->publish(test_msgs::msg::Empty()); + + // NOTE: intra-process communication is enabled, so the subscription will immediately see + // the new message, no risk of race conditions where spin_some gets called before the + // message has been delivered. + executor.spin_some(); + + // Verify that the callback is called as part of the spin above + EXPECT_EQ(callback_count, 1u); +} From 1a3dfaf45c5a4304a7ccb32a893e8c488ac3fddb Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Fri, 13 Sep 2024 00:04:59 +0800 Subject: [PATCH 427/455] Implement generic service (#2617) * Implement generic service Signed-off-by: Barry Xu * Add the required header files Signed-off-by: Barry Xu * Fix compiling errors on Windows Signed-off-by: Barry Xu * Fix compiling errors on Windows Signed-off-by: Barry Xu * Fix compiling errors on Windows Signed-off-by: Barry Xu --------- Signed-off-by: Barry Xu --- rclcpp/CMakeLists.txt | 1 + .../include/rclcpp/create_generic_service.hpp | 102 +++++ .../include/rclcpp/exceptions/exceptions.hpp | 9 + rclcpp/include/rclcpp/generic_service.hpp | 308 +++++++++++++ rclcpp/include/rclcpp/node.hpp | 19 + rclcpp/include/rclcpp/node_impl.hpp | 20 + rclcpp/src/rclcpp/create_generic_service.cpp | 49 ++ rclcpp/src/rclcpp/generic_service.cpp | 172 +++++++ rclcpp/test/rclcpp/CMakeLists.txt | 12 + rclcpp/test/rclcpp/test_generic_service.cpp | 422 ++++++++++++++++++ 10 files changed, 1114 insertions(+) create mode 100644 rclcpp/include/rclcpp/create_generic_service.hpp create mode 100644 rclcpp/include/rclcpp/generic_service.hpp create mode 100644 rclcpp/src/rclcpp/create_generic_service.cpp create mode 100644 rclcpp/src/rclcpp/generic_service.cpp create mode 100644 rclcpp/test/rclcpp/test_generic_service.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index b1dbfec227..8712b48856 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -77,6 +77,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/future_return_code.cpp src/rclcpp/generic_client.cpp src/rclcpp/generic_publisher.cpp + src/rclcpp/generic_service.cpp src/rclcpp/generic_subscription.cpp src/rclcpp/graph_listener.cpp src/rclcpp/guard_condition.cpp diff --git a/rclcpp/include/rclcpp/create_generic_service.hpp b/rclcpp/include/rclcpp/create_generic_service.hpp new file mode 100644 index 0000000000..9cb032ef76 --- /dev/null +++ b/rclcpp/include/rclcpp/create_generic_service.hpp @@ -0,0 +1,102 @@ +// Copyright 2024 Sony Group Corporation. +// +// 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__CREATE_GENERIC_SERVICE_HPP_ +#define RCLCPP__CREATE_GENERIC_SERVICE_HPP_ + +#include +#include +#include + +#include "rclcpp/generic_service.hpp" +#include "rclcpp/node_interfaces/get_node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/get_node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rmw/rmw.h" + +namespace rclcpp +{ +/// Create a generic service with a given type. +/** + * \param[in] node_base NodeBaseInterface implementation of the node on which + * to create the generic service. + * \param[in] node_services NodeServicesInterface implementation of the node on + * which to create the service. + * \param[in] service_name The name on which the service is accessible. + * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool". + * \param[in] callback The callback to call when the service gets a request. + * \param[in] qos Quality of service profile for the service. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created service. + */ +template +typename rclcpp::GenericService::SharedPtr +create_generic_service( + std::shared_ptr node_base, + std::shared_ptr node_services, + const std::string & service_name, + const std::string & service_type, + CallbackT && callback, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + rclcpp::GenericServiceCallback any_service_callback; + any_service_callback.set(std::forward(callback)); + + rcl_service_options_t service_options = rcl_service_get_default_options(); + service_options.qos = qos.get_rmw_qos_profile(); + + auto serv = GenericService::make_shared( + node_base->get_shared_rcl_node_handle(), + service_name, service_type, any_service_callback, service_options); + auto serv_base_ptr = std::dynamic_pointer_cast(serv); + node_services->add_service(serv_base_ptr, group); + return serv; +} + +/// Create a generic service with a given type. +/** + * The NodeT type needs to have NodeBaseInterface implementation and NodeServicesInterface + * implementation of the node which to create the generic service. + * + * \param[in] node The node on which to create the generic service. + * \param[in] service_name The name on which the service is accessible. + * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool". + * \param[in] callback The callback to call when the service gets a request. + * \param[in] qos Quality of service profile for the service. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created service. + */ +template +typename rclcpp::GenericService::SharedPtr +create_generic_service( + NodeT node, + const std::string & service_name, + const std::string & service_type, + CallbackT && callback, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return create_generic_service( + rclcpp::node_interfaces::get_node_base_interface(node), + rclcpp::node_interfaces::get_node_services_interface(node), + service_name, + service_type, + std::forward(callback), qos.get_rmw_qos_profile(), group); +} +} // namespace rclcpp + +#endif // RCLCPP__CREATE_GENERIC_SERVICE_HPP_ diff --git a/rclcpp/include/rclcpp/exceptions/exceptions.hpp b/rclcpp/include/rclcpp/exceptions/exceptions.hpp index b3a53373ed..08c6b88250 100644 --- a/rclcpp/include/rclcpp/exceptions/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions/exceptions.hpp @@ -100,6 +100,15 @@ class InvalidServiceNameError : public NameValidationError {} }; +class InvalidServiceTypeError : public std::runtime_error +{ +public: + InvalidServiceTypeError() + : std::runtime_error("Service type is invalid.") {} + explicit InvalidServiceTypeError(const std::string & msg) + : std::runtime_error(msg) {} +}; + class UnimplementedError : public std::runtime_error { public: diff --git a/rclcpp/include/rclcpp/generic_service.hpp b/rclcpp/include/rclcpp/generic_service.hpp new file mode 100644 index 0000000000..b4c8d5d9a7 --- /dev/null +++ b/rclcpp/include/rclcpp/generic_service.hpp @@ -0,0 +1,308 @@ +// Copyright 2024 Sony Group Corporation. +// +// 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__GENERIC_SERVICE_HPP_ +#define RCLCPP__GENERIC_SERVICE_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/typesupport_helpers.hpp" + +#include "rosidl_runtime_c/service_type_support_struct.h" +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" +#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp" + +#include "service.hpp" + +namespace rclcpp +{ +class GenericService; + +class GenericServiceCallback +{ +public: + using SharedRequest = std::shared_ptr; + using SharedResponse = std::shared_ptr; + + GenericServiceCallback() + : callback_(std::monostate{}) + {} + + template< + typename CallbackT, + typename std::enable_if_t::value, int> = 0> + void + set(CallbackT && callback) + { + // Workaround Windows issue with std::bind + if constexpr ( + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT, can't satisfy both cpplint and uncrustify + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrWithRequestHeaderCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrDeferResponseCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrDeferResponseCallbackWithServiceHandle + >::value) + { + callback_.template emplace(callback); + } else { + // the else clause is not needed, but anyways we should only be doing this instead + // of all the above workaround ... + callback_ = std::forward(callback); + } + } + + template< + typename CallbackT, + typename std::enable_if_t::value, int> = 0> + void + set(CallbackT && callback) + { + if (!callback) { + throw std::invalid_argument("AnyServiceCallback::set(): callback cannot be nullptr"); + } + // Workaround Windows issue with std::bind + if constexpr ( + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrWithRequestHeaderCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrDeferResponseCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrDeferResponseCallbackWithServiceHandle + >::value) + { + callback_.template emplace(callback); + } else { + // the else clause is not needed, but anyways we should only be doing this instead + // of all the above workaround ... + callback_ = std::forward(callback); + } + } + + SharedResponse + dispatch( + const std::shared_ptr & service_handle, + const std::shared_ptr & request_header, + SharedRequest request, + SharedRequest response) + { + TRACETOOLS_TRACEPOINT(callback_start, static_cast(this), false); + if (std::holds_alternative(callback_)) { + // TODO(ivanpauno): Remove the set method, and force the users of this class + // to pass a callback at construnciton. + throw std::runtime_error{"unexpected request without any callback set"}; + } + if (std::holds_alternative(callback_)) { + const auto & cb = std::get(callback_); + cb(request_header, std::move(request)); + return nullptr; + } + if (std::holds_alternative(callback_)) { + const auto & cb = std::get(callback_); + cb(service_handle, request_header, std::move(request)); + return nullptr; + } + + if (std::holds_alternative(callback_)) { + (void)request_header; + const auto & cb = std::get(callback_); + cb(std::move(request), std::move(response)); + } else if (std::holds_alternative(callback_)) { + const auto & cb = std::get(callback_); + cb(request_header, std::move(request), std::move(response)); + } + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); + return response; + } + + void register_callback_for_tracing() + { +#ifndef TRACETOOLS_DISABLED + std::visit( + [this](auto && arg) { + if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) { + char * symbol = tracetools::get_symbol(arg); + TRACETOOLS_DO_TRACEPOINT( + rclcpp_callback_register, + static_cast(this), + symbol); + std::free(symbol); + } + }, callback_); +#endif // TRACETOOLS_DISABLED + } + +private: + using SharedPtrCallback = std::function; + using SharedPtrWithRequestHeaderCallback = std::function< + void ( + std::shared_ptr, + SharedRequest, + SharedResponse + )>; + using SharedPtrDeferResponseCallback = std::function< + void ( + std::shared_ptr, + SharedRequest + )>; + using SharedPtrDeferResponseCallbackWithServiceHandle = std::function< + void ( + std::shared_ptr, + std::shared_ptr, + SharedRequest + )>; + + std::variant< + std::monostate, + SharedPtrCallback, + SharedPtrWithRequestHeaderCallback, + SharedPtrDeferResponseCallback, + SharedPtrDeferResponseCallbackWithServiceHandle> callback_; +}; + +class GenericService + : public ServiceBase, + public std::enable_shared_from_this +{ +public: + using Request = void *; // Serialized/Deserialized data pointer of request message + using Response = void *; // Serialized/Deserialized data pointer of response message + using SharedRequest = std::shared_ptr; + using SharedResponse = std::shared_ptr; + using CallbackType = std::function; + + using CallbackWithHeaderType = + std::function, + const SharedRequest, + SharedResponse)>; + + RCLCPP_SMART_PTR_DEFINITIONS(GenericService) + + /// Default constructor. + /** + * The constructor for a Service is almost never called directly. + * Instead, services should be instantiated through the function + * rclcpp::create_service(). + * + * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup. + * \param[in] service_name Name of the topic to publish to. + * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool". + * \param[in] any_callback User defined callback to call when a client request is received. + * \param[in] service_options options for the service. + */ + RCLCPP_PUBLIC + GenericService( + std::shared_ptr node_handle, + const std::string & service_name, + const std::string & service_type, + GenericServiceCallback any_callback, + rcl_service_options_t & service_options); + + GenericService() = delete; + + RCLCPP_PUBLIC + virtual ~GenericService() {} + + /// Take the next request from the service. + /** + * \sa ServiceBase::take_type_erased_request(). + * + * \param[out] request_out The reference to a service deserialized request object + * into which the middleware will copy the taken request. + * \param[out] request_id_out The output id for the request which can be used + * to associate response with this request in the future. + * \returns true if the request was taken, otherwise false. + * \throws rclcpp::exceptions::RCLError based exceptions if the underlying + * rcl calls fail. + */ + RCLCPP_PUBLIC + bool + take_request(SharedRequest request_out, rmw_request_id_t & request_id_out); + + RCLCPP_PUBLIC + std::shared_ptr + create_request() override; + + RCLCPP_PUBLIC + std::shared_ptr + create_response(); + + RCLCPP_PUBLIC + std::shared_ptr + create_request_header() override; + + RCLCPP_PUBLIC + void + handle_request( + std::shared_ptr request_header, + std::shared_ptr request) override; + + RCLCPP_PUBLIC + void + send_response(rmw_request_id_t & req_id, SharedResponse & response); + +private: + RCLCPP_DISABLE_COPY(GenericService) + + GenericServiceCallback any_callback_; + + std::shared_ptr ts_lib_; + const rosidl_typesupport_introspection_cpp::MessageMembers * request_members_; + const rosidl_typesupport_introspection_cpp::MessageMembers * response_members_; +}; + +} // namespace rclcpp +#endif // RCLCPP__GENERIC_SERVICE_HPP_ diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index f395bf8ea3..930bf419f1 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -44,6 +44,7 @@ #include "rclcpp/event.hpp" #include "rclcpp/generic_client.hpp" #include "rclcpp/generic_publisher.hpp" +#include "rclcpp/generic_service.hpp" #include "rclcpp/generic_subscription.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" @@ -303,6 +304,24 @@ class Node : public std::enable_shared_from_this const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a GenericService. + /** + * \param[in] service_name The topic to service on. + * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool" + * \param[in] callback User-defined callback function. + * \param[in] qos Quality of service profile for the service. + * \param[in] group Callback group to call the service. + * \return Shared pointer to the created service. + */ + template + typename rclcpp::GenericService::SharedPtr + create_generic_service( + const std::string & service_name, + const std::string & service_type, + CallbackT && callback, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a GenericPublisher. /** * The returned pointer will never be empty, but this function can throw various exceptions, for diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 9ca2c42c2d..5b81bdcba4 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -40,6 +40,7 @@ #include "rclcpp/create_generic_subscription.hpp" #include "rclcpp/create_publisher.hpp" #include "rclcpp/create_service.hpp" +#include "rclcpp/create_generic_service.hpp" #include "rclcpp/create_subscription.hpp" #include "rclcpp/create_timer.hpp" #include "rclcpp/detail/resolve_enable_topic_statistics.hpp" @@ -171,6 +172,25 @@ Node::create_service( group); } +template +typename rclcpp::GenericService::SharedPtr +Node::create_generic_service( + const std::string & service_name, + const std::string & service_type, + CallbackT && callback, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_generic_service( + node_base_, + node_services_, + extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), + service_type, + std::forward(callback), + qos, + group); +} + template std::shared_ptr Node::create_generic_publisher( diff --git a/rclcpp/src/rclcpp/create_generic_service.cpp b/rclcpp/src/rclcpp/create_generic_service.cpp new file mode 100644 index 0000000000..492635beb2 --- /dev/null +++ b/rclcpp/src/rclcpp/create_generic_service.cpp @@ -0,0 +1,49 @@ +// Copyright 2024 Sony Group Corporation. +// +// 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. + +#include +#include + +#include "rclcpp/create_generic_service.hpp" +#include "rclcpp/generic_service.hpp" + +namespace rclcpp +{ +rclcpp::GenericService::SharedPtr +create_generic_service( + std::shared_ptr node_base, + std::shared_ptr node_graph, + std::shared_ptr node_services, + const std::string & service_name, + const std::string & service_type, + GenericServiceCallback any_callback, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + rcl_service_options_t options = rcl_service_get_default_options(); + options.qos = qos.get_rmw_qos_profile(); + + auto srv = rclcpp::GenericService::make_shared( + node_base.get(), + node_graph, + service_name, + service_type, + any_callback, + options); + + auto srv_base_ptr = std::dynamic_pointer_cast(srv); + node_services->add_service(srv_base_ptr, group); + return srv; +} +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/generic_service.cpp b/rclcpp/src/rclcpp/generic_service.cpp new file mode 100644 index 0000000000..75b34993bf --- /dev/null +++ b/rclcpp/src/rclcpp/generic_service.cpp @@ -0,0 +1,172 @@ +// Copyright 2024 Sony Group Corporation. +// +// 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. + +#include "rclcpp/generic_service.hpp" + +namespace rclcpp +{ +GenericService::GenericService( + std::shared_ptr node_handle, + const std::string & service_name, + const std::string & service_type, + GenericServiceCallback any_callback, + rcl_service_options_t & service_options) +: ServiceBase(node_handle), + any_callback_(any_callback) +{ + const rosidl_service_type_support_t * service_ts; + try { + ts_lib_ = get_typesupport_library( + service_type, "rosidl_typesupport_cpp"); + + service_ts = get_service_typesupport_handle( + service_type, "rosidl_typesupport_cpp", *ts_lib_); + + auto request_type_support_intro = get_message_typesupport_handle( + service_ts->request_typesupport, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + request_members_ = static_cast( + request_type_support_intro->data); + + auto response_type_support_intro = get_message_typesupport_handle( + service_ts->response_typesupport, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + response_members_ = static_cast( + response_type_support_intro->data); + } catch (std::runtime_error & err) { + RCLCPP_ERROR( + rclcpp::get_node_logger(node_handle_.get()).get_child("rclcpp"), + "Invalid service type: %s", + err.what()); + throw rclcpp::exceptions::InvalidServiceTypeError(err.what()); + } + + // rcl does the static memory allocation here + service_handle_ = std::shared_ptr( + new rcl_service_t, [handle = node_handle_, service_name](rcl_service_t * service) + { + if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_node_logger(handle.get()).get_child("rclcpp"), + "Error in destruction of rcl service handle: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete service; + }); + *service_handle_.get() = rcl_get_zero_initialized_service(); + + rcl_ret_t ret = rcl_service_init( + service_handle_.get(), + node_handle.get(), + service_ts, + service_name.c_str(), + &service_options); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_SERVICE_NAME_INVALID) { + auto rcl_node_handle = get_rcl_node_handle(); + // this will throw on any validation problem + rcl_reset_error(); + expand_topic_or_service_name( + service_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + true); + } + + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service"); + } + TRACETOOLS_TRACEPOINT( + rclcpp_service_callback_added, + static_cast(get_service_handle().get()), + static_cast(&any_callback_)); +#ifndef TRACETOOLS_DISABLED + any_callback_.register_callback_for_tracing(); +#endif +} + +bool +GenericService::take_request( + SharedRequest request_out, + rmw_request_id_t & request_id_out) +{ + request_out = create_request(); + return this->take_type_erased_request(request_out.get(), request_id_out); +} + +std::shared_ptr +GenericService::create_request() +{ + Request request = new uint8_t[request_members_->size_of_]; + request_members_->init_function(request, rosidl_runtime_cpp::MessageInitialization::ZERO); + return std::shared_ptr( + request, + [this](void * p) + { + request_members_->fini_function(p); + delete[] reinterpret_cast(p); + }); +} + +std::shared_ptr +GenericService::create_response() +{ + Response response = new uint8_t[response_members_->size_of_]; + response_members_->init_function(response, rosidl_runtime_cpp::MessageInitialization::ZERO); + return std::shared_ptr( + response, + [this](void * p) + { + response_members_->fini_function(p); + delete[] reinterpret_cast(p); + }); +} + +std::shared_ptr +GenericService::create_request_header() +{ + return std::make_shared(); +} + +void +GenericService::handle_request( + std::shared_ptr request_header, + std::shared_ptr request) +{ + auto response = any_callback_.dispatch( + this->shared_from_this(), request_header, request, create_response()); + if (response) { + send_response(*request_header, response); + } +} + +void +GenericService::send_response(rmw_request_id_t & req_id, SharedResponse & response) +{ + rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, response.get()); + + if (ret == RCL_RET_TIMEOUT) { + RCLCPP_WARN( + node_logger_.get_child("rclcpp"), + "failed to send response to %s (timeout): %s", + this->get_service_name(), rcl_get_error_string().str); + rcl_reset_error(); + return; + } + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response"); + } +} + +} // namespace rclcpp diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 1c6fafe94a..f9e341087f 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -83,6 +83,18 @@ if(TARGET test_generic_client) ${test_msgs_TARGETS} ) endif() +ament_add_gtest(test_generic_service test_generic_service.cpp) +ament_add_test_label(test_generic_service mimick) +if(TARGET test_generic_service) + target_link_libraries(test_generic_service ${PROJECT_NAME} + mimick + ${rcl_interfaces_TARGETS} + rmw::rmw + rosidl_runtime_cpp::rosidl_runtime_cpp + rosidl_typesupport_cpp::rosidl_typesupport_cpp + ${test_msgs_TARGETS} + ) +endif() ament_add_gtest(test_client_common test_client_common.cpp) ament_add_test_label(test_client_common mimick) if(TARGET test_client_common) diff --git a/rclcpp/test/rclcpp/test_generic_service.cpp b/rclcpp/test/rclcpp/test_generic_service.cpp new file mode 100644 index 0000000000..554fdf0c0f --- /dev/null +++ b/rclcpp/test/rclcpp/test_generic_service.cpp @@ -0,0 +1,422 @@ +// Copyright 2024 Sony Group Corporation. +// +// 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. + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "rcl_interfaces/srv/list_parameters.hpp" +#include "test_msgs/srv/empty.hpp" +#include "test_msgs/srv/basic_types.hpp" + +using namespace std::chrono_literals; + +class TestGenericService : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_node", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +class TestGenericServiceSub : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_node", "/ns"); + subnode = node->create_sub_node("sub_ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::Node::SharedPtr subnode; +}; + +/* + Testing service construction and destruction. + */ +TEST_F(TestGenericService, construction_and_destruction) { + auto callback = []( + rclcpp::GenericService::SharedRequest, + rclcpp::GenericService::SharedResponse) {}; + { + auto generic_service = node->create_generic_service( + "test_generic_service", "rcl_interfaces/srv/ListParameters", callback); + EXPECT_NE(nullptr, generic_service->get_service_handle()); + const rclcpp::ServiceBase * const_service_base = generic_service.get(); + EXPECT_NE(nullptr, const_service_base->get_service_handle()); + } + + { + ASSERT_THROW( + { + auto generic_service = node->create_generic_service( + "invalid_service?", "test_msgs/srv/Empty", callback); + }, rclcpp::exceptions::InvalidServiceNameError); + } + + { + ASSERT_THROW( + { + auto generic_service = node->create_generic_service( + "test_generic_service", "test_msgs/srv/NotExist", callback); + }, rclcpp::exceptions::InvalidServiceTypeError); + } +} + +/* + Testing service construction and destruction for subnodes. + */ +TEST_F(TestGenericServiceSub, construction_and_destruction) { + auto callback = []( + rclcpp::GenericService::SharedRequest, + rclcpp::GenericService::SharedResponse) {}; + { + auto generic_service = subnode->create_generic_service( + "test_generic_service", "rcl_interfaces/srv/ListParameters", callback); + EXPECT_STREQ(generic_service->get_service_name(), "/ns/sub_ns/test_generic_service"); + } + + { + ASSERT_THROW( + { + auto generic_service = subnode->create_generic_service( + "invalid_service?", "test_msgs/srv/Empty", callback); + }, rclcpp::exceptions::InvalidServiceNameError); + } + + { + ASSERT_THROW( + { + auto generic_service = subnode->create_generic_service( + "test_generic_service", "test_msgs/srv/NotExist", callback); + }, rclcpp::exceptions::InvalidServiceTypeError); + } +} + +TEST_F(TestGenericService, construction_and_destruction_rcl_errors) { + auto callback = []( + rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {}; + + { + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_service_init, RCL_RET_ERROR); + // reset() isn't necessary for this exception, it just avoids unused return value warning + EXPECT_THROW( + node->create_generic_service("service", "test_msgs/srv/Empty", callback).reset(), + rclcpp::exceptions::RCLError); + } + { + // reset() is required for this one + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_service_fini, RCL_RET_ERROR); + EXPECT_NO_THROW( + node->create_generic_service("service", "test_msgs/srv/Empty", callback).reset()); + } +} + +TEST_F(TestGenericService, generic_service_take_request) { + auto callback = []( + rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {}; + auto generic_service = + node->create_generic_service("test_service", "test_msgs/srv/Empty", callback); + { + auto request_id = generic_service->create_request_header(); + auto request = generic_service->create_request(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_request, RCL_RET_OK); + EXPECT_TRUE(generic_service->take_request(request, *request_id.get())); + } + { + auto request_id = generic_service->create_request_header(); + auto request = generic_service->create_request(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_request, RCL_RET_SERVICE_TAKE_FAILED); + EXPECT_FALSE(generic_service->take_request(request, *request_id.get())); + } + { + auto request_id = generic_service->create_request_header(); + auto request = generic_service->create_request(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_request, RCL_RET_ERROR); + EXPECT_THROW( + generic_service->take_request(request, *request_id.get()), rclcpp::exceptions::RCLError); + } +} + +TEST_F(TestGenericService, generic_service_send_response) { + auto callback = []( + const rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {}; + auto generic_service = + node->create_generic_service("test_service", "test_msgs/srv/Empty", callback); + + { + auto request_id = generic_service->create_request_header(); + auto response = generic_service->create_response(); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_response, RCL_RET_OK); + EXPECT_NO_THROW(generic_service->send_response(*request_id.get(), response)); + } + + { + auto request_id = generic_service->create_request_header(); + auto response = generic_service->create_response(); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_response, RCL_RET_ERROR); + EXPECT_THROW( + generic_service->send_response(*request_id.get(), response), + rclcpp::exceptions::RCLError); + } +} + +/* + Testing on_new_request callbacks. + */ +TEST_F(TestGenericService, generic_service_on_new_request_callback) { + auto server_callback = []( + const rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {FAIL();}; + rclcpp::ServicesQoS service_qos; + service_qos.keep_last(3); + auto generic_service = node->create_generic_service( + "~/test_service", "test_msgs/srv/Empty", server_callback, service_qos); + + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; + generic_service->set_on_new_request_callback(increase_c1_cb); + + auto client = node->create_client( + "~/test_service", service_qos); + { + 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;}; + generic_service->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); + + generic_service->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;}; + generic_service->set_on_new_request_callback(increase_c3_cb); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c3 < 3 && 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(generic_service->set_on_new_request_callback(invalid_cb), std::invalid_argument); +} + +TEST_F(TestGenericService, rcl_service_response_publisher_get_actual_qos_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_response_publisher_get_actual_qos, nullptr); + auto callback = []( + const rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {}; + auto generic_service = + node->create_generic_service("test_service", "test_msgs/srv/Empty", callback); + RCLCPP_EXPECT_THROW_EQ( + generic_service->get_response_publisher_actual_qos(), + std::runtime_error("failed to get service's response publisher qos settings: error not set")); +} + +TEST_F(TestGenericService, rcl_service_request_subscription_get_actual_qos_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_request_subscription_get_actual_qos, nullptr); + auto callback = []( + const rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {}; + auto generic_service = + node->create_generic_service("test_service", "test_msgs/srv/Empty", callback); + RCLCPP_EXPECT_THROW_EQ( + generic_service->get_request_subscription_actual_qos(), + std::runtime_error("failed to get service's request subscription qos settings: error not set")); +} + +TEST_F(TestGenericService, generic_service_qos) { + rclcpp::ServicesQoS qos_profile; + qos_profile.liveliness(rclcpp::LivelinessPolicy::Automatic); + rclcpp::Duration duration(std::chrono::nanoseconds(1)); + qos_profile.deadline(duration); + qos_profile.lifespan(duration); + qos_profile.liveliness_lease_duration(duration); + + auto callback = []( + const rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {}; + auto generic_service = + node->create_generic_service("test_service", "test_msgs/srv/Empty", callback, qos_profile); + + auto rs_qos = generic_service->get_request_subscription_actual_qos(); + auto rp_qos = generic_service->get_response_publisher_actual_qos(); + + EXPECT_EQ(qos_profile, rp_qos); + // Lifespan has no meaning for subscription/readers + rs_qos.lifespan(qos_profile.lifespan()); + EXPECT_EQ(qos_profile, rs_qos); +} + +TEST_F(TestGenericService, generic_service_qos_depth) { + uint64_t server_cb_count_ = 0; + auto server_callback = [&]( + const rclcpp::GenericService::SharedRequest, + rclcpp::GenericService::SharedResponse) {server_cb_count_++;}; + + auto server_node = std::make_shared("server_node", "/ns"); + + rclcpp::QoS server_qos_profile(2); + + auto generic_service = server_node->create_generic_service( + "test_qos_depth", "test_msgs/srv/Empty", std::move(server_callback), server_qos_profile); + + rclcpp::QoS client_qos_profile(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + auto client = node->create_client("test_qos_depth", client_qos_profile); + + ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); + auto request = std::make_shared(); + + auto client_callback = [&request_result]( + rclcpp::Client::SharedFuture future_response) { + if (nullptr == future_response.get()) { + request_result = ::testing::AssertionFailure() << "Future response was null"; + } + }; + + uint64_t client_requests = 5; + for (uint64_t i = 0; i < client_requests; i++) { + client->async_send_request(request, client_callback); + std::this_thread::sleep_for(10ms); + } + + auto start = std::chrono::steady_clock::now(); + while ((server_cb_count_ < server_qos_profile.depth()) && + (std::chrono::steady_clock::now() - start) < 1s) + { + rclcpp::spin_some(server_node); + std::this_thread::sleep_for(1ms); + } + + // Spin an extra time to check if server QoS depth has been ignored, + // so more server responses might be processed than expected. + rclcpp::spin_some(server_node); + + EXPECT_EQ(server_cb_count_, server_qos_profile.depth()); +} + +TEST_F(TestGenericService, generic_service_and_client) { + const std::string service_name = "test_service"; + const std::string service_type = "test_msgs/srv/BasicTypes"; + int64_t expected_change = 87654321; + + auto callback = [&expected_change]( + const rclcpp::GenericService::SharedRequest request, + rclcpp::GenericService::SharedResponse response) { + auto typed_request = static_cast(request.get()); + auto typed_response = static_cast(response.get()); + + typed_response->int64_value = typed_request->int64_value + expected_change; + }; + auto generic_service = node->create_generic_service(service_name, service_type, callback); + + auto client = node->create_client(service_name); + + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(5))); + ASSERT_TRUE(client->service_is_ready()); + + auto request = std::make_shared(); + request->int64_value = 12345678; + + auto generic_client_callback = [&request, &expected_change]( + std::shared_future future) { + auto response = future.get(); + EXPECT_EQ(response->int64_value, (request->int64_value + expected_change)); + }; + + auto future = + client->async_send_request(request, generic_client_callback); + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), future, std::chrono::seconds(5)); +} From 97c386ce403b1c01f181519b5b1cb026e5999949 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Thu, 12 Sep 2024 09:49:53 -0700 Subject: [PATCH 428/455] LifecycleNode bugfix and add test cases (#2562) * LifecycleNode base class resource needs to be reset via dtor. Signed-off-by: Tomoya Fujita * add debug notice that prints LifecycleNode is not shutdown in dtor. Currently it is user application responsibility to manage the all state control. See more details for https://github.com/ros2/rclcpp/issues/2520. Signed-off-by: Tomoya Fujita * add test cases to call shutdown from each primary state. Signed-off-by: Tomoya Fujita * address review comments. Signed-off-by: Tomoya Fujita --------- Signed-off-by: Tomoya Fujita --- rclcpp_lifecycle/src/lifecycle_node.cpp | 12 ++++ rclcpp_lifecycle/test/test_lifecycle_node.cpp | 65 +++++++++++++++++++ 2 files changed, 77 insertions(+) diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 0c448cf5e6..03ece2e58b 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -152,8 +152,19 @@ LifecycleNode::LifecycleNode( LifecycleNode::~LifecycleNode() { + auto current_state = LifecycleNode::get_current_state().id(); + if (current_state != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { + // This might be leaving sensors and devices without shutting down unintentionally. + // It is user's responsibility to call shutdown to avoid leaving them unknow states. + RCLCPP_WARN( + rclcpp::get_logger("rclcpp_lifecycle"), + "LifecycleNode is not shut down: Node still in state(%u) in destructor", + current_state); + } + // release sub-interfaces in an order that allows them to consult with node_base during tear-down node_waitables_.reset(); + node_type_descriptions_.reset(); node_time_source_.reset(); node_parameters_.reset(); node_clock_.reset(); @@ -162,6 +173,7 @@ LifecycleNode::~LifecycleNode() node_timers_.reset(); node_logging_.reset(); node_graph_.reset(); + node_base_.reset(); } const char * diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 8e0286086e..70993af6d5 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -431,6 +431,71 @@ TEST_F(TestDefaultStateMachine, bad_mood) { EXPECT_EQ(1u, test_node->number_of_callbacks); } +TEST_F(TestDefaultStateMachine, shutdown_from_each_primary_state) { + auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + + // PRIMARY_STATE_UNCONFIGURED to shutdown + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + } + + // PRIMARY_STATE_INACTIVE to shutdown + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + ret = reset_key; + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + } + + // PRIMARY_STATE_ACTIVE to shutdown + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + ret = reset_key; + auto activated = test_node->activate(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); + ret = reset_key; + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + } + + // PRIMARY_STATE_FINALIZED to shutdown + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + ret = reset_key; + auto activated = test_node->activate(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); + ret = reset_key; + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + ret = reset_key; + auto finalized_again = test_node->shutdown(ret); + EXPECT_EQ(reset_key, ret); + EXPECT_EQ(finalized_again.id(), State::PRIMARY_STATE_FINALIZED); + } +} + TEST_F(TestDefaultStateMachine, lifecycle_subscriber) { auto test_node = std::make_shared>("testnode"); From 918363d081c0c5a85ba5fec83ca0f80768cbb9c2 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Mon, 16 Sep 2024 23:53:48 +0800 Subject: [PATCH 429/455] Use InvalidServiceTypeError for unavailable service type in GenericClient (#2629) Signed-off-by: Barry Xu --- rclcpp/src/rclcpp/generic_client.cpp | 33 ++++++++++++++-------- rclcpp/test/rclcpp/test_generic_client.cpp | 2 +- 2 files changed, 22 insertions(+), 13 deletions(-) diff --git a/rclcpp/src/rclcpp/generic_client.cpp b/rclcpp/src/rclcpp/generic_client.cpp index fdcfc70aab..987975d803 100644 --- a/rclcpp/src/rclcpp/generic_client.cpp +++ b/rclcpp/src/rclcpp/generic_client.cpp @@ -31,22 +31,31 @@ GenericClient::GenericClient( rcl_client_options_t & client_options) : ClientBase(node_base, node_graph) { - ts_lib_ = get_typesupport_library( - service_type, "rosidl_typesupport_cpp"); - - auto service_ts_ = get_service_typesupport_handle( - service_type, "rosidl_typesupport_cpp", *ts_lib_); - - auto response_type_support_intro = get_message_typesupport_handle( - service_ts_->response_typesupport, - rosidl_typesupport_introspection_cpp::typesupport_identifier); - response_members_ = static_cast( - response_type_support_intro->data); + const rosidl_service_type_support_t * service_ts; + try { + ts_lib_ = get_typesupport_library( + service_type, "rosidl_typesupport_cpp"); + + service_ts = get_service_typesupport_handle( + service_type, "rosidl_typesupport_cpp", *ts_lib_); + + auto response_type_support_intro = get_message_typesupport_handle( + service_ts->response_typesupport, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + response_members_ = static_cast( + response_type_support_intro->data); + } catch (std::runtime_error & err) { + RCLCPP_ERROR( + rclcpp::get_node_logger(node_handle_.get()).get_child("rclcpp"), + "Invalid service type: %s", + err.what()); + throw rclcpp::exceptions::InvalidServiceTypeError(err.what()); + } rcl_ret_t ret = rcl_client_init( this->get_client_handle().get(), this->get_rcl_node_handle(), - service_ts_, + service_ts, service_name.c_str(), &client_options); if (ret != RCL_RET_OK) { diff --git a/rclcpp/test/rclcpp/test_generic_client.cpp b/rclcpp/test/rclcpp/test_generic_client.cpp index be65ea1f53..496b21ab63 100644 --- a/rclcpp/test/rclcpp/test_generic_client.cpp +++ b/rclcpp/test/rclcpp/test_generic_client.cpp @@ -105,7 +105,7 @@ TEST_F(TestGenericClient, construction_and_destruction) { ASSERT_THROW( { auto client = node->create_generic_client("test_service", "test_msgs/srv/InvalidType"); - }, std::runtime_error); + }, rclcpp::exceptions::InvalidServiceTypeError); } } From a78d0cbd33b8fe0b4db25c04f7e10017bfca6061 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Mon, 16 Sep 2024 18:42:47 +0200 Subject: [PATCH 430/455] add smart pointer macros definitions to action server and client base classes (#2631) Signed-off-by: Alberto Soragna --- rclcpp_action/include/rclcpp_action/client.hpp | 2 ++ rclcpp_action/include/rclcpp_action/server.hpp | 2 ++ 2 files changed, 4 insertions(+) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 2993f28bc3..40a326702a 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -62,6 +62,8 @@ class ClientBaseImpl; class ClientBase : public rclcpp::Waitable { public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase) + RCLCPP_ACTION_PUBLIC virtual ~ClientBase(); diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index dc64991c46..a885383614 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -72,6 +72,8 @@ enum class CancelResponse : int8_t class ServerBase : public rclcpp::Waitable { public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServerBase) + /// Enum to identify entities belonging to the action server enum class EntityType : std::size_t { From 63105cd8a6edb6906efa71c7cf56cd56d0bccdda Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 23 Sep 2024 20:19:04 +0200 Subject: [PATCH 431/455] Skip rmw zenoh content filtering tests (#2627) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../rclcpp/test_subscription_content_filter.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp index 4b8baf4a24..95a1874d4e 100644 --- a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp +++ b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp @@ -175,6 +175,11 @@ TEST_F(TestContentFilterSubscription, set_content_filter) TEST_F(TestContentFilterSubscription, content_filter_get_begin) { + std::string rmw_implementation_str = std::string(rmw_get_implementation_identifier()); + if (rmw_implementation_str == "rmw_zenoh_cpp") { + GTEST_SKIP(); + } + using namespace std::chrono_literals; { test_msgs::msg::BasicTypes msg; @@ -218,6 +223,11 @@ TEST_F(TestContentFilterSubscription, content_filter_get_begin) TEST_F(TestContentFilterSubscription, content_filter_get_later) { + std::string rmw_implementation_str = std::string(rmw_get_implementation_identifier()); + if (rmw_implementation_str == "rmw_zenoh_cpp") { + GTEST_SKIP(); + } + using namespace std::chrono_literals; { test_msgs::msg::BasicTypes msg; @@ -266,6 +276,11 @@ TEST_F(TestContentFilterSubscription, content_filter_get_later) TEST_F(TestContentFilterSubscription, content_filter_reset) { + std::string rmw_implementation_str = std::string(rmw_get_implementation_identifier()); + if (rmw_implementation_str == "rmw_zenoh_cpp") { + GTEST_SKIP(); + } + using namespace std::chrono_literals; { test_msgs::msg::BasicTypes msg; From 1f408e3b19cde3036272f62b3ddb91fbe8084dec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 24 Sep 2024 18:36:54 +0200 Subject: [PATCH 432/455] Shutdown the context before context's destructor is invoked in tests (#2633) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * zenoh: Shutdown the node properly in component tests Signed-off-by: Alejandro Hernández Cordero * Call rclcpp::shutdown when tearing down tests in rclcpp Signed-off-by: Yadunund * Call rclcpp::shutdown when tearing down tests in rclcpp_lifecycle Signed-off-by: Yadunund * Ensure context is initialized for all tests in test_publisher Signed-off-by: Yadunund * Added feedback Signed-off-by: Alejandro Hernández Cordero * make linters happy Signed-off-by: Alejandro Hernández Cordero --------- Signed-off-by: Alejandro Hernández Cordero Signed-off-by: Yadunund Co-authored-by: Yadunund --- .../executors/test_multi_threaded_executor.cpp | 5 +++++ .../node_interfaces/test_get_node_interfaces.cpp | 1 + .../rclcpp/node_interfaces/test_node_graph.cpp | 8 ++++++-- .../rclcpp/test_externally_defined_services.cpp | 5 +++++ rclcpp/test/rclcpp/test_find_weak_nodes.cpp | 5 +++++ rclcpp/test/rclcpp/test_guard_condition.cpp | 5 +++++ rclcpp/test/rclcpp/test_node_global_args.cpp | 5 +++++ rclcpp/test/rclcpp/test_parameter.cpp | 5 +++++ rclcpp/test/rclcpp/test_parameter_client.cpp | 5 +++++ .../test/rclcpp/test_parameter_event_handler.cpp | 5 +++++ rclcpp/test/rclcpp/test_publisher.cpp | 15 ++++++++++++--- rclcpp/test/rclcpp/test_qos_event.cpp | 5 +++++ rclcpp/test/rclcpp/test_subscription_options.cpp | 5 +++++ rclcpp/test/rclcpp/test_wait_set.cpp | 5 +++++ rclcpp_components/test/test_component_manager.cpp | 5 +++++ .../test/test_component_manager_api.cpp | 5 +++++ .../test/test_callback_exceptions.cpp | 5 +++++ .../test/test_register_custom_callbacks.cpp | 5 +++++ rclcpp_lifecycle/test/test_state_machine_info.cpp | 5 +++++ 19 files changed, 99 insertions(+), 5 deletions(-) diff --git a/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp index b6fd2c3fda..09dfa03f90 100644 --- a/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp @@ -32,6 +32,11 @@ class TestMultiThreadedExecutor : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; constexpr std::chrono::milliseconds PERIOD_MS = 1000ms; diff --git a/rclcpp/test/rclcpp/node_interfaces/test_get_node_interfaces.cpp b/rclcpp/test/rclcpp/node_interfaces/test_get_node_interfaces.cpp index d91570e3be..d890f74d08 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_get_node_interfaces.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_get_node_interfaces.cpp @@ -38,6 +38,7 @@ class TestGetNodeInterfaces : public ::testing::Test { node.reset(); wrapped_node.reset(); + rclcpp::shutdown(); } static rclcpp::Node::SharedPtr node; diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp index 3882d2b71c..a2f19aa82b 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -47,9 +47,13 @@ constexpr char absolute_namespace[] = "/ns"; class TestNodeGraph : public ::testing::Test { public: - void SetUp() + static void SetUpTestCase() { rclcpp::init(0, nullptr); + } + + void SetUp() + { node_ = std::make_shared(node_name, node_namespace); // This dynamic cast is not necessary for the unittests, but instead is used to ensure @@ -59,7 +63,7 @@ class TestNodeGraph : public ::testing::Test ASSERT_NE(nullptr, node_graph_); } - void TearDown() + static void TearDownTestCase() { rclcpp::shutdown(); } diff --git a/rclcpp/test/rclcpp/test_externally_defined_services.cpp b/rclcpp/test/rclcpp/test_externally_defined_services.cpp index 563f7f6ec3..21a7d11142 100644 --- a/rclcpp/test/rclcpp/test_externally_defined_services.cpp +++ b/rclcpp/test/rclcpp/test_externally_defined_services.cpp @@ -34,6 +34,11 @@ class TestExternallyDefinedServices : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; void diff --git a/rclcpp/test/rclcpp/test_find_weak_nodes.cpp b/rclcpp/test/rclcpp/test_find_weak_nodes.cpp index d402f02a11..739a247b39 100644 --- a/rclcpp/test/rclcpp/test_find_weak_nodes.cpp +++ b/rclcpp/test/rclcpp/test_find_weak_nodes.cpp @@ -27,6 +27,11 @@ class TestFindWeakNodes : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; TEST_F(TestFindWeakNodes, allocator_strategy_with_weak_nodes) { diff --git a/rclcpp/test/rclcpp/test_guard_condition.cpp b/rclcpp/test/rclcpp/test_guard_condition.cpp index ed54074a4e..c9fc138ccc 100644 --- a/rclcpp/test/rclcpp/test_guard_condition.cpp +++ b/rclcpp/test/rclcpp/test_guard_condition.cpp @@ -27,6 +27,11 @@ class TestGuardCondition : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; /* diff --git a/rclcpp/test/rclcpp/test_node_global_args.cpp b/rclcpp/test/rclcpp/test_node_global_args.cpp index febef6bb37..c3b0726d66 100644 --- a/rclcpp/test/rclcpp/test_node_global_args.cpp +++ b/rclcpp/test/rclcpp/test_node_global_args.cpp @@ -32,6 +32,11 @@ class TestNodeWithGlobalArgs : public ::testing::Test const int argc = sizeof(args) / sizeof(const char *); rclcpp::init(argc, args); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; TEST_F(TestNodeWithGlobalArgs, local_arguments_before_global) { diff --git a/rclcpp/test/rclcpp/test_parameter.cpp b/rclcpp/test/rclcpp/test_parameter.cpp index 0d798dc451..a2e0548089 100644 --- a/rclcpp/test/rclcpp/test_parameter.cpp +++ b/rclcpp/test/rclcpp/test_parameter.cpp @@ -31,6 +31,11 @@ class TestParameter : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; TEST_F(TestParameter, construct_destruct) { diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index caefa0d6c8..a709d8cc68 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -60,6 +60,11 @@ class TestParameterClient : public ::testing::Test node_with_option.reset(); } + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + // "start_type_description_service" and "use_sim_time" const uint64_t builtin_param_count = 2; rclcpp::Node::SharedPtr node; diff --git a/rclcpp/test/rclcpp/test_parameter_event_handler.cpp b/rclcpp/test/rclcpp/test_parameter_event_handler.cpp index c80ca2f6d5..0cf4a7a445 100644 --- a/rclcpp/test/rclcpp/test_parameter_event_handler.cpp +++ b/rclcpp/test/rclcpp/test_parameter_event_handler.cpp @@ -107,6 +107,11 @@ class TestNode : public ::testing::Test param_handler.reset(); } + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + rcl_interfaces::msg::ParameterEvent::SharedPtr same_node_int; rcl_interfaces::msg::ParameterEvent::SharedPtr same_node_double; rcl_interfaces::msg::ParameterEvent::SharedPtr diff_node_int; diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 4284ea48bb..e6e09bcb7b 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -38,9 +38,7 @@ class TestPublisher : public ::testing::Test public: static void SetUpTestCase() { - if (!rclcpp::ok()) { - rclcpp::init(0, nullptr); - } + rclcpp::init(0, nullptr); } protected: @@ -54,6 +52,11 @@ class TestPublisher : public ::testing::Test node.reset(); } + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + rclcpp::Node::SharedPtr node; }; @@ -81,6 +84,7 @@ class TestPublisherSub : public ::testing::Test protected: static void SetUpTestCase() { + rclcpp::init(0, nullptr); } void SetUp() @@ -94,6 +98,11 @@ class TestPublisherSub : public ::testing::Test node.reset(); } + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + rclcpp::Node::SharedPtr node; rclcpp::Node::SharedPtr subnode; }; diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index ddd6b8db1c..8b7d8f973c 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -52,6 +52,11 @@ class TestQosEvent : public ::testing::Test node.reset(); } + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + static constexpr char topic_name[] = "test_topic"; rclcpp::Node::SharedPtr node; std::function message_callback; diff --git a/rclcpp/test/rclcpp/test_subscription_options.cpp b/rclcpp/test/rclcpp/test_subscription_options.cpp index d1122333bc..72ca5cac93 100644 --- a/rclcpp/test/rclcpp/test_subscription_options.cpp +++ b/rclcpp/test/rclcpp/test_subscription_options.cpp @@ -51,6 +51,11 @@ class TestSubscriptionOptions : public ::testing::Test node.reset(); } + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + rclcpp::Node::SharedPtr node; }; diff --git a/rclcpp/test/rclcpp/test_wait_set.cpp b/rclcpp/test/rclcpp/test_wait_set.cpp index bf7ee26ed0..78e102f390 100644 --- a/rclcpp/test/rclcpp/test_wait_set.cpp +++ b/rclcpp/test/rclcpp/test_wait_set.cpp @@ -31,6 +31,11 @@ class TestWaitSet : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; /* diff --git a/rclcpp_components/test/test_component_manager.cpp b/rclcpp_components/test/test_component_manager.cpp index d4df8e7d08..486e1c6a28 100644 --- a/rclcpp_components/test/test_component_manager.cpp +++ b/rclcpp_components/test/test_component_manager.cpp @@ -28,6 +28,11 @@ class TestComponentManager : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; TEST_F(TestComponentManager, get_component_resources_invalid) diff --git a/rclcpp_components/test/test_component_manager_api.cpp b/rclcpp_components/test/test_component_manager_api.cpp index dfb9db76a2..71f32b480c 100644 --- a/rclcpp_components/test/test_component_manager_api.cpp +++ b/rclcpp_components/test/test_component_manager_api.cpp @@ -33,6 +33,11 @@ class TestComponentManager : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; // TODO(hidmic): split up tests once Node bring up/tear down races diff --git a/rclcpp_lifecycle/test/test_callback_exceptions.cpp b/rclcpp_lifecycle/test/test_callback_exceptions.cpp index 7c4117ae7b..cd6683459f 100644 --- a/rclcpp_lifecycle/test/test_callback_exceptions.cpp +++ b/rclcpp_lifecycle/test/test_callback_exceptions.cpp @@ -34,6 +34,11 @@ class TestCallbackExceptions : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; class PositiveCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode diff --git a/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp b/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp index 3c146e4207..731e89234a 100644 --- a/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp +++ b/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp @@ -34,6 +34,11 @@ class TestRegisterCustomCallbacks : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; class CustomLifecycleNode : public rclcpp_lifecycle::LifecycleNode diff --git a/rclcpp_lifecycle/test/test_state_machine_info.cpp b/rclcpp_lifecycle/test/test_state_machine_info.cpp index 452002e563..addf7d12f1 100644 --- a/rclcpp_lifecycle/test/test_state_machine_info.cpp +++ b/rclcpp_lifecycle/test/test_state_machine_info.cpp @@ -34,6 +34,11 @@ class TestStateMachineInfo : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; TEST_F(TestStateMachineInfo, available_states) { From 51dfdc3708f37b880d82550b056d985fb8a4e446 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 30 Sep 2024 10:03:51 +0200 Subject: [PATCH 433/455] Skip some tests in test_qos_event and run others with event types supported by rmw_zenoh (#2626) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero Signed-off-by: Yadunund Co-authored-by: Yadunund --- rclcpp/test/rclcpp/test_qos_event.cpp | 212 ++++++++++++++------------ 1 file changed, 116 insertions(+), 96 deletions(-) diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index 8b7d8f973c..634d4837a5 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -33,13 +33,14 @@ using namespace std::chrono_literals; class TestQosEvent : public ::testing::Test { protected: - static void SetUpTestCase() + void SetUp() { + // We initialize and shutdown the context (and hence also the rmw_context), + // for each test case to reset the ROS graph for each test case. rclcpp::init(0, nullptr); - } - void SetUp() - { + rmw_implementation_str = std::string(rmw_get_implementation_identifier()); + node = std::make_shared("test_qos_event", "/ns"); message_callback = [node = node.get()](test_msgs::msg::Empty::ConstSharedPtr /*msg*/) { @@ -50,13 +51,10 @@ class TestQosEvent : public ::testing::Test void TearDown() { node.reset(); - } - - static void TearDownTestCase() - { rclcpp::shutdown(); } + std::string rmw_implementation_str; static constexpr char topic_name[] = "test_topic"; rclcpp::Node::SharedPtr node; std::function message_callback; @@ -75,28 +73,29 @@ TEST_F(TestQosEvent, test_publisher_constructor) auto publisher = node->create_publisher( topic_name, 10, options); - // options arg with one of the callbacks - options.event_callbacks.deadline_callback = - [node = node.get()](rclcpp::QOSDeadlineOfferedInfo & event) { - RCLCPP_INFO( - node->get_logger(), - "Offered deadline missed - total %d (delta %d)", - event.total_count, event.total_count_change); - }; - publisher = node->create_publisher( - topic_name, 10, options); - - // options arg with two of the callbacks - options.event_callbacks.liveliness_callback = - [node = node.get()](rclcpp::QOSLivelinessLostInfo & event) { - RCLCPP_INFO( - node->get_logger(), - "Liveliness lost - total %d (delta %d)", - event.total_count, event.total_count_change); - }; - publisher = node->create_publisher( - topic_name, 10, options); - + if (rmw_implementation_str != "rmw_zenoh_cpp") { + // options arg with one of the callbacks + options.event_callbacks.deadline_callback = + [node = node.get()](rclcpp::QOSDeadlineOfferedInfo & event) { + RCLCPP_INFO( + node->get_logger(), + "Offered deadline missed - total %d (delta %d)", + event.total_count, event.total_count_change); + }; + publisher = node->create_publisher( + topic_name, 10, options); + + // options arg with two of the callbacks + options.event_callbacks.liveliness_callback = + [node = node.get()](rclcpp::QOSLivelinessLostInfo & event) { + RCLCPP_INFO( + node->get_logger(), + "Liveliness lost - total %d (delta %d)", + event.total_count, event.total_count_change); + }; + publisher = node->create_publisher( + topic_name, 10, options); + } // options arg with three of the callbacks options.event_callbacks.incompatible_qos_callback = [node = node.get()](rclcpp::QOSOfferedIncompatibleQoSInfo & event) { @@ -114,35 +113,38 @@ TEST_F(TestQosEvent, test_publisher_constructor) */ TEST_F(TestQosEvent, test_subscription_constructor) { + // While rmw_zenoh does not support Deadline/LivelinessChanged events, + // it does support IncompatibleQoS rclcpp::SubscriptionOptions options; // options arg with no callbacks auto subscription = node->create_subscription( topic_name, 10, message_callback, options); - // options arg with one of the callbacks - options.event_callbacks.deadline_callback = - [node = node.get()](rclcpp::QOSDeadlineRequestedInfo & event) { - RCLCPP_INFO( - node->get_logger(), - "Requested deadline missed - total %d (delta %d)", - event.total_count, event.total_count_change); - }; - subscription = node->create_subscription( - topic_name, 10, message_callback, options); - - // options arg with two of the callbacks - options.event_callbacks.liveliness_callback = - [node = node.get()](rclcpp::QOSLivelinessChangedInfo & event) { - RCLCPP_INFO( - node->get_logger(), - "Liveliness changed - alive %d (delta %d), not alive %d (delta %d)", - event.alive_count, event.alive_count_change, - event.not_alive_count, event.not_alive_count_change); - }; - subscription = node->create_subscription( - topic_name, 10, message_callback, options); - + if (rmw_implementation_str != "rmw_zenoh_cpp") { + // options arg with one of the callbacks + options.event_callbacks.deadline_callback = + [node = node.get()](rclcpp::QOSDeadlineRequestedInfo & event) { + RCLCPP_INFO( + node->get_logger(), + "Requested deadline missed - total %d (delta %d)", + event.total_count, event.total_count_change); + }; + subscription = node->create_subscription( + topic_name, 10, message_callback, options); + + // options arg with two of the callbacks + options.event_callbacks.liveliness_callback = + [node = node.get()](rclcpp::QOSLivelinessChangedInfo & event) { + RCLCPP_INFO( + node->get_logger(), + "Liveliness changed - alive %d (delta %d), not alive %d (delta %d)", + event.alive_count, event.alive_count_change, + event.not_alive_count, event.not_alive_count_change); + }; + subscription = node->create_subscription( + topic_name, 10, message_callback, options); + } // options arg with three of the callbacks options.event_callbacks.incompatible_qos_callback = [node = node.get()](rclcpp::QOSRequestedIncompatibleQoSInfo & event) { @@ -209,14 +211,19 @@ TEST_F(TestQosEvent, test_default_incompatible_qos_callbacks) const auto timeout = std::chrono::seconds(10); ex.spin_until_future_complete(log_msgs_future, timeout); - EXPECT_EQ( - "New subscription discovered on topic '/ns/test_topic', requesting incompatible QoS. " - "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", - pub_log_msg); - EXPECT_EQ( - "New publisher discovered on topic '/ns/test_topic', offering incompatible QoS. " - "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", - sub_log_msg); + if (rmw_implementation_str == "rmw_zenoh_cpp") { + EXPECT_EQ(rclcpp::QoSCompatibility::Ok, + qos_check_compatible(qos_profile_publisher, qos_profile_subscription).compatibility); + } else { + EXPECT_EQ( + "New subscription discovered on topic '/ns/test_topic', requesting incompatible QoS. " + "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", + pub_log_msg); + EXPECT_EQ( + "New publisher discovered on topic '/ns/test_topic', offering incompatible QoS. " + "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", + sub_log_msg); + } rcutils_logging_set_output_handler(original_output_handler); } @@ -228,7 +235,8 @@ TEST_F(TestQosEvent, construct_destruct_rcl_error) { // This callback requires some type of parameter, but it could be anything auto callback = [](int) {}; - const rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; + const rcl_publisher_event_type_t event_type = rmw_implementation_str == "rmw_zenoh_cpp" ? + RCL_PUBLISHER_MATCHED : RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; { // Logs error and returns @@ -265,13 +273,16 @@ TEST_F(TestQosEvent, construct_destruct_rcl_error) { } TEST_F(TestQosEvent, execute) { + if (rmw_implementation_str == "rmw_zenoh_cpp") { + GTEST_SKIP(); + } auto publisher = node->create_publisher(topic_name, 10); auto rcl_handle = publisher->get_publisher_handle(); bool handler_callback_executed = false; // This callback requires some type of parameter, but it could be anything auto callback = [&handler_callback_executed](int) {handler_callback_executed = true;}; - rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; + const rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; rclcpp::EventHandler handler( callback, rcl_publisher_event_init, rcl_handle, event_type); @@ -297,8 +308,9 @@ TEST_F(TestQosEvent, add_to_wait_set) { // This callback requires some type of parameter, but it could be anything auto callback = [](int) {}; - rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; - rclcpp::EventHandler handler( + const rcl_publisher_event_type_t event_type = rmw_implementation_str == "rmw_zenoh_cpp" ? + RCL_PUBLISHER_MATCHED : RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; + rclcpp::EventHandler handler( callback, rcl_publisher_event_init, rcl_handle, event_type); EXPECT_EQ(1u, handler.get_number_of_ready_events()); @@ -319,6 +331,10 @@ TEST_F(TestQosEvent, add_to_wait_set) { TEST_F(TestQosEvent, test_on_new_event_callback) { + if (rmw_implementation_str == "rmw_zenoh_cpp") { + GTEST_SKIP(); + } + auto offered_deadline = rclcpp::Duration(std::chrono::milliseconds(1)); auto requested_deadline = rclcpp::Duration(std::chrono::milliseconds(2)); @@ -364,18 +380,19 @@ TEST_F(TestQosEvent, test_invalid_on_new_event_callback) auto sub = node->create_subscription(topic_name, 10, message_callback); auto dummy_cb = [](size_t count_events) {(void)count_events;}; - EXPECT_NO_THROW( - pub->set_on_new_qos_event_callback(dummy_cb, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED)); + if (rmw_implementation_str != "rmw_zenoh_cpp") { + 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->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->set_on_new_qos_event_callback(dummy_cb, RCL_PUBLISHER_LIVELINESS_LOST)); - EXPECT_NO_THROW( + 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)); @@ -388,18 +405,19 @@ TEST_F(TestQosEvent, test_invalid_on_new_event_callback) EXPECT_NO_THROW( pub->clear_on_new_qos_event_callback(RCL_PUBLISHER_MATCHED)); - EXPECT_NO_THROW( - sub->set_on_new_qos_event_callback(dummy_cb, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED)); + if (rmw_implementation_str == "rmw_zenoh_cpp") { + 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->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_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)); @@ -412,24 +430,26 @@ TEST_F(TestQosEvent, test_invalid_on_new_event_callback) EXPECT_NO_THROW( sub->clear_on_new_qos_event_callback(RCL_SUBSCRIPTION_MATCHED)); - std::function invalid_cb; + if (rmw_implementation_str != "rmw_zenoh_cpp") { + 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); + rclcpp::SubscriptionOptions sub_options; + sub_options.event_callbacks.deadline_callback = [](auto) {}; + sub = node->create_subscription( + topic_name, 10, message_callback, sub_options); - EXPECT_THROW( - sub->set_on_new_qos_event_callback(invalid_cb, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED), - std::invalid_argument); + EXPECT_THROW( + 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); + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.deadline_callback = [](auto) {}; + pub = node->create_publisher(topic_name, 10, pub_options); - EXPECT_THROW( - pub->set_on_new_qos_event_callback(invalid_cb, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED), - std::invalid_argument); + EXPECT_THROW( + pub->set_on_new_qos_event_callback(invalid_cb, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED), + std::invalid_argument); + } } TEST_F(TestQosEvent, test_pub_matched_event_by_set_event_callback) From 2813045a96f721abdd0aafcdf32165180f72c0b1 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Tue, 1 Oct 2024 07:15:58 -0700 Subject: [PATCH 434/455] verify client gid uniqueness for a single service event. (#2636) Signed-off-by: Tomoya Fujita --- rclcpp/test/rclcpp/test_service_introspection.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/rclcpp/test/rclcpp/test_service_introspection.cpp b/rclcpp/test/rclcpp/test_service_introspection.cpp index 94c94a58ce..ba9d80e1f7 100644 --- a/rclcpp/test/rclcpp/test_service_introspection.cpp +++ b/rclcpp/test/rclcpp/test_service_introspection.cpp @@ -124,6 +124,20 @@ TEST_F(TestServiceIntrospection, service_introspection_nominal) ASSERT_THAT( client_gid_arr, testing::Eq(event_map[ServiceEventInfo::REQUEST_SENT]->info.client_gid)); + // TODO(@fujitatomoya): Remove this if statement once other rmw implementations support test. + // Only rmw_connextdds can pass this test requirement for now. + // See more details for https://github.com/ros2/rmw/issues/357 + if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { + ASSERT_THAT( + client_gid_arr, + testing::Eq(event_map[ServiceEventInfo::REQUEST_RECEIVED]->info.client_gid)); + ASSERT_THAT( + client_gid_arr, + testing::Eq(event_map[ServiceEventInfo::RESPONSE_SENT]->info.client_gid)); + } + ASSERT_THAT( + client_gid_arr, + testing::Eq(event_map[ServiceEventInfo::RESPONSE_RECEIVED]->info.client_gid)); ASSERT_EQ( event_map[ServiceEventInfo::REQUEST_SENT]->info.sequence_number, From 1a0092a65b2d0f97b57a31321d44827f07b0320e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 2 Oct 2024 19:59:48 +0200 Subject: [PATCH 435/455] Fixed test qos rmw zenoh (#2639) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fixed test qos rmw zenoh Signed-off-by: Alejandro Hernández Cordero * Update rclcpp/test/rclcpp/test_qos.cpp Co-authored-by: Yadu Signed-off-by: Alejandro Hernández Cordero --------- Signed-off-by: Alejandro Hernández Cordero Co-authored-by: Yadu --- rclcpp/test/rclcpp/test_qos.cpp | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/rclcpp/test/rclcpp/test_qos.cpp b/rclcpp/test/rclcpp/test_qos.cpp index b2446beed3..eecfaf97a7 100644 --- a/rclcpp/test/rclcpp/test_qos.cpp +++ b/rclcpp/test/rclcpp/test_qos.cpp @@ -17,6 +17,7 @@ #include #include "rclcpp/qos.hpp" +#include "rmw/rmw.h" #include "rmw/types.h" @@ -241,13 +242,20 @@ TEST(TestQoS, qos_check_compatible) // TODO(jacobperron): programmatically check if current RMW is one of the officially // supported DDS middlewares before running the following tests + // If the RMW implementation is rmw_zenoh_cpp, we do not expect any QoS incompatibilities. + std::string rmw_implementation_str = std::string(rmw_get_implementation_identifier()); // Incompatible { rclcpp::QoS pub_qos = rclcpp::QoS(1).best_effort(); rclcpp::QoS sub_qos = rclcpp::QoS(1).reliable(); rclcpp::QoSCheckCompatibleResult ret = rclcpp::qos_check_compatible(pub_qos, sub_qos); - EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Error); - EXPECT_FALSE(ret.reason.empty()); + if (rmw_implementation_str == "rmw_zenoh_cpp") { + EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Ok); + EXPECT_TRUE(ret.reason.empty()); + } else { + EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Error); + EXPECT_FALSE(ret.reason.empty()); + } } // Warn of possible incompatibility @@ -255,7 +263,12 @@ TEST(TestQoS, qos_check_compatible) rclcpp::SystemDefaultsQoS pub_qos; rclcpp::QoS sub_qos = rclcpp::QoS(1).reliable(); rclcpp::QoSCheckCompatibleResult ret = rclcpp::qos_check_compatible(pub_qos, sub_qos); - EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Warning); - EXPECT_FALSE(ret.reason.empty()); + if (rmw_implementation_str == "rmw_zenoh_cpp") { + EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Ok); + EXPECT_TRUE(ret.reason.empty()); + } else { + EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Warning); + EXPECT_FALSE(ret.reason.empty()); + } } } From 50a1e50133dd2cb63b0ebe25a586cb8111f83fb4 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 3 Oct 2024 10:33:15 -0400 Subject: [PATCH 436/455] Increase the timeout for the cppcheck on rclcpp_action. (#2640) The default is 300 seconds, but on Windows this is taking between 250 and 300 seconds (I'm seeing it timeout sometimes). Up the timeout to 600 seconds, which should be more than enough. Signed-off-by: Chris Lalancette --- rclcpp_action/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index f5537d4f30..0fc1065d55 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -135,3 +135,8 @@ if(BUILD_TESTING) endif() ament_package() + +if(TEST cppcheck) + # must set the property after ament_package() + set_tests_properties(cppcheck PROPERTIES TIMEOUT 600) +endif() From e133cc65f6582bc38fba2198c546822dbf710db9 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 3 Oct 2024 16:06:42 +0000 Subject: [PATCH 437/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 24 ++++++++++++++++++++++++ rclcpp_action/CHANGELOG.rst | 6 ++++++ rclcpp_components/CHANGELOG.rst | 6 ++++++ rclcpp_lifecycle/CHANGELOG.rst | 7 +++++++ 4 files changed, 43 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 185a1de7ff..0421d3896f 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,30 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fixed test qos rmw zenoh (`#2639 `_) +* verify client gid uniqueness for a single service event. (`#2636 `_) +* Skip some tests in test_qos_event and run others with event types supported by rmw_zenoh (`#2626 `_) +* Shutdown the context before context's destructor is invoked in tests (`#2633 `_) +* Skip rmw zenoh content filtering tests (`#2627 `_) +* Use InvalidServiceTypeError for unavailable service type in GenericClient (`#2629 `_) +* Implement generic service (`#2617 `_) +* fix events-executor warm-up bug and add unit-tests (`#2591 `_) +* remove unnecessary gtest-skip in test_executors (`#2600 `_) +* Correct node name in service test code (`#2615 `_) +* Minor naming fixes for ParameterValue to_string() function (`#2609 `_) +* Removed clang warnings (`#2605 `_) +* Fix a couple of issues in the documentation. (`#2608 `_) +* deprecate the static single threaded executor (`#2598 `_) +* Fix name of ParameterEventHandler class in doc (`#2604 `_) +* subscriber_statistics_collectors\_ should be protected by mutex. (`#2592 `_) +* Fix bug in timers lifecycle for events executor (`#2586 `_) +* fix rclcpp/test/rclcpp/CMakeLists.txt to check for the correct targets existance (`#2596 `_) +* Shut down context during init if logging config fails (`#2594 `_) +* Make more of the Waitable API abstract (`#2593 `_) +* Contributors: Alberto Soragna, Alejandro Hernández Cordero, Alexis Pojomovsky, Barry Xu, Chris Lalancette, Christophe Bedard, Kang, Hsin-Yi, Tomoya Fujita + 28.3.3 (2024-07-29) ------------------- * Only compile the tests once. (`#2590 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index ae8bccb987..3bdf2c6d0c 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,12 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Increase the timeout for the cppcheck on rclcpp_action. (`#2640 `_) +* add smart pointer macros definitions to action server and client base classes (`#2631 `_) +* Contributors: Alberto Soragna, Chris Lalancette + 28.3.3 (2024-07-29) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 157a9868bd..b80a20cc99 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Shutdown the context before context's destructor is invoked in tests (`#2633 `_) +* Fix typo in rclcpp_components benchmark_components (`#2602 `_) +* Contributors: Alejandro Hernández Cordero, Christophe Bedard + 28.3.3 (2024-07-29) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index bc0c86ccd0..56b2498293 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,13 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Shutdown the context before context's destructor is invoked in tests (`#2633 `_) +* LifecycleNode bugfix and add test cases (`#2562 `_) +* Properly test get_service_names_and_types_by_node in rclcpp_lifecycle (`#2599 `_) +* Contributors: Alejandro Hernández Cordero, Christophe Bedard, Tomoya Fujita + 28.3.3 (2024-07-29) ------------------- From 16290fb35208058d5f274da14a396c8d80ed13df Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 3 Oct 2024 16:06:52 +0000 Subject: [PATCH 438/455] 29.0.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 0421d3896f..9037f15241 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +29.0.0 (2024-10-03) +------------------- * Fixed test qos rmw zenoh (`#2639 `_) * verify client gid uniqueness for a single service event. (`#2636 `_) * Skip some tests in test_qos_event and run others with event types supported by rmw_zenoh (`#2626 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index b260140f43..1ea6ec3eb2 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 28.3.3 + 29.0.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 3bdf2c6d0c..b7ce2410d0 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +29.0.0 (2024-10-03) +------------------- * Increase the timeout for the cppcheck on rclcpp_action. (`#2640 `_) * add smart pointer macros definitions to action server and client base classes (`#2631 `_) * Contributors: Alberto Soragna, Chris Lalancette diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 993091c7e4..c6f6ab3b31 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 28.3.3 + 29.0.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index b80a20cc99..438e86f1b9 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +29.0.0 (2024-10-03) +------------------- * Shutdown the context before context's destructor is invoked in tests (`#2633 `_) * Fix typo in rclcpp_components benchmark_components (`#2602 `_) * Contributors: Alejandro Hernández Cordero, Christophe Bedard diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index e2d94ae31f..d798af409d 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 28.3.3 + 29.0.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 56b2498293..fce56fe8bc 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +29.0.0 (2024-10-03) +------------------- * Shutdown the context before context's destructor is invoked in tests (`#2633 `_) * LifecycleNode bugfix and add test cases (`#2562 `_) * Properly test get_service_names_and_types_by_node in rclcpp_lifecycle (`#2599 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 74b66e93f3..322265b95a 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 28.3.3 + 29.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From f8aea8cc51116ff27fe29fd170d4f23148e9f709 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Fri, 4 Oct 2024 18:12:00 +0800 Subject: [PATCH 439/455] Implement callback support of async_send_request for service generic client (#2614) Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/generic_client.hpp | 106 ++++++++++++++++++++- rclcpp/src/rclcpp/generic_client.cpp | 19 ++++ rclcpp/test/rclcpp/test_generic_client.cpp | 69 ++++++++++++++ 3 files changed, 189 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/generic_client.hpp b/rclcpp/include/rclcpp/generic_client.hpp index d6073decfc..cff0eb25a1 100644 --- a/rclcpp/include/rclcpp/generic_client.hpp +++ b/rclcpp/include/rclcpp/generic_client.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -46,6 +47,8 @@ class GenericClient : public ClientBase using Future = std::future; using SharedFuture = std::shared_future; + using CallbackType = std::function; + RCLCPP_SMART_PTR_DEFINITIONS(GenericClient) /// A convenient GenericClient::Future and request id pair. @@ -76,6 +79,20 @@ class GenericClient : public ClientBase ~FutureAndRequestId() = default; }; + /// A convenient GenericClient::SharedFuture and request id pair. + /** + * Public members: + * - future: a std::shared_future. + * - request_id: the request id associated with the future. + * + * All the other methods are equivalent to the ones std::shared_future provides. + */ + struct SharedFutureAndRequestId + : detail::FutureAndRequestId> + { + using detail::FutureAndRequestId>::FutureAndRequestId; + }; + GenericClient( rclcpp::node_interfaces::NodeBaseInterface * node_base, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, @@ -106,16 +123,16 @@ class GenericClient : public ClientBase * If the future never completes, * e.g. the call to Executor::spin_until_future_complete() times out, * GenericClient::remove_pending_request() must be called to clean the client internal state. - * Not doing so will make the `Client` instance to use more memory each time a response is not - * received from the service server. + * Not doing so will make the `GenericClient` instance to use more memory each time a response is + * not received from the service server. * * ```cpp - * auto future = client->async_send_request(my_request); + * auto future = generic_client->async_send_request(my_request); * if ( * rclcpp::FutureReturnCode::TIMEOUT == * executor->spin_until_future_complete(future, timeout)) * { - * client->remove_pending_request(future); + * generic_client->remove_pending_request(future); * // handle timeout * } else { * handle_response(future.get()); @@ -129,6 +146,45 @@ class GenericClient : public ClientBase FutureAndRequestId async_send_request(const Request request); + /// Send a request to the service server and schedule a callback in the executor. + /** + * Similar to the previous overload, but a callback will automatically be called when a response + * is received. + * + * If the callback is never called, because we never got a reply for the service server, + * remove_pending_request() has to be called with the returned request id or + * prune_pending_requests(). + * Not doing so will make the `GenericClient` instance use more memory each time a response is not + * received from the service server. + * In this case, it's convenient to setup a timer to cleanup the pending requests. + * + * \param[in] request request to be send. + * \param[in] cb callback that will be called when we get a response for this request. + * \return the request id representing the request just sent. + */ + template< + typename CallbackT, + typename std::enable_if< + rclcpp::function_traits::same_arguments< + CallbackT, + CallbackType + >::value + >::type * = nullptr + > + SharedFutureAndRequestId + async_send_request(const Request request, CallbackT && cb) + { + Promise promise; + auto shared_future = promise.get_future().share(); + auto req_id = async_send_request_impl( + request, + std::make_tuple( + CallbackType{std::forward(cb)}, + shared_future, + std::move(promise))); + return SharedFutureAndRequestId{std::move(shared_future), req_id}; + } + /// Clean all pending requests older than a time_point. /** * \param[in] time_point Requests that were sent before this point are going to be removed. @@ -149,15 +205,52 @@ class GenericClient : public ClientBase pruned_requests); } + /// Clean all pending requests. + /** + * \return number of pending requests that were removed. + */ RCLCPP_PUBLIC size_t prune_pending_requests(); + /// Cleanup a pending request. + /** + * This notifies the client that we have waited long enough for a response from the server + * to come, we have given up and we are not waiting for a response anymore. + * + * Not calling this will make the client start using more memory for each request + * that never got a reply from the server. + * + * \param[in] request_id request id returned by async_send_request(). + * \return true when a pending request was removed, false if not (e.g. a response was received). + */ RCLCPP_PUBLIC bool remove_pending_request( int64_t request_id); + /// Cleanup a pending request. + /** + * Convenient overload, same as: + * + * `GenericClient::remove_pending_request(this, future.request_id)`. + */ + RCLCPP_PUBLIC + bool + remove_pending_request( + const FutureAndRequestId & future); + + /// Cleanup a pending request. + /** + * Convenient overload, same as: + * + * `GenericClient::remove_pending_request(this, future.request_id)`. + */ + RCLCPP_PUBLIC + bool + remove_pending_request( + const SharedFutureAndRequestId & future); + /// Take the next response for this client. /** * \sa ClientBase::take_type_erased_response(). @@ -179,9 +272,12 @@ class GenericClient : public ClientBase } protected: + using CallbackTypeValueVariant = std::tuple; using CallbackInfoVariant = std::variant< - std::promise>; // Use variant for extension + std::promise, + CallbackTypeValueVariant>; // Use variant for extension + RCLCPP_PUBLIC int64_t async_send_request_impl( const Request request, diff --git a/rclcpp/src/rclcpp/generic_client.cpp b/rclcpp/src/rclcpp/generic_client.cpp index 987975d803..0ac9a86e15 100644 --- a/rclcpp/src/rclcpp/generic_client.cpp +++ b/rclcpp/src/rclcpp/generic_client.cpp @@ -109,6 +109,13 @@ GenericClient::handle_response( if (std::holds_alternative(value)) { auto & promise = std::get(value); promise.set_value(std::move(response)); + } else if (std::holds_alternative(value)) { + auto & inner = std::get(value); + const auto & callback = std::get(inner); + auto & promise = std::get(inner); + auto & future = std::get(inner); + promise.set_value(std::move(response)); + callback(std::move(future)); } } @@ -128,6 +135,18 @@ GenericClient::remove_pending_request(int64_t request_id) return pending_requests_.erase(request_id) != 0u; } +bool +GenericClient::remove_pending_request(const FutureAndRequestId & future) +{ + return this->remove_pending_request(future.request_id); +} + +bool +GenericClient::remove_pending_request(const SharedFutureAndRequestId & future) +{ + return this->remove_pending_request(future.request_id); +} + std::optional GenericClient::get_and_erase_pending_request(int64_t request_number) { diff --git a/rclcpp/test/rclcpp/test_generic_client.cpp b/rclcpp/test/rclcpp/test_generic_client.cpp index 496b21ab63..433348220b 100644 --- a/rclcpp/test/rclcpp/test_generic_client.cpp +++ b/rclcpp/test/rclcpp/test_generic_client.cpp @@ -14,6 +14,8 @@ #include +#include +#include #include #include #include @@ -28,6 +30,7 @@ #include "../mocking_utils/patch.hpp" #include "test_msgs/srv/empty.hpp" +#include "test_msgs/srv/basic_types.hpp" using namespace std::chrono_literals; @@ -228,3 +231,69 @@ TEST_F(TestGenericClientSub, construction_and_destruction) { }, rclcpp::exceptions::InvalidServiceNameError); } } + +TEST_F(TestGenericClientSub, async_send_request_with_request) { + const std::string service_name = "test_service"; + int64_t expected_change = 1111; + + auto client = node->create_generic_client(service_name, "test_msgs/srv/BasicTypes"); + + auto callback = [&expected_change]( + const test_msgs::srv::BasicTypes::Request::SharedPtr request, + test_msgs::srv::BasicTypes::Response::SharedPtr response) { + response->int64_value = request->int64_value + expected_change; + }; + + auto service = + node->create_service(service_name, std::move(callback)); + + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(5))); + ASSERT_TRUE(client->service_is_ready()); + + test_msgs::srv::BasicTypes::Request request; + request.int64_value = 12345678; + + auto future = client->async_send_request(static_cast(&request)); + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), future, std::chrono::seconds(5)); + ASSERT_TRUE(future.valid()); + auto get_untyped_response = future.get(); + auto typed_response = + static_cast(get_untyped_response.get()); + EXPECT_EQ(typed_response->int64_value, (request.int64_value + expected_change)); +} + +TEST_F(TestGenericClientSub, async_send_request_with_request_and_callback) { + const std::string service_name = "test_service"; + int64_t expected_change = 2222; + + auto client = node->create_generic_client(service_name, "test_msgs/srv/BasicTypes"); + + auto server_callback = [&expected_change]( + const test_msgs::srv::BasicTypes::Request::SharedPtr request, + test_msgs::srv::BasicTypes::Response::SharedPtr response) { + response->int64_value = request->int64_value + expected_change; + }; + + auto service = + node->create_service(service_name, std::move(server_callback)); + + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(5))); + ASSERT_TRUE(client->service_is_ready()); + + test_msgs::srv::BasicTypes::Request request; + request.int64_value = 12345678; + + auto client_callback = [&request, &expected_change]( + rclcpp::GenericClient::SharedFuture future) { + auto untyped_response = future.get(); + auto typed_response = + static_cast(untyped_response.get()); + EXPECT_EQ(typed_response->int64_value, (request.int64_value + expected_change)); + }; + + auto future = + client->async_send_request(static_cast(&request), client_callback); + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), future, std::chrono::seconds(5)); +} From c50f0c9c3d7d08e20d9af1e303f0f42b6ab3fb3f Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Wed, 9 Oct 2024 03:26:26 -0700 Subject: [PATCH 440/455] Fix error message in rclcpp_lifecycle::State::reset() (#2647) The function that may not complete successfully is `rcl_lifecycle_state_fini()`, not `rcl_lifecycle_transition_fini()`. Signed-off-by: Christophe Bedard --- rclcpp_lifecycle/src/state.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp_lifecycle/src/state.cpp b/rclcpp_lifecycle/src/state.cpp index e69078dfba..85d59d4af6 100644 --- a/rclcpp_lifecycle/src/state.cpp +++ b/rclcpp_lifecycle/src/state.cpp @@ -167,7 +167,7 @@ State::reset() noexcept if (ret != RCL_RET_OK) { RCLCPP_ERROR( rclcpp::get_logger("rclcpp_lifecycle"), - "rcl_lifecycle_transition_fini did not complete successfully, leaking memory"); + "rcl_lifecycle_state_fini did not complete successfully, leaking memory"); } } From 41d3a27437973e85cf8fbb5612beb82fb6811e98 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Wed, 9 Oct 2024 06:22:46 -0700 Subject: [PATCH 441/455] print warning if event callback is not supported instead of passing exception. (#2648) Signed-off-by: Tomoya Fujita --- rclcpp/src/rclcpp/publisher_base.cpp | 52 ++++++++++++++------ rclcpp/src/rclcpp/subscription_base.cpp | 65 ++++++++++++++++++------- 2 files changed, 83 insertions(+), 34 deletions(-) diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index 111246ed96..0dc9b01a7d 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -135,15 +135,28 @@ void PublisherBase::bind_event_callbacks( const PublisherEventCallbacks & event_callbacks, bool use_default_callbacks) { - if (event_callbacks.deadline_callback) { - this->add_event_handler( - event_callbacks.deadline_callback, - RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); + try { + if (event_callbacks.deadline_callback) { + this->add_event_handler( + event_callbacks.deadline_callback, + RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); + } + } catch (const UnsupportedEventTypeException & /*exc*/) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for deadline; not supported"); } - if (event_callbacks.liveliness_callback) { - this->add_event_handler( - event_callbacks.liveliness_callback, - RCL_PUBLISHER_LIVELINESS_LOST); + + try { + if (event_callbacks.liveliness_callback) { + this->add_event_handler( + event_callbacks.liveliness_callback, + RCL_PUBLISHER_LIVELINESS_LOST); + } + } catch (const UnsupportedEventTypeException & /*exc*/) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for liveliness; not supported"); } QOSOfferedIncompatibleQoSCallbackType incompatible_qos_cb; @@ -160,9 +173,9 @@ PublisherBase::bind_event_callbacks( this->add_event_handler(incompatible_qos_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); } } catch (const UnsupportedEventTypeException & /*exc*/) { - RCLCPP_DEBUG( + RCLCPP_WARN( rclcpp::get_logger("rclcpp"), - "Failed to add event handler for incompatible qos; wrong callback type"); + "Failed to add event handler for incompatible qos; not supported"); } IncompatibleTypeCallbackType incompatible_type_cb; @@ -179,14 +192,21 @@ PublisherBase::bind_event_callbacks( this->add_event_handler(incompatible_type_cb, RCL_PUBLISHER_INCOMPATIBLE_TYPE); } } catch (UnsupportedEventTypeException & /*exc*/) { - RCLCPP_DEBUG( + RCLCPP_WARN( rclcpp::get_logger("rclcpp"), - "Failed to add event handler for incompatible type; wrong callback type"); + "Failed to add event handler for incompatible type; not supported"); } - if (event_callbacks.matched_callback) { - this->add_event_handler( - event_callbacks.matched_callback, - RCL_PUBLISHER_MATCHED); + + try { + if (event_callbacks.matched_callback) { + this->add_event_handler( + event_callbacks.matched_callback, + RCL_PUBLISHER_MATCHED); + } + } catch (const UnsupportedEventTypeException & /*exc*/) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for matched; not supported"); } } diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index f7c238a910..7dca16a1a9 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -112,16 +112,28 @@ void SubscriptionBase::bind_event_callbacks( const SubscriptionEventCallbacks & event_callbacks, bool use_default_callbacks) { - if (event_callbacks.deadline_callback) { - this->add_event_handler( - event_callbacks.deadline_callback, - RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); + try { + if (event_callbacks.deadline_callback) { + this->add_event_handler( + event_callbacks.deadline_callback, + RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); + } + } catch (const UnsupportedEventTypeException & /*exc*/) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for deadline; not supported"); } - if (event_callbacks.liveliness_callback) { - this->add_event_handler( - event_callbacks.liveliness_callback, - RCL_SUBSCRIPTION_LIVELINESS_CHANGED); + try { + if (event_callbacks.liveliness_callback) { + this->add_event_handler( + event_callbacks.liveliness_callback, + RCL_SUBSCRIPTION_LIVELINESS_CHANGED); + } + } catch (const UnsupportedEventTypeException & /*exc*/) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for liveliness; not supported"); } QOSRequestedIncompatibleQoSCallbackType incompatible_qos_cb; @@ -139,7 +151,9 @@ SubscriptionBase::bind_event_callbacks( this->add_event_handler(incompatible_qos_cb, RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); } } catch (const UnsupportedEventTypeException & /*exc*/) { - // pass + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for incompatible qos; not supported"); } IncompatibleTypeCallbackType incompatible_type_cb; @@ -156,18 +170,33 @@ SubscriptionBase::bind_event_callbacks( this->add_event_handler(incompatible_type_cb, RCL_SUBSCRIPTION_INCOMPATIBLE_TYPE); } } catch (UnsupportedEventTypeException & /*exc*/) { - // pass + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for incompatible type; not supported"); } - if (event_callbacks.message_lost_callback) { - this->add_event_handler( - event_callbacks.message_lost_callback, - RCL_SUBSCRIPTION_MESSAGE_LOST); + try { + if (event_callbacks.message_lost_callback) { + this->add_event_handler( + event_callbacks.message_lost_callback, + RCL_SUBSCRIPTION_MESSAGE_LOST); + } + } catch (const UnsupportedEventTypeException & /*exc*/) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for message lost; not supported"); } - if (event_callbacks.matched_callback) { - this->add_event_handler( - event_callbacks.matched_callback, - RCL_SUBSCRIPTION_MATCHED); + + try { + if (event_callbacks.matched_callback) { + this->add_event_handler( + event_callbacks.matched_callback, + RCL_SUBSCRIPTION_MATCHED); + } + } catch (const UnsupportedEventTypeException & /*exc*/) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for matched; not supported"); } } From 0be8aa013e4ed94bcab88527887e85130816d256 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Mon, 14 Oct 2024 12:24:42 -0700 Subject: [PATCH 442/455] rmw_fastrtps supports service event gid uniqueness test. (#2638) Signed-off-by: Tomoya Fujita --- rclcpp/test/rclcpp/test_service_introspection.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rclcpp/test/rclcpp/test_service_introspection.cpp b/rclcpp/test/rclcpp/test_service_introspection.cpp index ba9d80e1f7..af60eec281 100644 --- a/rclcpp/test/rclcpp/test_service_introspection.cpp +++ b/rclcpp/test/rclcpp/test_service_introspection.cpp @@ -124,10 +124,10 @@ TEST_F(TestServiceIntrospection, service_introspection_nominal) ASSERT_THAT( client_gid_arr, testing::Eq(event_map[ServiceEventInfo::REQUEST_SENT]->info.client_gid)); - // TODO(@fujitatomoya): Remove this if statement once other rmw implementations support test. - // Only rmw_connextdds can pass this test requirement for now. + // TODO(@fujitatomoya): Remove this if statement once rmw implementations support test. + // rmw_cyclonedds_cpp does not pass this test requirement for now. // See more details for https://github.com/ros2/rmw/issues/357 - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { + if (std::string(rmw_get_implementation_identifier()).find("rmw_cyclonedds_cpp") != 0) { ASSERT_THAT( client_gid_arr, testing::Eq(event_map[ServiceEventInfo::REQUEST_RECEIVED]->info.client_gid)); From 4567b6ec80724bbeaa1615fd050e210f177a1370 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 17 Oct 2024 18:20:47 +0200 Subject: [PATCH 443/455] Fixed test_events_executors in zenoh (#2643) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- rclcpp/test/rclcpp/CMakeLists.txt | 2 +- .../rclcpp/executors/test_events_executor.cpp | 23 ++++++++++++------- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index f9e341087f..35fa8fdd32 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -555,7 +555,7 @@ if(TARGET test_executor_notify_waitable) target_link_libraries(test_executor_notify_waitable ${PROJECT_NAME} mimick rcpputils::rcpputils) endif() -ament_add_gtest(test_events_executor executors/test_events_executor.cpp TIMEOUT 5) +ament_add_gtest(test_events_executor executors/test_events_executor.cpp TIMEOUT 60) if(TARGET test_events_executor) target_link_libraries(test_events_executor ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() diff --git a/rclcpp/test/rclcpp/executors/test_events_executor.cpp b/rclcpp/test/rclcpp/executors/test_events_executor.cpp index 13092b7067..2c6504426e 100644 --- a/rclcpp/test/rclcpp/executors/test_events_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_events_executor.cpp @@ -479,14 +479,21 @@ TEST_F(TestEventsExecutor, test_default_incompatible_qos_callbacks) const auto timeout = std::chrono::seconds(10); ex.spin_until_future_complete(log_msgs_future, timeout); - EXPECT_EQ( - "New subscription discovered on topic '/test_topic', requesting incompatible QoS. " - "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", - pub_log_msg); - EXPECT_EQ( - "New publisher discovered on topic '/test_topic', offering incompatible QoS. " - "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", - sub_log_msg); + rclcpp::QoSCheckCompatibleResult qos_compatible = rclcpp::qos_check_compatible( + publisher->get_actual_qos(), subscription->get_actual_qos()); + if (qos_compatible.compatibility == rclcpp::QoSCompatibility::Error) { + EXPECT_EQ( + "New subscription discovered on topic '/test_topic', requesting incompatible QoS. " + "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", + pub_log_msg); + EXPECT_EQ( + "New publisher discovered on topic '/test_topic', offering incompatible QoS. " + "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", + sub_log_msg); + } else { + EXPECT_EQ("", pub_log_msg); + EXPECT_EQ("", sub_log_msg); + } rcutils_logging_set_output_handler(original_output_handler); } From bcc14755f93d8321709e7ce4fab6efa58f17f35e Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 18 Oct 2024 18:55:25 -0400 Subject: [PATCH 444/455] Fix test_intra_process_manager.cpp with rmw_zenoh_cpp (#2653) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Cleanup of test_intra_process_manager.cpp In particular, make every pub and sub have to pass in both a topic name and a QoS when they are constructing mock pubs and subs for the intra-process manager test. This just makes it easier to tell whether the pubs and subs should be matched or not. Signed-off-by: Chris Lalancette * Make sure to check QoS compatibility in the intra_process_manager tests. It turns out that the intra_process_manager will attempt to match QoS between publishers and subscriptions as they are added to the IPM. This is exactly correct, but the tests were not following the same pattern. Thus, when running these tests under Zenoh, the tests would fail because more things would match than the tests were expecting. Update the test to take the differences in QoS into account, which fixes the test under rmw_zenoh_cpp (and keeps it working for the existing DDS RMWs). Signed-off-by: Chris Lalancette * Added feedback Signed-off-by: Alejandro Hernández Cordero --------- Signed-off-by: Chris Lalancette Signed-off-by: Alejandro Hernández Cordero Co-authored-by: Alejandro Hernández Cordero --- .../rclcpp/test_intra_process_manager.cpp | 136 ++++++++++-------- 1 file changed, 79 insertions(+), 57 deletions(-) diff --git a/rclcpp/test/rclcpp/test_intra_process_manager.cpp b/rclcpp/test/rclcpp/test_intra_process_manager.cpp index 046a5228da..6a4bfe56d8 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager.cpp @@ -123,6 +123,7 @@ class IntraProcessBuffer : public IntraProcessBufferBase return result; } +private: // need to store the messages somewhere otherwise the memory address will be reused ConstMessageSharedPtr shared_msg; MessageUniquePtr unique_msg; @@ -158,9 +159,9 @@ class PublisherBase public: RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase) - explicit PublisherBase(rclcpp::QoS qos = rclcpp::QoS(10)) - : qos_profile(qos), - topic_name("topic") + explicit PublisherBase(const std::string & topic, const rclcpp::QoS & qos) + : topic_name(topic), + qos_profile(qos) {} virtual ~PublisherBase() @@ -205,10 +206,12 @@ class PublisherBase return false; } - rclcpp::QoS qos_profile; - std::string topic_name; uint64_t intra_process_publisher_id_; IntraProcessManagerWeakPtr weak_ipm_; + +private: + std::string topic_name; + rclcpp::QoS qos_profile; }; template> @@ -223,8 +226,8 @@ class Publisher : public PublisherBase RCLCPP_SMART_PTR_DEFINITIONS(Publisher) - explicit Publisher(rclcpp::QoS qos = rclcpp::QoS(10)) - : PublisherBase(qos) + explicit Publisher(const std::string & topic, const rclcpp::QoS & qos) + : PublisherBase(topic, qos) { auto allocator = std::make_shared(); message_allocator_ = std::make_shared(*allocator.get()); @@ -258,9 +261,9 @@ class SubscriptionIntraProcessBase explicit SubscriptionIntraProcessBase( rclcpp::Context::SharedPtr context, - const std::string & topic = "topic", - rclcpp::QoS qos = rclcpp::QoS(10)) - : qos_profile(qos), topic_name(topic) + const std::string & topic, + const rclcpp::QoS & qos) + : topic_name(topic), qos_profile(qos) { (void)context; } @@ -292,8 +295,8 @@ class SubscriptionIntraProcessBase size_t available_capacity() const = 0; - rclcpp::QoS qos_profile; std::string topic_name; + rclcpp::QoS qos_profile; }; template< @@ -307,8 +310,8 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase public: RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcessBuffer) - explicit SubscriptionIntraProcessBuffer(rclcpp::QoS qos) - : SubscriptionIntraProcessBase(nullptr, "topic", qos), take_shared_method(false) + explicit SubscriptionIntraProcessBuffer(const std::string & topic, const rclcpp::QoS & qos) + : SubscriptionIntraProcessBase(nullptr, topic, qos), take_shared_method(false) { buffer = std::make_unique>(); } @@ -375,8 +378,8 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< public: RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess) - explicit SubscriptionIntraProcess(rclcpp::QoS qos = rclcpp::QoS(10)) - : SubscriptionIntraProcessBuffer(qos) + explicit SubscriptionIntraProcess(const std::string & topic, const rclcpp::QoS & qos) + : SubscriptionIntraProcessBuffer(topic, qos) { } }; @@ -466,12 +469,11 @@ TEST(TestIntraProcessManager, add_pub_sub) { auto ipm = std::make_shared(); - auto p1 = std::make_shared(rclcpp::QoS(10).best_effort()); + auto p1 = std::make_shared("topic", rclcpp::QoS(10).best_effort()); - auto p2 = std::make_shared(rclcpp::QoS(10).best_effort()); - p2->topic_name = "different_topic_name"; + auto p2 = std::make_shared("different_topic_name", rclcpp::QoS(10).best_effort()); - auto s1 = std::make_shared(rclcpp::QoS(10).best_effort()); + auto s1 = std::make_shared("topic", rclcpp::QoS(10).best_effort()); auto p1_id = ipm->add_publisher(p1); auto p2_id = ipm->add_publisher(p2); @@ -480,24 +482,42 @@ TEST(TestIntraProcessManager, add_pub_sub) { bool unique_ids = p1_id != p2_id && p2_id != s1_id; ASSERT_TRUE(unique_ids); + // p1 has 1 subcription, s1 size_t p1_subs = ipm->get_subscription_count(p1_id); + // p2 has 0 subscriptions size_t p2_subs = ipm->get_subscription_count(p2_id); + // Non-existent publisher_id has 0 subscriptions size_t non_existing_pub_subs = ipm->get_subscription_count(42); ASSERT_EQ(1u, p1_subs); ASSERT_EQ(0u, p2_subs); ASSERT_EQ(0u, non_existing_pub_subs); - auto p3 = std::make_shared(rclcpp::QoS(10).reliable()); + auto p3 = std::make_shared("topic", rclcpp::QoS(10).reliable()); - auto s2 = std::make_shared(rclcpp::QoS(10).reliable()); + auto s2 = std::make_shared("topic", rclcpp::QoS(10).reliable()); + // s2 may be able to communicate with p1 depending on the RMW auto s2_id = ipm->template add_subscription(s2); + // p3 can definitely communicate with s2, may be able to communicate with s1 depending on the RMW auto p3_id = ipm->add_publisher(p3); + // p1 definitely matches subscription s1, since the topic name and QoS match exactly. + // If the RMW can match best-effort publishers to reliable subscriptions (like Zenoh can), + // then p1 will also match s2. p1_subs = ipm->get_subscription_count(p1_id); + // No subscriptions with a topic name of "different_topic_name" were added. p2_subs = ipm->get_subscription_count(p2_id); + // On all current RMWs (DDS and Zenoh), a reliable publisher like p3 can communicate with both + // reliable and best-effort subscriptions (s1 and s2). size_t p3_subs = ipm->get_subscription_count(p3_id); - ASSERT_EQ(1u, p1_subs); + + rclcpp::QoSCheckCompatibleResult qos_compatible = + rclcpp::qos_check_compatible(p1->get_actual_qos(), s2->get_actual_qos()); + if (qos_compatible.compatibility == rclcpp::QoSCompatibility::Error) { + ASSERT_EQ(1u, p1_subs); + } else { + ASSERT_EQ(2u, p1_subs); + } ASSERT_EQ(0u, p2_subs); ASSERT_EQ(2u, p3_subs); @@ -528,11 +548,11 @@ TEST(TestIntraProcessManager, single_subscription) { auto ipm = std::make_shared(); - auto p1 = std::make_shared(); + auto p1 = std::make_shared("topic", rclcpp::QoS(10)); auto p1_id = ipm->add_publisher(p1); p1->set_intra_process_manager(p1_id, ipm); - auto s1 = std::make_shared(); + auto s1 = std::make_shared("topic", rclcpp::QoS(10)); s1->take_shared_method = false; auto s1_id = ipm->template add_subscription(s1); @@ -543,7 +563,7 @@ TEST(TestIntraProcessManager, single_subscription) { ASSERT_EQ(original_message_pointer, received_message_pointer_1); ipm->remove_subscription(s1_id); - auto s2 = std::make_shared(); + auto s2 = std::make_shared("topic", rclcpp::QoS(10)); s2->take_shared_method = true; auto s2_id = ipm->template add_subscription(s2); (void)s2_id; @@ -582,15 +602,15 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { auto ipm = std::make_shared(); - auto p1 = std::make_shared(); + auto p1 = std::make_shared("topic", rclcpp::QoS(10)); auto p1_id = ipm->add_publisher(p1); p1->set_intra_process_manager(p1_id, ipm); - auto s1 = std::make_shared(); + auto s1 = std::make_shared("topic", rclcpp::QoS(10)); s1->take_shared_method = false; auto s1_id = ipm->template add_subscription(s1); - auto s2 = std::make_shared(); + auto s2 = std::make_shared("topic", rclcpp::QoS(10)); s2->take_shared_method = false; auto s2_id = ipm->template add_subscription(s2); @@ -606,11 +626,11 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { ipm->remove_subscription(s1_id); ipm->remove_subscription(s2_id); - auto s3 = std::make_shared(); + auto s3 = std::make_shared("topic", rclcpp::QoS(10)); s3->take_shared_method = true; auto s3_id = ipm->template add_subscription(s3); - auto s4 = std::make_shared(); + auto s4 = std::make_shared("topic", rclcpp::QoS(10)); s4->take_shared_method = true; auto s4_id = ipm->template add_subscription(s4); @@ -625,11 +645,11 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { ipm->remove_subscription(s3_id); ipm->remove_subscription(s4_id); - auto s5 = std::make_shared(); + auto s5 = std::make_shared("topic", rclcpp::QoS(10)); s5->take_shared_method = false; auto s5_id = ipm->template add_subscription(s5); - auto s6 = std::make_shared(); + auto s6 = std::make_shared("topic", rclcpp::QoS(10)); s6->take_shared_method = false; auto s6_id = ipm->template add_subscription(s6); @@ -645,12 +665,12 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { ipm->remove_subscription(s5_id); ipm->remove_subscription(s6_id); - auto s7 = std::make_shared(); + auto s7 = std::make_shared("topic", rclcpp::QoS(10)); s7->take_shared_method = true; auto s7_id = ipm->template add_subscription(s7); (void)s7_id; - auto s8 = std::make_shared(); + auto s8 = std::make_shared("topic", rclcpp::QoS(10)); s8->take_shared_method = true; auto s8_id = ipm->template add_subscription(s8); (void)s8_id; @@ -688,15 +708,15 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { auto ipm = std::make_shared(); - auto p1 = std::make_shared(); + auto p1 = std::make_shared("topic", rclcpp::QoS(10)); auto p1_id = ipm->add_publisher(p1); p1->set_intra_process_manager(p1_id, ipm); - auto s1 = std::make_shared(); + auto s1 = std::make_shared("topic", rclcpp::QoS(10)); s1->take_shared_method = true; auto s1_id = ipm->template add_subscription(s1); - auto s2 = std::make_shared(); + auto s2 = std::make_shared("topic", rclcpp::QoS(10)); s2->take_shared_method = false; auto s2_id = ipm->template add_subscription(s2); @@ -711,15 +731,15 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { ipm->remove_subscription(s1_id); ipm->remove_subscription(s2_id); - auto s3 = std::make_shared(); + auto s3 = std::make_shared("topic", rclcpp::QoS(10)); s3->take_shared_method = false; auto s3_id = ipm->template add_subscription(s3); - auto s4 = std::make_shared(); + auto s4 = std::make_shared("topic", rclcpp::QoS(10)); s4->take_shared_method = false; auto s4_id = ipm->template add_subscription(s4); - auto s5 = std::make_shared(); + auto s5 = std::make_shared("topic", rclcpp::QoS(10)); s5->take_shared_method = true; auto s5_id = ipm->template add_subscription(s5); @@ -743,19 +763,19 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { ipm->remove_subscription(s4_id); ipm->remove_subscription(s5_id); - auto s6 = std::make_shared(); + auto s6 = std::make_shared("topic", rclcpp::QoS(10)); s6->take_shared_method = true; auto s6_id = ipm->template add_subscription(s6); - auto s7 = std::make_shared(); + auto s7 = std::make_shared("topic", rclcpp::QoS(10)); s7->take_shared_method = true; auto s7_id = ipm->template add_subscription(s7); - auto s8 = std::make_shared(); + auto s8 = std::make_shared("topic", rclcpp::QoS(10)); s8->take_shared_method = false; auto s8_id = ipm->template add_subscription(s8); - auto s9 = std::make_shared(); + auto s9 = std::make_shared("topic", rclcpp::QoS(10)); s9->take_shared_method = false; auto s9_id = ipm->template add_subscription(s9); @@ -781,12 +801,12 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { ipm->remove_subscription(s8_id); ipm->remove_subscription(s9_id); - auto s10 = std::make_shared(); + auto s10 = std::make_shared("topic", rclcpp::QoS(10)); s10->take_shared_method = false; auto s10_id = ipm->template add_subscription(s10); (void)s10_id; - auto s11 = std::make_shared(); + auto s11 = std::make_shared("topic", rclcpp::QoS(10)); s11->take_shared_method = true; auto s11_id = ipm->template add_subscription(s11); (void)s11_id; @@ -831,10 +851,12 @@ TEST(TestIntraProcessManager, lowest_available_capacity) { auto ipm = std::make_shared(); - auto p1 = std::make_shared(rclcpp::QoS(history_depth).best_effort()); + auto p1 = std::make_shared("topic", rclcpp::QoS(history_depth).best_effort()); - auto s1 = std::make_shared(rclcpp::QoS(history_depth).best_effort()); - auto s2 = std::make_shared(rclcpp::QoS(history_depth).best_effort()); + auto s1 = + std::make_shared("topic", rclcpp::QoS(history_depth).best_effort()); + auto s2 = + std::make_shared("topic", rclcpp::QoS(history_depth).best_effort()); auto p1_id = ipm->add_publisher(p1); p1->set_intra_process_manager(p1_id, ipm); @@ -902,7 +924,7 @@ TEST(TestIntraProcessManager, transient_local_invalid_buffer) { auto ipm = std::make_shared(); - auto p1 = std::make_shared(rclcpp::QoS(history_depth).transient_local()); + auto p1 = std::make_shared("topic", rclcpp::QoS(history_depth).transient_local()); ASSERT_THROW( { @@ -926,14 +948,14 @@ TEST(TestIntraProcessManager, transient_local) { auto ipm = std::make_shared(); - auto p1 = std::make_shared(rclcpp::QoS(history_depth).transient_local()); + auto p1 = std::make_shared("topic", rclcpp::QoS(history_depth).transient_local()); - auto s1 = - std::make_shared(rclcpp::QoS(history_depth).transient_local()); - auto s2 = - std::make_shared(rclcpp::QoS(history_depth).transient_local()); - auto s3 = - std::make_shared(rclcpp::QoS(history_depth).transient_local()); + auto s1 = std::make_shared( + "topic", rclcpp::QoS(history_depth).transient_local()); + auto s2 = std::make_shared( + "topic", rclcpp::QoS(history_depth).transient_local()); + auto s3 = std::make_shared( + "topic", rclcpp::QoS(history_depth).transient_local()); s1->take_shared_method = false; s2->take_shared_method = true; From f12e3c69dcc5823b474d43ae1d7bf3c596154cbd Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Sat, 19 Oct 2024 14:21:20 -0700 Subject: [PATCH 445/455] set QoS History KEEP_ALL explicitly for statistics publisher. (#2650) * set QoS History KEEP_ALL explicitly for statistics publisher. Signed-off-by: Tomoya Fujita * test_subscription_options adjustment. Signed-off-by: Tomoya Fujita --------- Signed-off-by: Tomoya Fujita --- rclcpp/include/rclcpp/subscription_options.hpp | 3 ++- rclcpp/test/rclcpp/test_subscription_options.cpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 71f90fe15d..0dd738ee60 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -80,7 +80,8 @@ struct SubscriptionOptionsBase // An optional QoS which can provide topic_statistics with a stable QoS separate from // the subscription's current QoS settings which could be unstable. - rclcpp::QoS qos = SystemDefaultsQoS(); + // Explicitly set the enough depth to avoid missing the statistics messages. + rclcpp::QoS qos = SystemDefaultsQoS().keep_last(10); }; TopicStatisticsOptions topic_stats_options; diff --git a/rclcpp/test/rclcpp/test_subscription_options.cpp b/rclcpp/test/rclcpp/test_subscription_options.cpp index 72ca5cac93..1bcde41130 100644 --- a/rclcpp/test/rclcpp/test_subscription_options.cpp +++ b/rclcpp/test/rclcpp/test_subscription_options.cpp @@ -65,7 +65,7 @@ TEST_F(TestSubscriptionOptions, topic_statistics_options_default_and_set) { EXPECT_EQ(options.topic_stats_options.state, rclcpp::TopicStatisticsState::NodeDefault); EXPECT_EQ(options.topic_stats_options.publish_topic, defaultPublishTopic); EXPECT_EQ(options.topic_stats_options.publish_period, 1s); - EXPECT_EQ(options.topic_stats_options.qos, rclcpp::SystemDefaultsQoS()); + EXPECT_EQ(options.topic_stats_options.qos, rclcpp::SystemDefaultsQoS().keep_last(10)); options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; options.topic_stats_options.publish_topic = "topic_statistics"; From 9b654942f99f17850e0e95480958abdbb508bc00 Mon Sep 17 00:00:00 2001 From: Romain DESILLE Date: Wed, 30 Oct 2024 12:45:19 +0100 Subject: [PATCH 446/455] Fix NodeOptions assignment operator (#2656) * Fix NodeOptions assignment operator Also copy the enable_logger_service_ member during the assignment operation * Add more checks for NodeOptions copy test * Set non default values by avoiding the copy-assignement Signed-off-by: Romain DESILLE Co-authored-by: Christophe Bedard --- rclcpp/src/rclcpp/node_options.cpp | 1 + rclcpp/test/rclcpp/test_node_options.cpp | 51 ++++++++++++++++++++++++ 2 files changed, 52 insertions(+) diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index ada87ce5fe..ca58154eb4 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -80,6 +80,7 @@ NodeOptions::operator=(const NodeOptions & other) this->clock_type_ = other.clock_type_; this->clock_qos_ = other.clock_qos_; this->use_clock_thread_ = other.use_clock_thread_; + this->enable_logger_service_ = other.enable_logger_service_; this->parameter_event_qos_ = other.parameter_event_qos_; this->rosout_qos_ = other.rosout_qos_; this->parameter_event_publisher_options_ = other.parameter_event_publisher_options_; diff --git a/rclcpp/test/rclcpp/test_node_options.cpp b/rclcpp/test/rclcpp/test_node_options.cpp index 49af93aa8a..7abc36f38e 100644 --- a/rclcpp/test/rclcpp/test_node_options.cpp +++ b/rclcpp/test/rclcpp/test_node_options.cpp @@ -200,6 +200,57 @@ TEST(TestNodeOptions, copy) { rcl_arguments_get_count_unparsed(&other_rcl_options->arguments), rcl_arguments_get_count_unparsed(&rcl_options->arguments)); } + + { + // The following scope test is missing: + // "arguments" because it is already tested in the above scopes + // "parameter_event_publisher_options" because it can not be directly compared with EXPECT_EQ + // "allocator" because it can not be directly compared with EXPECT_EQ + + // We separate attribute modification from variable initialisation (copy assignment operator) + // to be sure the "non_default_options"'s properties are correctly set before testing the + // assignment operator. + auto non_default_options = rclcpp::NodeOptions(); + non_default_options + .parameter_overrides({rclcpp::Parameter("foo", 0), rclcpp::Parameter("bar", "1")}) + .use_global_arguments(false) + .enable_rosout(false) + .use_intra_process_comms(true) + .enable_topic_statistics(true) + .start_parameter_services(false) + .enable_logger_service(true) + .start_parameter_event_publisher(false) + .clock_type(RCL_SYSTEM_TIME) + .clock_qos(rclcpp::SensorDataQoS()) + .use_clock_thread(false) + .parameter_event_qos(rclcpp::ClockQoS()) + .rosout_qos(rclcpp::ParameterEventsQoS()) + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true); + + auto copied_options = non_default_options; + EXPECT_EQ(non_default_options.parameter_overrides(), copied_options.parameter_overrides()); + EXPECT_EQ(non_default_options.use_global_arguments(), copied_options.use_global_arguments()); + EXPECT_EQ(non_default_options.enable_rosout(), copied_options.enable_rosout()); + EXPECT_EQ(non_default_options.use_intra_process_comms(), + copied_options.use_intra_process_comms()); + EXPECT_EQ(non_default_options.enable_topic_statistics(), + copied_options.enable_topic_statistics()); + EXPECT_EQ(non_default_options.start_parameter_services(), + copied_options.start_parameter_services()); + EXPECT_EQ(non_default_options.enable_logger_service(), copied_options.enable_logger_service()); + EXPECT_EQ(non_default_options.start_parameter_event_publisher(), + copied_options.start_parameter_event_publisher()); + EXPECT_EQ(non_default_options.clock_type(), copied_options.clock_type()); + EXPECT_EQ(non_default_options.clock_qos(), copied_options.clock_qos()); + EXPECT_EQ(non_default_options.use_clock_thread(), copied_options.use_clock_thread()); + EXPECT_EQ(non_default_options.parameter_event_qos(), copied_options.parameter_event_qos()); + EXPECT_EQ(non_default_options.rosout_qos(), copied_options.rosout_qos()); + EXPECT_EQ(non_default_options.allow_undeclared_parameters(), + copied_options.allow_undeclared_parameters()); + EXPECT_EQ(non_default_options.automatically_declare_parameters_from_overrides(), + copied_options.automatically_declare_parameters_from_overrides()); + } } TEST(TestNodeOptions, append_parameter_override) { From 37e36880260f10c85fb20809accec7cc77a3b1dd Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Tue, 5 Nov 2024 21:07:02 +0800 Subject: [PATCH 447/455] Correct the incorrect comments in generic_client.hpp (#2662) Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/generic_client.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/generic_client.hpp b/rclcpp/include/rclcpp/generic_client.hpp index cff0eb25a1..1b853e0b6e 100644 --- a/rclcpp/include/rclcpp/generic_client.hpp +++ b/rclcpp/include/rclcpp/generic_client.hpp @@ -36,8 +36,8 @@ namespace rclcpp class GenericClient : public ClientBase { public: - using Request = void *; // Serialized data pointer of request message - using Response = void *; // Serialized data pointer of response message + using Request = void *; // Deserialized data pointer of request message + using Response = void *; // Deserialized data pointer of response message using SharedResponse = std::shared_ptr; From 1e6767ac13670ec9d2dffeb9b403b8a69d58e172 Mon Sep 17 00:00:00 2001 From: YR <73534920+anacondrai@users.noreply.github.com> Date: Sun, 10 Nov 2024 16:54:41 +0100 Subject: [PATCH 448/455] Fix documentation typo in server_goal_handle.hpp (#2669) Signed-off-by: Yurii Rovinskyi --- rclcpp_action/include/rclcpp_action/server_goal_handle.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index 7d17819189..07873f4c71 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -43,7 +43,7 @@ class ServerGoalHandleBase { public: /// Indicate if client has requested this goal be cancelled. - /// \return true if a cancelation request has been accepted for this goal. + /// \return true if a cancellation request has been accepted for this goal. RCLCPP_ACTION_PUBLIC bool is_canceling() const; From 88ebea94e9c79499c34f825cf6565827c5ba958e Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Mon, 11 Nov 2024 13:39:38 -0800 Subject: [PATCH 449/455] Make sure callback_end tracepoint is triggered in AnyServiceCallback (#2670) Signed-off-by: Christophe Bedard --- rclcpp/include/rclcpp/any_service_callback.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp/include/rclcpp/any_service_callback.hpp index 918d8e5a29..5d6f3ee7b7 100644 --- a/rclcpp/include/rclcpp/any_service_callback.hpp +++ b/rclcpp/include/rclcpp/any_service_callback.hpp @@ -165,11 +165,13 @@ class AnyServiceCallback if (std::holds_alternative(callback_)) { const auto & cb = std::get(callback_); cb(request_header, std::move(request)); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); return nullptr; } if (std::holds_alternative(callback_)) { const auto & cb = std::get(callback_); cb(service_handle, request_header, std::move(request)); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); return nullptr; } // auto response = allocate_shared(); From e854bb29cdb0a3fe8e53c4d683b848124c249cdf Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Tue, 19 Nov 2024 04:14:45 -0800 Subject: [PATCH 450/455] a couple of typo fixes in doc section for LoanedMessage. (#2676) * rephrase doc section of LoanedMessage Class. Signed-off-by: Tomoya Fujita Co-authored-by: Chris Lalancette Co-authored-by: Christophe Bedard --- rclcpp/include/rclcpp/loaned_message.hpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/loaned_message.hpp b/rclcpp/include/rclcpp/loaned_message.hpp index 282fb6c801..3fd86f0753 100644 --- a/rclcpp/include/rclcpp/loaned_message.hpp +++ b/rclcpp/include/rclcpp/loaned_message.hpp @@ -40,12 +40,11 @@ class LoanedMessage * The constructor of this class allocates memory for a given message type * and associates this with a given publisher. * - * Given the publisher instance, a case differentiation is being performaned - * which decides whether the underlying middleware is able to allocate the appropriate - * memory for this message type or not. - * In the case that the middleware can not loan messages, the passed in allocator instance - * is being used to allocate the message within the scope of this class. - * Otherwise, the allocator is being ignored and the allocation is solely performaned + * The underlying middleware is queried to determine whether it is able to allocate the + * appropriate memory for this message type or not. + * In the case that the middleware cannot loan messages, the passed in allocator instance + * is used to allocate the message within the scope of this class. + * Otherwise, the allocator is ignored and the allocation is solely performed * in the underlying middleware with its appropriate allocation strategy. * The need for this arises as the user code can be written explicitly targeting a middleware * capable of loaning messages. @@ -53,7 +52,7 @@ class LoanedMessage * a middleware which doesn't support message loaning in which case the allocator will be used. * * \param[in] pub rclcpp::Publisher instance to which the memory belongs - * \param[in] allocator Allocator instance in case middleware can not allocate messages + * \param[in] allocator Allocator instance in case middleware cannot allocate messages * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. */ LoanedMessage( From 322b5f5d794ffc942602b8efdd82610eac8a2223 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 20 Nov 2024 15:53:48 +0000 Subject: [PATCH 451/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 14 ++++++++++++++ rclcpp_action/CHANGELOG.rst | 5 +++++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 5 +++++ 4 files changed, 27 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 9037f15241..89f4c5a92e 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* a couple of typo fixes in doc section for LoanedMessage. (`#2676 `_) +* Make sure callback_end tracepoint is triggered in AnyServiceCallback (`#2670 `_) +* Correct the incorrect comments in generic_client.hpp (`#2662 `_) +* Fix NodeOptions assignment operator (`#2656 `_) +* set QoS History KEEP_ALL explicitly for statistics publisher. (`#2650 `_) +* Fix test_intra_process_manager.cpp with rmw_zenoh_cpp (`#2653 `_) +* Fixed test_events_executors in zenoh (`#2643 `_) +* rmw_fastrtps supports service event gid uniqueness test. (`#2638 `_) +* print warning if event callback is not supported instead of passing exception. (`#2648 `_) +* Implement callback support of async_send_request for service generic client (`#2614 `_) +* Contributors: Alejandro Hernández Cordero, Barry Xu, Chris Lalancette, Christophe Bedard, Romain DESILLE, Tomoya Fujita + 29.0.0 (2024-10-03) ------------------- * Fixed test qos rmw zenoh (`#2639 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index b7ce2410d0..f36b16b65e 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix documentation typo in server_goal_handle.hpp (`#2669 `_) +* Contributors: YR + 29.0.0 (2024-10-03) ------------------- * Increase the timeout for the cppcheck on rclcpp_action. (`#2640 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 438e86f1b9..fa0da73787 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 29.0.0 (2024-10-03) ------------------- * Shutdown the context before context's destructor is invoked in tests (`#2633 `_) diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index fce56fe8bc..dda6141795 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix error message in rclcpp_lifecycle::State::reset() (`#2647 `_) +* Contributors: Christophe Bedard + 29.0.0 (2024-10-03) ------------------- * Shutdown the context before context's destructor is invoked in tests (`#2633 `_) From 64dd644469d7fb6f2ed64e609e5bd48b6b23f4ce Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 20 Nov 2024 15:53:57 +0000 Subject: [PATCH 452/455] 29.1.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 89f4c5a92e..ed5c4202d3 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +29.1.0 (2024-11-20) +------------------- * a couple of typo fixes in doc section for LoanedMessage. (`#2676 `_) * Make sure callback_end tracepoint is triggered in AnyServiceCallback (`#2670 `_) * Correct the incorrect comments in generic_client.hpp (`#2662 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 1ea6ec3eb2..1454dc5b6a 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 29.0.0 + 29.1.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index f36b16b65e..b174f11a12 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +29.1.0 (2024-11-20) +------------------- * Fix documentation typo in server_goal_handle.hpp (`#2669 `_) * Contributors: YR diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index c6f6ab3b31..33c97415a0 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 29.0.0 + 29.1.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index fa0da73787..68042ce904 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +29.1.0 (2024-11-20) +------------------- 29.0.0 (2024-10-03) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index d798af409d..8566a743ea 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 29.0.0 + 29.1.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index dda6141795..6288b0d6e5 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +29.1.0 (2024-11-20) +------------------- * Fix error message in rclcpp_lifecycle::State::reset() (`#2647 `_) * Contributors: Christophe Bedard diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 322265b95a..4b93d45253 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 29.0.0 + 29.1.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 7e9ff5f4c774e6c0b28268f2d2b1b03fc52383a2 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Thu, 21 Nov 2024 13:50:59 -0800 Subject: [PATCH 453/455] accept custom allocator for LoanedMessage. (#2672) * accept custom allocator for LoanedMessage. Signed-off-by: Tomoya Fujita * move message allocator using directives to public. Signed-off-by: Tomoya Fujita --------- Signed-off-by: Tomoya Fujita --- rclcpp/include/rclcpp/loaned_message.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/loaned_message.hpp b/rclcpp/include/rclcpp/loaned_message.hpp index 3fd86f0753..34a1633ef4 100644 --- a/rclcpp/include/rclcpp/loaned_message.hpp +++ b/rclcpp/include/rclcpp/loaned_message.hpp @@ -31,10 +31,10 @@ namespace rclcpp template> class LoanedMessage { +public: using MessageAllocatorTraits = rclcpp::allocator::AllocRebind; using MessageAllocator = typename MessageAllocatorTraits::allocator_type; -public: /// Constructor of the LoanedMessage class. /** * The constructor of this class allocates memory for a given message type @@ -57,7 +57,7 @@ class LoanedMessage */ LoanedMessage( const rclcpp::PublisherBase & pub, - std::allocator allocator) + MessageAllocator allocator) : pub_(pub), message_(nullptr), message_allocator_(std::move(allocator)) From 226044022ae9cad2093f65d4ff2d491b3d0f7cfd Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2024 14:38:05 +0000 Subject: [PATCH 454/455] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 5 +++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 3 +++ 4 files changed, 14 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index ed5c4202d3..cee69cb188 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* accept custom allocator for LoanedMessage. (`#2672 `_) +* Contributors: Tomoya Fujita + 29.1.0 (2024-11-20) ------------------- * a couple of typo fixes in doc section for LoanedMessage. (`#2676 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index b174f11a12..969f1346d3 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 29.1.0 (2024-11-20) ------------------- * Fix documentation typo in server_goal_handle.hpp (`#2669 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 68042ce904..12d4970ee0 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 29.1.0 (2024-11-20) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 6288b0d6e5..b6a7a6cbfc 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 29.1.0 (2024-11-20) ------------------- * Fix error message in rclcpp_lifecycle::State::reset() (`#2647 `_) From 83c282a161318ea2fec39fc474ea5ef2f0b887d3 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 25 Nov 2024 14:38:16 +0000 Subject: [PATCH 455/455] 29.2.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index cee69cb188..07f7d2b4d2 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +29.2.0 (2024-11-25) +------------------- * accept custom allocator for LoanedMessage. (`#2672 `_) * Contributors: Tomoya Fujita diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 1454dc5b6a..abbd14f8dc 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 29.1.0 + 29.2.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 969f1346d3..a8a7e3899d 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +29.2.0 (2024-11-25) +------------------- 29.1.0 (2024-11-20) ------------------- diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 33c97415a0..6fb522f1fe 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 29.1.0 + 29.2.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 12d4970ee0..cc55217333 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +29.2.0 (2024-11-25) +------------------- 29.1.0 (2024-11-20) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 8566a743ea..ff9c20a182 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 29.1.0 + 29.2.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index b6a7a6cbfc..0cf68a5891 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +29.2.0 (2024-11-25) +------------------- 29.1.0 (2024-11-20) ------------------- diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 4b93d45253..065992fdc8 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 29.1.0 + 29.2.0 Package containing a prototype for lifecycle implementation Ivan Paunovic