Pluginizing BT Navigators (#3345)

* initial prototype

* linting

Author: SteveMacenski

nav2_bringup/params/nav2_multirobot_params_1.yaml

@@ -45,6 +45,11 @@ bt_navigator:
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
+    navigators: ["navigate_to_pose", "navigate_through_poses"]
+    navigate_to_pose:
+      plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
+    navigate_through_poses:
+      plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator"
     # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
     # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
     # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml

nav2_bringup/params/nav2_multirobot_params_2.yaml

@@ -45,6 +45,11 @@ bt_navigator:
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
+    navigators: ["navigate_to_pose", "navigate_through_poses"]
+    navigate_to_pose:
+      plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
+    navigate_through_poses:
+      plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator"
     # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
     # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
     # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml

nav2_bringup/params/nav2_params.yaml

@@ -45,6 +45,11 @@ bt_navigator:
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
+    navigators: ["navigate_to_pose", "navigate_through_poses"]
+    navigate_to_pose:
+      plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
+    navigate_through_poses:
+      plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator"
     # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
     # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
     # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml

nav2_bt_navigator/CMakeLists.txt

@@ -17,6 +17,7 @@ find_package(std_srvs REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(nav2_core REQUIRED)
 find_package(tf2_ros REQUIRED)
+find_package(pluginlib REQUIRED)
 
 nav2_package()
 
@@ -47,12 +48,11 @@ set(dependencies
   nav2_util
   nav2_core
   tf2_ros
+  pluginlib
 )
 
 add_library(${library_name} SHARED
   src/bt_navigator.cpp
-  src/navigators/navigate_to_pose.cpp
-  src/navigators/navigate_through_poses.cpp
 )
 
 ament_target_dependencies(${executable_name}
@@ -65,9 +65,16 @@ ament_target_dependencies(${library_name}
   ${dependencies}
 )
 
+add_library(nav2_navigate_to_pose_navigator SHARED src/navigators/navigate_to_pose.cpp)
+ament_target_dependencies(nav2_navigate_to_pose_navigator ${dependencies})
+
+add_library(nav2_navigate_through_poses SHARED src/navigators/navigate_through_poses.cpp)
+ament_target_dependencies(nav2_navigate_through_poses ${dependencies})
+
+pluginlib_export_plugin_description_file(nav2_core navigator_plugins.xml)
 rclcpp_components_register_nodes(${library_name} "nav2_bt_navigator::BtNavigator")
 
-install(TARGETS ${library_name}
+install(TARGETS ${library_name} nav2_navigate_to_pose_navigator nav2_navigate_through_poses
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -89,6 +96,6 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories(include)
-ament_export_libraries(${library_name})
+ament_export_libraries(${library_name} nav2_navigate_to_pose_navigator nav2_navigate_through_poses)
 ament_export_dependencies(${dependencies})
 ament_package()

nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp

@@ -1,4 +1,5 @@
 // Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2023 Samsung Research America
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -25,8 +26,8 @@
 #include "tf2_ros/buffer.h"
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/create_timer_ros.h"
-#include "nav2_bt_navigator/navigators/navigate_to_pose.hpp"
-#include "nav2_bt_navigator/navigators/navigate_through_poses.hpp"
+#include "nav2_core/behavior_tree_navigator.hpp"
+#include "pluginlib/class_loader.hpp"
 
 namespace nav2_bt_navigator
 {
@@ -53,7 +54,7 @@ class BtNavigator : public nav2_util::LifecycleNode
   /**
    * @brief Configures member variables
    *
-   * Initializes action server for "NavigationToPose"; subscription to
+   * Initializes action servers for navigator plugins; subscription to
    * "goal_sub"; and builds behavior tree from xml file.
    * @param state Reference to LifeCycle node state
    * @return SUCCESS or FAILURE
@@ -85,10 +86,9 @@ class BtNavigator : public nav2_util::LifecycleNode
   nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
 
   // To handle all the BT related execution
-  std::unique_ptr<nav2_bt_navigator::Navigator<nav2_msgs::action::NavigateToPose>> pose_navigator_;
-  std::unique_ptr<nav2_bt_navigator::Navigator<nav2_msgs::action::NavigateThroughPoses>>
-  poses_navigator_;
-  nav2_bt_navigator::NavigatorMuxer plugin_muxer_;
+  pluginlib::ClassLoader<nav2_core::NavigatorBase> class_loader_;
+  std::vector<pluginlib::UniquePtr<nav2_core::NavigatorBase>> navigators_;
+  nav2_core::NavigatorMuxer plugin_muxer_;
 
   // Odometry smoother object
   std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
@@ -99,7 +99,7 @@ class BtNavigator : public nav2_util::LifecycleNode
   double transform_tolerance_;
   std::string odom_topic_;
 
-  // Spinning transform that can be used by the BT nodes
+  // Spinning transform that can be used by the node
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 };

nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2021 Samsung Research
+// Copyright (c) 2021-2023 Samsung Research
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -21,7 +21,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "nav2_bt_navigator/navigator.hpp"
+#include "nav2_core/behavior_tree_navigator.hpp"
 #include "nav2_msgs/action/navigate_through_poses.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "nav2_util/robot_utils.hpp"
@@ -36,7 +36,7 @@ namespace nav2_bt_navigator
  * @brief A navigator for navigating to a a bunch of intermediary poses
  */
 class NavigateThroughPosesNavigator
-  : public nav2_bt_navigator::Navigator<nav2_msgs::action::NavigateThroughPoses>
+  : public nav2_core::BehaviorTreeNavigator<nav2_msgs::action::NavigateThroughPoses>
 {
 public:
   using ActionT = nav2_msgs::action::NavigateThroughPoses;
@@ -46,7 +46,7 @@ class NavigateThroughPosesNavigator
    * @brief A constructor for NavigateThroughPosesNavigator
    */
   NavigateThroughPosesNavigator()
-  : Navigator() {}
+  : BehaviorTreeNavigator() {}
 
   /**
    * @brief A configure state transition to configure navigator's state

nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2021 Samsung Research
+// Copyright (c) 2021-2023 Samsung Research
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -21,7 +21,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "nav2_bt_navigator/navigator.hpp"
+#include "nav2_core/behavior_tree_navigator.hpp"
 #include "nav2_msgs/action/navigate_to_pose.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
@@ -36,7 +36,7 @@ namespace nav2_bt_navigator
  * @brief A navigator for navigating to a specified pose
  */
 class NavigateToPoseNavigator
-  : public nav2_bt_navigator::Navigator<nav2_msgs::action::NavigateToPose>
+  : public nav2_core::BehaviorTreeNavigator<nav2_msgs::action::NavigateToPose>
 {
 public:
   using ActionT = nav2_msgs::action::NavigateToPose;
@@ -45,7 +45,7 @@ class NavigateToPoseNavigator
    * @brief A constructor for NavigateToPoseNavigator
    */
   NavigateToPoseNavigator()
-  : Navigator() {}
+  : BehaviorTreeNavigator() {}
 
   /**
    * @brief A configure state transition to configure navigator's state

nav2_bt_navigator/navigator_plugins.xml

@@ -0,0 +1,18 @@
+<class_libraries>
+	<library path="nav2_navigate_to_pose_navigator">
+	  <class
+	  	name="nav2_bt_navigator/NavigateToPoseNavigator"
+	  	type="nav2_bt_navigator::NavigateToPoseNavigator"
+	  	base_class_type="nav2_core::NavigatorBase">
+	    <description>Navigate to Pose Navigator</description>
+	  </class>
+	</library>
+  <library path="nav2_navigate_through_poses">
+	  <class
+	  	name="nav2_bt_navigator/NavigateThroughPosesNavigator"
+	  	type="nav2_bt_navigator::NavigateThroughPosesNavigator"
+	  	base_class_type="nav2_core::NavigatorBase">
+	    <description>Navigate through Poses Navigator</description>
+	  </class>
+	</library>
+</class_libraries>

nav2_bt_navigator/package.xml

@@ -22,6 +22,7 @@
   <build_depend>geometry_msgs</build_depend>
   <build_depend>std_srvs</build_depend>
   <build_depend>nav2_util</build_depend>
+  <build_depend>pluginlib</build_depend>
   <build_depend>nav2_core</build_depend>
 
   <exec_depend>behaviortree_cpp_v3</exec_depend>
@@ -34,12 +35,14 @@
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>nav2_util</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>pluginlib</exec_depend>
   <exec_depend>nav2_core</exec_depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
+    <nav2_core prefix="$(prefix)/navigator_plugins.xml"/>
   </export>
 </package>

nav2_bt_navigator/src/bt_navigator.cpp

@@ -27,7 +27,8 @@ namespace nav2_bt_navigator
 {
 
 BtNavigator::BtNavigator(const rclcpp::NodeOptions & options)
-: nav2_util::LifecycleNode("bt_navigator", "", options)
+: nav2_util::LifecycleNode("bt_navigator", "", options),
+  class_loader_("nav2_core", "nav2_core::NavigatorBase")
 {
   RCLCPP_INFO(get_logger(), "Creating");
 
@@ -111,28 +112,56 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
   // Libraries to pull plugins (BT Nodes) from
   auto plugin_lib_names = get_parameter("plugin_lib_names").as_string_array();
 
-  pose_navigator_ = std::make_unique<nav2_bt_navigator::NavigateToPoseNavigator>();
-  poses_navigator_ = std::make_unique<nav2_bt_navigator::NavigateThroughPosesNavigator>();
-
-  nav2_bt_navigator::FeedbackUtils feedback_utils;
+  nav2_core::FeedbackUtils feedback_utils;
   feedback_utils.tf = tf_;
   feedback_utils.global_frame = global_frame_;
   feedback_utils.robot_frame = robot_frame_;
   feedback_utils.transform_tolerance = transform_tolerance_;
 
   // Odometry smoother object for getting current speed
-  odom_smoother_ = std::make_shared<nav2_util::OdomSmoother>(shared_from_this(), 0.3, odom_topic_);
+  auto node = shared_from_this();
+  odom_smoother_ = std::make_shared<nav2_util::OdomSmoother>(node, 0.3, odom_topic_);
+
+  // Navigator defaults
+  const std::vector<std::string> default_navigator_ids = {
+    "navigate_to_pose",
+    "navigate_through_poses"
+  };
+  const std::vector<std::string> default_navigator_types = {
+    "nav2_bt_navigator/NavigateToPoseNavigator",
+    "nav2_bt_navigator/NavigateThroughPosesNavigator"
+  };
 
-  if (!pose_navigator_->on_configure(
-      shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_, odom_smoother_))
-  {
-    return nav2_util::CallbackReturn::FAILURE;
+  std::vector<std::string> navigator_ids;
+  declare_parameter("navigators", default_navigator_ids);
+  get_parameter("navigators", navigator_ids);
+  if (navigator_ids == default_navigator_ids) {
+    for (size_t i = 0; i < default_navigator_ids.size(); ++i) {
+      declare_parameter(default_navigator_ids[i] + ".plugin", default_navigator_types[i]);
+    }
   }
 
-  if (!poses_navigator_->on_configure(
-      shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_, odom_smoother_))
-  {
-    return nav2_util::CallbackReturn::FAILURE;
+  // Load navigator plugins
+  for (size_t i = 0; i != navigator_ids.size(); i++) {
+    std::string navigator_type = nav2_util::get_plugin_type_param(node, navigator_ids[i]);
+    try {
+      RCLCPP_INFO(
+        get_logger(), "Creating navigator id %s of type %s",
+        navigator_ids[i].c_str(), navigator_type.c_str());
+      navigators_.push_back(class_loader_.createUniqueInstance(navigator_type));
+      if (!navigators_.back()->on_configure(
+          node, plugin_lib_names, feedback_utils,
+          &plugin_muxer_, odom_smoother_))
+      {
+        return nav2_util::CallbackReturn::FAILURE;
+      }
+    } catch (const pluginlib::PluginlibException & ex) {
+      RCLCPP_FATAL(
+        get_logger(), "Failed to create navigator id %s of type %s."
+        " Exception: %s", navigator_ids[i].c_str(), navigator_type.c_str(),
+        ex.what());
+      return nav2_util::CallbackReturn::FAILURE;
+    }
   }
 
   return nav2_util::CallbackReturn::SUCCESS;
@@ -142,9 +171,10 @@ nav2_util::CallbackReturn
 BtNavigator::on_activate(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Activating");
-
-  if (!poses_navigator_->on_activate() || !pose_navigator_->on_activate()) {
-    return nav2_util::CallbackReturn::FAILURE;
+  for (size_t i = 0; i != navigators_.size(); i++) {
+    if (!navigators_[i]->on_activate()) {
+      return nav2_util::CallbackReturn::FAILURE;
+    }
   }
 
   // create bond connection
@@ -157,9 +187,10 @@ nav2_util::CallbackReturn
 BtNavigator::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Deactivating");
-
-  if (!poses_navigator_->on_deactivate() || !pose_navigator_->on_deactivate()) {
-    return nav2_util::CallbackReturn::FAILURE;
+  for (size_t i = 0; i != navigators_.size(); i++) {
+    if (!navigators_[i]->on_deactivate()) {
+      return nav2_util::CallbackReturn::FAILURE;
+    }
   }
 
   // destroy bond connection
@@ -177,13 +208,13 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   tf_listener_.reset();
   tf_.reset();
 
-  if (!poses_navigator_->on_cleanup() || !pose_navigator_->on_cleanup()) {
-    return nav2_util::CallbackReturn::FAILURE;
+  for (size_t i = 0; i != navigators_.size(); i++) {
+    if (!navigators_[i]->on_cleanup()) {
+      return nav2_util::CallbackReturn::FAILURE;
+    }
   }
 
-  poses_navigator_.reset();
-  pose_navigator_.reset();
-
+  navigators_.clear();
   RCLCPP_INFO(get_logger(), "Completed Cleaning up");
   return nav2_util::CallbackReturn::SUCCESS;
 }

nav2_bt_navigator/src/navigators/navigate_through_poses.cpp

@@ -217,3 +217,8 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr
 }
 
 }  // namespace nav2_bt_navigator
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(
+  nav2_bt_navigator::NavigateThroughPosesNavigator,
+  nav2_core::NavigatorBase)

nav2_bt_navigator/src/navigators/navigate_to_pose.cpp

@@ -225,3 +225,8 @@ NavigateToPoseNavigator::onGoalPoseReceived(const geometry_msgs::msg::PoseStampe
 }
 
 }  // namespace nav2_bt_navigator
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(
+  nav2_bt_navigator::NavigateToPoseNavigator,
+  nav2_core::NavigatorBase)