Merge branch 'master' into 828

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_;

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

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_;

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();
 }
 

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();

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_;

nav2_util/include/nav2_util/lifecycle_node.hpp

@@ -19,6 +19,7 @@
 #include <string>
 #include <thread>
 
+#include "nav2_util/node_thread.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "rclcpp/rclcpp.hpp"
 
@@ -123,8 +124,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

nav2_util/include/nav2_util/node_thread.hpp

+// 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_

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}

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

nav2_util/src/node_thread.cpp

+// 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