From e7e357dd7d50665da9ec0bdcd060c7fb709f884b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 3 Jan 2023 17:16:12 -0800 Subject: [PATCH] Backport #3345 to Galactic * initial prototype * linting --- .../params/nav2_multirobot_params_1.yaml | 5 + .../params/nav2_multirobot_params_2.yaml | 5 + nav2_bringup/bringup/params/nav2_params.yaml | 5 + nav2_bt_navigator/CMakeLists.txt | 17 ++- .../nav2_bt_navigator/bt_navigator.hpp | 16 +-- .../navigators/navigate_through_poses.hpp | 8 +- .../navigators/navigate_to_pose.hpp | 8 +- nav2_bt_navigator/navigator_plugins.xml | 18 +++ nav2_bt_navigator/package.xml | 3 + nav2_bt_navigator/src/bt_navigator.cpp | 81 ++++++++---- .../src/navigators/navigate_through_poses.cpp | 5 + .../src/navigators/navigate_to_pose.cpp | 5 + nav2_core/CMakeLists.txt | 4 +- nav2_core/README.md | 1 + .../nav2_core/behavior_tree_navigator.hpp | 116 +++++++++++++----- nav2_core/package.xml | 1 + 16 files changed, 220 insertions(+), 78 deletions(-) create mode 100644 nav2_bt_navigator/navigator_plugins.xml rename nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp => nav2_core/include/nav2_core/behavior_tree_navigator.hpp (70%) diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index 371b223a44..522da7b7c3 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/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 diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index 6396005c8e..5556871efc 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/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 diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 6574399703..437e6bef8f 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/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 diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt index df14b1e15a..b5029a5ce7 100644 --- a/nav2_bt_navigator/CMakeLists.txt +++ b/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() diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 2793a9d3d3..fe70735724 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/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> pose_navigator_; - std::unique_ptr> - poses_navigator_; - nav2_bt_navigator::NavigatorMuxer plugin_muxer_; + pluginlib::ClassLoader class_loader_; + std::vector> navigators_; + nav2_core::NavigatorMuxer plugin_muxer_; // Odometry smoother object std::shared_ptr 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 tf_; std::shared_ptr tf_listener_; }; diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index eb0b204515..7acc214259 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/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 + : public nav2_core::BehaviorTreeNavigator { 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 diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp index 00dfeaffdb..c48612f40e 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp +++ b/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 + : public nav2_core::BehaviorTreeNavigator { 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 diff --git a/nav2_bt_navigator/navigator_plugins.xml b/nav2_bt_navigator/navigator_plugins.xml new file mode 100644 index 0000000000..e00a0c2969 --- /dev/null +++ b/nav2_bt_navigator/navigator_plugins.xml @@ -0,0 +1,18 @@ + + + + Navigate to Pose Navigator + + + + + Navigate through Poses Navigator + + + diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 13cd2f6f96..b9db06ab3c 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -22,6 +22,7 @@ geometry_msgs std_srvs nav2_util + pluginlib nav2_core behaviortree_cpp_v3 @@ -34,6 +35,7 @@ std_msgs nav2_util geometry_msgs + pluginlib nav2_core ament_lint_common @@ -41,5 +43,6 @@ ament_cmake + diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index ee0ce1b41b..a414bfe54e 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/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(); - poses_navigator_ = std::make_unique(); - - 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(shared_from_this(), 0.3, odom_topic_); + auto node = shared_from_this(); + odom_smoother_ = std::make_shared(node, 0.3, odom_topic_); + + // Navigator defaults + const std::vector default_navigator_ids = { + "navigate_to_pose", + "navigate_through_poses" + }; + const std::vector 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 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; } diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 462b1da6c5..aca4af926c 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/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) diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index cf1d1970fc..0b17b76559 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/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) diff --git a/nav2_core/CMakeLists.txt b/nav2_core/CMakeLists.txt index 1e72865dd7..988f940864 100644 --- a/nav2_core/CMakeLists.txt +++ b/nav2_core/CMakeLists.txt @@ -9,10 +9,10 @@ find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(pluginlib REQUIRED) -find_package(nav_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(nav2_util REQUIRED) find_package(nav_msgs REQUIRED) +find_package(nav2_behavior_tree REQUIRED) nav2_package() @@ -26,6 +26,8 @@ set(dependencies visualization_msgs nav_msgs tf2_ros + nav2_util + nav2_behavior_tree ) install(DIRECTORY include/ diff --git a/nav2_core/README.md b/nav2_core/README.md index 062fda5a4d..9b9f58a901 100644 --- a/nav2_core/README.md +++ b/nav2_core/README.md @@ -1,6 +1,7 @@ # Nav2 Core This package hosts the abstract interface (virtual base classes) for plugins to be used with the following: +- navigators (e.g., `navigate_to_pose`) - global planner (e.g., `nav2_navfn_planner`) - controller (i.e, path execution controller, e.g `nav2_dwb_controller`) - goal checker diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp similarity index 70% rename from nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp rename to nav2_core/include/nav2_core/behavior_tree_navigator.hpp index f4a4349513..46c1276b8f 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp +++ b/nav2_core/include/nav2_core/behavior_tree_navigator.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Samsung Research America +// Copyright (c) 2021-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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_BT_NAVIGATOR__NAVIGATOR_HPP_ -#define NAV2_BT_NAVIGATOR__NAVIGATOR_HPP_ +#ifndef NAV2_CORE__BEHAVIOR_TREE_NAVIGATOR_HPP_ +#define NAV2_CORE__BEHAVIOR_TREE_NAVIGATOR_HPP_ #include #include @@ -27,7 +27,7 @@ #include "pluginlib/class_loader.hpp" #include "nav2_behavior_tree/bt_action_server.hpp" -namespace nav2_bt_navigator +namespace nav2_core { /** @@ -107,19 +107,65 @@ class NavigatorMuxer }; /** - * @class Navigator + * @class NavigatorBase + * @brief Navigator interface to allow navigators to be stored in a vector and + * accessed via pluginlib due to templates. These functions will be implemented + * by BehaviorTreeNavigator, not the user. The user should implement the virtual + * methods from BehaviorTreeNavigator to implement their navigator action. + */ +class NavigatorBase +{ +public: + NavigatorBase() = default; + ~NavigatorBase() = default; + + /** + * @brief Configuration of the navigator's backend BT and actions + * @return bool If successful + */ + virtual bool on_configure( + rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node, + const std::vector & plugin_lib_names, + const FeedbackUtils & feedback_utils, + nav2_core::NavigatorMuxer * plugin_muxer, + std::shared_ptr odom_smoother) = 0; + + /** + * @brief Activation of the navigator's backend BT and actions + * @return bool If successful + */ + virtual bool on_activate() = 0; + + /** + * @brief Deactivation of the navigator's backend BT and actions + * @return bool If successful + */ + virtual bool on_deactivate() = 0; + + /** + * @brief Cleanup a navigator + * @return bool If successful + */ + virtual bool on_cleanup() = 0; +}; + +/** + * @class BehaviorTreeNavigator * @brief Navigator interface that acts as a base class for all BT-based Navigator action's plugins + * All methods from NavigatorBase are marked as final so they may not be overrided by derived + * methods - instead, users should use the appropriate APIs provided after BT Action handling. */ template -class Navigator +class BehaviorTreeNavigator : public NavigatorBase { public: - using Ptr = std::shared_ptr>; + using Ptr = std::shared_ptr>; /** * @brief A Navigator constructor */ - Navigator() + BehaviorTreeNavigator() + : NavigatorBase() { plugin_muxer_ = nullptr; } @@ -127,7 +173,7 @@ class Navigator /** * @brief Virtual destructor */ - virtual ~Navigator() = default; + virtual ~BehaviorTreeNavigator() = default; /** * @brief Configuration to setup the navigator's backend BT and actions @@ -143,8 +189,8 @@ class Navigator rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node, const std::vector & plugin_lib_names, const FeedbackUtils & feedback_utils, - nav2_bt_navigator::NavigatorMuxer * plugin_muxer, - std::shared_ptr odom_smoother) + nav2_core::NavigatorMuxer * plugin_muxer, + std::shared_ptr odom_smoother) final { auto node = parent_node.lock(); logger_ = node->get_logger(); @@ -161,10 +207,12 @@ class Navigator getName(), plugin_lib_names, default_bt_xml_filename, - std::bind(&Navigator::onGoalReceived, this, std::placeholders::_1), - std::bind(&Navigator::onLoop, this), - std::bind(&Navigator::onPreempt, this, std::placeholders::_1), - std::bind(&Navigator::onCompletion, this, std::placeholders::_1)); + std::bind(&BehaviorTreeNavigator::onGoalReceived, this, std::placeholders::_1), + std::bind(&BehaviorTreeNavigator::onLoop, this), + std::bind(&BehaviorTreeNavigator::onPreempt, this, std::placeholders::_1), + std::bind( + &BehaviorTreeNavigator::onCompletion, this, + std::placeholders::_1, std::placeholders::_2)); bool ok = true; if (!bt_action_server_->on_configure()) { @@ -180,10 +228,10 @@ class Navigator } /** - * @brief Actiation of the navigator's backend BT and actions + * @brief Activation of the navigator's backend BT and actions * @return bool If successful */ - bool on_activate() + bool on_activate() final { bool ok = true; @@ -195,10 +243,10 @@ class Navigator } /** - * @brief Dectiation of the navigator's backend BT and actions + * @brief Deactivation of the navigator's backend BT and actions * @return bool If successful */ - bool on_deactivate() + bool on_deactivate() final { bool ok = true; if (!bt_action_server_->on_deactivate()) { @@ -212,7 +260,7 @@ class Navigator * @brief Cleanup a navigator * @return bool If successful */ - bool on_cleanup() + bool on_cleanup() final { bool ok = true; if (!bt_action_server_->on_cleanup()) { @@ -224,14 +272,14 @@ class Navigator return cleanup() && ok; } + virtual std::string getDefaultBTFilepath(rclcpp_lifecycle::LifecycleNode::WeakPtr node) = 0; + /** * @brief Get the action name of this navigator to expose * @return string Name of action to expose */ virtual std::string getName() = 0; - virtual std::string getDefaultBTFilepath(rclcpp_lifecycle::LifecycleNode::WeakPtr node) = 0; - protected: /** * @brief An intermediate goal reception function to mux navigators. @@ -256,12 +304,14 @@ class Navigator } /** - * @brief An intermediate compution function to mux navigators + * @brief An intermediate completion function to mux navigators */ - void onCompletion(typename ActionT::Result::SharedPtr result) + void onCompletion( + typename ActionT::Result::SharedPtr result, + const nav2_behavior_tree::BtStatus final_bt_status) { plugin_muxer_->stopNavigating(getName()); - goalCompleted(result); + goalCompleted(result, final_bt_status); } /** @@ -283,17 +333,19 @@ class Navigator virtual void onPreempt(typename ActionT::Goal::ConstSharedPtr goal) = 0; /** - * @brief A callback that is called when a the action is completed, can fill in + * @brief A callback that is called when a the action is completed; Can fill in * action result message or indicate that this action is done. */ - virtual void goalCompleted(typename ActionT::Result::SharedPtr result) = 0; + virtual void goalCompleted( + typename ActionT::Result::SharedPtr result, + const nav2_behavior_tree::BtStatus final_bt_status) = 0; /** * @param Method to configure resources. */ virtual bool configure( rclcpp_lifecycle::LifecycleNode::WeakPtr /*node*/, - std::shared_ptr /*odom_smoother*/) + std::shared_ptr/*odom_smoother*/) { return true; } @@ -304,12 +356,12 @@ class Navigator virtual bool cleanup() {return true;} /** - * @brief Method to active and any threads involved in execution. + * @brief Method to activate any threads involved in execution. */ virtual bool activate() {return true;} /** - * @brief Method to deactive and any threads involved in execution. + * @brief Method to deactivate and any threads involved in execution. */ virtual bool deactivate() {return true;} @@ -320,6 +372,6 @@ class Navigator NavigatorMuxer * plugin_muxer_; }; -} // namespace nav2_bt_navigator +} // namespace nav2_core -#endif // NAV2_BT_NAVIGATOR__NAVIGATOR_HPP_ +#endif // NAV2_CORE__BEHAVIOR_TREE_NAVIGATOR_HPP_ diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 1535e89682..981c04f004 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -20,6 +20,7 @@ nav_msgs tf2_ros nav2_util + nav2_behavior_tree ament_lint_common ament_lint_auto