(WIP) Recoveries always return success, regardless if they're able to complete their required tasks or not

nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp

@@ -32,9 +32,19 @@ class BackUpActionServer : public TestActionServer<nav2_msgs::action::BackUp>
 
 protected:
   void execute(
-    const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::BackUp>>)
+    const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::BackUp>>
+    goal_handle)
   override
-  {}
+  {
+    nav2_msgs::action::BackUp::Result::SharedPtr result =
+      std::make_shared<nav2_msgs::action::BackUp::Result>();
+    bool return_success = getReturnSuccess();
+    if (return_success) {
+      goal_handle->succeed(result);
+    } else {
+      goal_handle->abort(result);
+    }
+  }
 };
 
 class BackUpActionTestFixture : public ::testing::Test
@@ -154,6 +164,34 @@ TEST_F(BackUpActionTestFixture, test_tick)
   EXPECT_EQ(goal->speed, 0.26f);
 }
 
+TEST_F(BackUpActionTestFixture, test_failure)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <BackUp backup_dist="2" backup_speed="0.26" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  action_server_->setReturnSuccess(false);
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0);
+
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
+    tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
+  {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
+
+  auto goal = action_server_->getCurrentGoal();
+  EXPECT_EQ(goal->target.x, 2.0);
+  EXPECT_EQ(goal->speed, 0.26f);
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

nav2_behavior_tree/test/plugins/action/test_spin_action.cpp

@@ -32,9 +32,19 @@ class SpinActionServer : public TestActionServer<nav2_msgs::action::Spin>
 
 protected:
   void execute(
-    const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin>>)
+    const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin>>
+    goal_handle)
   override
-  {}
+  {
+    nav2_msgs::action::Spin::Result::SharedPtr result =
+      std::make_shared<nav2_msgs::action::Spin::Result>();
+    bool return_success = getReturnSuccess();
+    if (return_success) {
+      goal_handle->succeed(result);
+    } else {
+      goal_handle->abort(result);
+    }
+  }
 };
 
 class SpinActionTestFixture : public ::testing::Test
@@ -149,6 +159,33 @@ TEST_F(SpinActionTestFixture, test_tick)
   EXPECT_EQ(action_server_->getCurrentGoal()->target_yaw, 3.14f);
 }
 
+TEST_F(SpinActionTestFixture, test_failure)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <Spin spin_dist="3.14" />
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  action_server_->setReturnSuccess(false);
+
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0);
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
+    tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
+  {
+    tree_->rootNode()->executeTick();
+  }
+
+  std::cout << tree_->rootNode()->status();
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE);
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
+  EXPECT_EQ(action_server_->getCurrentGoal()->target_yaw, 3.14f);
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

nav2_behavior_tree/test/test_action_server.hpp

@@ -48,6 +48,16 @@ class TestActionServer : public rclcpp::Node
     return current_goal_;
   }
 
+  void setReturnSuccess(bool return_success)
+  {
+    return_success_ = return_success;
+  }
+
+  bool getReturnSuccess(void)
+  {
+    return return_success_;
+  }
+
 protected:
   virtual rclcpp_action::GoalResponse handle_goal(
     const rclcpp_action::GoalUUID &,
@@ -77,6 +87,7 @@ class TestActionServer : public rclcpp::Node
 private:
   typename rclcpp_action::Server<ActionT>::SharedPtr action_server_;
   std::shared_ptr<const typename ActionT::Goal> current_goal_;
+  bool return_success_ = true;
 };
 
 #endif  // TEST_ACTION_SERVER_HPP_

nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml

@@ -20,12 +20,15 @@
       </PipelineSequence>
       <ReactiveFallback name="RecoveryFallback">
         <GoalUpdated/>
-        <SequenceStar name="RecoveryActions">
-          <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
-          <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
+        <RoundRobin name="RecoveryActions">
+          <Sequence name="ClearingActions">
+            <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
+            <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
+          </Sequence>
           <Spin spin_dist="1.57"/>
           <Wait wait_duration="5"/>
