Adding RecoveryNode to Behavior Tree (#857)

* added recovery node

nav2_bt_navigator/CMakeLists.txt

@@ -33,6 +33,7 @@ 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/behavior_trees/navigate_w_replanning_and_recovery.xml

@@ -4,25 +4,21 @@
 -->
 <root main_tree_to_execute="MainTree">
   <BehaviorTree ID="MainTree">
-    <RetryUntilSuccesful name="retry_navigate" num_attempts="6">
-      <Fallback>
-        <Sequence>
-          <RateController hz="1.0">
-            <Fallback>
-              <GoalReached/>
-              <ComputePathToPose goal="${goal}" path="${path}"/>
-            </Fallback>
-          </RateController>
-          <FollowPath path="${path}"/>
-        </Sequence>
-        <ForceFailure>
-          <SequenceStar name="recovery">
-            <clearEntirelyCostmapServiceRequest service_name="/local_costmap/clear_entirely_local_costmap"/>
-            <clearEntirelyCostmapServiceRequest service_name="/global_costmap/clear_entirely_global_costmap"/>
-            <Spin/>
-          </SequenceStar>
-        </ForceFailure>
-      </Fallback>
-    </RetryUntilSuccesful>
+    <RecoveryNode number_of_retries="6">
+      <Sequence name="NavigateWithReplanning">
+        <RateController hz="1.0">
+          <Fallback>
+            <GoalReached/>
+             <ComputePathToPose goal="${goal}" path="${path}"/>
+          </Fallback>
+        </RateController>
+        <FollowPath path="${path}"/>
+      </Sequence>
+      <SequenceStar name="RecoveryActions">
+        <clearEntirelyCostmapServiceRequest service_name="/local_costmap/clear_entirely_local_costmap"/>
+        <clearEntirelyCostmapServiceRequest service_name="/global_costmap/clear_entirely_global_costmap"/>
+        <Spin/>
+      </SequenceStar>
+    </RecoveryNode>
   </BehaviorTree>
 </root>

nav2_bt_navigator/include/nav2_bt_navigator/recovery_node.hpp

+// Copyright (c) 2019 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__RECOVERY_NODE_HPP_
+#define NAV2_BT_NAVIGATOR__RECOVERY_NODE_HPP_
+
+#include <string>
+#include "behaviortree_cpp/control_node.h"
+
+namespace nav2_bt_navigator
+{
+/**
+ * @brief The RecoveryNode has only two children and returns SUCCESS if and only if the first child
+ * returns SUCCESS.
+ *
+ * - If the first child returns FAILURE, the second child will be executed.  After that the first
+ * child is executed again if the second child returns SUCCESS.
+ *
+ * - If the first or second child returns RUNNING, this node returns RUNNING.
+ *
+ * - If the second child returns FAILURE, this control node will stop the loop and returns FAILURE.
+ *
+ */
+class RecoveryNode : public BT::ControlNode
+{
+public:
+  RecoveryNode(const std::string & name, const BT::NodeParameters & params);
+
+  ~RecoveryNode() override = default;
+
+  // Any BT node that accepts parameters must provide a requiredNodeParameters method
+  static const BT::NodeParameters & requiredNodeParameters()
+  {
+    static BT::NodeParameters params = {{"number_of_retries", "1"}};
+    return params;
+  }
+
+private:
+  unsigned int current_child_idx_;
+  unsigned int number_of_retries_;
+  unsigned int retry_count_;
+  BT::NodeStatus tick() override;
+};
+}  // namespace nav2_bt_navigator
+
+#endif  // NAV2_BT_NAVIGATOR__RECOVERY_NODE_HPP_

nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include "nav2_bt_navigator/navigate_to_pose_behavior_tree.hpp"
+#include "nav2_bt_navigator/recovery_node.hpp"
 
 #include <memory>
 #include <string>
@@ -52,6 +53,9 @@ NavigateToPoseBehaviorTree::NavigateToPoseBehaviorTree()
   // Register our custom decorator nodes
   factory_.registerNodeType<nav2_tasks::RateController>("RateController");
 
+  // Register our custom control nodes
+  factory_.registerNodeType<nav2_bt_navigator::RecoveryNode>("RecoveryNode");
+
   // Register our simple action nodes
   factory_.registerSimpleAction("globalLocalizationServiceRequest",
     std::bind(&NavigateToPoseBehaviorTree::globalLocalizationServiceRequest, this));

nav2_bt_navigator/src/recovery_node.cpp

+// Copyright (c) 2019 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 <string>
+#include "nav2_bt_navigator/recovery_node.hpp"
+
+namespace nav2_bt_navigator
+{
+RecoveryNode::RecoveryNode(const std::string & name, const BT::NodeParameters & params)
+: BT::ControlNode::ControlNode(name, params), current_child_idx_(0), retry_count_(0)
+{
+  getParam<unsigned int>("number_of_retries", number_of_retries_);
+}
+
+BT::NodeStatus RecoveryNode::tick()
+{
+  const unsigned children_count = children_nodes_.size();
+
+  if (children_count != 2) {
+    throw BT::BehaviorTreeException("Recovery Node '" + name() + "' must only have 2 children.");
+  }
+
+  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::SUCCESS:
+          {
+            retry_count_ = 0;
+            return BT::NodeStatus::SUCCESS;
+          }
+          break;
+
+        case BT::NodeStatus::FAILURE:
+          {
+            // tick second child
+            if (retry_count_ <= number_of_retries_) {
+              current_child_idx_++;
+              break;
+            } else {
+              haltChildren(0);
+              return BT::NodeStatus::FAILURE;
+            }
+          }
+          break;
+
+        case BT::NodeStatus::RUNNING:
+          {
+            return BT::NodeStatus::RUNNING;
+          }
+          break;
+
+        default:
+          {
+          }
+      }  // end switch
+
+    } else if (current_child_idx_ == 1) {
+      switch (child_status) {
+        case BT::NodeStatus::SUCCESS:
+          {
+            retry_count_++;
+            current_child_idx_--;
+            haltChildren(1);
+          }
+          break;
+
+        case BT::NodeStatus::FAILURE:
+          {
+            current_child_idx_--;
+            retry_count_ = 0;
+            return BT::NodeStatus::FAILURE;
+          }
+          break;
+
+        case BT::NodeStatus::RUNNING:
+          {
+            return BT::NodeStatus::RUNNING;
+          }
+          break;
+
+        default:
+          {
+          }
+      }  // end switch
+    }
+  }  // end while loop
+  retry_count_ = 0;
+  return BT::NodeStatus::FAILURE;
+}
+
+}  // namespace nav2_bt_navigator