Make RecoveryNode return Running (#4777)

* Add IsStoppedBTNode Signed-off-by: Tony Najjar <[email protected]> * add topic name + reformat Signed-off-by: Tony Najjar <[email protected]> * fix comment Signed-off-by: Tony Najjar <[email protected]> * fix abs Signed-off-by: Tony Najjar <[email protected]> * remove log Signed-off-by: Tony Najjar <[email protected]> * add getter functions for raw twist Signed-off-by: Tony Najjar <[email protected]> * remove unused code Signed-off-by: Tony Najjar <[email protected]> * use odomsmoother Signed-off-by: Tony Najjar <[email protected]> * fix formatting Signed-off-by: Tony Najjar <[email protected]> * update groot Signed-off-by: Tony Najjar <[email protected]> * Add test Signed-off-by: Tony Najjar <[email protected]> * reset at success Signed-off-by: Tony Najjar <[email protected]> * FIX velocity_threshold_ Signed-off-by: Tony Najjar <[email protected]> * Fix stopped Node Signed-off-by: Tony Najjar <[email protected]> * Add tests to odometry_utils Signed-off-by: Tony Najjar <[email protected]> * fix linting Signed-off-by: Tony Najjar <[email protected]> * Make RecoveryNode return RUNNING Signed-off-by: Tony Najjar <[email protected]> * PR review Signed-off-by: Tony Najjar <[email protected]> * add halt at the end Signed-off-by: Tony Najjar <[email protected]> --------- Signed-off-by: Tony Najjar <[email protected]>
ros-navigation · Dec 4, 2024 · 9284c8a · 9284c8a
1 parent 3ee384d
commit 9284c8a
Show file tree

Hide file tree

Showing 2 changed files with 76 additions and 71 deletions.
diff --git a/nav2_behavior_tree/plugins/control/recovery_node.cpp b/nav2_behavior_tree/plugins/control/recovery_node.cpp
@@ -37,83 +37,83 @@ BT::NodeStatus RecoveryNode::tick()
     throw BT::BehaviorTreeException("Recovery Node '" + name() + "' must only have 2 children.");
   }
 
+  if (retry_count_ > number_of_retries_) {
+    halt();
+    return BT::NodeStatus::FAILURE;
+  }
   setStatus(BT::NodeStatus::RUNNING);
 
