From bef61705f70b4f1f30d9a950433f041fe7629ec9 Mon Sep 17 00:00:00 2001 From: bpwilcox Date: Tue, 18 Jun 2019 14:18:55 -0700 Subject: [PATCH 1/3] use executor to spin and stop node in thread initialize executor set spin_thread to true in initThread Signed-off-by: bpwilcox --- tf2_ros/include/tf2_ros/transform_listener.h | 3 ++- tf2_ros/src/transform_listener.cpp | 11 ++++++++--- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h index 02d3c0963..28fb2a238 100644 --- a/tf2_ros/include/tf2_ros/transform_listener.h +++ b/tf2_ros/include/tf2_ros/transform_listener.h @@ -116,7 +116,8 @@ class TransformListener rclcpp::Subscription::SharedPtr message_subscription_tf_; rclcpp::Subscription::SharedPtr message_subscription_tf_static_; tf2::BufferCore & buffer_; - std::atomic stop_thread_{false}; + std::unique_ptr executor_; + bool spin_thread_{false}; tf2::TimePoint last_update_; }; } // namespace tf2_ros diff --git a/tf2_ros/src/transform_listener.cpp b/tf2_ros/src/transform_listener.cpp index 854436726..7426f1e97 100644 --- a/tf2_ros/src/transform_listener.cpp +++ b/tf2_ros/src/transform_listener.cpp @@ -54,19 +54,24 @@ TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread) TransformListener::~TransformListener() { - stop_thread_ = true; + if (spin_thread_) { + executor_->cancel(); + } } void TransformListener::initThread( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface) { - stop_thread_ = false; + executor_ = std::make_unique(); + spin_thread_ = true; // This lambda is required because `std::thread` cannot infer the correct // rclcpp::spin, since there are more than one versions of it (overloaded). // see: http://stackoverflow.com/a/27389714/671658 // I (wjwwood) chose to use the lamda rather than the static cast solution. auto run_func = [&](rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface) { - while (!stop_thread_ && rclcpp::ok()) {rclcpp::spin_some(node_base_interface);} + executor_->add_node(node_base_interface); + executor_->spin(); + executor_->remove_node(node_base_interface); }; dedicated_listener_thread_ = thread_ptr( new std::thread(run_func, node_base_interface), From 249fe8738881425860e392633e6278a5f8dbd4eb Mon Sep 17 00:00:00 2001 From: bpwilcox Date: Wed, 17 Jul 2019 11:38:03 -0700 Subject: [PATCH 2/3] remove spin_thread_ bool --- tf2_ros/include/tf2_ros/transform_listener.h | 1 - tf2_ros/src/transform_listener.cpp | 3 +-- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h index 28fb2a238..a3ae52629 100644 --- a/tf2_ros/include/tf2_ros/transform_listener.h +++ b/tf2_ros/include/tf2_ros/transform_listener.h @@ -117,7 +117,6 @@ class TransformListener rclcpp::Subscription::SharedPtr message_subscription_tf_static_; tf2::BufferCore & buffer_; std::unique_ptr executor_; - bool spin_thread_{false}; tf2::TimePoint last_update_; }; } // namespace tf2_ros diff --git a/tf2_ros/src/transform_listener.cpp b/tf2_ros/src/transform_listener.cpp index 7426f1e97..76d9b6b70 100644 --- a/tf2_ros/src/transform_listener.cpp +++ b/tf2_ros/src/transform_listener.cpp @@ -54,7 +54,7 @@ TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread) TransformListener::~TransformListener() { - if (spin_thread_) { + if (executor_) { executor_->cancel(); } } @@ -63,7 +63,6 @@ void TransformListener::initThread( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface) { executor_ = std::make_unique(); - spin_thread_ = true; // This lambda is required because `std::thread` cannot infer the correct // rclcpp::spin, since there are more than one versions of it (overloaded). // see: http://stackoverflow.com/a/27389714/671658 From 74ee27b3303447ed81fb63c6d9c1f4249e911e5f Mon Sep 17 00:00:00 2001 From: bpwilcox Date: Wed, 24 Jul 2019 11:18:58 -0700 Subject: [PATCH 3/3] capture local executor in lambda remove executor member variable --- tf2_ros/include/tf2_ros/transform_listener.h | 1 - tf2_ros/src/transform_listener.cpp | 18 +++++++++--------- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h index a3ae52629..763355552 100644 --- a/tf2_ros/include/tf2_ros/transform_listener.h +++ b/tf2_ros/include/tf2_ros/transform_listener.h @@ -116,7 +116,6 @@ class TransformListener rclcpp::Subscription::SharedPtr message_subscription_tf_; rclcpp::Subscription::SharedPtr message_subscription_tf_static_; tf2::BufferCore & buffer_; - std::unique_ptr executor_; tf2::TimePoint last_update_; }; } // namespace tf2_ros diff --git a/tf2_ros/src/transform_listener.cpp b/tf2_ros/src/transform_listener.cpp index 76d9b6b70..94908b9b3 100644 --- a/tf2_ros/src/transform_listener.cpp +++ b/tf2_ros/src/transform_listener.cpp @@ -54,27 +54,27 @@ TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread) TransformListener::~TransformListener() { - if (executor_) { - executor_->cancel(); - } } void TransformListener::initThread( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface) { - executor_ = std::make_unique(); + auto executor = std::make_shared(); + // This lambda is required because `std::thread` cannot infer the correct // rclcpp::spin, since there are more than one versions of it (overloaded). // see: http://stackoverflow.com/a/27389714/671658 // I (wjwwood) chose to use the lamda rather than the static cast solution. - auto run_func = [&](rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface) { - executor_->add_node(node_base_interface); - executor_->spin(); - executor_->remove_node(node_base_interface); + auto run_func = + [executor](rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface) { + executor->add_node(node_base_interface); + executor->spin(); + executor->remove_node(node_base_interface); }; dedicated_listener_thread_ = thread_ptr( new std::thread(run_func, node_base_interface), - [](std::thread * t) { + [executor](std::thread * t) { + executor->cancel(); t->join(); delete t; // TODO(tfoote) reenable callback queue processing