Performance optimisations for big trees

[pepeRossRobotics]

Hello I am working with a tree made programmatic via a UI.

When the user makes really big trees, the loading of the tree and the execution is quite slow.

Actions that are immediate one after the other take sometimes 9 seconds in between, when the tree is small the action is instant.

When the tree is small it loads quite fast, but when the tree is very big it starts loafing fast then slows down to snail pace. sometimes taking 15 minutes to load. any pointers how to debug this?

[facontidavide]

Hi, I would love to help you debugging this.
It is definitively not expected and I am sure that there must be an easy fix.

Is there any way we can work together? Can I reproduce the problem somehow on my own machine?

If you need to share confidential information, reach me out at dfaconti@aurynrobotics.com

[pepeRossRobotics]

Thanks for offering help, I will try to give as much information here to see if there are some obvious issues.

Background, I am using ROS2 Humble and behaviortree_cpp_v3 that comes with it.
I am running the Behavior Tree (BT) from within an action server, so that the user can dynamically load and cancel trees.

This is the action server execute function

  void execute(const std::shared_ptr<GoalHandleBt> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "Executing goal");
    const auto goal = goal_handle->get_goal();
    auto feedback = std::make_shared<BtAction::Feedback>();
    auto result = std::make_shared<BtAction::Result>();

    if (!std::filesystem::exists(goal->mission_file)) {
      RCLCPP_ERROR(this->get_logger(), "The provided BT path was not valid. Aborting.");
      result->success = false;
      goal_handle->abort(result);
      executing_goal = false;
      return;
    }

    std::string bt_file_path_ = goal->mission_file;

    auto blackboard = Blackboard::create();
    blackboard->set<rclcpp::Node::SharedPtr>(
      "node", std::make_shared<rclcpp::Node>("bt_engine_node"));

    rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr _diagnostics_publisher;
    rclcpp::Publisher<mk4_interfaces::msg::Log>::SharedPtr _log_publisher;

    blackboard->set<rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr>(
      "diagnostics_publisher",
      create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 10));

    blackboard->set<rclcpp::Publisher<mk4_interfaces::msg::Log>::SharedPtr>(
      "log_publisher",
      create_publisher<mk4_interfaces::msg::Log>("/logger", 10));


    factory_.clearRegisteredBehaviorTrees();

    RCLCPP_INFO(this->get_logger(), "Loading trees from folder: %s", bt_file_path_.c_str());
    using std::filesystem::directory_iterator;

    for (auto const & entry : directory_iterator(bt_file_path_.c_str())) {
      if (entry.path().extension() == ".xml") {
        factory_.registerBehaviorTreeFromFile(entry.path().string());
        RCLCPP_INFO(this->get_logger(), "Adding file %s", entry.path().string().c_str());
      }
    }

    RCLCPP_INFO(this->get_logger(), "Loading Main Tree ...");
    auto tree_ = std::make_shared<Tree>(factory_.createTree("MainTree", blackboard));
    RCLCPP_INFO(this->get_logger(), "... tree loaded.");

    auto start_time = rclcpp::Clock().now();
    RCLCPP_INFO(this->get_logger(), "Running Main Tree.");
    while (rclcpp::ok()) {
      if (goal_handle->is_canceling()) {
        goal_handle->canceled(result);
        tree_->haltTree();
        executing_goal = false;
        RCLCPP_INFO(this->get_logger(), "Main Tree execution canceled");
        return;
      }

      feedback->mission_time = rclcpp::Clock().now() - start_time;
      goal_handle->publish_feedback(feedback);
      tree_->tickRoot();
      tree_->sleep(std::chrono::milliseconds(100));
    }

Now, the three is mainly composed of Action server, services and some publishers. There are the templates for each.

The initialisation of each of these are:
SERVICE

BtService(const std::string & name, const BT::NodeConfiguration & config)
  : BT::ActionNodeBase(name, config)
  {
    _server_name = getInput<std::string>("server_name").value();
    _server_timeout = getInput<std::chrono::milliseconds>("server_timeout").value();

    config.blackboard->template get<rclcpp::Node::SharedPtr>("node", _node);
    config.blackboard->template
      get<rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr>(
      "diagnostics_publisher",
      _diagnostics_publisher);
    config.blackboard->template get<rclcpp::Publisher<mk4_interfaces::msg::Log>::SharedPtr>(
      "log_publisher",
      _log_publisher);

    RCLCPP_INFO(_node->get_logger(), "Initialising service: %s", _server_name.c_str());
    _client = _node->create_client<ServiceT>(_server_name);

    publishDiagnostics();
  }

