blackboard.h

#pragma once
 
#include "behaviortree_cpp/basic_types.h"
#ifdef BTCPP_SYSTEM_JSON
#include <nlohmann/json.hpp>
#else
#include "behaviortree_cpp/contrib/json.hpp"
#endif
#include "behaviortree_cpp/exceptions.h"
#include "behaviortree_cpp/utils/locked_reference.hpp"
#include "behaviortree_cpp/utils/polymorphic_cast_registry.hpp"
#include "behaviortree_cpp/utils/safe_any.hpp"
 
#include <memory>
#include <mutex>
#include <shared_mutex>
#include <string>
#include <unordered_map>
 
namespace BT
{
 
/// This type contains a pointer to Any, protected
/// with a locked mutex as long as the object is in scope
using AnyPtrLocked = LockedPtr<Any>;
 
template <typename T>
struct StampedValue
{
  T value;
  Timestamp stamp;
};
 
/**
 * @brief The Blackboard is the mechanism used by BehaviorTrees to exchange
 * typed data.
 */
class Blackboard
{
public:
  using Ptr = std::shared_ptr<Blackboard>;
 
protected:
  // This is intentionally protected. Use Blackboard::create instead
  Blackboard(Blackboard::Ptr parent) : parent_bb_(parent)
  {}
 
public:
  Blackboard(const Blackboard&) = delete;
  Blackboard& operator=(const Blackboard&) = delete;
  Blackboard(Blackboard&&) = delete;
  Blackboard& operator=(Blackboard&&) = delete;
 
  struct Entry
  {
    Any value;
    TypeInfo info;
    StringConverter string_converter;
    mutable std::mutex entry_mutex;
 
    uint64_t sequence_id = 0;
    // timestamp since epoch
    std::chrono::nanoseconds stamp = std::chrono::nanoseconds{ 0 };
 
    Entry(const TypeInfo& _info) : info(_info)
    {}
 
    ~Entry() = default;
    Entry(const Entry&) = delete;
    Entry& operator=(const Entry&) = delete;
    Entry(Entry&&) = delete;
    Entry& operator=(Entry&&) = delete;
  };
 
  /** Use this static method to create an instance of the BlackBoard
    *   to share among all your NodeTrees.
    */
  static Blackboard::Ptr create(Blackboard::Ptr parent = {})
  {
    return std::shared_ptr<Blackboard>(new Blackboard(parent));
  }
 
  virtual ~Blackboard() = default;
 
  void enableAutoRemapping(bool remapping);
 
  [[nodiscard]] const std::shared_ptr<Entry> getEntry(const std::string& key) const;
 
  [[nodiscard]] std::shared_ptr<Blackboard::Entry> getEntry(const std::string& key);
 
  [[nodiscard]] AnyPtrLocked getAnyLocked(const std::string& key);
 
  [[nodiscard]] AnyPtrLocked getAnyLocked(const std::string& key) const;
 
  [[deprecated("Use getAnyLocked instead")]] const Any*
  getAny(const std::string& key) const;
 
  [[deprecated("Use getAnyLocked instead")]] Any* getAny(const std::string& key);
 
  /** Return true if the entry with the given key was found.
   *  Note that this method may throw an exception if the cast to T failed.
   */
  template <typename T>
  [[nodiscard]] bool get(const std::string& key, T& value) const;
 
  template <typename T>
  [[nodiscard]] Expected<Timestamp> getStamped(const std::string& key, T& value) const;
 
  /**
   * Version of get() that throws if it fails.
   */
  template <typename T>
  [[nodiscard]] T get(const std::string& key) const;
 
  template <typename T>
  [[nodiscard]] Expected<StampedValue<T>> getStamped(const std::string& key) const;
 
  /// Update the entry with the given key
  template <typename T>
  void set(const std::string& key, const T& value);
 
  void unset(const std::string& key);
 
  [[nodiscard]] const TypeInfo* entryInfo(const std::string& key);
 
  void addSubtreeRemapping(StringView internal, StringView external);
 
  void debugMessage() const;
 
  /// The names of all the entries stored in this blackboard (copies).
  [[nodiscard]] std::vector<std::string> getKeyNames() const;
 
  // The views point into the storage: they dangle as soon as another thread
  // removes the entry.
  [[deprecated(
      "The views may dangle: use getKeyNames")]] [[nodiscard]] std::vector<StringView>
  getKeys() const;
 
  [[deprecated("This command is unsafe. Consider using Backup/Restore instead")]] void
  clear();
 
  void createEntry(const std::string& key, const TypeInfo& info);
 
