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,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

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<nav2_util::GlobalLocalizationServiceClient> global_localization_client_;
+
   // The factory that will be used to dynamically construct the behavior tree
   BT::BehaviorTreeFactory factory_;
 };

nav2_bt_navigator/include/nav2_bt_navigator/recovery_node.hpp → 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 <string>
 #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_

nav2_behavior_tree/package.xml

@@ -22,6 +22,7 @@
   <build_depend>rosidl_default_generators</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>nav2_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
   <build_depend>tf2</build_depend>
   <build_depend>tf2_geometry_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
@@ -39,6 +40,7 @@
   <exec_depend>rosidl_default_runtime</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>nav2_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
   <exec_depend>tf2</exec_depend>
   <exec_depend>tf2_geometry_msgs</exec_depend>
   <exec_depend>nav2_util</exec_depend>

nav2_behavior_tree/src/behavior_tree_engine.cpp

@@ -18,7 +18,17 @@
 #include <string>
 
 #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<nav2_behavior_tree::ComputePathToPoseAction>("ComputePathToPose");
+  factory_.registerNodeType<nav2_behavior_tree::FollowPathAction>("FollowPath");
+  factory_.registerNodeType<nav2_behavior_tree::BackUpAction>("BackUp");
+  factory_.registerNodeType<nav2_behavior_tree::SpinAction>("Spin");
+
+  // Register our custom condition nodes
+  factory_.registerNodeType<nav2_behavior_tree::IsStuckCondition>("IsStuck");
+  factory_.registerNodeType<nav2_behavior_tree::IsLocalizedCondition>("IsLocalized");
+  factory_.registerNodeType<nav2_behavior_tree::GoalReachedCondition>("GoalReached");
+
+  // Register our simple condition nodes
+  factory_.registerSimpleCondition("initialPoseReceived",
+    std::bind(&BehaviorTreeEngine::initialPoseReceived, this, std::placeholders::_1));
+
+  // Register our custom decorator nodes
+  factory_.registerNodeType<nav2_behavior_tree::RateController>("RateController");
+
+  // Register our custom control nodes
+  factory_.registerNodeType<nav2_behavior_tree::RecoveryNode>("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<nav2_util::GlobalLocalizationServiceClient>("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<std_srvs::srv::Empty::Request>();
+  auto response = std::make_shared<std_srvs::srv::Empty::Response>();
+
+  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<bool>("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<std::string>("service_name", service_name);
+
+  nav2_util::ClearEntirelyCostmapServiceClient clear_entirely_costmap(service_name);
+  auto request = std::make_shared<nav2_msgs::srv::ClearEntireCostmap::Request>();
+  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

nav2_bt_navigator/src/recovery_node.cpp → nav2_behavior_tree/src/recovery_node.cpp

@@ -13,9 +13,9 @@
 // limitations under the License.
 
 #include <string>
-#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

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

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<NavigateToPoseBehaviorTree> bt_;
+  std::unique_ptr<nav2_behavior_tree::BehaviorTreeEngine> bt_;
 
   // The complete behavior tree that results from parsing the incoming XML
   std::unique_ptr<BT::Tree> tree_;

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<NavigateToPoseBehaviorTree>();
+  bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>();
 
   // Create the path that will be returned from ComputePath and sent to FollowPath
   goal_ = std::make_shared<geometry_msgs::msg::PoseStamped>();