ACTION

BtAction(const std::string & name, const BT::NodeConfiguration & config)
  : BT::ActionNodeBase(name, config)
  {
    _server_name = getInput<std::string>("server_name").value();
    _server_timeout = getInput<std::chrono::milliseconds>("server_timeout").value();
    _cancel_timeout = getInput<std::chrono::milliseconds>("cancel_timeout").value();

    config.blackboard->template get<rclcpp::Node::SharedPtr>("node", _node);
    config.blackboard->template
      get<rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr>(
      "diagnostics_publisher",
      _diagnostics_publisher);
    config.blackboard->template get<rclcpp::Publisher<mk4_interfaces::msg::Log>::SharedPtr>(
      "log_publisher",
      _log_publisher);

    RCLCPP_INFO(_node->get_logger(), "Initialising action: %s", _server_name.c_str());
    _action_client = rclcpp_action::create_client<ActionT>(_node, _server_name);

    publishDiagnostics();

  }

PUBLISHER

  BtPublisher(const std::string & name, const BT::NodeConfiguration & config)
  : BT::ActionNodeBase(name, config)
  {
    _topic_name = getInput<std::string>("topic_name").value();

    config.blackboard->template get<rclcpp::Node::SharedPtr>("node", _node);
    RCLCPP_INFO(_node->get_logger(), "Initialising publisher: %s", _topic_name.c_str());
    auto test = config.blackboard->getKeys();

    _publisher = _node->create_publisher<PublisherT>(_topic_name, 10);
  }

There is a chance that the performance drop is due to the ROS back end, but if there is anything I can do from the BT side I would love to do it as it is more straight forward to do for me.

The tree is quite complex to share it easilly as it is composed of 20+ files for each of the routines. But if needed I can share some parts of it.

Let me know if there is anything else I can provide to help debug.

[facontidavide]

First of all, you should have a look at BehaviorTree.ROS2, where a lot of work went into making these wrappers "right".
I can see that you have extra functionalities (logging/diagnostic), but you should still have a look at them, in particular in the way Executors are used.

Something that I hope people will stop writing is this:

config.blackboard->template get<rclcpp::Node::SharedPtr>("node", _node);

There is an entire tutorial explaining how to avoid doing this in an idiomatic way: Tutorial 8

Secondary, the delay is FOR SURE in the ROS backend, and we need to figure out exactly where.
Just looking at your code, is hard to know.

My suggestion is to try perf/hotspot: https://github.com/KDAB/hotspot
This will give you precise information about where your time is spent.

Operations in the constructors are always serialized in a single thread. If they are slow and you want to run them in parallel, there is a specific design pattern that you can use, implementing an initialize() method.

I can give you mode details, but first we need to find the specific problem.

[pepeRossRobotics]

This is gold, Let me have a look at the links that you sent and will try to implement the wrappers, might be not be right now, but will work towards them for sure. Will also modify how we pass the arguments to the constructors.
Will work on the hotspot and will post the results.

EDIT: I will I found the BehaviorTree.ROS2 sooner, it is quite good!

[pepeRossRobotics]

Sorry if this is too basic, but I am struggling applying the suggested method int the Tutorial 8. My code does the following:
on load, it registers the nodes based on the config file of a launch file using the following function

// Load the BT plugins of the config file
    for (const auto & p : plugins_) {
      RCLCPP_INFO(this->get_logger(), "Loading plugin: %s", SharedLibrary::getOSName(p).c_str());
      factory_.registerFromPlugin(SharedLibrary::getOSName(p));
    }

So when I try to implement the visitor like this (using one of my Nodes from the plugins MuxLockPub) as below