  /**
   * @brief cloneInto copies the values of the entries
   * into another blackboard.  Known limitations:
   *
   * - it doesn't update the remapping in dst
   * - it doesn't change the parent blackboard os dst
   *
   * @param dst destination, i.e. blackboard to be updated
   */
  void cloneInto(Blackboard& dst) const;
 
  Blackboard::Ptr parent();
 
  // recursively look for parent Blackboard, until you find the root
  Blackboard* rootBlackboard();
 
  const Blackboard* rootBlackboard() const;
 
  /**
   * @brief Set the polymorphic cast registry for this blackboard.
   *
   * The registry enables polymorphic shared_ptr conversions during get().
   * This is typically set automatically when creating trees via BehaviorTreeFactory.
   */
  void setPolymorphicCastRegistry(std::shared_ptr<PolymorphicCastRegistry> registry)
  {
    polymorphic_registry_ = std::move(registry);
  }
 
  /**
   * @brief Get the polymorphic cast registry (may be null).
   */
  [[nodiscard]] const PolymorphicCastRegistry* polymorphicCastRegistry() const
  {
    return polymorphic_registry_.get();
  }
 
  /**
   * @brief Cast Any value with polymorphic fallback for shared_ptr types.
   *
   * First attempts a direct cast. If that fails and T is a shared_ptr type,
   * tries a polymorphic cast via the registry. Returns Expected with error on failure.
   */
  template <typename T>
  [[nodiscard]] Expected<T> tryCastWithPolymorphicFallback(const Any* any) const;
 
private:
  mutable std::shared_mutex storage_mutex_;
  std::unordered_map<std::string, std::shared_ptr<Entry>> storage_;
  std::weak_ptr<Blackboard> parent_bb_;
  std::unordered_map<std::string, std::string> internal_to_external_;
 
  std::shared_ptr<Entry> createEntryImpl(const std::string& key, const TypeInfo& info);
 
  bool autoremapping_ = false;
 
