From 8c4960e82f928fb35513c40d1ed50044f66083df Mon Sep 17 00:00:00 2001 From: Michael Jeronimo Date: Wed, 7 Aug 2019 16:26:09 -0700 Subject: [PATCH] Reorganize some of the BehaviorTree-related code (#957) * Reorganize some of the BehaviorTree-related code. Eliminate the NavigateToPoseBehaviorTree class, merging it with the BehaviorTreeEngine. Put clear_entirely service client with the others in nav2_util. Move RecoveryNode to nav2_behavior_tree with the other BT nodes. * Remove method signature that isn't implemented yet * Add a missing dependency * Address a couple linter issues --- nav2_behavior_tree/CMakeLists.txt | 5 + .../behavior_tree_engine.hpp | 9 ++ .../nav2_behavior_tree}/recovery_node.hpp | 10 +- nav2_behavior_tree/package.xml | 2 + .../src/behavior_tree_engine.cpp | 77 ++++++++++++- .../src/recovery_node.cpp | 6 +- nav2_bt_navigator/CMakeLists.txt | 2 - .../nav2_bt_navigator/bt_navigator.hpp | 4 +- .../navigate_to_pose_behavior_tree.hpp | 47 -------- nav2_bt_navigator/src/bt_navigator.cpp | 2 +- .../src/navigate_to_pose_behavior_tree.cpp | 107 ------------------ .../clear_entirely_costmap_service_client.hpp | 4 +- 12 files changed, 105 insertions(+), 170 deletions(-) rename {nav2_bt_navigator/include/nav2_bt_navigator => nav2_behavior_tree/include/nav2_behavior_tree}/recovery_node.hpp (89%) rename {nav2_bt_navigator => nav2_behavior_tree}/src/recovery_node.cpp (96%) delete mode 100644 nav2_bt_navigator/include/nav2_bt_navigator/navigate_to_pose_behavior_tree.hpp delete mode 100644 nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp rename {nav2_behavior_tree/include/nav2_behavior_tree => nav2_util/include/nav2_util}/clear_entirely_costmap_service_client.hpp (95%) diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 66dfb847a3..10cffcde0b 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -5,10 +5,12 @@ find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(std_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_msgs REQUIRED) +find_package(nav_msgs REQUIRED) find_package(behaviortree_cpp REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) @@ -27,13 +29,16 @@ set(library_name ${PROJECT_NAME}) add_library(${library_name} SHARED src/behavior_tree_engine.cpp + src/recovery_node.cpp ) set(dependencies rclcpp rclcpp_action + rclcpp_lifecycle geometry_msgs nav2_msgs + nav_msgs behaviortree_cpp tf2 tf2_geometry_msgs diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 92c3e9fe82..650b1e8c97 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -22,6 +22,7 @@ #include "behaviortree_cpp/blackboard/blackboard_local.h" #include "behaviortree_cpp/bt_factory.h" #include "behaviortree_cpp/xml_parsing.h" +#include "nav2_util/global_localization_service_client.hpp" namespace nav2_behavior_tree { @@ -69,6 +70,14 @@ class BehaviorTreeEngine } protected: + // Methods used to register as (simple action) BT nodes + BT::NodeStatus globalLocalizationServiceRequest(); + BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node); + BT::NodeStatus clearEntirelyCostmapServiceRequest(BT::TreeNode & tree_node); + + // Service clients + std::unique_ptr global_localization_client_; + // The factory that will be used to dynamically construct the behavior tree BT::BehaviorTreeFactory factory_; }; diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/recovery_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/recovery_node.hpp similarity index 89% rename from nav2_bt_navigator/include/nav2_bt_navigator/recovery_node.hpp rename to nav2_behavior_tree/include/nav2_behavior_tree/recovery_node.hpp index a8fbfa4bdd..dc49d85c86 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/recovery_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/recovery_node.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_BT_NAVIGATOR__RECOVERY_NODE_HPP_ -#define NAV2_BT_NAVIGATOR__RECOVERY_NODE_HPP_ +#ifndef NAV2_BEHAVIOR_TREE__RECOVERY_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__RECOVERY_NODE_HPP_ #include #include "behaviortree_cpp/control_node.h" -namespace nav2_bt_navigator +namespace nav2_behavior_tree { /** * @brief The RecoveryNode has only two children and returns SUCCESS if and only if the first child @@ -53,6 +53,6 @@ class RecoveryNode : public BT::ControlNode BT::NodeStatus tick() override; void halt() override; }; -} // namespace nav2_bt_navigator +} // namespace nav2_behavior_tree -#endif // NAV2_BT_NAVIGATOR__RECOVERY_NODE_HPP_ +#endif // NAV2_BEHAVIOR_TREE__RECOVERY_NODE_HPP_ diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index bf6df44329..5a0e4622cc 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -22,6 +22,7 @@ rosidl_default_generators geometry_msgs nav2_msgs + nav_msgs tf2 tf2_geometry_msgs std_msgs @@ -39,6 +40,7 @@ rosidl_default_runtime geometry_msgs nav2_msgs + nav_msgs tf2 tf2_geometry_msgs nav2_util diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index b79a4ef861..6ed3d0f5ed 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -18,7 +18,17 @@ #include #include "behaviortree_cpp/blackboard/blackboard_local.h" +#include "nav2_behavior_tree/back_up_action.hpp" #include "nav2_behavior_tree/bt_conversions.hpp" +#include "nav2_behavior_tree/compute_path_to_pose_action.hpp" +#include "nav2_behavior_tree/follow_path_action.hpp" +#include "nav2_behavior_tree/goal_reached_condition.hpp" +#include "nav2_behavior_tree/is_localized_condition.hpp" +#include "nav2_behavior_tree/is_stuck_condition.hpp" +#include "nav2_behavior_tree/rate_controller_node.hpp" +#include "nav2_behavior_tree/recovery_node.hpp" +#include "nav2_behavior_tree/spin_action.hpp" +#include "nav2_util/clear_entirely_costmap_service_client.hpp" #include "rclcpp/rclcpp.hpp" using namespace std::chrono_literals; @@ -28,6 +38,37 @@ namespace nav2_behavior_tree BehaviorTreeEngine::BehaviorTreeEngine() { + // Register our custom action nodes so that they can be included in XML description + factory_.registerNodeType("ComputePathToPose"); + factory_.registerNodeType("FollowPath"); + factory_.registerNodeType("BackUp"); + factory_.registerNodeType("Spin"); + + // Register our custom condition nodes + factory_.registerNodeType("IsStuck"); + factory_.registerNodeType("IsLocalized"); + factory_.registerNodeType("GoalReached"); + + // Register our simple condition nodes + factory_.registerSimpleCondition("initialPoseReceived", + std::bind(&BehaviorTreeEngine::initialPoseReceived, this, std::placeholders::_1)); + + // Register our custom decorator nodes + factory_.registerNodeType("RateController"); + + // Register our custom control nodes + factory_.registerNodeType("RecoveryNode"); + + // Register our simple action nodes + factory_.registerSimpleAction("globalLocalizationServiceRequest", + std::bind(&BehaviorTreeEngine::globalLocalizationServiceRequest, this)); + + factory_.registerSimpleAction("clearEntirelyCostmapServiceRequest", + std::bind(&BehaviorTreeEngine::clearEntirelyCostmapServiceRequest, this, + std::placeholders::_1)); + + global_localization_client_ = + std::make_unique("bt_navigator"); } BtStatus @@ -71,7 +112,7 @@ BehaviorTreeEngine::run( rclcpp::WallRate loopRate(loopTimeout); BT::NodeStatus result = BT::NodeStatus::RUNNING; - // Loop until something happens with ROS or the node completes w/ success or failure + // Loop until something happens with ROS or the node completes while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { if (cancelRequested()) { tree->root_node->halt(); @@ -94,5 +135,39 @@ BehaviorTreeEngine::buildTreeFromText(std::string & xml_string, BT::Blackboard:: return BT::buildTreeFromText(factory_, xml_string, blackboard); } +BT::NodeStatus +BehaviorTreeEngine::globalLocalizationServiceRequest() +{ + auto request = std::make_shared(); + auto response = std::make_shared(); + + auto succeeded = global_localization_client_->invoke(request, response); + return succeeded ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; +} + +BT::NodeStatus +BehaviorTreeEngine::initialPoseReceived(BT::TreeNode & tree_node) +{ + auto initPoseReceived = tree_node.blackboard()->template get("initial_pose_received"); + return initPoseReceived ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; +} + +BT::NodeStatus +BehaviorTreeEngine::clearEntirelyCostmapServiceRequest( + BT::TreeNode & tree_node) +{ + std::string service_name = "/local_costmap/clear_entirely_local_costmap"; + tree_node.getParam("service_name", service_name); + + nav2_util::ClearEntirelyCostmapServiceClient clear_entirely_costmap(service_name); + auto request = std::make_shared(); + try { + clear_entirely_costmap.wait_for_service(std::chrono::seconds(3)); + auto result = clear_entirely_costmap.invoke(request, std::chrono::seconds(3)); + return BT::NodeStatus::SUCCESS; + } catch (std::runtime_error & e) { + return BT::NodeStatus::FAILURE; + } +} } // namespace nav2_behavior_tree diff --git a/nav2_bt_navigator/src/recovery_node.cpp b/nav2_behavior_tree/src/recovery_node.cpp similarity index 96% rename from nav2_bt_navigator/src/recovery_node.cpp rename to nav2_behavior_tree/src/recovery_node.cpp index 2503b49ebc..3e413e1986 100644 --- a/nav2_bt_navigator/src/recovery_node.cpp +++ b/nav2_behavior_tree/src/recovery_node.cpp @@ -13,9 +13,9 @@ // limitations under the License. #include -#include "nav2_bt_navigator/recovery_node.hpp" +#include "nav2_behavior_tree/recovery_node.hpp" -namespace nav2_bt_navigator +namespace nav2_behavior_tree { RecoveryNode::RecoveryNode(const std::string & name, const BT::NodeParameters & params) : BT::ControlNode::ControlNode(name, params), current_child_idx_(0), retry_count_(0) @@ -111,4 +111,4 @@ BT::NodeStatus RecoveryNode::tick() return BT::NodeStatus::FAILURE; } -} // namespace nav2_bt_navigator +} // namespace nav2_behavior_tree diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt index b83620566c..dc21541508 100644 --- a/nav2_bt_navigator/CMakeLists.txt +++ b/nav2_bt_navigator/CMakeLists.txt @@ -31,8 +31,6 @@ set(library_name ${executable_name}_core) add_library(${library_name} SHARED src/bt_navigator.cpp - src/navigate_to_pose_behavior_tree.cpp - src/recovery_node.cpp ) set(dependencies 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 e17f30589e..ee1abeff74 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -20,7 +20,7 @@ #include "behaviortree_cpp/blackboard/blackboard_local.h" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav2_bt_navigator/navigate_to_pose_behavior_tree.hpp" +#include "nav2_behavior_tree/behavior_tree_engine.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_msgs/msg/path.hpp" @@ -73,7 +73,7 @@ class BtNavigator : public nav2_util::LifecycleNode std::string xml_string_; // The wrapper class for the BT functionality - std::unique_ptr bt_; + std::unique_ptr bt_; // The complete behavior tree that results from parsing the incoming XML std::unique_ptr tree_; diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigate_to_pose_behavior_tree.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigate_to_pose_behavior_tree.hpp deleted file mode 100644 index 5df00eaf3c..0000000000 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigate_to_pose_behavior_tree.hpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright (c) 2018 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_BT_NAVIGATOR__NAVIGATE_TO_POSE_BEHAVIOR_TREE_HPP_ -#define NAV2_BT_NAVIGATOR__NAVIGATE_TO_POSE_BEHAVIOR_TREE_HPP_ - -#include - -#include "nav2_util/lifecycle_node.hpp" -#include "nav2_msgs/srv/clear_entire_costmap.hpp" -#include "nav2_behavior_tree/behavior_tree_engine.hpp" -#include "nav2_behavior_tree/clear_entirely_costmap_service_client.hpp" -#include "nav2_util/global_localization_service_client.hpp" -#include "rclcpp/rclcpp.hpp" - -namespace nav2_bt_navigator -{ - -class NavigateToPoseBehaviorTree : public nav2_behavior_tree::BehaviorTreeEngine -{ -public: - NavigateToPoseBehaviorTree(); - -private: - // Methods used to register as (simple action) BT nodes - BT::NodeStatus globalLocalizationServiceRequest(); - BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node); - BT::NodeStatus clearEntirelyCostmapServiceRequest(BT::TreeNode & tree_node); - - // Service clients - std::unique_ptr global_localization_client_; -}; - -} // namespace nav2_bt_navigator - -#endif // NAV2_BT_NAVIGATOR__NAVIGATE_TO_POSE_BEHAVIOR_TREE_HPP_ diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index d17d4b2a10..cbad2e5a1c 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -63,7 +63,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) std::bind(&BtNavigator::navigateToPose, this), false); // Create the class that registers our custom nodes and executes the BT - bt_ = std::make_unique(); + bt_ = std::make_unique(); // Create the path that will be returned from ComputePath and sent to FollowPath goal_ = std::make_shared(); diff --git a/nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp b/nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp deleted file mode 100644 index c1a00e67d2..0000000000 --- a/nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp +++ /dev/null @@ -1,107 +0,0 @@ -// Copyright (c) 2018 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 "nav2_bt_navigator/navigate_to_pose_behavior_tree.hpp" - -#include -#include - -#include "nav2_bt_navigator/recovery_node.hpp" -#include "nav2_behavior_tree/back_up_action.hpp" -#include "nav2_behavior_tree/compute_path_to_pose_action.hpp" -#include "nav2_behavior_tree/follow_path_action.hpp" -#include "nav2_behavior_tree/goal_reached_condition.hpp" -#include "nav2_behavior_tree/is_localized_condition.hpp" -#include "nav2_behavior_tree/is_stuck_condition.hpp" -#include "nav2_behavior_tree/rate_controller_node.hpp" -#include "nav2_behavior_tree/spin_action.hpp" -#include "nav2_behavior_tree/random_crawl_action.hpp" -#include "rclcpp/rclcpp.hpp" - -using namespace std::chrono_literals; - -namespace nav2_bt_navigator -{ - -NavigateToPoseBehaviorTree::NavigateToPoseBehaviorTree() -{ - // Register our custom action nodes so that they can be included in XML description - factory_.registerNodeType("ComputePathToPose"); - factory_.registerNodeType("FollowPath"); - factory_.registerNodeType("BackUp"); - factory_.registerNodeType("Spin"); - factory_.registerNodeType("RandomCrawl"); - - // Register our custom condition nodes - factory_.registerNodeType("IsStuck"); - factory_.registerNodeType("IsLocalized"); - factory_.registerNodeType("GoalReached"); - - // Register our simple condition nodes - factory_.registerSimpleCondition("initialPoseReceived", - std::bind(&NavigateToPoseBehaviorTree::initialPoseReceived, this, std::placeholders::_1)); - - // Register our custom decorator nodes - factory_.registerNodeType("RateController"); - - // Register our custom control nodes - factory_.registerNodeType("RecoveryNode"); - - // Register our simple action nodes - factory_.registerSimpleAction("globalLocalizationServiceRequest", - std::bind(&NavigateToPoseBehaviorTree::globalLocalizationServiceRequest, this)); - - factory_.registerSimpleAction("clearEntirelyCostmapServiceRequest", - std::bind(&NavigateToPoseBehaviorTree::clearEntirelyCostmapServiceRequest, this, - std::placeholders::_1)); - - global_localization_client_ = - std::make_unique("bt_navigator"); -} - -BT::NodeStatus -NavigateToPoseBehaviorTree::globalLocalizationServiceRequest() -{ - auto request = std::make_shared(); - auto response = std::make_shared(); - - auto succeeded = global_localization_client_->invoke(request, response); - return succeeded ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; -} - -BT::NodeStatus -NavigateToPoseBehaviorTree::initialPoseReceived(BT::TreeNode & tree_node) -{ - auto initPoseReceived = tree_node.blackboard()->template get("initial_pose_received"); - return initPoseReceived ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; -} - -BT::NodeStatus NavigateToPoseBehaviorTree::clearEntirelyCostmapServiceRequest( - BT::TreeNode & tree_node) -{ - std::string service_name = "/local_costmap/clear_entirely_local_costmap"; - tree_node.getParam("service_name", service_name); - - nav2_behavior_tree::ClearEntirelyCostmapServiceClient clear_entirely_costmap(service_name); - auto request = std::make_shared(); - try { - clear_entirely_costmap.wait_for_service(std::chrono::seconds(3)); - auto result = clear_entirely_costmap.invoke(request, std::chrono::seconds(3)); - return BT::NodeStatus::SUCCESS; - } catch (std::runtime_error & e) { - return BT::NodeStatus::FAILURE; - } -} - -} // namespace nav2_bt_navigator diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/clear_entirely_costmap_service_client.hpp b/nav2_util/include/nav2_util/clear_entirely_costmap_service_client.hpp similarity index 95% rename from nav2_behavior_tree/include/nav2_behavior_tree/clear_entirely_costmap_service_client.hpp rename to nav2_util/include/nav2_util/clear_entirely_costmap_service_client.hpp index 7b6acd0ff4..62f896fe96 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/clear_entirely_costmap_service_client.hpp +++ b/nav2_util/include/nav2_util/clear_entirely_costmap_service_client.hpp @@ -20,7 +20,7 @@ #include "std_srvs/srv/empty.hpp" #include "nav2_msgs/srv/clear_entire_costmap.hpp" -namespace nav2_behavior_tree +namespace nav2_util { class ClearEntirelyCostmapServiceClient @@ -38,6 +38,6 @@ class ClearEntirelyCostmapServiceClient nav2_util::ServiceClient::ResponseType; }; -} // namespace nav2_behavior_tree +} // namespace nav2_util #endif // NAV2_BEHAVIOR_TREE__CLEAR_ENTIRELY_COSTMAP_SERVICE_CLIENT_HPP_