From 43db06dad7639da965d5447961e4da0dbf6f59ec Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Wed, 9 Feb 2022 14:45:39 +0000 Subject: [PATCH 01/20] 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 02/20] 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 03/20] 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 04/20] 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 05/20] 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 06/20] 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 07/20] 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 558e9bce7961ae6cde4097381f01ce0708be1c7f Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Wed, 23 Feb 2022 18:06:50 +0000 Subject: [PATCH 08/20] Support action expire timers with callbacks approach Signed-off-by: Mauro Passerino --- .../include/rclcpp_action/create_server.hpp | 3 ++ .../include/rclcpp_action/server.hpp | 7 ++- rclcpp_action/src/server.cpp | 44 +++++++++++++++++-- 3 files changed, 50 insertions(+), 4 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index 3aa0e4dea7..2f525da51d 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -60,6 +60,7 @@ create_server( rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface, + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_interface, const std::string & name, typename Server::GoalCallback handle_goal, typename Server::CancelCallback handle_cancel, @@ -100,6 +101,7 @@ create_server( node_base_interface, node_clock_interface, node_logging_interface, + node_timers_interface, name, options, handle_goal, @@ -143,6 +145,7 @@ create_server( node->get_node_clock_interface(), node->get_node_logging_interface(), node->get_node_waitables_interface(), + node->get_node_timers_interface(), name, handle_goal, handle_cancel, diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 18535b13d4..95a138fa82 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -77,6 +77,7 @@ class ServerBase : public rclcpp::Waitable GoalService, ResultService, CancelService, + ExpireTimer, }; RCLCPP_ACTION_PUBLIC @@ -183,6 +184,7 @@ class ServerBase : public rclcpp::Waitable rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers, const std::string & name, const rosidl_action_type_support_t * type_support, const rcl_action_server_options_t & options); @@ -312,9 +314,10 @@ class ServerBase : public rclcpp::Waitable protected: // Mutex to protect the callbacks storage. - std::recursive_mutex listener_mutex_; + std::recursive_mutex callbacks_mutex_; // Storage for std::function callbacks to keep them in scope std::unordered_map> entity_type_to_on_ready_callback_; + std::function timer_expired_callback_{nullptr}; /// Set a callback to be called when the specified entity is ready RCLCPP_ACTION_PUBLIC @@ -381,6 +384,7 @@ class Server : public ServerBase, public std::enable_shared_from_this(), options), diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 7af05f1e5e..352d6b62dc 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -27,6 +27,7 @@ #include "action_msgs/msg/goal_status_array.hpp" #include "action_msgs/srv/cancel_goal.hpp" +#include "rclcpp/create_timer.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp_action/server.hpp" @@ -54,6 +55,9 @@ class ServerBaseImpl // Do not declare this before clock_ as this depends on clock_(see #1526) std::shared_ptr action_server_; + // Timer for expiring goals + rclcpp::TimerBase::SharedPtr timer_; + size_t num_subscriptions_ = 0; size_t num_timers_ = 0; size_t num_clients_ = 0; @@ -83,6 +87,7 @@ ServerBase::ServerBase( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers, const std::string & name, const rosidl_action_type_support_t * type_support, const rcl_action_server_options_t & options @@ -110,8 +115,26 @@ ServerBase::ServerBase( rcl_node_t * rcl_node = node_base->get_rcl_node_handle(); rcl_clock_t * rcl_clock = pimpl_->clock_->get_clock_handle(); + pimpl_->timer_ = create_timer( + node_base, + node_timers, + pimpl_->clock_, + rclcpp::Duration(std::chrono::nanoseconds(options.result_timeout.nanoseconds)), + [this]() { + std::lock_guard lock(callbacks_mutex_); + if (timer_expired_callback_) { + timer_expired_callback_(1); + } + }); + rcl_ret_t ret = rcl_action_server_init( - pimpl_->action_server_.get(), rcl_node, rcl_clock, type_support, name.c_str(), &options); + pimpl_->action_server_.get(), + rcl_node, + rcl_clock, + type_support, + name.c_str(), + &options, + pimpl_->timer_->get_timer_handle().get()); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -235,6 +258,12 @@ ServerBase::take_data_by_entity_id(int id) break; } + case EntityType::ExpireTimer: + { + pimpl_->goal_expired_ = true; + break; + } + default: throw std::runtime_error("ServerBase::take_data. Unknown entity type."); break; @@ -586,6 +615,8 @@ ServerBase::execute_check_expired_goals() pimpl_->goal_handles_.erase(uuid); } } + + pimpl_->goal_expired_ = false; } void @@ -725,6 +756,7 @@ ServerBase::set_on_ready_callback(std::function callback) set_callback_to_entity(EntityType::GoalService, callback); set_callback_to_entity(EntityType::ResultService, callback); set_callback_to_entity(EntityType::CancelService, callback); + set_callback_to_entity(EntityType::ExpireTimer, callback); } void @@ -753,6 +785,11 @@ ServerBase::set_callback_to_entity( } }; + if (entity_type == EntityType::ExpireTimer) { + std::lock_guard lock(callbacks_mutex_); + timer_expired_callback_ = new_callback; + return; + } // 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 @@ -762,7 +799,7 @@ ServerBase::set_callback_to_entity( rclcpp::detail::cpp_callback_trampoline, static_cast(&new_callback)); - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callbacks_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); @@ -836,12 +873,13 @@ ServerBase::set_on_ready_callback( void ServerBase::clear_on_ready_callback() { - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callbacks_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); + timer_expired_callback_ = nullptr; on_ready_callback_set_ = false; } 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 09/20] 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 68ccdd459bf216e88048058e9f59d55dc6e06e41 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Thu, 24 Feb 2022 11:07:53 +0000 Subject: [PATCH 10/20] Merge stuff --- rclcpp/CMakeLists.txt | 13 +++--- rclcpp/include/rclcpp/client.hpp | 6 +-- rclcpp/include/rclcpp/create_publisher.hpp | 2 +- .../rclcpp/detail/cpp_callback_trampoline.hpp | 2 +- .../subscription_intra_process_base.hpp | 21 ++++++--- .../subscription_intra_process_buffer.hpp | 8 ++-- rclcpp/include/rclcpp/qos_event.hpp | 6 +-- rclcpp/include/rclcpp/service.hpp | 6 +-- rclcpp/include/rclcpp/subscription_base.hpp | 6 +-- rclcpp/include/rclcpp/wait_for_message.hpp | 3 ++ .../test_allocator_memory_strategy.cpp | 10 +---- rclcpp/test/rclcpp/test_publisher.cpp | 11 +++++ rclcpp/test/rclcpp/test_qos_event.cpp | 43 ++++++------------- rclcpp/test/rclcpp/test_wait_for_message.cpp | 35 +++++++++++++++ rclcpp_action/CMakeLists.txt | 11 +++-- rclcpp_components/CMakeLists.txt | 14 +++--- .../component_manager_isolated.hpp | 37 ++++++++++++---- rclcpp_lifecycle/CMakeLists.txt | 12 ++++-- 18 files changed, 158 insertions(+), 88 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index a18cf19c40..9fa483e4b2 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -191,7 +191,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} @@ -221,11 +221,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) @@ -251,7 +254,7 @@ ament_package() install( DIRECTORY include/ ${CMAKE_CURRENT_BINARY_DIR}/include/ - DESTINATION include + DESTINATION include/${PROJECT_NAME} ) if(TEST cppcheck) @@ -259,4 +262,4 @@ if(TEST cppcheck) set_tests_properties(cppcheck PROPERTIES TIMEOUT 500) endif() -ament_cmake_gen_version_h() +ament_generate_version_header(${PROJECT_NAME}) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index a1deef73f6..e169b890bb 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -273,7 +273,7 @@ class ClientBase } }; - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callback_mutex_); // Set it temporarily to the new callback, while we replace the old one. // This two-step setting, prevents a gap where the old std::function has @@ -295,7 +295,7 @@ class ClientBase void clear_on_new_response_callback() { - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callback_mutex_); if (on_new_response_callback_) { set_on_new_response_callback(nullptr, nullptr); on_new_response_callback_ = nullptr; @@ -330,7 +330,7 @@ class ClientBase std::atomic in_use_by_wait_set_{false}; - std::recursive_mutex listener_mutex_; + std::recursive_mutex callback_mutex_; std::function on_new_response_callback_{nullptr}; }; diff --git a/rclcpp/include/rclcpp/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/include/rclcpp/detail/cpp_callback_trampoline.hpp b/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp index 7ea991c0de..be857e1b58 100644 --- a/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp +++ b/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp @@ -38,7 +38,7 @@ namespace detail * captures, and various kinds of std::bind instances. * * The interior of this function is likely to be executed within a C runtime, - * so no exceptins should be thrown at this point, and doing so results in + * so no exceptions should be thrown at this point, and doing so results in * undefined behavior. * * \tparam UserDataT Deduced type based on what is passed for user data, diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index db36690788..e16ac2ecef 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -103,7 +103,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable * The application should provide a generic callback function that will be then * forwarded by the waitable to all of its entities. * Before forwarding, a different value for the identifier argument will be - * bond to the function. + * bound to the function. * This implies that the provided callback can use the identifier to behave * differently depending on which entity triggered the waitable to become ready. * @@ -149,11 +149,11 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable } }; - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callback_mutex_); on_new_message_callback_ = new_callback; if (unread_count_ > 0) { - if (qos_profile_.history() == HistoryPolicy::KeepLast || qos_profile_.depth() == 0) { + if (qos_profile_.history() == HistoryPolicy::KeepAll) { on_new_message_callback_(unread_count_); } else { // Use qos profile depth as upper bound for unread_count_ @@ -167,12 +167,12 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable void clear_on_ready_callback() override { - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callback_mutex_); on_new_message_callback_ = nullptr; } protected: - std::recursive_mutex listener_mutex_; + std::recursive_mutex callback_mutex_; std::function on_new_message_callback_ {nullptr}; size_t unread_count_{0}; rclcpp::GuardCondition gc_; @@ -180,6 +180,17 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable virtual void trigger_guard_condition() = 0; + void + invoke_on_new_message() + { + std::lock_guard lock(this->callback_mutex_); + if (this->on_new_message_callback_) { + this->on_new_message_callback_(1); + } else { + this->unread_count_++; + } + } + private: std::string topic_name_; QoS qos_profile_; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 3f7dc48fa6..9fcbc043dc 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -125,7 +125,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff buffer_->add_shared(convert_ros_message_to_subscribed_type_unique_ptr(*message)); trigger_guard_condition(); } - invoke_on_new_message_listener(); + this->invoke_on_new_message(); } void @@ -138,7 +138,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff buffer_->add_unique(convert_ros_message_to_subscribed_type_unique_ptr(*message)); trigger_guard_condition(); } - invoke_on_new_message_listener(); + this->invoke_on_new_message(); } void @@ -146,7 +146,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff { buffer_->add_shared(std::move(message)); trigger_guard_condition(); - invoke_on_new_message_listener(); + this->invoke_on_new_message(); } void @@ -154,7 +154,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff { buffer_->add_unique(std::move(message)); trigger_guard_condition(); - invoke_on_new_message_listener(); + this->invoke_on_new_message(); } bool diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index c25789b86b..9d6f0964de 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -179,7 +179,7 @@ class QOSEventHandlerBase : public Waitable } }; - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callback_mutex_); // Set it temporarily to the new callback, while we replace the old one. // This two-step setting, prevents a gap where the old std::function has @@ -201,7 +201,7 @@ class QOSEventHandlerBase : public Waitable void clear_on_ready_callback() override { - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callback_mutex_); if (on_new_event_callback_) { set_on_new_event_callback(nullptr, nullptr); on_new_event_callback_ = nullptr; @@ -215,7 +215,7 @@ class QOSEventHandlerBase : public Waitable rcl_event_t event_handle_; size_t wait_set_event_index_; - std::recursive_mutex listener_mutex_; + std::recursive_mutex callback_mutex_; std::function on_new_event_callback_{nullptr}; }; diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index f1f8484436..47704a4c09 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -183,7 +183,7 @@ class ServiceBase } }; - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callback_mutex_); // Set it temporarily to the new callback, while we replace the old one. // This two-step setting, prevents a gap where the old std::function has @@ -205,7 +205,7 @@ class ServiceBase void clear_on_new_request_callback() { - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callback_mutex_); if (on_new_request_callback_) { set_on_new_request_callback(nullptr, nullptr); on_new_request_callback_ = nullptr; @@ -236,7 +236,7 @@ class ServiceBase std::atomic in_use_by_wait_set_{false}; - std::recursive_mutex listener_mutex_; + std::recursive_mutex callback_mutex_; std::function on_new_request_callback_{nullptr}; }; diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index eff4b64052..26650cd121 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -341,7 +341,7 @@ class SubscriptionBase : public std::enable_shared_from_this } }; - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callback_mutex_); // Set it temporarily to the new callback, while we replace the old one. // This two-step setting, prevents a gap where the old std::function has @@ -363,7 +363,7 @@ class SubscriptionBase : public std::enable_shared_from_this void clear_on_new_message_callback() { - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callback_mutex_); if (on_new_message_callback_) { set_on_new_message_callback(nullptr, nullptr); @@ -545,7 +545,7 @@ class SubscriptionBase : public std::enable_shared_from_this std::unordered_map> qos_events_in_use_by_wait_set_; - std::recursive_mutex listener_mutex_; + std::recursive_mutex callback_mutex_; std::function on_new_message_callback_{nullptr}; }; 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/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_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 1509e3f487..6671fa1176 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(); diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index d7ab0775f5..221e2bdf0a 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -39,9 +39,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*/) { @@ -56,7 +53,6 @@ class TestQosEvent : public ::testing::Test static constexpr char topic_name[] = "test_topic"; rclcpp::Node::SharedPtr node; - bool is_fastrtps; std::function message_callback; }; @@ -103,12 +99,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); } /* @@ -153,12 +145,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); } /* @@ -212,22 +200,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); } 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(); +} 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_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_; }; 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 79241a3cdc2815e6156f5f83aaceab1014db1847 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Fri, 25 Feb 2022 09:20:00 -0800 Subject: [PATCH 11/20] 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 e820c3cddb5b6412f8a24187c15f93b54ea24c65 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Mon, 28 Feb 2022 11:38:25 +0000 Subject: [PATCH 12/20] time_until_trigger returns max time if timer is cancelled Signed-off-by: Mauro Passerino --- rclcpp/src/rclcpp/timer.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 70414f38c6..25303de63a 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -116,8 +116,17 @@ TimerBase::is_ready() std::chrono::nanoseconds TimerBase::time_until_trigger() { + bool is_canceled = false; + rcl_ret_t ret = rcl_timer_is_canceled(timer_handle_.get(), &is_canceled); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get timer cancelled state"); + } + if (is_canceled) { + return std::chrono::nanoseconds::max(); + } + int64_t time_until_next_call = 0; - rcl_ret_t ret = rcl_timer_get_time_until_next_call( + ret = rcl_timer_get_time_until_next_call( timer_handle_.get(), &time_until_next_call); if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(ret, "Timer could not get time until next call"); From 360d853cc3b67d8c1e5e66c7e8dc2eb88c35cc00 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Mon, 28 Feb 2022 12:14:08 +0000 Subject: [PATCH 13/20] Add test for timer time_until_trigger api Signed-off-by: Mauro Passerino --- rclcpp/test/rclcpp/test_timer.cpp | 2 ++ 1 file changed, 2 insertions(+) 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 76a6dfe5c42289e053b0a78e15c17540f8891cd1 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Mon, 28 Feb 2022 15:20:22 +0000 Subject: [PATCH 14/20] create timer as part as server base Signed-off-by: Mauro Passerino --- rclcpp_action/include/rclcpp_action/server.hpp | 2 ++ rclcpp_action/src/server.cpp | 12 ++++++------ 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 95a138fa82..e896c459e7 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -317,6 +317,8 @@ class ServerBase : public rclcpp::Waitable std::recursive_mutex callbacks_mutex_; // Storage for std::function callbacks to keep them in scope std::unordered_map> entity_type_to_on_ready_callback_; + // Timer for expiring goals + rclcpp::TimerBase::SharedPtr timer_; std::function timer_expired_callback_{nullptr}; /// Set a callback to be called when the specified entity is ready diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 352d6b62dc..3574bb5c5a 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -55,9 +55,6 @@ class ServerBaseImpl // Do not declare this before clock_ as this depends on clock_(see #1526) std::shared_ptr action_server_; - // Timer for expiring goals - rclcpp::TimerBase::SharedPtr timer_; - size_t num_subscriptions_ = 0; size_t num_timers_ = 0; size_t num_clients_ = 0; @@ -115,14 +112,16 @@ ServerBase::ServerBase( rcl_node_t * rcl_node = node_base->get_rcl_node_handle(); rcl_clock_t * rcl_clock = pimpl_->clock_->get_clock_handle(); - pimpl_->timer_ = create_timer( + timer_ = create_timer( node_base, node_timers, - pimpl_->clock_, + node_clock->get_clock(), rclcpp::Duration(std::chrono::nanoseconds(options.result_timeout.nanoseconds)), [this]() { std::lock_guard lock(callbacks_mutex_); + std::cout << "if (timer_expired_callback_)" << std::endl; if (timer_expired_callback_) { + std::cout << "calling timer_expired_callback_" << std::endl; timer_expired_callback_(1); } }); @@ -134,7 +133,7 @@ ServerBase::ServerBase( type_support, name.c_str(), &options, - pimpl_->timer_->get_timer_handle().get()); + timer_->get_timer_handle().get()); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -594,6 +593,7 @@ ServerBase::execute_check_expired_goals() // Allocate expecting only one goal to expire at a time rcl_action_goal_info_t expired_goals[1]; size_t num_expired = 1; + std::cout << "execute_check_expired_goals" << std::endl; // Loop in case more than 1 goal expired while (num_expired > 0u) { From 4e3c6be76a8a1e91a3f1cafcfc80f52c7e072daf Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 1 Mar 2022 19:33:22 +0000 Subject: [PATCH 15/20] 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 16/20] 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 17/20] 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 18/20] 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 19/20] 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 c83b51b0e40123643789ecde7aaa9eea91dea162 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Mon, 14 Mar 2022 10:48:02 +0000 Subject: [PATCH 20/20] Add timers on_reset callback Signed-off-by: Mauro Passerino --- rclcpp/include/rclcpp/timer.hpp | 29 +++++++++ rclcpp/include/rclcpp/timers_manager.hpp | 7 +++ rclcpp/src/rclcpp/timer.cpp | 79 +++++++++++++++++++++++- rclcpp/src/rclcpp/timers_manager.cpp | 12 ++++ 4 files changed, 125 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index d55167e3ad..8bfab30eef 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -149,11 +149,40 @@ 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. + * + * 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 + */ + void + set_on_reset_callback(std::function callback); + + /// Unset the callback registered for reset timer + void + clear_on_reset_callback(); + protected: 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); + + std::recursive_mutex callback_mutex_; + std::function on_reset_callback_{nullptr}; }; diff --git a/rclcpp/include/rclcpp/timers_manager.hpp b/rclcpp/include/rclcpp/timers_manager.hpp index 37228a7fc0..81cbdc9a56 100644 --- a/rclcpp/include/rclcpp/timers_manager.hpp +++ b/rclcpp/include/rclcpp/timers_manager.hpp @@ -441,6 +441,13 @@ class TimersManager std::make_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater); } + void clear_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 diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 932e06db64..e29a8ac2b3 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() @@ -147,3 +152,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 { + (void)reset_calls; + callback(); + } 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, + 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, + 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_guard_condition_t * timer_gc = rcl_timer_get_guard_condition(timer_handle_.get()); + rcl_ret_t ret = rcl_guard_condition_set_on_trigger_callback(timer_gc, callback, user_data); + + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to set on reset callback"); + } +} diff --git a/rclcpp/src/rclcpp/timers_manager.cpp b/rclcpp/src/rclcpp/timers_manager.cpp index a25fe66b87..38ab311e77 100644 --- a/rclcpp/src/rclcpp/timers_manager.cpp +++ b/rclcpp/src/rclcpp/timers_manager.cpp @@ -49,6 +49,14 @@ void TimersManager::add_timer(rclcpp::TimerBase::SharedPtr timer) timers_updated_ = timers_updated_ || added; } + timer->set_on_reset_callback([this](){ + { + 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(); @@ -268,6 +276,9 @@ 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_callbacks(); weak_timers_heap_.clear(); timers_updated_ = true; @@ -291,4 +302,5 @@ void TimersManager::remove_timer(TimerPtr timer) // Notify timers thread such that it can re-compute its timeout timers_cv_.notify_one(); } + timer->clear_on_reset_callback(); }