-        </SequenceStar>
+          <BackUp backup_dist="0.15" backup_speed="0.025"/>
+        </RoundRobin>
       </ReactiveFallback>
     </RecoveryNode>
   </BehaviorTree>

nav2_recoveries/plugins/back_up.cpp

@@ -108,7 +108,7 @@ Status BackUp::onCycleUpdate()
   if (!isCollisionFree(distance, cmd_vel.get(), pose2d)) {
     stopRobot();
     RCLCPP_WARN(logger_, "Collision Ahead - Exiting BackUp");
-    return Status::SUCCEEDED;
+    return Status::FAILED;
   }
 
   vel_pub_->publish(std::move(cmd_vel));

nav2_recoveries/plugins/spin.cpp

@@ -119,9 +119,13 @@ Status Spin::onCycleUpdate()
   action_server_->publish_feedback(feedback_);
 
   double remaining_yaw = abs(cmd_yaw_) - abs(relative_yaw_);
-  if (remaining_yaw <= 0) {
+  if (remaining_yaw < 1e-6) {
     stopRobot();
     return Status::SUCCEEDED;
+  } else {
+    RCLCPP_INFO(
+      logger_, "Remaining Yaw: %lf",
+      remaining_yaw);
   }
 
   double vel = sqrt(2 * rotational_acc_lim_ * remaining_yaw);
@@ -138,7 +142,7 @@ Status Spin::onCycleUpdate()
   if (!isCollisionFree(relative_yaw_, cmd_vel.get(), pose2d)) {
     stopRobot();
     RCLCPP_WARN(logger_, "Collision Ahead - Exiting Spin");
-    return Status::SUCCEEDED;
+    return Status::FAILED;
   }
 
   vel_pub_->publish(std::move(cmd_vel));

nav2_system_tests/CMakeLists.txt

@@ -7,6 +7,7 @@ find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(nav2_map_server REQUIRED)
+find_package(nav2_behavior_tree REQUIRED)
 find_package(nav2_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
@@ -21,6 +22,7 @@ find_package(nav2_navfn_planner REQUIRED)
 find_package(nav2_planner REQUIRED)
 find_package(navigation2)
 find_package(angles REQUIRED)
+find_package(behaviortree_cpp_v3 REQUIRED)
 
 nav2_package()
 
@@ -33,6 +35,7 @@ set(dependencies
   visualization_msgs
   nav2_amcl
   nav2_lifecycle_manager
+  nav2_behavior_tree
   gazebo_ros_pkgs
   geometry_msgs
   std_msgs
@@ -41,6 +44,7 @@ set(dependencies
   nav2_planner
   nav2_navfn_planner
   angles
+  behaviortree_cpp_v3
 )
 
 if(BUILD_TESTING)
@@ -50,6 +54,7 @@ if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_cmake_pytest REQUIRED)
 
+  add_subdirectory(src/behavior_tree)
   add_subdirectory(src/planning)
   add_subdirectory(src/localization)
   add_subdirectory(src/system)

nav2_system_tests/package.xml

@@ -17,6 +17,7 @@
   <build_depend>nav2_msgs</build_depend>
   <build_depend>nav2_lifecycle_manager</build_depend>
   <build_depend>nav2_navfn_planner</build_depend>
+  <build_depend>nav2_behavior_tree</build_depend>
   <build_depend>nav_msgs</build_depend>
   <build_depend>visualization_msgs</build_depend>
   <build_depend>nav2_amcl</build_depend>
@@ -40,6 +41,7 @@
   <exec_depend>nav2_msgs</exec_depend>
   <exec_depend>nav2_lifecycle_manager</exec_depend>
   <exec_depend>nav2_navfn_planner</exec_depend>
+  <exec_depend>nav2_behavior_tree</exec_depend>
   <exec_depend>nav_msgs</exec_depend>
   <exec_depend>visualization_msgs</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>

nav2_system_tests/src/behavior_tree/CMakeLists.txt

@@ -0,0 +1,8 @@
+ament_add_gtest(test_behavior_tree_node
+  test_behavior_tree_node.cpp
+  server_handler.cpp
+)
+
+ament_target_dependencies(test_behavior_tree_node
+  ${dependencies}
+)