From b08160c126b1b84ba11df25bfbd18a1a6f982bcd Mon Sep 17 00:00:00 2001
From: Mohammad Haghighipanah <39755151+mhpanah@users.noreply.github.com>
Date: Tue, 18 Jun 2019 14:48:33 -0700
Subject: [PATCH] Adding RecoveryNode to Behavior Tree (#857)
* added recovery node
---
nav2_bt_navigator/CMakeLists.txt | 1 +
.../navigate_w_replanning_and_recovery.xml | 36 +++---
.../nav2_bt_navigator/recovery_node.hpp | 57 ++++++++++
.../src/navigate_to_pose_behavior_tree.cpp | 4 +
nav2_bt_navigator/src/recovery_node.cpp | 107 ++++++++++++++++++
5 files changed, 185 insertions(+), 20 deletions(-)
create mode 100644 nav2_bt_navigator/include/nav2_bt_navigator/recovery_node.hpp
create mode 100644 nav2_bt_navigator/src/recovery_node.cpp
diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt
index 31fb3c91..5d4fbbcf 100644
--- a/nav2_bt_navigator/CMakeLists.txt
+++ b/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
diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
index 6a4b0bdd..4691193b 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
+++ b/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>
diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/recovery_node.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/recovery_node.hpp
new file mode 100644
index 00000000..76f26eac
--- /dev/null
+++ b/nav2_bt_navigator/include/nav2_bt_navigator/recovery_node.hpp
@@ -0,0 +1,57 @@
+// 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_
diff --git a/nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp b/nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp
index e25fc0c0..5fd5bc9b 100644
--- a/nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp
+++ b/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));
diff --git a/nav2_bt_navigator/src/recovery_node.cpp b/nav2_bt_navigator/src/recovery_node.cpp
new file mode 100644
index 00000000..ae9cec3f
--- /dev/null
+++ b/nav2_bt_navigator/src/recovery_node.cpp
@@ -0,0 +1,107 @@
+// 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
--
GitLab