Backport 3345

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -322,6 +322,14 @@ class BtActionNode : public BT::ActionNodeBase
   {
     goal_result_available_ = false;
     auto send_goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions();
+
+    using GoalHandleT = typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr;
+    using FeedbackT = const std::shared_ptr<const typename ActionT::Feedback>;
+
+    send_goal_options.feedback_callback =
+      [this](GoalHandleT, FeedbackT feedback) {
+        config().blackboard->template set<FeedbackT>(action_name_ + "/feedback", feedback);
+    };
     send_goal_options.result_callback =
       [this](const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult & result) {
         if (future_goal_handle_) {

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

@@ -40,7 +40,7 @@ class BtActionServer
   typedef std::function<bool (typename ActionT::Goal::ConstSharedPtr)> OnGoalReceivedCallback;
   typedef std::function<void ()> OnLoopCallback;
   typedef std::function<void (typename ActionT::Goal::ConstSharedPtr)> OnPreemptCallback;
-  typedef std::function<void (typename ActionT::Result::SharedPtr)> OnCompletionCallback;
+  typedef std::function<void (typename ActionT::Result::SharedPtr, const nav2_behavior_tree::BtStatus)> OnCompletionCallback;
 
   /**
    * @brief A constructor for nav2_behavior_tree::BtActionServer class

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -245,7 +245,7 @@ void BtActionServer<ActionT>::executeCallback()
   // Give server an opportunity to populate the result message or simple give
   // an indication that the action is complete.
   auto result = std::make_shared<typename ActionT::Result>();
-  on_completion_callback_(result);
+  on_completion_callback_(result, rc);
 
   switch (rc) {
     case nav2_behavior_tree::BtStatus::SUCCEEDED:

nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml

@@ -55,6 +55,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/bringup/params/nav2_multirobot_params_2.yaml

@@ -55,6 +55,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/bringup/params/nav2_params.yaml

@@ -58,6 +58,11 @@ bt_navigator:
     enable_groot_monitoring: True
     groot_zmq_publisher_port: 1666
     groot_zmq_server_port: 1667
+    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

@@ -16,6 +16,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()
 
@@ -45,12 +46,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}
@@ -63,7 +63,16 @@ ament_target_dependencies(${library_name}
   ${dependencies}
 )
 
-install(TARGETS ${library_name}
+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} nav2_navigate_to_pose_navigator nav2_navigate_through_poses
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -85,6 +94,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,9 @@
 #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 "nav2_msgs/srv/get_string.hpp"
+#include "pluginlib/class_loader.hpp"
 
 namespace nav2_bt_navigator
 {
@@ -52,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
@@ -83,11 +85,13 @@ class BtNavigator : public nav2_util::LifecycleNode
    */
   nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
 
+  void get_navigator_callback(const nav2_msgs::srv::GetString::Request::SharedPtr req,
+    const nav2_msgs::srv::GetString::Response::SharedPtr res);
+
   // 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_;
@@ -98,9 +102,12 @@ 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_;
+
+  // Service to query current navigator
+  rclcpp::Service<nav2_msgs::srv::GetString>::SharedPtr get_navigator_srv_;
 };
 
 }  // namespace nav2_bt_navigator

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
@@ -96,7 +96,9 @@ class NavigateThroughPosesNavigator
    * action result message or indicate that this action is done.
    * @param result Action template result message to populate
    */
-  void goalCompleted(typename ActionT::Result::SharedPtr result) override;
+  void goalCompleted(
+    typename ActionT::Result::SharedPtr result,
+    const nav2_behavior_tree::BtStatus final_bt_status) override;
 
   /**
    * @brief Goal pose initialization on the blackboard

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
@@ -72,7 +72,7 @@ class NavigateToPoseNavigator
    * @brief Get action name for this navigator
    * @return string Name of action server
    */
-  std::string getName() {return std::string("navigate_to_pose");}
+  std::string getName() override {return std::string("navigate_to_pose");}
 
   /**
    * @brief Get navigator's default BT
@@ -107,7 +107,9 @@ class NavigateToPoseNavigator
    * action result message or indicate that this action is done.
    * @param result Action template result message to populate
    */
-  void goalCompleted(typename ActionT::Result::SharedPtr result) override;
+  void goalCompleted(
+    typename ActionT::Result::SharedPtr result,
+    const nav2_behavior_tree::BtStatus final_bt_status) override;
 
   /**
    * @brief Goal pose initialization on the blackboard

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>