tree_node.h

Go to the documentation of this file.

/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
*  Copyright (C) 2018-2023 Davide Faconti -  All Rights Reserved
*
*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
*
*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
 
#pragma once
 
#include <exception>
#include <map>
#include <utility>
 
#include "behaviortree_cpp/utils/signal.h"
#include "behaviortree_cpp/basic_types.h"
#include "behaviortree_cpp/blackboard.h"
#include "behaviortree_cpp/utils/strcat.hpp"
#include "behaviortree_cpp/utils/wakeup_signal.hpp"
#include "behaviortree_cpp/scripting/script_parser.hpp"
 
#ifdef _MSC_VER
#pragma warning(disable : 4127)
#endif
 
namespace BT
{
 
struct TreeNodeManifest
{
  NodeType type;
  std::string registration_ID;
  PortsList ports;
  KeyValueVector metadata;
};
 
using PortsRemapping = std::unordered_map<std::string, std::string>;
using NonPortAttributes = std::unordered_map<std::string, std::string>;
 
enum class PreCond
{
  // order of the enums also tell us the execution order
  FAILURE_IF = 0,
  SUCCESS_IF,
  SKIP_IF,
  WHILE_TRUE,
  COUNT_
};
 
static const std::array<std::string, 4> PreCondNames = {  //
  "_failureIf", "_successIf", "_skipIf", "_while"
};
 
enum class PostCond
{
  // order of the enums also tell us the execution order
  ON_HALTED = 0,
  ON_FAILURE,
  ON_SUCCESS,
  ALWAYS,
  COUNT_
};
 
static const std::array<std::string, 4> PostCondNames = {  //
  "_onHalted", "_onFailure", "_onSuccess", "_post"
};
 
template <>
[[nodiscard]] std::string toStr<BT::PostCond>(const BT::PostCond& cond);
 
template <>
[[nodiscard]] std::string toStr<BT::PreCond>(const BT::PreCond& cond);
 
using ScriptingEnumsRegistry = std::unordered_map<std::string, int>;
 
struct NodeConfig
{
  NodeConfig()
  {}
 
  // Pointer to the blackboard used by this node
  Blackboard::Ptr blackboard;
  // List of enums available for scripting
  std::shared_ptr<ScriptingEnumsRegistry> enums;
  // input ports
  PortsRemapping input_ports;
  // output ports
  PortsRemapping output_ports;
 
  // Any other attributes found in the xml that are not parsed as ports
  // or built-in identifier (e.g. anything with a leading '_')
  NonPortAttributes other_attributes;
 
  const TreeNodeManifest* manifest = nullptr;
 
  // Numberic unique identifier
  uint16_t uid = 0;
  // Unique human-readable name, that encapsulate the subtree
  // hierarchy, for instance, given 2 nested trees, it should be:
  //
  //   main_tree/nested_tree/my_action
  std::string path;
 
  std::map<PreCond, std::string> pre_conditions;
  std::map<PostCond, std::string> post_conditions;
};
 
// back compatibility
using NodeConfiguration = NodeConfig;
 
template <typename T>
inline constexpr bool hasNodeNameCtor()
{
  return std::is_constructible<T, const std::string&>::value;
}
 
template <typename T, typename... ExtraArgs>
inline constexpr bool hasNodeFullCtor()
{
  return std::is_constructible<T, const std::string&, const NodeConfig&,
                               ExtraArgs...>::value;
}
 
class TreeNode
{
public:
  typedef std::shared_ptr<TreeNode> Ptr;
 
  TreeNode(std::string name, NodeConfig config);
 
  TreeNode(const TreeNode& other) = delete;
  TreeNode& operator=(const TreeNode& other) = delete;
 
  TreeNode(TreeNode&& other) noexcept;
  TreeNode& operator=(TreeNode&& other) noexcept;
 
  virtual ~TreeNode();
 
  virtual BT::NodeStatus executeTick();
 
  void haltNode();
 
  [[nodiscard]] bool isHalted() const;
 
  [[nodiscard]] NodeStatus status() const;
 
  [[nodiscard]] const std::string& name() const;
 
  [[nodiscard]] BT::NodeStatus waitValidStatus();
 
  virtual NodeType type() const = 0;
 
  using StatusChangeSignal = Signal<TimePoint, const TreeNode&, NodeStatus, NodeStatus>;
  using StatusChangeSubscriber = StatusChangeSignal::Subscriber;
  using StatusChangeCallback = StatusChangeSignal::CallableFunction;
 
  using PreTickCallback = std::function<NodeStatus(TreeNode&)>;
  using PostTickCallback = std::function<NodeStatus(TreeNode&, NodeStatus)>;
  using TickMonitorCallback =
      std::function<void(TreeNode&, NodeStatus, std::chrono::microseconds)>;
 
  [[nodiscard]] StatusChangeSubscriber
  subscribeToStatusChange(StatusChangeCallback callback);
 
  void setPreTickFunction(PreTickCallback callback);
 