RCLCPP_INFO(this->get_logger(), "Loading trees from folder: %s", bt_file_path_.c_str());
    using std::filesystem::directory_iterator;

    for (auto const & entry : directory_iterator(bt_file_path_.c_str())) {
      if (entry.path().extension() == ".xml") {
        factory_.registerBehaviorTreeFromFile(entry.path().string());
        RCLCPP_INFO(this->get_logger(), "Adding file %s", entry.path().string().c_str());
      }
    }

    RCLCPP_INFO(this->get_logger(), "Loading Main Tree ...");
    auto tree_ = std::make_shared<Tree>(factory_.createTree("MainTree", blackboard));
    RCLCPP_INFO(this->get_logger(), "... tree loaded.");

    // visitor will initialize the instances of
    auto visitor = [](TreeNode *node)
    {
      if (auto action_B_node = dynamic_cast<MuxLockPub*>(node))
      {
        action_B_node->initialize();
      }
      auto node->initialize();
    };

    tree_.applyVisitor(visitor);

it gives the following errors

error: ‘MuxLockPub’ does not name a type
  163 |       if (auto action_B_node = dynamic_cast<MuxLockPub*>(node))
error: ‘class std::shared_ptr<BT::Tree>’ has no member named ‘applyVisitor’
  170 |     tree_.applyVisitor(visitor);

For the first error, I kinda guess that I need to provide the header files for the plugins, but can I do it just with the templates of the plugins?

For the second, I also tried applyRecursiveVisitor but it tells me that the class has no function named like this.

I assume I am just making basic errors due to lack of knowledge, sorry for that

[facontidavide]

Pass extra arguments (Tutorial 8) + Plugins

Check this, to see how that can be done:

https://github.com/BehaviorTree/BehaviorTree.ROS2/blob/humble/include/behaviortree_ros2/plugins.hpp

Other error

• MuxLockPub does not name a type : mising header file?
• has no member named ‘applyVisitor’: I think it should be tree_->applyVisitor(visitor);

[pepeRossRobotics]

Checking the code now, for the second error:
with tree_->applyVisitor(visitor);

error: ‘using element_type = class BT::Tree’ {aka ‘class BT::Tree’} has no member named ‘applyVisitor’
  173 |     tree_->applyVisitor(visitor);

still no luck

[pepeRossRobotics]

I cannot find a function applyVisitor() in https://github.com/BehaviorTree/BehaviorTree.CPP/blob/v3.8/include/behaviortree_cpp_v3/bt_factory.h

[facontidavide]

oh, you did not tell me that you were using the old version 3.8.

I should fix the documentation: https://github.com/BehaviorTree/BehaviorTree.CPP/blob/v3.8/examples/t08_additional_node_args.cpp#L105-L112

[pepeRossRobotics]

Many thanks will try this

[pepeRossRobotics]

So I managed to fix my issue, it had nothing to do with BTs. The issue was how fast and how many publishers, services and action clients I was initialising. The behavior where the loading got progressively slower was the DDS.

I figure this out by adding a ridiculous amount of nodes in the tree to the point of failure. This went to the point where the DDS crashed with this error:

behavior_trees_action_server: ./src/core/ddsc/src/dds_handles.c:430: dds_handle_drop_childref_and_pin: Assertion `(cf & HDL_REFCOUNT_MASK) > 0' failed.

For the record I am using Cyclone DDS

[facontidavide]

important question... do you have EXACTLY the same publisher/ Action Client / Service Client repeated several times?
By "same" I mean talking to the same subscriber/server.

Maybe a good solution, instead of creating one instance per TreeNode, is to use a singletone pattern to use the same instance in multiple Nodes.

Of course, this may have problems if you use a ParallelNodes and call concurrently the unique instance.

Thoughts?

[pepeRossRobotics]

Sounds like a really good idea, I could implement it at many places in the BT, maybe not all of them, but plenty I can think of.
I am self taught programmer so I am not super familiar with the singleton concept, is there any example you can point me to? I can prototype something from that and check how the performance changes.

[facontidavide]

I can not go into too much detail, but the idea that instead of calling this in each node:

_action_client = rclcpp_action::create_client<ActionT>(_node, _server_name);

You should use a static list (shared by all the instance of your Node) with a registry of the existing clients.
Reuse existing ones, if they exist.

static thread_local std::map<std::string, rclcpp_action::Client<ActionT>::SharedPtr> action_clients;
auto client_it = action_clients.find(_server_name);
if( it == action_clients.end())
{
  // create a new one and add it to the map
  _action_client = rclcpp_action::create_client<ActionT>(_node, _server_name);
  action_clients[_server_name] = _action_client;
}
else {
  // use exisitng one
  _action_client = it->second;
}