"For loop" decorator node

[SteveMacenski]

Hi,

I had a question and a potential feature request. I've been mulling over how to best create a behavior tree to do waypoint following with some task at each waypoint, for instance. For this, I might have a vector of N, unknowably sized, waypoints. I haven't been able to think of a great way to accomplish this without the existence of a "loop"ing mechanism within the behavior tree to trigger a subtree with a bunch of nodes to do its thing over N times.

It seems like a loop control flow node would be the best way to accomplish this, where it takes in some port for the current size of N to trigger and its up to the sub-tree below it to use the current state of N for its purposes.

Is this something that would be a useful node elsewhere to have included in the base BT.CPP library? Any thoughts on this?

[asasine]

Hey @SteveMacenski, we have accomplished something similar using two custom nodes that manage a queue on the blackboard, a custom action node to process the item, and KeepRunningUntilFailure + Sequence to control it all.

The custom action node CreateIntQueue does whatever you need for creating the queue and sets its output port queue with a std::shared_ptr<std::queue<int>>. You can change the templated type of queue to whatever you want such as waypoints. You could have multiple actions that modify the queue as well by pushing/popping items before the main loop.

The custom action node PopIntQueue has an input port queue of std::shared_ptr<std::queue<int>>. If the queue is empty, the tick returns FAILURE. Otherwise, the tick will pop an element, set the output port item to the popped element, and return SUCCESS.

The custom action node ProcessInt has an input port item and processes your item (e.g., controls your motors, calls a ROS action, you name it). It returns success or failure based on the outcome of the action.

By wrapping the inner sequence in KeepRunningUntilFailure, the main loop succeeds once the last element is popped and processed. Unfortunately, we haven't found a great way to discern between when PopIntQueue fails (indicating the queue is empty and we've successfully processed all items) vs. when ProcessInt fails (indicating a real failure to process an item). If you have any ideas in this area, I'd love to hear them.

<Sequence>
  <CreateIntQueue queue="{queue}"/>

  <KeepRunningUntilFailure>
    <Sequence>
      <PopIntQueue queue="{queue}" item="{item}"/>
      <ProcessInt item="{item}"/>
    </Sequence>
  </KeepRunningUntilFailure>
</Sequence>

[facontidavide]

I haven't been able to think of a great way to accomplish this without the existence of a "loop"ing mechanism within the behavior tree to trigger a subtree with a bunch of nodes to do its thing over N times.

So, if I understand you correctly, the difference of this "loop" when compared with the "Repeat" node is that N is somehow "passed" below.

Shall we just add an optional output port to Repeat?

[SteveMacenski]

I was thinking about if you provided a vector<T> if it would pass the next T down to the subtree below it for all T's in the vector. Though thinking about that now seems alot less reasonable than what you suggest and then just putting a node in the subtree to take i from the repeat node and dish it out itself for whatever the specific type is.

[emrecanbulut]

@SteveMacenski I needed this feature as well. I came up with a custom decorator for this purpose as shown below (with its own syntax for parsing the range input)

This is my tree.xml:

<?xml version="1.0"?>
<root main_tree_to_execute="main_tree">
    <!-- ////////// -->
    <BehaviorTree ID="main_tree">
        <Decorator ID="CustomRangeLoop" current_iteration="current_iteration" range="0-4" step="1">
            <Action ID="SomeActionNode" array_index="{current_iteration}"/>
        </Decorator>
    </BehaviorTree>
    <!-- ////////// -->
    <TreeNodesModel>
        <Decorator ID="CustomRangeLoop">
            <output_port default="current_iteration" name="current_iteration"/>
            <input_port name="range"/>
            <input_port default="1" name="step">The increment at each iteration</input_port>
        </Decorator>
        <Action ID="SomeActionNode">
            <input_port name="array_index"/>
        </Action>
    </TreeNodesModel>
    <!-- ////////// -->
</root>

And I read the ports at the CustomRangeLoop constructor, as they are not supposed to change (except for the current_iteration which is changed at each tick() of the Decorator):

CustomRangeLoop::CustomRangeLoop(const std::string& name, const BT::NodeConfiguration& config):
DecoratorNode(name, config)
{
  std::string port_key_1 = "range";
  auto input_1 = getInput<std::string>(port_key_1);
  if (!input_1)
  {
    throw BT::RuntimeError("missing required input for "+this->name()+" "+ port_key_1 +": ", input_1.error() );
  }

  auto range_tuple = this->getRange(input_1.value(), '-');
  
  this->_start = std::get<0>(range_tuple);
  this->_end = std::get<1>(range_tuple);

  std::string port_key_2 = "step";
  auto input_2 = getInput<std::string>(port_key_2);
  if (!input_2)
  {
    throw BT::RuntimeError("missing required input for "+this->name()+" "+ port_key_2 +": ", input_2.error() );
  }

  this->_step = std::stoi(input_2.value());

  if( (_end > _start && _step < 0) || (_end < _start && _step > 0))
  {
    throw BT::RuntimeError("Ill-defined CustomRangeLoop. Please check your inputs." );
  }

  std::cout << "CustomRangeLoop " << this->name() << "," << port_key_1 <<":" << input_1.value() 
                                                  << "," << port_key_2 <<":" << input_2.value() << std::endl;
}

[emrecanbulut]

Hi, After refining the CustomRangeLoop a little bit, I would like to share our temporary solution which is a bit more generic than before:

customrangeloop.h

/**
 * The decorator node providing a loop functionality with an output to track
 * the loop's current iteration.
 */
class CustomRangeLoop : public BT::DecoratorNode {
 public:
  using DecoratorNode::DecoratorNode;

  static BT::PortsList providedPorts();

  BT::NodeStatus tick() override;

 private:
  int start_{0};
  int stop_{0};
  int step_{1};
};

// The input ports in our scenario are mostly mandatory ports. So, this part has nothing to do with 
// the CustomRangeLoop. I think this would also be a very good feature to add to the BT.
template <class T>
inline T getInputValue(const BT::TreeNode& node, const std::string& key) {
  T value{};
  auto result = node.getInput(key, value);
  if (!result) {
    throw BT::RuntimeError(
        node.name() + ": missing required input [" + key + "]: ",
        result.error());
  }
  return value;
}

customrangeloop.cpp

BT::PortsList CustomRangeLoop::providedPorts() {
  return {BT::InputPort<int>("start"), BT::InputPort<int>("stop"),
          BT::InputPort<int>("step"), BT::OutputPort<int>("current")};
}

BT::NodeStatus CustomRangeLoop::tick() {
  if (status() == BT::NodeStatus::IDLE) {
    start_ = getInputValue<int>(*this, "start");
    stop_ = getInputValue<int>(*this, "stop");
    step_ = getInputValue<int>(*this, "step");

    if (step_ == 0) {
      throw BT::RuntimeError(name() + ": [step] cannot be zero");
    }
  }

  this->setStatus(BT::NodeStatus::RUNNING);

  for (; step_ >= 0 ? (start_ < stop_) : (start_ > stop_); start_ += step_) {
    setOutput("current", start_);

    switch (child()->executeTick()) {
      case BT::NodeStatus::SUCCESS:
        haltChild();
        break;
      case BT::NodeStatus::FAILURE:
        haltChild();
        return BT::NodeStatus::FAILURE;
      case BT::NodeStatus::RUNNING:
        return BT::NodeStatus::RUNNING;
      default:
        throw BT::LogicError("A child node must never return IDLE");
    }
  }

  return BT::NodeStatus::SUCCESS;
}

I share this as an intermediate solution until it is built-in to BT.

Cheers

[facontidavide]

There is now a Loop node and specific example