  // Optional registry for polymorphic shared_ptr conversions
  std::shared_ptr<PolymorphicCastRegistry> polymorphic_registry_;
};
 
/**
 * @brief ExportBlackboardToJSON will create a JSON
 * that contains the current values of the blackboard.
 * Complex types must be registered with JsonExporter::get()
 */
nlohmann::json ExportBlackboardToJSON(const Blackboard& blackboard);
 
/**
 * @brief ImportBlackboardFromJSON will append elements to the blackboard,
 * using the values parsed from the JSON file created using ExportBlackboardToJSON.
 * Complex types must be registered with JsonExporter::get()
 */
void ImportBlackboardFromJSON(const nlohmann::json& json, Blackboard& blackboard);
 
//------------------------------------------------------
 
template <typename T>
inline Expected<T> Blackboard::tryCastWithPolymorphicFallback(const Any* any) const
{
  // Try direct cast first
  auto result = any->tryCast<T>();
  if(result)
  {
    return result.value();
  }
 
  // For shared_ptr types, try polymorphic cast via registry (Issue #943)
  if constexpr(is_shared_ptr<T>::value)
  {
    if(polymorphic_registry_)
    {
      auto poly_result = any->tryCastWithRegistry<T>(*polymorphic_registry_);
      if(poly_result)
      {
        return poly_result.value();
      }
    }
  }
 
  return nonstd::make_unexpected(result.error());
}
 
template <typename T>
inline T Blackboard::get(const std::string& key) const
{
  if(auto any_ref = getAnyLocked(key))
  {
    const auto& any = any_ref.get();
    if(any->empty())
    {
      throw RuntimeError("Blackboard::get() error. Entry [", key,
                         "] hasn't been initialized, yet");
    }
    auto result = tryCastWithPolymorphicFallback<T>(any);
    if(!result)
    {
      throw std::runtime_error(result.error());
    }
    return result.value();
  }
  throw RuntimeError("Blackboard::get() error. Missing key [", key, "]");
}
 
inline void Blackboard::unset(const std::string& key)
{
  // the entry (i.e. the stored value) is destroyed outside the lock
  std::shared_ptr<Entry> removed;
  std::unique_lock storage_lock(storage_mutex_);
 
  // check local storage
  auto it = storage_.find(key);
  if(it == storage_.end())
  {
    // No entry, nothing to do.
    return;
  }
 
  removed = std::move(it->second);
  storage_.erase(it);
}
 
template <typename T>
inline void Blackboard::set(const std::string& key, const T& value)
{
  if(StartWith(key, '@'))
  {
    rootBlackboard()->set(key.substr(1, key.size() - 1), value);
    return;
  }
  // getEntry() follows the remapping to the parent blackboard, exactly like
  // the readers do. A key that resolves to an existing entry (local or
  // remapped) must go through the type check below: createEntryImpl() would
  // follow the same remapping and return the pre-existing entry, and writing
  // to it blindly bypasses the type lock and the string conversion.
  auto entry_ptr = getEntry(key);
  if(!entry_ptr)
  {
    // create a new entry
    Any new_value(value);
    std::shared_ptr<Blackboard::Entry> entry;
    // if a new generic port is created with a string, it's type should be AnyTypeAllowed
    if constexpr(std::is_same_v<std::string, T>)
    {
      entry = createEntryImpl(key, PortInfo(PortDirection::INOUT));
    }
    else
    {
      PortInfo new_port(PortDirection::INOUT, new_value.type(),
                        GetAnyFromStringFunctor<T>());
      entry = createEntryImpl(key, new_port);
    }
 
    // Lock entry_mutex before writing to prevent data races with
    // concurrent readers (BUG-1/BUG-8 fix).
    std::scoped_lock entry_lock(entry->entry_mutex);
    entry->value = new_value;
    entry->sequence_id++;
    entry->stamp = std::chrono::steady_clock::now().time_since_epoch();
  }
  else
  {
    // this is not the first time we set this entry, we need to check
    // if the type is the same or not.
    Entry& entry = *entry_ptr;
 
    std::scoped_lock scoped_lock(entry.entry_mutex);
 
    Any& previous_any = entry.value;
    Any new_value(value);
 
    // special case: entry exists but it is not strongly typed... yet
    if(!entry.info.isStronglyTyped())
    {
      // Use the new type to create a new entry that is strongly typed.
      entry.info = TypeInfo::Create<T>();
      entry.sequence_id++;
      entry.stamp = std::chrono::steady_clock::now().time_since_epoch();
      previous_any = std::move(new_value);
      return;
    }
 
    std::type_index previous_type = entry.info.type();
 
    // check type mismatch
    if(previous_type != std::type_index(typeid(T)) && previous_type != new_value.type())
    {
      bool mismatching = true;
      if(std::is_constructible<StringView, T>::value)
      {
        Any any_from_string = entry.info.parseString(value);
        if(any_from_string.empty() == false)
        {
          mismatching = false;
          new_value = std::move(any_from_string);
        }
      }
      // check if we are doing a safe cast between numbers
      // for instance, it is safe to use int(100) to set
      // a uint8_t port, but not int(-42) or int(300)
      if constexpr(std::is_arithmetic_v<T>)
      {
        if(mismatching && isCastingSafe(previous_type, value))
        {
          mismatching = false;
        }
      }
 
      if(mismatching)
      {
        debugMessage();
 
        auto msg = StrCat("Blackboard::set(", key,
                          "): once declared, "
                          "the type of a port shall not change. "
                          "Previously declared type [",
                          BT::demangle(previous_type), "], current type [",
                          BT::demangle(typeid(T)), "]");
        throw LogicError(msg);
      }
    }
    // if doing set<BT::Any>, skip type check
    if constexpr(std::is_same_v<Any, T>)
    {
      previous_any = new_value;
    }
    else
    {
      // copy only if the type is compatible
      new_value.copyInto(previous_any);
    }
    entry.sequence_id++;
    entry.stamp = std::chrono::steady_clock::now().time_since_epoch();
  }
}
 
template <typename T>
inline bool Blackboard::get(const std::string& key, T& value) const
{
  if(auto any_ref = getAnyLocked(key))
  {
    const auto& any = any_ref.get();
    if(any->empty())
    {
      return false;
    }
    auto result = tryCastWithPolymorphicFallback<T>(any);
    if(!result)
    {
      throw std::runtime_error(result.error());
    }
    value = result.value();
    return true;
  }
  return false;
}
 
template <typename T>
inline Expected<Timestamp> Blackboard::getStamped(const std::string& key, T& value) const
{
  if(auto entry = getEntry(key))
  {
    std::unique_lock lk(entry->entry_mutex);
    if(entry->value.empty())
    {
      return nonstd::make_unexpected(StrCat("Blackboard::getStamped() error. Entry [",
                                            key, "] hasn't been initialized, yet"));
    }
    auto result = tryCastWithPolymorphicFallback<T>(&entry->value);
    if(result)
    {
      value = result.value();
      return Timestamp{ entry->sequence_id, entry->stamp };
    }
    return nonstd::make_unexpected(result.error());
  }
  return nonstd::make_unexpected(
      StrCat("Blackboard::getStamped() error. Missing key [", key, "]"));
}
 
template <typename T>
inline Expected<StampedValue<T>> Blackboard::getStamped(const std::string& key) const
{
  StampedValue<T> out;
  if(auto res = getStamped<T>(key, out.value))
  {
    out.stamp = *res;
    return out;
  }
  else
  {
    return nonstd::make_unexpected(res.error());
  }
}
 
}  // namespace BT