From b04673dee5695fbe754fd47f462d2f7c2a26e39e Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Wed, 18 Nov 2026 17:20:19 +0100 Subject: [PATCH 1/2] Graph changes should reach all interested nodes Signed-off-by: Tim Clephas --- rclcpp/test/rclcpp/test_graph_listener.cpp | 26 ++++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/rclcpp/test/rclcpp/test_graph_listener.cpp b/rclcpp/test/rclcpp/test_graph_listener.cpp index 2dd8050545..e0623d5deb 100644 --- a/rclcpp/test/rclcpp/test_graph_listener.cpp +++ b/rclcpp/test/rclcpp/test_graph_listener.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include @@ -200,6 +201,31 @@ TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_timeout) { std::runtime_error("rcl_wait unexpectedly timed out")); } +/* A graph change wakes up the graph events of every node in the context, not only the first. + * The nodes share one rmw graph guard condition, which the GraphListener must not consume + * on behalf of a single node. */ +TEST_F(TestGraphListener, graph_change_notifies_all_nodes_in_context) { + using namespace std::chrono_literals; + auto node_b = std::make_shared("node_b", node_namespace); + auto event_a = node()->get_graph_event(); + auto event_b = node_b->get_graph_event(); + event_a->check_and_clear(); + event_b->check_and_clear(); + + // Creating a node is a graph change. + auto node_c = std::make_shared("node_c", node_namespace); + + auto wait_for_event = [](rclcpp::Node & node, const rclcpp::Event::SharedPtr & event) { + const auto deadline = std::chrono::steady_clock::now() + 5s; + while (!event->check() && std::chrono::steady_clock::now() < deadline) { + node.wait_for_graph_change(event, 100ms); + } + return event->check(); + }; + EXPECT_TRUE(wait_for_event(*node(), event_a)); + EXPECT_TRUE(wait_for_event(*node_b, event_b)); +} + /* Add/Remove node usage */ TEST_F(TestGraphListener, test_graph_listener_add_remove_node) { EXPECT_FALSE(graph_listener()->has_node(node_graph())); From 0019820d03cdf8afa9efbc8057afe3d650ab60c5 Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Wed, 18 Nov 2026 17:24:30 +0100 Subject: [PATCH 2/2] And now all nodes do get the event Signed-off-by: Tim Clephas --- rclcpp/src/rclcpp/graph_listener.cpp | 38 +++++++++++++++++++++++----- 1 file changed, 32 insertions(+), 6 deletions(-) diff --git a/rclcpp/src/rclcpp/graph_listener.cpp b/rclcpp/src/rclcpp/graph_listener.cpp index 38ce92ccb2..b32a03a13f 100644 --- a/rclcpp/src/rclcpp/graph_listener.cpp +++ b/rclcpp/src/rclcpp/graph_listener.cpp @@ -14,12 +14,14 @@ #include "rclcpp/graph_listener.hpp" +#include #include #include #include #include #include #include "rcl/error_handling.h" +#include "rcl/guard_condition.h" #include "rcl/types.h" #include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" #include "rclcpp/exceptions.hpp" @@ -138,23 +140,34 @@ GraphListener::run_loop() // Put the interrupt guard condition in the wait set. detail::add_guard_condition_to_rcl_wait_set(wait_set_, interrupt_guard_condition_); - // Put graph guard conditions for each node into the wait set. - std::vector graph_gc_indexes(node_graph_interfaces_size, 0u); + // Put the graph guard conditions of the nodes into the wait set. + // Each node has its own rcl graph guard condition, but they usually wrap one rmw guard + // condition shared by all nodes of the context. rmw_wait() resets a triggered guard condition + // when it reports it, so a guard condition added twice is reported only once. Hence each rmw + // guard condition is added once and all nodes sharing it are notified below. + std::vector waited_graph_gcs; + waited_graph_gcs.reserve(node_graph_interfaces_size); for (size_t i = 0u; i < node_graph_interfaces_size; ++i) { auto node_ptr = node_graph_interfaces_[i]; // Only wait on graph changes if some user of the node is watching. if (node_ptr->count_graph_users() == 0) { continue; } - // Add the graph guard condition for the node to the wait set. auto graph_gc = node_ptr->get_graph_guard_condition(); if (!graph_gc) { throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition"); } - ret = rcl_wait_set_add_guard_condition(&wait_set_, graph_gc, &graph_gc_indexes[i]); + const auto rmw_graph_gc = rcl_guard_condition_get_rmw_handle(graph_gc); + if (std::find(waited_graph_gcs.begin(), waited_graph_gcs.end(), rmw_graph_gc) != + waited_graph_gcs.end()) + { + continue; + } + ret = rcl_wait_set_add_guard_condition(&wait_set_, graph_gc, nullptr); if (RCL_RET_OK != ret) { throw_from_rcl_error(ret, "failed to add graph guard condition to wait set"); } + waited_graph_gcs.push_back(rmw_graph_gc); } // Wait for: graph changes, interrupt, or shutdown/SIGINT @@ -166,14 +179,27 @@ GraphListener::run_loop() throw_from_rcl_error(ret, "failed to wait on wait set"); } - // Notify nodes who's guard conditions are set (triggered). + // Collect the triggered rmw graph guard conditions. + // rcl_wait() leaves triggered guard conditions in the wait set and sets the others to NULL. + std::vector triggered_graph_gcs; + for (size_t i = 0u; i < wait_set_.size_of_guard_conditions; ++i) { + if (wait_set_.guard_conditions[i]) { + triggered_graph_gcs.push_back( + rcl_guard_condition_get_rmw_handle(wait_set_.guard_conditions[i])); + } + } + + // Notify every node whose graph guard condition is set (triggered). for (size_t i = 0u; i < node_graph_interfaces_size; ++i) { const auto node_ptr = node_graph_interfaces_[i]; auto graph_gc = node_ptr->get_graph_guard_condition(); if (!graph_gc) { throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition"); } - if (graph_gc == wait_set_.guard_conditions[graph_gc_indexes[i]]) { + if (std::find( + triggered_graph_gcs.begin(), triggered_graph_gcs.end(), + rcl_guard_condition_get_rmw_handle(graph_gc)) != triggered_graph_gcs.end()) + { node_ptr->notify_graph_change(); } if (is_shutdown()) {