  void setPostTickFunction(PostTickCallback callback);
 
  void setTickMonitorCallback(TickMonitorCallback callback);
 
  [[nodiscard]] uint16_t UID() const;
 
  [[nodiscard]] const std::string& fullPath() const;
 
  [[nodiscard]] const std::string& registrationName() const;
 
  [[nodiscard]] const NodeConfig& config() const;
 
  template <typename T>
  Result getInput(const std::string& key, T& destination) const;
 
  template <typename T>
  [[nodiscard]] Expected<Timestamp> getInputStamped(const std::string& key,
                                                    T& destination) const;
 
  template <typename T>
  [[nodiscard]] Expected<T> getInput(const std::string& key) const
  {
    T out{};
    auto res = getInput(key, out);
    return (res) ? Expected<T>(out) : nonstd::make_unexpected(res.error());
  }
 
  template <typename T>
  [[nodiscard]] Expected<StampedValue<T>> getInputStamped(const std::string& key) const
  {
    StampedValue<T> out;
    if(auto res = getInputStamped(key, out.value))
    {
      out.stamp = *res;
      return out;
    }
    else
    {
      return nonstd::make_unexpected(res.error());
    }
  }
 
  template <typename T>
  Result setOutput(const std::string& key, const T& value);
 
  [[nodiscard]] AnyPtrLocked getLockedPortContent(const std::string& key);
 
  // function provided mostly for debugging purpose to see the raw value
  // in the port (no remapping and no conversion to a type)
  [[nodiscard]] StringView getRawPortValue(const std::string& key) const;
 
  [[nodiscard]] static bool isBlackboardPointer(StringView str,
                                                StringView* stripped_pointer = nullptr);
 
  [[nodiscard]] static StringView stripBlackboardPointer(StringView str);
 
  [[nodiscard]] static Expected<StringView> getRemappedKey(StringView port_name,
                                                           StringView remapped_port);
 
  void emitWakeUpSignal();
 
  [[nodiscard]] bool requiresWakeUp() const;
 
  template <class DerivedT, typename... ExtraArgs>
  static std::unique_ptr<TreeNode> Instantiate(const std::string& name,
                                               const NodeConfig& config,
                                               ExtraArgs... args)
  {
    static_assert(hasNodeFullCtor<DerivedT, ExtraArgs...>() ||
                  hasNodeNameCtor<DerivedT>());
 
    if constexpr(hasNodeFullCtor<DerivedT, ExtraArgs...>())
    {
      return std::make_unique<DerivedT>(name, config, args...);
    }
    else if constexpr(hasNodeNameCtor<DerivedT>())
    {
      auto node_ptr = new DerivedT(name, args...);
      node_ptr->config() = config;
      return std::unique_ptr<DerivedT>(node_ptr);
    }
  }
 
protected:
  friend class BehaviorTreeFactory;
  friend class DecoratorNode;
  friend class ControlNode;
  friend class Tree;
 
  [[nodiscard]] NodeConfig& config();
 
  virtual BT::NodeStatus tick() = 0;
 
  void resetStatus();
 
  // Only BehaviorTreeFactory should call this
  void setRegistrationID(StringView ID);
 
  void setWakeUpInstance(std::shared_ptr<WakeUpSignal> instance);
 
  void modifyPortsRemapping(const PortsRemapping& new_remapping);
 
  void setStatus(NodeStatus new_status);
 
  using PreScripts = std::array<ScriptFunction, size_t(PreCond::COUNT_)>;
  using PostScripts = std::array<ScriptFunction, size_t(PostCond::COUNT_)>;
 
  PreScripts& preConditionsScripts();
  PostScripts& postConditionsScripts();
 
  template <typename T>
  T parseString(const std::string& str) const;
 
private:
  struct PImpl;
  std::unique_ptr<PImpl> _p;
 
  Expected<NodeStatus> checkPreConditions();
  void checkPostConditions(NodeStatus status);
 
