From e1d2656fe8139f12b24796a45b55d0cd505c71c8 Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Thu, 29 Jul 2021 20:36:04 +0800 Subject: [PATCH 1/3] reduce transform listener nodes Signed-off-by: zhenpeng ge --- tf2_ros/include/tf2_ros/transform_listener.h | 55 +++++++++++--------- tf2_ros/src/transform_listener.cpp | 32 ++---------- 2 files changed, 36 insertions(+), 51 deletions(-) diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h index fc3cfb0f5..b6d346de7 100644 --- a/tf2_ros/include/tf2_ros/transform_listener.h +++ b/tf2_ros/include/tf2_ros/transform_listener.h @@ -101,7 +101,6 @@ class TransformListener : buffer_(buffer) { init(node, spin_thread, qos, static_qos, options, static_options); - node_logging_interface_ = node->get_node_logging_interface(); } TF2_ROS_PUBLIC @@ -117,6 +116,8 @@ class TransformListener const rclcpp::SubscriptionOptionsWithAllocator & options, const rclcpp::SubscriptionOptionsWithAllocator & static_options) { + spin_thread_ = spin_thread; + node_base_interface_ = node->get_node_base_interface(); node_logging_interface_ = node->get_node_logging_interface(); using callback_t = std::function; @@ -125,36 +126,41 @@ class TransformListener callback_t static_cb = std::bind( &TransformListener::subscription_callback, this, std::placeholders::_1, true); - message_subscription_tf_ = rclcpp::create_subscription( - node, - "/tf", - qos, - std::move(cb), - options); - message_subscription_tf_static_ = rclcpp::create_subscription( - node, - "/tf_static", - static_qos, - std::move(static_cb), - static_options); - - if (spin_thread) { - initThread(node->get_node_base_interface()); + if (spin_thread_) { + // Create new callback group for message_subscription of tf and tf_static + callback_group_ = node_base_interface_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + // Duplicate to modify option of subscription + rclcpp::SubscriptionOptionsWithAllocator options2 = options; + rclcpp::SubscriptionOptionsWithAllocator static_options2 = static_options; + options2.callback_group = callback_group_; + static_options2.callback_group = callback_group_; + message_subscription_tf_ = rclcpp::create_subscription( + node, "/tf", qos, std::move(cb), options2); + message_subscription_tf_static_ = rclcpp::create_subscription( + node, "/tf_static", static_qos, std::move(static_cb), static_options2); + // Create executor with dedicated thread to spin. + executor_ = std::make_shared(); + executor_->add_callback_group(callback_group_, node_base_interface_); + dedicated_listener_thread_ = std::make_unique([&]() {executor_->spin();}); + // Tell the buffer we have a dedicated thread to enable timeouts + buffer_.setUsingDedicatedThread(true); + } else { + message_subscription_tf_ = rclcpp::create_subscription( + node, "/tf", qos, std::move(cb), options); + message_subscription_tf_static_ = rclcpp::create_subscription( + node, "/tf_static", static_qos, std::move(static_cb), static_options); } } - - TF2_ROS_PUBLIC - void initThread( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface); - /// Callback function for ros message subscriptoin TF2_ROS_PUBLIC void subscription_callback(tf2_msgs::msg::TFMessage::SharedPtr msg, bool is_static); // ros::CallbackQueue tf_message_callback_queue_; - using thread_ptr = - std::unique_ptr>; - thread_ptr dedicated_listener_thread_; + bool spin_thread_{false}; + std::unique_ptr dedicated_listener_thread_; + rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr}; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; rclcpp::Node::SharedPtr optional_default_node_ = nullptr; rclcpp::Subscription::SharedPtr message_subscription_tf_; @@ -162,6 +168,7 @@ class TransformListener tf2::BufferCore & buffer_; tf2::TimePoint last_update_; rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_; }; } // namespace tf2_ros diff --git a/tf2_ros/src/transform_listener.cpp b/tf2_ros/src/transform_listener.cpp index 844bf3453..d8400440d 100644 --- a/tf2_ros/src/transform_listener.cpp +++ b/tf2_ros/src/transform_listener.cpp @@ -52,6 +52,7 @@ TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread) options.start_parameter_event_publisher(false); options.start_parameter_services(false); optional_default_node_ = rclcpp::Node::make_shared("_", options); + init( optional_default_node_, spin_thread, DynamicListenerQoS(), StaticListenerQoS(), detail::get_default_transform_listener_sub_options(), @@ -60,35 +61,12 @@ TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread) TransformListener::~TransformListener() { + if (spin_thread_) { + executor_->cancel(); + dedicated_listener_thread_->join(); + } } -void TransformListener::initThread( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface) -{ - 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 = - [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), - [executor](std::thread * t) { - executor->cancel(); - t->join(); - delete t; - // TODO(tfoote) reenable callback queue processing - // tf_message_callback_queue_.callAvailable(ros::WallDuration(0.01)); - }); - // Tell the buffer we have a dedicated thread to enable timeouts - buffer_.setUsingDedicatedThread(true); -} void TransformListener::subscription_callback( const tf2_msgs::msg::TFMessage::SharedPtr msg, From 8cb5278202d5b68153d77f703cdc91aef1442a31 Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Fri, 30 Jul 2021 07:33:33 +0800 Subject: [PATCH 2/3] remove a blank line Signed-off-by: zhenpeng ge --- tf2_ros/src/transform_listener.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/tf2_ros/src/transform_listener.cpp b/tf2_ros/src/transform_listener.cpp index d8400440d..d5ec0d34d 100644 --- a/tf2_ros/src/transform_listener.cpp +++ b/tf2_ros/src/transform_listener.cpp @@ -52,7 +52,6 @@ TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread) options.start_parameter_event_publisher(false); options.start_parameter_services(false); optional_default_node_ = rclcpp::Node::make_shared("_", options); - init( optional_default_node_, spin_thread, DynamicListenerQoS(), StaticListenerQoS(), detail::get_default_transform_listener_sub_options(), From 74523b36ed24d3a744996c80021daed6be58cc83 Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Fri, 30 Jul 2021 21:50:32 +0800 Subject: [PATCH 3/3] improve code style Signed-off-by: zhenpeng ge --- tf2_ros/include/tf2_ros/transform_listener.h | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h index b6d346de7..7684d913c 100644 --- a/tf2_ros/include/tf2_ros/transform_listener.h +++ b/tf2_ros/include/tf2_ros/transform_listener.h @@ -131,14 +131,16 @@ class TransformListener callback_group_ = node_base_interface_->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); // Duplicate to modify option of subscription - rclcpp::SubscriptionOptionsWithAllocator options2 = options; - rclcpp::SubscriptionOptionsWithAllocator static_options2 = static_options; - options2.callback_group = callback_group_; - static_options2.callback_group = callback_group_; + rclcpp::SubscriptionOptionsWithAllocator tf_options = options; + rclcpp::SubscriptionOptionsWithAllocator tf_static_options = static_options; + tf_options.callback_group = callback_group_; + tf_static_options.callback_group = callback_group_; + message_subscription_tf_ = rclcpp::create_subscription( - node, "/tf", qos, std::move(cb), options2); + node, "/tf", qos, std::move(cb), tf_options); message_subscription_tf_static_ = rclcpp::create_subscription( - node, "/tf_static", static_qos, std::move(static_cb), static_options2); + node, "/tf_static", static_qos, std::move(static_cb), tf_static_options); + // Create executor with dedicated thread to spin. executor_ = std::make_shared(); executor_->add_callback_group(callback_group_, node_base_interface_);