-  while (current_child_idx_ < children_count && retry_count_ <= number_of_retries_) {
-    TreeNode * child_node = children_nodes_[current_child_idx_];
-    const BT::NodeStatus child_status = child_node->executeTick();
-
-    if (current_child_idx_ == 0) {
-      switch (child_status) {
-        case BT::NodeStatus::SKIPPED:
-          // If first child is skipped, the entire branch is considered skipped
-          halt();
-          return BT::NodeStatus::SKIPPED;
-
-        case BT::NodeStatus::SUCCESS:
-          // reset node and return success when first child returns success
-          halt();
-          return BT::NodeStatus::SUCCESS;
-
-        case BT::NodeStatus::RUNNING:
-          return BT::NodeStatus::RUNNING;
-
-        case BT::NodeStatus::FAILURE:
-          {
-            if (retry_count_ < number_of_retries_) {
-              // halt first child and tick second child in next iteration
-              ControlNode::haltChild(0);
-              current_child_idx_++;
-              break;
-            } else {
-              // reset node and return failure when max retries has been exceeded
-              halt();
-              return BT::NodeStatus::FAILURE;
-            }
-          }
-
-        default:
-          throw BT::LogicError("A child node must never return IDLE");
-      }  // end switch
-
-    } else if (current_child_idx_ == 1) {
-      switch (child_status) {
-        case BT::NodeStatus::SKIPPED:
-          {
-            // if we skip the recovery (maybe a precondition fails), then we
-            // should assume that no recovery is possible. For this reason,
-            // we should return FAILURE and reset the index.
-            // This does not count as a retry.
-            current_child_idx_ = 0;
-            ControlNode::haltChild(1);
+  TreeNode * child_node = children_nodes_[current_child_idx_];
+  const BT::NodeStatus child_status = child_node->executeTick();
+
+  if (current_child_idx_ == 0) {
+    switch (child_status) {
+      case BT::NodeStatus::SKIPPED:
+        // If first child is skipped, the entire branch is considered skipped
+        halt();
+        return BT::NodeStatus::SKIPPED;
+
+      case BT::NodeStatus::SUCCESS:
+        // reset node and return success when first child returns success
+        halt();
+        return BT::NodeStatus::SUCCESS;
+
+      case BT::NodeStatus::RUNNING:
+        return BT::NodeStatus::RUNNING;
+
+      case BT::NodeStatus::FAILURE:
+        {
+          if (retry_count_ < number_of_retries_) {
+            // halt first child and tick second child in next iteration
+            ControlNode::haltChild(0);
+            current_child_idx_ = 1;
+            return BT::NodeStatus::RUNNING;
+          } else {
+            // reset node and return failure when max retries has been exceeded
+            halt();
             return BT::NodeStatus::FAILURE;
           }
-        case BT::NodeStatus::RUNNING:
-          return child_status;
-
-        case BT::NodeStatus::SUCCESS:
-          {
-            // halt second child, increment recovery count, and tick first child in next iteration
-            ControlNode::haltChild(1);
-            retry_count_++;
-            current_child_idx_ = 0;
-          }
-          break;
-
-        case BT::NodeStatus::FAILURE:
-          // reset node and return failure if second child fails
-          halt();
+        }
+
+      default:
+        throw BT::LogicError("A child node must never return IDLE");
+    }  // end switch
+
+  } else if (current_child_idx_ == 1) {
+    switch (child_status) {
+      case BT::NodeStatus::SKIPPED:
+        {
+          // if we skip the recovery (maybe a precondition fails), then we
+          // should assume that no recovery is possible. For this reason,
+          // we should return FAILURE and reset the index.
+          // This does not count as a retry.
+          current_child_idx_ = 0;
+          ControlNode::haltChild(1);
           return BT::NodeStatus::FAILURE;
+        }
+      case BT::NodeStatus::RUNNING:
+        return child_status;
+
+      case BT::NodeStatus::SUCCESS:
+        {
+          // halt second child, increment recovery count, and tick first child in next iteration
+          ControlNode::haltChild(1);
+          retry_count_++;
+          current_child_idx_ = 0;
+          return BT::NodeStatus::RUNNING;
+        }
 
-        default:
-          throw BT::LogicError("A child node must never return IDLE");
-      }  // end switch
-    }
-  }  // end while loop
+      case BT::NodeStatus::FAILURE:
+        // reset node and return failure if second child fails
+        halt();
+        return BT::NodeStatus::FAILURE;
 
-  // reset node and return failure
+      default:
+        throw BT::LogicError("A child node must never return IDLE");
+    }  // end switch
+  }
   halt();
-  return BT::NodeStatus::FAILURE;
+  throw BT::LogicError("A recovery node has exactly 2 children and should never reach here");
 }
 
 void RecoveryNode::halt()

diff --git a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp
@@ -105,6 +105,7 @@ TEST_F(RecoveryNodeTestFixture, test_failure_on_idle_child)
   first_child_->changeStatus(BT::NodeStatus::IDLE);
   EXPECT_THROW(bt_node_->executeTick(), BT::LogicError);
   first_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
   second_child_->changeStatus(BT::NodeStatus::IDLE);
   EXPECT_THROW(bt_node_->executeTick(), BT::LogicError);
 }
@@ -119,8 +120,9 @@ TEST_F(RecoveryNodeTestFixture, test_success_one_retry)
   first_child_->returnSuccessOn(1);
   first_child_->changeStatus(BT::NodeStatus::FAILURE);
   second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
   EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS);
-  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::SUCCESS);
   EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
   EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
 }
@@ -130,15 +132,17 @@ TEST_F(RecoveryNodeTestFixture, test_failure_one_retry)
   // first child fails, second child fails
   first_child_->changeStatus(BT::NodeStatus::FAILURE);
   second_child_->changeStatus(BT::NodeStatus::FAILURE);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
   EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
-  EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE);
   EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
   EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE);
 
   // first child fails, second child succeeds, then first child fails (one retry)
   first_child_->returnFailureOn(1);
   first_child_->changeStatus(BT::NodeStatus::FAILURE);
   second_child_->changeStatus(BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
   EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
   EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE);
   EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);
@@ -158,6 +162,7 @@ TEST_F(RecoveryNodeTestFixture, test_skipping)
   // first child fails, second child skipped
   first_child_->changeStatus(BT::NodeStatus::FAILURE);
   second_child_->changeStatus(BT::NodeStatus::SKIPPED);
+  EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING);
   EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE);
   EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE);
   EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE);