basic_types.h

#pragma once
 
#include "behaviortree_cpp/contrib/expected.hpp"
#include "behaviortree_cpp/exceptions.h"
#include "behaviortree_cpp/utils/safe_any.hpp"
 
#include <chrono>
#include <functional>
#include <iostream>
#include <string_view>
#include <typeinfo>
#include <unordered_map>
#include <utility>
#include <variant>
#include <vector>
 
namespace BT
{
/// Enumerates the possible types of nodes
enum class NodeType
{
  UNDEFINED = 0,
  ACTION,
  CONDITION,
  CONTROL,
  DECORATOR,
  SUBTREE
};
 
/// Enumerates the states every node can be in after execution during a particular
/// time step.
/// IMPORTANT: Your custom nodes should NEVER return IDLE.
enum class NodeStatus
{
  IDLE = 0,
  RUNNING = 1,
  SUCCESS = 2,
  FAILURE = 3,
  SKIPPED = 4,
};
 
inline bool isStatusActive(const NodeStatus& status)
{
  return status != NodeStatus::IDLE && status != NodeStatus::SKIPPED;
}
 
inline bool isStatusCompleted(const NodeStatus& status)
{
  return status == NodeStatus::SUCCESS || status == NodeStatus::FAILURE;
}
 
enum class PortDirection
{
  INPUT,
  OUTPUT,
  INOUT
};
 
using StringView = std::string_view;
 
bool StartWith(StringView str, StringView prefix);
 
bool StartWith(StringView str, char prefix);
 
// vector of key/value pairs
using KeyValueVector = std::vector<std::pair<std::string, std::string>>;
 
/** Usage: given a function/method like this:
 *
 *     Expected<double> getAnswer();
 *
 * User code can check result and error message like this:
 *
 *     auto res = getAnswer();
 *     if( res )
 *     {
 *         std::cout << "answer was: " << res.value() << std::endl;
 *     }
 *     else{
 *         std::cerr << "failed to get the answer: " << res.error() << std::endl;
 *     }
 *
 * */
template <typename T>
using Expected = nonstd::expected<T, std::string>;
 
struct AnyTypeAllowed
{
};
 
/**
 * @brief convertFromJSON will parse a json string and use JsonExporter
 * to convert its content to a given type. It will work only if
 * the type was previously registered. May throw if it fails.
 *
 * @param json_text a valid JSON string
 * @param type you must specify the typeid()
 * @return the object, wrapped in Any.
 */
[[nodiscard]] Any convertFromJSON(StringView json_text, std::type_index type);
 
/// Same as the non template version, but with automatic casting
template <typename T>
[[nodiscard]] inline T convertFromJSON(StringView str)
{
  return convertFromJSON(str, typeid(T)).cast<T>();
}
 
/**
 * convertFromString is used to convert a string into a custom type.
 *
 * This function is invoked under the hood by TreeNode::getInput(), but only when the
 * input port contains a string.
 *
 * If you have a custom type, you need to implement the corresponding
 * template specialization.
 *
 * If the string starts with the prefix "json:", it will
 * fall back to convertFromJSON()
 */
template <typename T>
[[nodiscard]] inline T convertFromString(StringView str)
{
  // if string starts with "json:{", try to parse it as json
  if(StartWith(str, "json:"))
  {
    str.remove_prefix(5);
    return convertFromJSON<T>(str);
  }
 
  auto type_name = BT::demangle(typeid(T));
 
  std::cerr << "You (maybe indirectly) called BT::convertFromString() for type ["
            << type_name << "], but I can't find the template specialization.\n"
            << std::endl;
 
  throw LogicError(std::string("You didn't implement the template specialization of "
                               "convertFromString for this type: ") +
                   type_name);
}
 
template <>
[[nodiscard]] std::string convertFromString<std::string>(StringView str);
 
template <>
[[nodiscard]] const char* convertFromString<const char*>(StringView str);
 
template <>
[[nodiscard]] int8_t convertFromString<int8_t>(StringView str);
 
template <>
[[nodiscard]] int16_t convertFromString<int16_t>(StringView str);
 
template <>
[[nodiscard]] int32_t convertFromString<int32_t>(StringView str);
 
template <>
[[nodiscard]] int64_t convertFromString<int64_t>(StringView str);
 
template <>
[[nodiscard]] uint8_t convertFromString<uint8_t>(StringView str);
 
template <>
[[nodiscard]] uint16_t convertFromString<uint16_t>(StringView str);
 
template <>
[[nodiscard]] uint32_t convertFromString<uint32_t>(StringView str);
 
template <>
[[nodiscard]] uint64_t convertFromString<uint64_t>(StringView str);
 
template <>
[[nodiscard]] float convertFromString<float>(StringView str);
 
template <>
[[nodiscard]] double convertFromString<double>(StringView str);
 
/**
 * @brief Parse a double from a string using the semantics of
 * std::from_chars(std::chars_format::general): locale-independent (only '.' as
 * the decimal separator), rejecting leading whitespace, a leading '+', and hex
 * floats. Includes a thread-safe fallback for standard libraries that lack the
 * floating-point std::from_chars overload (e.g. Apple libc++).
 *
 * @param str  the input string.
 * @param out  set to the parsed value on success (left untouched on failure).
 * @param require_full_consumption  when true, the whole string must be a valid
 *        double (trailing characters cause failure); when false, parsing stops
 *        at the first non-numeric character.
 * @return true on success.
 */
[[nodiscard]] bool parseDouble(StringView str, double& out,
                               bool require_full_consumption);
 
// Integer numbers separated by the character ";"
template <>
[[nodiscard]] std::vector<int> convertFromString<std::vector<int>>(StringView str);
 
// Real numbers separated by the character ";"
template <>
[[nodiscard]] std::vector<double> convertFromString<std::vector<double>>(StringView str);
 
// Boolean values separated by the character ";"
template <>
[[nodiscard]] std::vector<bool> convertFromString<std::vector<bool>>(StringView str);
 
// Strings separated by the character ";"
template <>
[[nodiscard]] std::vector<std::string>
convertFromString<std::vector<std::string>>(StringView str);
 
// This recognizes either 0/1, true/false, TRUE/FALSE
template <>
[[nodiscard]] bool convertFromString<bool>(StringView str);
 
// Names with all capital letters
template <>
[[nodiscard]] NodeStatus convertFromString<NodeStatus>(StringView str);
 
// Names with all capital letters
template <>
[[nodiscard]] NodeType convertFromString<NodeType>(StringView str);
 
template <>
[[nodiscard]] PortDirection convertFromString<PortDirection>(StringView str);
 
using StringConverter = std::function<Any(StringView)>;
 
using StringConvertersMap = std::unordered_map<const std::type_info*, StringConverter>;
 
// helper function
template <typename T>
[[nodiscard]] inline StringConverter GetAnyFromStringFunctor()
{
  if constexpr(std::is_constructible_v<StringView, T>)
  {
    return [](StringView str) { return Any(str); };
  }
  else if constexpr(std::is_same_v<BT::AnyTypeAllowed, T> || std::is_enum_v<T>)
  {
    return {};
  }
  else
  {
    return [](StringView str) { return Any(convertFromString<T>(str)); };
  }
}
 
template <>
[[nodiscard]] inline StringConverter GetAnyFromStringFunctor<void>()
{
  return {};
}
 
//------------------------------------------------------------------
 
template <typename T>
constexpr bool IsConvertibleToString()
{
  return std::is_convertible_v<T, std::string> ||
         std::is_convertible_v<T, std::string_view>;
}
 
Expected<std::string> toJsonString(const Any& value);
 
/**
 * @brief toStr is the reverse operation of convertFromString.
 *
 * If T is a custom type and there is no template specialization,
 * it will try to fall back to toJsonString()
 */
template <typename T>
[[nodiscard]] std::string toStr(const T& value)
{
  if constexpr(IsConvertibleToString<T>())
  {
    return static_cast<std::string>(value);
  }
  else if constexpr(!std::is_arithmetic_v<T>)
  {
    if(auto str = toJsonString(Any(value)))
    {
      return *str;
    }
 
    throw LogicError(StrCat("Function BT::toStr<T>() not specialized for type [",
                            BT::demangle(typeid(T)), "]"));
  }
  else
  {
    return std::to_string(value);
  }
}
 
template <>
[[nodiscard]] std::string toStr<bool>(const bool& value);
 
template <>
[[nodiscard]] std::string toStr<std::string>(const std::string& value);
 
template <>
[[nodiscard]] std::string toStr<BT::NodeStatus>(const BT::NodeStatus& status);
 
/**
 * @brief toStr converts NodeStatus to string. Optionally colored.
 */
[[nodiscard]] std::string toStr(BT::NodeStatus status, bool colored);
 
std::ostream& operator<<(std::ostream& os, const BT::NodeStatus& status);
 
template <>
[[nodiscard]] std::string toStr<BT::NodeType>(const BT::NodeType& type);
 
std::ostream& operator<<(std::ostream& os, const BT::NodeType& type);
 
template <>
[[nodiscard]] std::string toStr<BT::PortDirection>(const BT::PortDirection& direction);
 
std::ostream& operator<<(std::ostream& os, const BT::PortDirection& type);
 
// Small utility, unless you want to use <boost/algorithm/string.hpp>
[[nodiscard]] std::vector<StringView> splitString(const StringView& strToSplit,
                                                  char delimeter);
 
template <typename Predicate>
using enable_if = typename std::enable_if<Predicate::value>::type*;
 
template <typename Predicate>
using enable_if_not = typename std::enable_if<!Predicate::value>::type*;
 
#ifdef USE_BTCPP3_OLD_NAMES
// note: we also use the name Optional instead of expected because it is more intuitive
// for users that are not up to date with "modern" C++
template <typename T>
using Optional = nonstd::expected<T, std::string>;
#endif
 
/** Usage: given a function/method like:
 *
 *     Result DoSomething();
 *
 * User code can check result and error message like this:
 *
 *     auto res = DoSomething();
 *     if( res )
 *     {
 *         std::cout << "DoSomething() done " << std::endl;
 *     }
 *     else{
 *         std::cerr << "DoSomething() failed with message: " << res.error() << std::endl;
 *     }
 *
 * */
using Result = Expected<std::monostate>;
 
struct Timestamp
{
  // Number being incremented every time a new value is written
  uint64_t seq = 0;
  // Last update time. Nanoseconds since epoch
  std::chrono::nanoseconds time = std::chrono::nanoseconds(0);
};
 
[[nodiscard]] bool IsAllowedPortName(StringView str);
 
[[nodiscard]] bool IsReservedAttribute(StringView str);
 
/// Returns the first forbidden character found in the name, or '\0' if valid.
/// Forbidden characters include: space, tab, newline, CR, < > & " ' / \ : * ? | .
/// and control characters (ASCII 0-31, 127). UTF-8 multibyte sequences are allowed.
[[nodiscard]] char findForbiddenChar(StringView name);
 
class TypeInfo
{
public:
  template <typename T>
  static TypeInfo Create()
  {
    return TypeInfo{ typeid(T), GetAnyFromStringFunctor<T>() };
  }
 
