tree_node.h

Go to the documentation of this file.

/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
*  Copyright (C) 2018-2020 Davide Faconti, Eurecat -  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 <condition_variable>
#include <mutex>
#include <map>

#include "behaviortree_cpp_v3/utils/signal.h"
#include "behaviortree_cpp_v3/exceptions.h"
#include "behaviortree_cpp_v3/basic_types.h"
#include "behaviortree_cpp_v3/blackboard.h"
#include "behaviortree_cpp_v3/utils/strcat.hpp"
#include "behaviortree_cpp_v3/utils/wakeup_signal.hpp"

#ifdef _MSC_VER
#pragma warning(disable : 4127)
#endif

namespace BT
{
struct TreeNodeManifest
{
  NodeType type;
  std::string registration_ID;
  PortsList ports;
  std::string description;
};

typedef std::unordered_map<std::string, std::string> PortsRemapping;

struct NodeConfiguration
{
  NodeConfiguration()
  {}

  Blackboard::Ptr blackboard;
  PortsRemapping input_ports;
  PortsRemapping output_ports;
};

class TreeNode
{
public:
  typedef std::shared_ptr<TreeNode> Ptr;

  TreeNode(std::string name, NodeConfiguration config);

  virtual ~TreeNode() = default;

  virtual BT::NodeStatus executeTick();

  virtual void halt() = 0;

  bool isHalted() const;

  NodeStatus status() const;

  const std::string& name() const;

  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 PreTickOverrideCallback =
      std::function<Optional<NodeStatus>(TreeNode&, NodeStatus)>;
  using PostTickOverrideCallback =
      std::function<Optional<NodeStatus>(TreeNode&, NodeStatus, NodeStatus)>;

  StatusChangeSubscriber subscribeToStatusChange(StatusChangeCallback callback);

  void setPreTickOverrideFunction(PreTickOverrideCallback callback);

  void setPostTickOverrideFunction(PostTickOverrideCallback callback);

  // get an unique identifier of this instance of treeNode
  uint16_t UID() const;

  const std::string& registrationName() const;

  const NodeConfiguration& config() const;

  template <typename T>
  Result getInput(const std::string& key, T& destination) const;

  template <typename T>
  Optional<T> getInput(const std::string& key) const
  {
    T out;
    auto res = getInput(key, out);
    return (res) ? Optional<T>(out) : nonstd::make_unexpected(res.error());
  }

  template <typename T>
  Result setOutput(const std::string& key, const T& value);

  // function provide mostrly for debugging purpose to see the raw value
  // in the port (no remapping and no conversion to a type)
  StringView getRawPortValue(const std::string& key) const;

  static bool isBlackboardPointer(StringView str);

  static StringView stripBlackboardPointer(StringView str);

  static Optional<StringView> getRemappedKey(StringView port_name,
                                             StringView remapping_value);

  // Notify the tree should be ticked again()
  void emitStateChanged();

protected:
  virtual BT::NodeStatus tick() = 0;

  friend class BehaviorTreeFactory;
  friend class DecoratorNode;
  friend class ControlNode;
  friend class Tree;

  // 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);

  void resetStatus();

private:
  const std::string name_;

  NodeStatus status_;

  std::condition_variable state_condition_variable_;

  mutable std::mutex state_mutex_;

  StatusChangeSignal state_change_signal_;

  const uint16_t uid_;

  NodeConfiguration config_;

  std::string registration_ID_;

  PreTickOverrideCallback pre_condition_callback_;

  PostTickOverrideCallback post_condition_callback_;

  std::shared_ptr<WakeUpSignal> wake_up_;
};

//-------------------------------------------------------
template <typename T>
inline Result TreeNode::getInput(const std::string& key, T& destination) const
{
  auto remap_it = config_.input_ports.find(key);
  if (remap_it == config_.input_ports.end())
  {
    return nonstd::make_unexpected(StrCat("getInput() failed because "
                                          "NodeConfiguration::input_ports "
                                          "does not contain the key: [",
                                          key, "]"));
  }
  auto remapped_res = getRemappedKey(key, remap_it->second);
  try
  {
    if (!remapped_res)
    {
      destination = convertFromString<T>(remap_it->second);
      return {};
    }
    const auto& remapped_key = remapped_res.value();

    if (!config_.blackboard)
    {
      return nonstd::make_unexpected("getInput() trying to access a Blackboard(BB) "
                                     "entry, "
                                     "but BB is invalid");
    }

    std::unique_lock<std::mutex> entry_lock(config_.blackboard->entryMutex());
    const Any* val = config_.blackboard->getAny(static_cast<std::string>(remapped_key));

    if(!val)
    {
      return nonstd::make_unexpected(StrCat("getInput() failed because it was unable to "
                                     "find the port [", key,
                                     "] remapped to BB [", remapped_key, "]"));
    }

    if(val->empty())
    {
      return nonstd::make_unexpected(StrCat("getInput() failed because the port [", key,
                                            "] remapped to BB [", remapped_key, "] was found,"
                                            "but its content was not initialized correctly"));
    }

    if (!std::is_same<T, std::string>::value && val->type() == typeid(std::string))
    {
      destination = convertFromString<T>(val->cast<std::string>());
    }
    else
    {
      destination = val->cast<T>();
    }
    return {};
  }
  catch (std::exception& err)
  {
    return nonstd::make_unexpected(err.what());
  }
}

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: "
                                          "NodeConfiguration::output_ports "
                                          "does not "
                                          "contain the key: [",
                                          key, "]"));
  }
  StringView remapped_key = remap_it->second;
  if (remapped_key == "=")
  {
    remapped_key = key;
  }
  if (isBlackboardPointer(remapped_key))
  {
    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(NodeConfiguration& config)
{
  for (const auto& it : getProvidedPorts<T>())
  {
    const auto& port_name = it.first;
    const auto direction = it.second.direction();
    if (direction != PortDirection::OUTPUT)
    {
      config.input_ports[port_name] = "=";
    }
    if (direction != PortDirection::INPUT)
    {
      config.output_ports[port_name] = "=";
    }
  }
}

}   // namespace BT