  virtual void halt() = 0;
};
 
//-------------------------------------------------------
 
template <typename T>
T TreeNode::parseString(const std::string& str) const
{
  if constexpr(std::is_enum_v<T> && !std::is_same_v<T, NodeStatus>)
  {
    auto it = config().enums->find(str);
    // conversion available
    if(it != config().enums->end())
    {
      return static_cast<T>(it->second);
    }
    else
    {
      // hopefully str contains a number that can be parsed. May throw
      return static_cast<T>(convertFromString<int>(str));
    }
  }
  return convertFromString<T>(str);
}
 
template <typename T>
inline Expected<Timestamp> TreeNode::getInputStamped(const std::string& key,
                                                     T& destination) const
{
  std::string port_value_str;
 
  auto input_port_it = config().input_ports.find(key);
  if(input_port_it != config().input_ports.end())
  {
    port_value_str = input_port_it->second;
  }
  else if(!config().manifest)
  {
    return nonstd::make_unexpected(StrCat("getInput() of node '", fullPath(),
                                          "' failed because the manifest is "
                                          "nullptr (WTF?) and the key: [",
                                          key, "] is missing"));
  }
  else
  {
    // maybe it is declared with a default value in the manifest
    auto port_manifest_it = config().manifest->ports.find(key);
    if(port_manifest_it == config().manifest->ports.end())
    {
      return nonstd::make_unexpected(StrCat("getInput() of node '", fullPath(),
                                            "' failed because the manifest doesn't "
                                            "contain the key: [",
                                            key, "]"));
    }
    const auto& port_info = port_manifest_it->second;
    // there is a default value
    if(port_info.defaultValue().empty())
    {
      return nonstd::make_unexpected(StrCat("getInput() of node '", fullPath(),
                                            "' failed because nor the manifest or the "
                                            "XML contain the key: [",
                                            key, "]"));
    }
    if(port_info.defaultValue().isString())
    {
      port_value_str = port_info.defaultValue().cast<std::string>();
    }
    else
    {
      destination = port_info.defaultValue().cast<T>();
      return Timestamp{};
    }
  }
 
  auto blackboard_ptr = getRemappedKey(key, port_value_str);
  try
  {
    // pure string, not a blackboard key
    if(!blackboard_ptr)
    {
      try
      {
        destination = parseString<T>(port_value_str);
      }
      catch(std::exception& ex)
      {
        return nonstd::make_unexpected(StrCat("getInput(): ", ex.what()));
      }
      return Timestamp{};
    }
    const auto& blackboard_key = blackboard_ptr.value();
 
    if(!config().blackboard)
    {
      return nonstd::make_unexpected("getInput(): trying to access "
                                     "an invalid Blackboard");
    }
 
    if(auto entry = config().blackboard->getEntry(std::string(blackboard_key)))
    {
      std::unique_lock lk(entry->entry_mutex);
      auto& any_value = entry->value;
 
      // support getInput<Any>()
      if constexpr(std::is_same_v<T, Any>)
      {
        destination = any_value;
        return Timestamp{ entry->sequence_id, entry->stamp };
      }
 
      if(!entry->value.empty())
      {
        if(!std::is_same_v<T, std::string> && any_value.isString())
        {
          destination = parseString<T>(any_value.cast<std::string>());
        }
        else
        {
          destination = any_value.cast<T>();
        }
        return Timestamp{ entry->sequence_id, entry->stamp };
      }
    }
 
    return nonstd::make_unexpected(StrCat("getInput() failed because it was unable to "
                                          "find the key [",
                                          key, "] remapped to [", blackboard_key, "]"));
  }
  catch(std::exception& err)
  {
    return nonstd::make_unexpected(err.what());
  }
}
 
template <typename T>
inline Result TreeNode::getInput(const std::string& key, T& destination) const
{
  auto res = getInputStamped(key, destination);
  if(!res)
  {
    return nonstd::make_unexpected(res.error());
  }
  return {};
}
 
template <typename T>
inline Result TreeNode::setOutput(const std::string& key, const T& value)
{
  if(!config().blackboard)
  {
    return nonstd::make_unexpected("setOutput() failed: trying to access a "
                                   "Blackboard(BB) entry, but BB is invalid");
  }
 
  auto remap_it = config().output_ports.find(key);
  if(remap_it == config().output_ports.end())
  {
    return nonstd::make_unexpected(StrCat("setOutput() failed: "
                                          "NodeConfig::output_ports "
                                          "does not contain the key: [",
                                          key, "]"));
  }
  StringView remapped_key = remap_it->second;
  if(remapped_key == "{=}" || remapped_key == "=")
  {
    config().blackboard->set(static_cast<std::string>(key), value);
    return {};
  }
 
  if(!isBlackboardPointer(remapped_key))
  {
    return nonstd::make_unexpected("setOutput requires a blackboard pointer. Use {}");
  }
 
  if constexpr(std::is_same_v<BT::Any, T>)
  {
    auto port_type = config().manifest->ports.at(key).type();
    if(port_type != typeid(BT::Any) && port_type != typeid(BT::AnyTypeAllowed))
    {
      throw LogicError("setOutput<Any> is not allowed, unless the port "
                       "was declared using OutputPort<Any>");
    }
  }
 
  remapped_key = stripBlackboardPointer(remapped_key);
  config().blackboard->set(static_cast<std::string>(remapped_key), value);
 
  return {};
}
 
// Utility function to fill the list of ports using T::providedPorts();
template <typename T>
inline void assignDefaultRemapping(NodeConfig& config)
{
  for(const auto& it : getProvidedPorts<T>())
  {
    const auto& port_name = it.first;
    const auto direction = it.second.direction();
    if(direction != PortDirection::OUTPUT)
    {
      // PortDirection::{INPUT,INOUT}
      config.input_ports[port_name] = "{=}";
    }
    if(direction != PortDirection::INPUT)
    {
      // PortDirection::{OUTPUT,INOUT}
      config.output_ports[port_name] = "{=}";
    }
  }
}
 
}  // namespace BT