add humble-navigation2

2025-05-27 19:03:40 +08:00
parent 974abb5e1e
commit e74ec539c2
1280 changed files with 204114 additions and 0 deletions
@@ -0,0 +1,80 @@
+// 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 <stdexcept>
+#include <sstream>
+#include <string>
+
+#include "nav2_behavior_tree/plugins/control/pipeline_sequence.hpp"
+
+namespace nav2_behavior_tree
+{
+
+PipelineSequence::PipelineSequence(const std::string & name)
+: BT::ControlNode(name, {})
+{
+}
+
+PipelineSequence::PipelineSequence(
+  const std::string & name,
+  const BT::NodeConfiguration & config)
+: BT::ControlNode(name, config)
+{
+}
+
+BT::NodeStatus PipelineSequence::tick()
+{
+  for (std::size_t i = 0; i < children_nodes_.size(); ++i) {
+    auto status = children_nodes_[i]->executeTick();
+    switch (status) {
+      case BT::NodeStatus::FAILURE:
+        ControlNode::haltChildren();
+        last_child_ticked_ = 0;  // reset
+        return status;
+      case BT::NodeStatus::SUCCESS:
+        // do nothing and continue on to the next child. If it is the last child
+        // we'll exit the loop and hit the wrap-up code at the end of the method.
+        break;
+      case BT::NodeStatus::RUNNING:
+        if (i >= last_child_ticked_) {
+          last_child_ticked_ = i;
+          return status;
+        }
+        // else do nothing and continue on to the next child
+        break;
+      default:
+        std::stringstream error_msg;
+        error_msg << "Invalid node status. Received status " << status <<
+          "from child " << children_nodes_[i]->name();
+        throw std::runtime_error(error_msg.str());
+    }
+  }
+  // Wrap up.
+  ControlNode::haltChildren();
+  last_child_ticked_ = 0;  // reset
+  return BT::NodeStatus::SUCCESS;
+}
+
+void PipelineSequence::halt()
+{
+  BT::ControlNode::halt();
+  last_child_ticked_ = 0;
+}
+
+}  // namespace nav2_behavior_tree
+
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::PipelineSequence>("PipelineSequence");
+}
@@ -0,0 +1,129 @@
+// 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_behavior_tree/plugins/control/recovery_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+RecoveryNode::RecoveryNode(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::ControlNode::ControlNode(name, conf),
+  current_child_idx_(0),
+  number_of_retries_(1),
+  retry_count_(0)
+{
+  getInput("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:
+          {
+            // reset node and return success when first child returns success
+            halt();
+            return BT::NodeStatus::SUCCESS;
+          }
+
+        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;
+            }
+          }
+
+        case BT::NodeStatus::RUNNING:
+          {
+            return BT::NodeStatus::RUNNING;
+          }
+
+        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::SUCCESS:
+          {
+            // halt second child, increment recovery count, and tick first child in next iteration
+            ControlNode::haltChild(1);
+            retry_count_++;
+            current_child_idx_--;
+          }
+          break;
+
+        case BT::NodeStatus::FAILURE:
+          {
+            // reset node and return failure if second child fails
+            halt();
+            return BT::NodeStatus::FAILURE;
+          }
+
+        case BT::NodeStatus::RUNNING:
+          {
+            return BT::NodeStatus::RUNNING;
+          }
+
+        default:
+          {
+            throw BT::LogicError("A child node must never return IDLE");
+          }
+      }  // end switch
+    }
+  }  // end while loop
+
+  // reset node and return failure
+  halt();
+  return BT::NodeStatus::FAILURE;
+}
+
+void RecoveryNode::halt()
+{
+  ControlNode::halt();
+  retry_count_ = 0;
+  current_child_idx_ = 0;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::RecoveryNode>("RecoveryNode");
+}
@@ -0,0 +1,93 @@
+// 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_behavior_tree/plugins/control/round_robin_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+RoundRobinNode::RoundRobinNode(const std::string & name)
+: BT::ControlNode::ControlNode(name, {})
+{
+}
+
+RoundRobinNode::RoundRobinNode(
+  const std::string & name,
+  const BT::NodeConfiguration & config)
+: BT::ControlNode(name, config)
+{
+}
+
+BT::NodeStatus RoundRobinNode::tick()
+{
+  const auto num_children = children_nodes_.size();
+
+  setStatus(BT::NodeStatus::RUNNING);
+
+  while (num_failed_children_ < num_children) {
+    TreeNode * child_node = children_nodes_[current_child_idx_];
+    const BT::NodeStatus child_status = child_node->executeTick();
+
+    switch (child_status) {
+      case BT::NodeStatus::SUCCESS:
+        {
+          // Wrap around to the first child
+          if (++current_child_idx_ >= num_children) {
+            current_child_idx_ = 0;
+          }
+          num_failed_children_ = 0;
+          ControlNode::haltChildren();
+          return BT::NodeStatus::SUCCESS;
+        }
+
+      case BT::NodeStatus::FAILURE:
+        {
+          if (++current_child_idx_ >= num_children) {
+            current_child_idx_ = 0;
+          }
+          num_failed_children_++;
+          break;
+        }
+
+      case BT::NodeStatus::RUNNING:
+        {
+          return BT::NodeStatus::RUNNING;
+        }
+
+      default:
+        {
+          throw BT::LogicError("Invalid status return from BT node");
+        }
+    }
+  }
+
+  halt();
+  return BT::NodeStatus::FAILURE;
+}
+
+void RoundRobinNode::halt()
+{
+  ControlNode::halt();
+  current_child_idx_ = 0;
+  num_failed_children_ = 0;
+}
+
+}  // namespace nav2_behavior_tree
+
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::RoundRobinNode>("RoundRobin");
+}