Backport ros-navigation#3345 to Galactic

* initial prototype

* linting

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,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
 {
@@ -52,7 +53,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
@@ -84,10 +85,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_;
@@ -98,7 +98,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

@@ -29,7 +29,8 @@ namespace nav2_bt_navigator
 {
 
 BtNavigator::BtNavigator()
-: nav2_util::LifecycleNode("bt_navigator", "", false)
+: nav2_util::LifecycleNode("bt_navigator", "", false),
+ class_loader_("nav2_core", "nav2_core::NavigatorBase")
 {
  RCLCPP_INFO(get_logger(), "Creating");
 
@@ -99,28 +100,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;
@@ -130,9 +159,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
@@ -145,9 +175,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
@@ -165,13 +196,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

@@ -215,3 +215,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

@@ -224,3 +224,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)