behavior_tree_engine.hpp

// 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.
 
#ifndef NAV2_BEHAVIOR_TREE__BEHAVIOR_TREE_ENGINE_HPP_
#define NAV2_BEHAVIOR_TREE__BEHAVIOR_TREE_ENGINE_HPP_
 
#include <memory>
#include <string>
#include <vector>
 
#include "behaviortree_cpp/behavior_tree.h"
#include "behaviortree_cpp/bt_factory.h"
#include "behaviortree_cpp/loggers/groot2_publisher.h"
#include "behaviortree_cpp/xml_parsing.h"
 
#include "nav2_ros_common/lifecycle_node.hpp"
 
namespace nav2_behavior_tree
{
 
enum class BtStatus { SUCCEEDED, FAILED, CANCELED };
 
struct BTInfo
{
  std::string main_id;
  std::vector<std::string> behavior_tree_ids;
};
 
class BehaviorTreeEngine
{
public:
  explicit BehaviorTreeEngine(
    const std::vector<std::string> & plugin_libraries,
    nav2::LifecycleNode::SharedPtr node);
  virtual ~BehaviorTreeEngine() {}
 
  BtStatus run(
    BT::Tree * tree,
    std::function<void()> onLoop,
    std::function<bool()> cancelRequested,
    std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10));
 
  BT::Tree createTreeFromText(
    const std::string & xml_string,
    BT::Blackboard::Ptr blackboard);
 
  BT::Tree createTreeFromFile(
    const std::string & file_path,
    BT::Blackboard::Ptr blackboard);
 
  BTInfo parseTreeInfo(const std::string & filename);
 
  BT::Tree createTree(
    const std::string & tree_id,
    BT::Blackboard::Ptr blackboard);
 
  void addGrootMonitoring(BT::Tree * tree, uint16_t server_port);
 
  void resetGrootMonitor();
 
  void registerTreeFromFile(const std::string & file_path);
 
  void haltAllActions(BT::Tree & tree);
 
protected:
  // The factory that will be used to dynamically construct the behavior tree
  BT::BehaviorTreeFactory factory_;
 
  // Node handle used to obtain clocks at run time
  nav2::LifecycleNode::WeakPtr node_;
 
  // Groot2 monitor
  std::unique_ptr<BT::Groot2Publisher> groot_monitor_;
};
 
}  // namespace nav2_behavior_tree
 
#endif  // NAV2_BEHAVIOR_TREE__BEHAVIOR_TREE_ENGINE_HPP_