behavior_tree_engine.cpp

// Copyright (c) 2018 Intel Corporation
// Copyright (c) 2020 Florian Gramss
//
// 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/behavior_tree_engine.hpp"
 
#include <memory>
#include <string>
#include <vector>
#include "tinyxml2.h" //NOLINT
 
#include "rclcpp/logger.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/utilities.hpp"
#include "behaviortree_cpp/json_export.h"
#include "behaviortree_cpp/utils/shared_library.h"
#include "nav2_behavior_tree/json_utils.hpp"
#include "nav2_behavior_tree/utils/loop_rate.hpp"
#include "nav2_ros_common/rate.hpp"
 
namespace nav2_behavior_tree
{
 
BehaviorTreeEngine::BehaviorTreeEngine(
  const std::vector<std::string> & plugin_libraries,
  const nav2::LifecycleNode::SharedPtr node)
{
  BT::SharedLibrary loader;
  for (const auto & p : plugin_libraries) {
    factory_.registerFromPlugin(loader.getOSName(p));
  }
 
  node_ = node;
}
 
BtStatus
BehaviorTreeEngine::run(
  BT::Tree * tree,
  std::function<void()> onLoop,
  std::function<bool()> cancelRequested,
  std::chrono::milliseconds loopTimeout)
{
  auto node = node_.lock();
  if (!node) {
    RCLCPP_ERROR(
      rclcpp::get_logger("BehaviorTreeEngine"),
      "BehaviorTreeEngine node expired. Exiting with failure.");
    return BtStatus::FAILED;
  }
 
  auto rate_clock = nav2::selectSteadyOrSimClock(node);
  nav2_behavior_tree::LoopRate loopRate(loopTimeout, tree, rate_clock);
  BT::NodeStatus result = BT::NodeStatus::RUNNING;
 
  // Loop until something happens with ROS or the node completes
  try {
    while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) {
      if (cancelRequested()) {
        tree->haltTree();
        return BtStatus::CANCELED;
      }
 
      result = tree->tickOnce();
 
      if (result == BT::NodeStatus::RUNNING || result == BT::NodeStatus::IDLE) {
        onLoop();
      }
 
      if (!loopRate.sleep()) {
        RCLCPP_DEBUG_THROTTLE(
          rclcpp::get_logger("BehaviorTreeEngine"),
          *rate_clock, 1000,
          "Behavior Tree tick rate %0.2f was exceeded!",
          1.0 / (loopRate.period().count() * 1.0e-9));
      }
    }
  } catch (const BT::NodeExecutionError & ex) {
    RCLCPP_ERROR(
      rclcpp::get_logger("BehaviorTreeEngine"),
      "BT Exception at Node: [%s] (Path: %s). Original error: %s. Exiting with failure.",
      ex.failedNode().registration_name.c_str(),
      ex.failedNode().node_path.c_str(),
      ex.originalMessage().c_str());
    return BtStatus::FAILED;
  } catch (const std::exception & ex) {
    RCLCPP_ERROR(
      rclcpp::get_logger("BehaviorTreeEngine"),
      "Behavior tree threw exception: %s. Exiting with failure.", ex.what());
    return BtStatus::FAILED;
  }
 
  return (result == BT::NodeStatus::SUCCESS) ? BtStatus::SUCCEEDED : BtStatus::FAILED;
}
 
BT::Tree
BehaviorTreeEngine::createTreeFromText(
  const std::string & xml_string,
  BT::Blackboard::Ptr blackboard)
{
  return factory_.createTreeFromText(xml_string, blackboard);
}
 
BT::Tree
BehaviorTreeEngine::createTreeFromFile(
  const std::string & file_path,
  BT::Blackboard::Ptr blackboard)
{
  return factory_.createTreeFromFile(file_path, blackboard);
}
 
BTInfo BehaviorTreeEngine::parseTreeInfo(const std::string & filename)
{
  BTInfo info;
  if (filename.empty()) {
    RCLCPP_ERROR(rclcpp::get_logger("BehaviorTreeEngine"), "Empty BT file path.");
    return info;
  }
 
  tinyxml2::XMLDocument doc;
  if (doc.LoadFile(filename.c_str()) != tinyxml2::XML_SUCCESS) {
    RCLCPP_ERROR(rclcpp::get_logger("BehaviorTreeEngine"), "Could not parse: %s", filename.c_str());
    return info;
  }
 
  tinyxml2::XMLElement * root = doc.RootElement();
  if (!root) {
    RCLCPP_ERROR(rclcpp::get_logger("BehaviorTreeEngine"), "No root element in: %s",
      filename.c_str());
    return info;
  }
 
  // Loop through all BehaviorTree elements to get all IDs
  for (auto * bt = root->FirstChildElement("BehaviorTree"); bt;
    bt = bt->NextSiblingElement("BehaviorTree"))
  {
    const char * id = bt->Attribute("ID");
    if (id) {
      info.behavior_tree_ids.emplace_back(id);
    }
  }
 
  // First try to get main_tree_to_execute attribute
  const char * main_attr = root->Attribute("main_tree_to_execute");
  if (main_attr) {
    info.main_id = main_attr;
  }
 
  // If main_tree_to_execute attribute is not set, we first check the number of BehaviorTree tags
  if (info.main_id.empty()) {
    // If only one BehaviorTree tag is found, we can use that as the main ID
    // If multiple are found, we throw an error since we don't know
    // which one to use as the main tree
    if (info.behavior_tree_ids.size() == 1) {
      info.main_id = info.behavior_tree_ids[0];
    } else if (info.behavior_tree_ids.size() > 1) {
      throw std::runtime_error(
              "Multiple BehaviorTree elements found in " + filename +
              " but no main_tree_to_execute attribute specified. Unable to determine main tree.");
    }
  }
 
  return info;
}
 
BT::Tree
BehaviorTreeEngine::createTree(
  const std::string & tree_id,
  BT::Blackboard::Ptr blackboard)
{
  return factory_.createTree(tree_id, blackboard);
}
 
void BehaviorTreeEngine::registerTreeFromFile(
  const std::string & file_path)
{
  factory_.registerBehaviorTreeFromFile(file_path);
}
 
void
BehaviorTreeEngine::addGrootMonitoring(
  BT::Tree * tree,
  uint16_t server_port)
{
  // This logger publish status changes using Groot2
  groot_monitor_ = std::make_unique<BT::Groot2Publisher>(*tree, server_port);
 
  // Register common types JSON definitions
  BT::RegisterJsonDefinition<builtin_interfaces::msg::Time>();
  BT::RegisterJsonDefinition<std_msgs::msg::Header>();
}
 
void
BehaviorTreeEngine::resetGrootMonitor()
{
  if (groot_monitor_) {
    groot_monitor_.reset();
  }
}
 
// In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state
void
BehaviorTreeEngine::haltAllActions(BT::Tree & tree)
{
  // this halt signal should propagate through the entire tree.
  tree.haltTree();
}
 
}  // namespace nav2_behavior_tree