  TypeInfo() : type_info_(typeid(AnyTypeAllowed)), type_str_("AnyTypeAllowed")
  {}
 
  TypeInfo(std::type_index type_info, StringConverter conv)
    : type_info_(type_info), converter_(conv), type_str_(BT::demangle(type_info))
  {}
 
  [[nodiscard]] const std::type_index& type() const;
 
  [[nodiscard]] const std::string& typeName() const;
 
  [[nodiscard]] Any parseString(const char* str) const;
 
  [[nodiscard]] Any parseString(const std::string& str) const;
 
  template <typename T>
  [[nodiscard]] Any parseString(const T&) const
  {
    // avoid compilation errors
    return {};
  }
 
  [[nodiscard]] bool isStronglyTyped() const
  {
    return type_info_ != typeid(AnyTypeAllowed) && type_info_ != typeid(BT::Any);
  }
 
  [[nodiscard]] const StringConverter& converter() const
  {
    return converter_;
  }
 
private:
  std::type_index type_info_;
  StringConverter converter_;
  std::string type_str_;
};
 
class PortInfo : public TypeInfo
{
public:
  PortInfo(PortDirection direction = PortDirection::INOUT)
    : TypeInfo(), direction_(direction)
  {}
 
  PortInfo(PortDirection direction, std::type_index type_info, StringConverter conv)
    : TypeInfo(type_info, conv), direction_(direction)
  {}
 
