diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index a6c84e4aa0..6131681b80 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -122,9 +122,13 @@ class NodeBase : public NodeBaseInterface, public std::enable_shared_from_this notify_guard_condition_; bool notify_guard_condition_is_valid_; }; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index fd4f64b22b..eb87771eab 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -152,9 +152,14 @@ class NodeBaseInterface */ RCLCPP_PUBLIC virtual - rclcpp::GuardCondition & + rclcpp::GuardCondition::SharedPtr get_notify_guard_condition() = 0; + RCLCPP_PUBLIC + virtual + void + trigger_notify_guard_condition() = 0; + /// Return the default preference for using intra process communication. RCLCPP_PUBLIC virtual diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 7b4a70d411..2b08b746b6 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -45,7 +45,7 @@ NodeBase::NodeBase( node_handle_(nullptr), default_callback_group_(default_callback_group), associated_with_executor_(false), - notify_guard_condition_(context), + notify_guard_condition_(std::make_shared(context)), notify_guard_condition_is_valid_(false) { // Create the rcl node and store it in a shared_ptr with a custom destructor. @@ -254,7 +254,7 @@ NodeBase::get_associated_with_executor_atomic() return associated_with_executor_; } -rclcpp::GuardCondition & +rclcpp::GuardCondition::SharedPtr NodeBase::get_notify_guard_condition() { std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); @@ -264,6 +264,16 @@ NodeBase::get_notify_guard_condition() return notify_guard_condition_; } +void +NodeBase::trigger_notify_guard_condition() +{ + std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); + if (!notify_guard_condition_is_valid_) { + throw std::runtime_error("failed to trigger notify guard condition because it is invalid"); + } + notify_guard_condition_->trigger(); +} + bool NodeBase::get_use_intra_process_default() const { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index e13ec7cd4c..bf47a1e3b5 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -533,9 +533,8 @@ NodeGraph::notify_graph_change() } } graph_cv_.notify_all(); - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string("failed to notify wait set on graph change: ") + ex.what()); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp index 2f1afd3224..e9c4a5266e 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp @@ -42,9 +42,8 @@ NodeServices::add_service( group->add_service(service_base_ptr); // Notify the executor that a new service was created using the parent Node. - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( @@ -69,9 +68,8 @@ NodeServices::add_client( group->add_client(client_base_ptr); // Notify the executor that a new client was created using the parent Node. - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( diff --git a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp index d2e821a9e6..96097def22 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp @@ -42,9 +42,8 @@ NodeTimers::add_timer( } callback_group->add_timer(timer); - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); callback_group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index 167a35f35d..659129d62c 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -70,9 +70,8 @@ NodeTopics::add_publisher( } // Notify the executor that a new publisher was created using the parent Node. - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); callback_group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( @@ -119,9 +118,8 @@ NodeTopics::add_subscription( } // Notify the executor that a new subscription was created using the parent Node. - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); callback_group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( diff --git a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp index 1d1fe2ce59..02a9de82b0 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp @@ -42,9 +42,8 @@ NodeWaitables::add_waitable( group->add_waitable(waitable_ptr); // Notify the executor that a new waitable was created using the parent Node. - auto & node_gc = node_base_->get_notify_guard_condition(); try { - node_gc.trigger(); + node_base_->trigger_notify_guard_condition(); group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( diff --git a/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp b/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp index 8c9aaad819..a286de5fa8 100644 --- a/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp +++ b/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp @@ -61,10 +61,10 @@ TEST_F(TestExecutorNotifyWaitable, add_remove_guard_conditions) { std::make_shared(on_execute_callback); auto node = std::make_shared("my_node", "/ns"); - auto & notify_guard_condition = node->get_node_base_interface()->get_notify_guard_condition(); + auto notify_guard_condition = node->get_node_base_interface()->get_notify_guard_condition(); - EXPECT_NO_THROW(waitable->add_guard_condition(¬ify_guard_condition)); - EXPECT_NO_THROW(waitable->remove_guard_condition(¬ify_guard_condition)); + EXPECT_NO_THROW(waitable->add_guard_condition(notify_guard_condition)); + EXPECT_NO_THROW(waitable->remove_guard_condition(notify_guard_condition)); } TEST_F(TestExecutorNotifyWaitable, wait) { @@ -75,8 +75,8 @@ TEST_F(TestExecutorNotifyWaitable, wait) { std::make_shared(on_execute_callback); auto node = std::make_shared("my_node", "/ns"); - auto & notify_guard_condition = node->get_node_base_interface()->get_notify_guard_condition(); - EXPECT_NO_THROW(waitable->add_guard_condition(¬ify_guard_condition)); + auto notify_guard_condition = node->get_node_base_interface()->get_notify_guard_condition(); + EXPECT_NO_THROW(waitable->add_guard_condition(notify_guard_condition)); auto default_cbg = node->get_node_base_interface()->get_default_callback_group(); ASSERT_NE(nullptr, default_cbg->get_notify_guard_condition());