diff --git a/nav2_controller/include/nav2_controller/nav2_controller.hpp b/nav2_controller/include/nav2_controller/nav2_controller.hpp index 05835ce86c4df635088962becdc1c774cf2577e9..26c3928360667593d4ea8b48e2d06680dc052512 100644 --- a/nav2_controller/include/nav2_controller/nav2_controller.hpp +++ b/nav2_controller/include/nav2_controller/nav2_controller.hpp @@ -68,7 +68,7 @@ protected: // The local controller needs a costmap node std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_; - std::unique_ptr<std::thread> costmap_thread_; + std::unique_ptr<nav2_util::NodeThread> costmap_thread_; // Publishers and subscribers std::unique_ptr<nav_2d_utils::OdomSubscriber> odom_sub_; @@ -78,9 +78,6 @@ protected: pluginlib::ClassLoader<nav2_core::LocalPlanner> lp_loader_; nav2_core::LocalPlanner::Ptr local_planner_; - // An executor used to spin the costmap node - rclcpp::executors::SingleThreadedExecutor costmap_executor_; - std::unique_ptr<ProgressChecker> progress_checker_; double controller_frequency_; diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index 0207ef3403a6cb322668d7aaa9034196c738355f..da63aff07b4aac7ed607214a529fb09c95bd90f7 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -41,22 +41,12 @@ ControllerServer::ControllerServer() "local_costmap", std::string{get_namespace()}, "local_costmap"); // Launch a thread to run the costmap node - costmap_thread_ = std::make_unique<std::thread>( - [&](rclcpp_lifecycle::LifecycleNode::SharedPtr node) - { - // TODO(mjeronimo): Once Brian pushes his change upstream to rlcpp executors, we'll - // be able to provide our own executor to spin(), reducing this to a single line - costmap_executor_.add_node(node->get_node_base_interface()); - costmap_executor_.spin(); - costmap_executor_.remove_node(node->get_node_base_interface()); - }, costmap_ros_); + costmap_thread_ = std::make_unique<nav2_util::NodeThread>(costmap_ros_); } ControllerServer::~ControllerServer() { RCLCPP_INFO(get_logger(), "Destroying"); - costmap_executor_.cancel(); - costmap_thread_->join(); } nav2_util::CallbackReturn diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index c295a1c9ebc5ac6955e7bb1fcb0d4ca0e5b466a5..873c3f199efc012730a15b9707d9e72efe959286 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -75,9 +75,8 @@ protected: // Global Costmap std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_; + std::unique_ptr<nav2_util::NodeThread> costmap_thread_; nav2_costmap_2d::Costmap2D * costmap_; - std::unique_ptr<std::thread> costmap_thread_; - rclcpp::executors::SingleThreadedExecutor costmap_executor_; // Publishers for the path rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr plan_publisher_; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 3345aa7bf7fa77dfc01f619cbece98ece0c559d3..79f8e7242bb5626a3406ef9d703e3811b2514f4d 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -48,21 +48,12 @@ PlannerServer::PlannerServer() "global_costmap", std::string{get_namespace()}, "global_costmap"); // Launch a thread to run the costmap node - costmap_thread_ = std::make_unique<std::thread>( - [&](rclcpp_lifecycle::LifecycleNode::SharedPtr node) - { - // TODO(mjeronimo): Once Brian pushes his change upstream to rlcpp executors, we'll - costmap_executor_.add_node(node->get_node_base_interface()); - costmap_executor_.spin(); - costmap_executor_.remove_node(node->get_node_base_interface()); - }, costmap_ros_); + costmap_thread_ = std::make_unique<nav2_util::NodeThread>(costmap_ros_); } PlannerServer::~PlannerServer() { RCLCPP_INFO(get_logger(), "Destroying"); - costmap_executor_.cancel(); - costmap_thread_->join(); planner_.reset(); } diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp index 0c94a20a57f417a80376a19025de0ee77a6ac73d..071a881e91daf5a2c52794a79af37608686ee659 100644 --- a/nav2_system_tests/src/planning/planner_tester.cpp +++ b/nav2_system_tests/src/planning/planner_tester.cpp @@ -54,13 +54,7 @@ void PlannerTester::activate() is_active_ = true; // Launch a thread to process the messages for this node - spin_thread_ = std::make_unique<std::thread>( - [&]() - { - executor_.add_node(this->get_node_base_interface()); - executor_.spin(); - executor_.remove_node(this->get_node_base_interface()); - }); + spin_thread_ = std::make_unique<nav2_util::NodeThread>(this); // We start with a 10x10 grid with no obstacles costmap_ = std::make_unique<Costmap>(this); @@ -87,8 +81,6 @@ void PlannerTester::deactivate() } is_active_ = false; - executor_.cancel(); - spin_thread_->join(); spin_thread_.reset(); auto state = rclcpp_lifecycle::State(); diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index de523f4c6399f7636140d72deba9dce7af55fc18..3f1d9e52908a70191f68f6211d0df0ea85429c83 100644 --- a/nav2_system_tests/src/planning/planner_tester.hpp +++ b/nav2_system_tests/src/planning/planner_tester.hpp @@ -29,6 +29,7 @@ #include "nav2_msgs/srv/get_costmap.hpp" #include "visualization_msgs/msg/marker.hpp" #include "nav2_util/costmap.hpp" +#include "nav2_util/node_thread.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -166,9 +167,8 @@ private: // The global planner std::shared_ptr<NavFnPlannerTester> planner_tester_; - // A thread for spinning the ROS node and the executor used - std::unique_ptr<std::thread> spin_thread_; - rclcpp::executors::SingleThreadedExecutor executor_; + // A thread for spinning the ROS node + std::unique_ptr<nav2_util::NodeThread> spin_thread_; // The tester must provide the robot pose through a transform std::unique_ptr<geometry_msgs::msg::TransformStamped> base_transform_; diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index 6f54721f146d186738774a39fc15a8be1175f6f7..3182f80cd37a5a2df2fbb94e39690dd623fa1b22 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -20,6 +20,7 @@ #include <thread> #include "nav2_util/lifecycle_helper_interface.hpp" +#include "nav2_util/node_thread.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp/rclcpp.hpp" @@ -122,8 +123,7 @@ protected: rclcpp::Node::SharedPtr rclcpp_node_; // When creating a local node, this class will launch a separate thread created to spin the node - std::unique_ptr<std::thread> rclcpp_thread_; - rclcpp::executors::SingleThreadedExecutor rclcpp_exec_; + std::unique_ptr<NodeThread> rclcpp_thread_; }; } // namespace nav2_util diff --git a/nav2_util/include/nav2_util/node_thread.hpp b/nav2_util/include/nav2_util/node_thread.hpp new file mode 100644 index 0000000000000000000000000000000000000000..97ab46cee6a97033425e6c37dedf234ec6e7d886 --- /dev/null +++ b/nav2_util/include/nav2_util/node_thread.hpp @@ -0,0 +1,45 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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 NAV2_UTIL__NODE_THREAD_HPP_ +#define NAV2_UTIL__NODE_THREAD_HPP_ + +#include <memory> + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_util +{ + +class NodeThread +{ +public: + explicit NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base); + + template<typename NodeT> + explicit NodeThread(NodeT node) + : NodeThread(node->get_node_base_interface()) + {} + + ~NodeThread(); + +protected: + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_; + std::unique_ptr<std::thread> thread_; + rclcpp::executors::SingleThreadedExecutor executor_; +}; + +} // namespace nav2_util + +#endif // NAV2_UTIL__NODE_THREAD_HPP_ diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index 3862ef25aed4d08ebdcff7634ea516ae9fa53095..70f1c7b62479d049f524dacf0787e953a8553007 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -6,6 +6,7 @@ add_library(${library_name} SHARED lifecycle_utils.cpp lifecycle_node.cpp robot_utils.cpp + node_thread.cpp ) ament_target_dependencies(${library_name} diff --git a/nav2_util/src/lifecycle_node.cpp b/nav2_util/src/lifecycle_node.cpp index 2d2c4edb0e791d35c42de41dd8abb659fdcbcf51..e6efb5f6a9bd2df81b486a5e4c9cc31a409b0268 100644 --- a/nav2_util/src/lifecycle_node.cpp +++ b/nav2_util/src/lifecycle_node.cpp @@ -55,13 +55,7 @@ LifecycleNode::LifecycleNode( new_args.push_back("--"); rclcpp_node_ = std::make_shared<rclcpp::Node>( "_", namespace_, rclcpp::NodeOptions(options).arguments(new_args)); - rclcpp_thread_ = std::make_unique<std::thread>( - [&](rclcpp::Node::SharedPtr node) { - rclcpp_exec_.add_node(node); - rclcpp_exec_.spin(); - rclcpp_exec_.remove_node(node); - }, - rclcpp_node_); + rclcpp_thread_ = std::make_unique<NodeThread>(rclcpp_node_); } } @@ -74,11 +68,6 @@ LifecycleNode::~LifecycleNode() on_deactivate(get_current_state()); on_cleanup(get_current_state()); } - - if (use_rclcpp_node_) { - rclcpp_exec_.cancel(); - rclcpp_thread_->join(); - } } } // namespace nav2_util diff --git a/nav2_util/src/node_thread.cpp b/nav2_util/src/node_thread.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e498e1371d3d00b99ce0235c15c522b698c017d3 --- /dev/null +++ b/nav2_util/src/node_thread.cpp @@ -0,0 +1,41 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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. + +#include <memory> + +#include "nav2_util/node_thread.hpp" + +namespace nav2_util +{ + +NodeThread::NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base) +: node_(node_base) +{ + thread_ = std::make_unique<std::thread>( + [&]() + { + executor_.add_node(node_); + executor_.spin(); + executor_.remove_node(node_); + }); +} + + +NodeThread::~NodeThread() +{ + executor_.cancel(); + thread_->join(); +} + +} // namespace nav2_util