  [[nodiscard]] PortDirection direction() const;
 
  void setDescription(StringView description);
 
  template <typename T>
  void setDefaultValue(const T& default_value)
  {
    default_value_ = Any(default_value);
    try
    {
      default_value_str_ = BT::toStr(default_value);
    }
    // NOLINTNEXTLINE(bugprone-empty-catch)
    catch(LogicError&)
    {
      // conversion to string not available for this type, ignore
    }
  }
 
  [[nodiscard]] const std::string& description() const;
 
  [[nodiscard]] const Any& defaultValue() const;
 
  [[nodiscard]] const std::string& defaultValueString() const;
 
private:
  PortDirection direction_;
  std::string description_;
  Any default_value_;
  std::string default_value_str_;
};
 
template <typename T = AnyTypeAllowed>
[[nodiscard]] std::pair<std::string, PortInfo> CreatePort(PortDirection direction,
                                                          StringView name,
                                                          StringView description = {})
{
  auto sname = static_cast<std::string>(name);
  if(!IsAllowedPortName(sname))
  {
    throw RuntimeError("The name of a port must not be `name` or `ID` "
                       "and must start with an alphabetic character. "
                       "Underscore is reserved.");
  }
 
  std::pair<std::string, PortInfo> out;
 
  if(std::is_same<T, void>::value)
  {
    out = { sname, PortInfo(direction) };
  }
  else
  {
    out = { sname, PortInfo(direction, typeid(T), GetAnyFromStringFunctor<T>()) };
  }
  if(!description.empty())
  {
    out.second.setDescription(description);
  }
  return out;
}
 
//----------
/** Syntactic sugar to invoke CreatePort<T>(PortDirection::INPUT, ...)
 *
 *  @param name the name of the port
 *  @param description optional human-readable description
 */
template <typename T = AnyTypeAllowed>
[[nodiscard]] inline std::pair<std::string, PortInfo>
InputPort(StringView name, StringView description = {})
{
  return CreatePort<T>(PortDirection::INPUT, name, description);
}
 
/** Syntactic sugar to invoke CreatePort<T>(PortDirection::OUTPUT,...)
 *
 *  @param name the name of the port
 *  @param description optional human-readable description
 */
template <typename T = AnyTypeAllowed>
[[nodiscard]] inline std::pair<std::string, PortInfo>
OutputPort(StringView name, StringView description = {})
{
  return CreatePort<T>(PortDirection::OUTPUT, name, description);
}
 
/** Syntactic sugar to invoke CreatePort<T>(PortDirection::INOUT,...)
 *
 *  @param name the name of the port
 *  @param description optional human-readable description
 */
template <typename T = AnyTypeAllowed>
[[nodiscard]] inline std::pair<std::string, PortInfo>
BidirectionalPort(StringView name, StringView description = {})
{
  return CreatePort<T>(PortDirection::INOUT, name, description);
}
//----------
 
namespace details
{
 
template <typename T = AnyTypeAllowed, typename DefaultT = T>
[[nodiscard]] inline std::pair<std::string, PortInfo>
PortWithDefault(PortDirection direction, StringView name, const DefaultT& default_value,
                StringView description)
{
  static_assert(IsConvertibleToString<DefaultT>() || std::is_convertible_v<T, DefaultT> ||
                    std::is_constructible_v<T, DefaultT>,
                "The default value must be either the same of the port or string");
 
  auto out = CreatePort<T>(direction, name, description);
 
  if constexpr(std::is_constructible_v<T, DefaultT>)
  {
    out.second.setDefaultValue(T(default_value));
  }
  else if constexpr(IsConvertibleToString<DefaultT>())
  {
    out.second.setDefaultValue(std::string(default_value));
  }
  else
  {
    out.second.setDefaultValue(default_value);
  }
  return out;
}
 
}  // end namespace details
 
/** Syntactic sugar to invoke CreatePort<T>(PortDirection::INPUT,...)
 *  It also sets the PortInfo::defaultValue()
 *
 *  @param name the name of the port
 *  @param default_value default value of the port, either type T of BlackboardKey
 *  @param description optional human-readable description
 */
template <typename T = AnyTypeAllowed, typename DefaultT = T>
[[nodiscard]] inline std::pair<std::string, PortInfo>
InputPort(StringView name, const DefaultT& default_value, StringView description)
{
  return details::PortWithDefault<T, DefaultT>(PortDirection::INPUT, name, default_value,
                                               description);
}
 
/** Syntactic sugar to invoke CreatePort<T>(PortDirection::INOUT,...)
 *  It also sets the PortInfo::defaultValue()
 *
 *  @param name the name of the port
 *  @param default_value default value of the port, either type T of BlackboardKey
 *  @param description optional human-readable description
 */
template <typename T = AnyTypeAllowed, typename DefaultT = T>
[[nodiscard]] inline std::pair<std::string, PortInfo>
BidirectionalPort(StringView name, const DefaultT& default_value, StringView description)
{
  return details::PortWithDefault<T, DefaultT>(PortDirection::INOUT, name, default_value,
                                               description);
}
 
/** Syntactic sugar to invoke CreatePort<T>(PortDirection::OUTPUT,...)
 *  It also sets the PortInfo::defaultValue()
 *
 *  @param name the name of the port
 *  @param default_value default blackboard entry where the output is written
 *  @param description optional human-readable description
 */
template <typename T = AnyTypeAllowed>
[[nodiscard]] inline std::pair<std::string, PortInfo> OutputPort(StringView name,
                                                                 StringView default_value,
                                                                 StringView description)
{
  if(default_value.empty() || default_value.front() != '{' || default_value.back() != '}')
  {
    throw LogicError("Output port can only refer to blackboard entries, i.e. use the "
                     "syntax '{port_name}'");
  }
  auto out = CreatePort<T>(PortDirection::OUTPUT, name, description);
  out.second.setDefaultValue(default_value);
  return out;
}
 
//----------
 
using PortsList = std::unordered_map<std::string, PortInfo>;
 
template <typename T, typename = void>
struct has_static_method_providedPorts : std::false_type
{
};
 
template <typename T>
struct has_static_method_providedPorts<
    T, typename std::enable_if<
           std::is_same<decltype(T::providedPorts()), PortsList>::value>::type>
  : std::true_type
{
};
 
template <typename T, typename = void>
struct has_static_method_metadata : std::false_type
{
};
 
template <typename T>
struct has_static_method_metadata<
    T, typename std::enable_if<
           std::is_same<decltype(T::metadata()), KeyValueVector>::value>::type>
  : std::true_type
{
};
 
template <typename T>
[[nodiscard]] inline PortsList
getProvidedPorts(enable_if<has_static_method_providedPorts<T>> = nullptr)
{
  return T::providedPorts();
}
 
template <typename T>
[[nodiscard]] inline PortsList
getProvidedPorts(enable_if_not<has_static_method_providedPorts<T>> = nullptr)
{
  return {};
}
 
using TimePoint = std::chrono::high_resolution_clock::time_point;
using Duration = std::chrono::high_resolution_clock::duration;
 
}  // namespace BT