persistent_sequence.cpp

// Copyright (c) 2025 Enjoy Robotics
//
// 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 "nav2_behavior_tree/plugins/control/persistent_sequence.hpp"
#include "behaviortree_cpp/action_node.h"
#include "behaviortree_cpp/bt_factory.h"
 
namespace nav2_behavior_tree
{
 
PersistentSequenceNode::PersistentSequenceNode(
  const std::string & name,
  const BT::NodeConfiguration & conf)
: BT::ControlNode::ControlNode(name, conf) {}
 
BT::NodeStatus PersistentSequenceNode::tick()
{
  const int children_count = children_nodes_.size();
 
  int current_child_idx;
  if (!getInput("current_child_idx", current_child_idx)) {
    throw BT::RuntimeError(
      "Missing required input [current_child_idx] in PersistentSequenceNode. "
      "Set via <Script code=\"current_child_idx := 0\" />");
  }
 
  setStatus(BT::NodeStatus::RUNNING);
 
  while (current_child_idx < children_count) {
    TreeNode * current_child_node = children_nodes_[current_child_idx];
    const BT::NodeStatus child_status = current_child_node->executeTick();
 
    switch (child_status) {
      case BT::NodeStatus::RUNNING:
        return child_status;
 
      case BT::NodeStatus::FAILURE:
        // Reset on failure
        resetChildren();
        current_child_idx = 0;
        setOutput("current_child_idx", 0);
        return child_status;
 
      case BT::NodeStatus::SUCCESS:
      case BT::NodeStatus::SKIPPED:
        // Skip the child node
        current_child_idx++;
        setOutput("current_child_idx", current_child_idx);
        break;
 
      case BT::NodeStatus::IDLE:
        throw std::runtime_error("A child node must never return IDLE");
    }  // end switch
  }  // end while loop
 
  // The entire while loop completed. This means that all the children returned SUCCESS.
  if (current_child_idx >= children_count) {
    resetChildren();
    setOutput("current_child_idx", 0);
  }
  return BT::NodeStatus::SUCCESS;
}
 
}  // namespace nav2_behavior_tree
 
BT_REGISTER_NODES(factory)
{
  BT::NodeBuilder builder =
    [](const std::string & name, const BT::NodeConfiguration & config)
    {
      return std::make_unique<nav2_behavior_tree::PersistentSequenceNode>(
        name, config);
    };
 
  factory.registerBuilder<nav2_behavior_tree::PersistentSequenceNode>(
    "PersistentSequence", builder);
}