DataStore.h

Go to the documentation of this file.

/*
 * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_rtc/logging.h>
#include <mc_rtc/type_name.h>
#include <mc_rtc/utils_api.h>
 
#include <functional>
#include <memory>
#include <stdexcept>
#include <unordered_map>
#include <vector>
 
#include <Eigen/Core>
 
namespace mc_rtc
{
 
namespace internal
{
template<typename T>
bool is_valid_hash(std::size_t h)
{
  return h == typeid(T).hash_code();
}
 
template<typename T, typename U, typename... Args>
bool is_valid_hash(std::size_t h)
{
  return is_valid_hash<T>(h) || is_valid_hash<U, Args...>(h);
}
 
template<typename T>
bool is_valid_name(const std::string & name)
{
  return name == type_name<T>();
}
 
template<typename T, typename U, typename... Args>
bool is_valid_name(const std::string & name)
{
  return is_valid_name<T>(name) || is_valid_name<U, Args...>(name);
}
 
template<typename T>
struct lambda_traits : public lambda_traits<decltype(&T::operator())>
{
};
 
template<typename C, typename RetT, typename... Args>
struct lambda_traits<RetT (C::*)(Args...) const>
{
  using fn_t = std::function<RetT(Args...)>;
};
 
template<typename C, typename RetT, typename... Args>
struct lambda_traits<RetT (C::*)(Args...)>
{
  using fn_t = std::function<RetT(Args...)>;
};
 
template<typename T>
struct args_t
{
  using decay_t = typename std::decay<T>::type;
  static constexpr bool is_arithmetic = std::is_arithmetic<decay_t>::value;
  using type = typename std::conditional<is_arithmetic, decay_t, T>::type;
};
 
template<typename T, typename = void>
struct Allocator : public std::allocator<T>
{
};
 
template<typename T>
struct Allocator<T, typename T::eigen_aligned_operator_new_marker_type> : public Eigen::aligned_allocator<T>
{
};
 
} // namespace internal
 
struct DataStore
{
  DataStore() = default;
  DataStore(const DataStore &) = delete;
  DataStore & operator=(const DataStore &) = delete;
  DataStore(DataStore &&) = default;
  DataStore & operator=(DataStore &&) = default;
 
  inline bool has(const std::string & name) const noexcept { return datas_.find(name) != datas_.end(); }
 
  inline std::vector<std::string> keys() const noexcept
  {
    std::vector<std::string> out;
    out.reserve(datas_.size());
    for(const auto & d : datas_) { out.push_back(d.first); }
    return out;
  }
 
  template<typename T>
  T & get(const std::string & name)
  {
    return const_cast<T &>(get_<T>(name));
  }
 
  template<typename T>
  const T & get(const std::string & name) const
  {
    return get_<T>(name);
  }
 
  template<typename T>
  void get(const std::string & name, T & data)
  {
    auto it = datas_.find(name);
    if(it != datas_.end()) { data = safe_cast<T>(it->second, name); }
  }
 
  template<typename T>
  const T & get(const std::string & name, const T & defaultValue) const
  {
    auto it = datas_.find(name);
    if(it != datas_.end()) { return safe_cast<T>(it->second, name); }
    return defaultValue;
  }
 
  template<typename T>
  void assign(const std::string & name, const T & data)
  {
    get<T>(name) = data;
  }
 
  template<typename T, typename... ArgsT, typename... Args>
  T & make(const std::string & name, Args &&... args)
  {
    auto & data = datas_[name];
    if(data.buffer) { log::error_and_throw("[{}] An object named {} already exists on the datastore.", name_, name); }
    data.allocate<T>(name_, name);
    new(data.buffer.get()) T(std::forward<Args>(args)...);
    return data.setup<T, ArgsT...>();
  }
 
  template<typename T>
  auto make_call(const std::string & name, T fn) -> typename internal::lambda_traits<T>::fn_t &
  {
    using fn_t = typename internal::lambda_traits<T>::fn_t;
    return make<fn_t>(name, fn_t(fn));
  }
 
  template<typename T, typename... ArgsT, typename... Args>
  T & make_initializer(const std::string & name, Args &&... args)
  {
    auto & data = datas_[name];
    if(data.buffer) { log::error_and_throw("[{}] An object named {} already exists on the datastore.", name_, name); }
    data.allocate<T>(name_, name);
    new(data.buffer.get()) T{std::forward<Args>(args)...};
    return data.setup<T, ArgsT...>();
  }
 
  template<typename RetT,
           typename... FuncArgsT,
           typename... ArgsT,
           typename std::enable_if<sizeof...(FuncArgsT) == sizeof...(ArgsT)
                                       && !std::is_same<std::tuple<FuncArgsT...>, std::tuple<ArgsT...>>::value,
                                   int>::type = 0>
  RetT call(const std::string & name, ArgsT &&... args) const
  {
    return safe_call<RetT, FuncArgsT...>(name, std::forward<ArgsT>(args)...);
  }
 
  template<typename RetT = void, typename... ArgsT>
  RetT call(const std::string & name, ArgsT &&... args) const
  {
    return safe_call<RetT, typename internal::args_t<ArgsT>::type...>(name, std::forward<ArgsT>(args)...);
  }
 
  inline void remove(const std::string & name) noexcept
  {
    auto it = datas_.find(name);
    if(it == datas_.end())
    {
      log::error("[{}] Failed to remove element \"{}\" (element does not exist)", name_, name);
      return;
    }
    datas_.erase(it);
  }
 
  inline void clear() noexcept { datas_.clear(); }
 
  inline const std::string & name() const noexcept { return name_; }
 
  inline void name(const std::string & name) noexcept { name_ = name; }
 
private:
  struct Data
  {
    Data() = default;
    Data(const Data &) = delete;
    Data & operator=(const Data &) = delete;
    Data(Data &&) = default;
    Data & operator=(Data &&) = default;
 
    std::unique_ptr<uint8_t[]> buffer;
    std::string (*type)();
    bool (*same)(std::size_t);
    bool (*same_name)(const std::string &);
    void (*destroy)(Data &);
    ~Data()
    {
      if(buffer) { destroy(*this); }
    }
 
    template<typename T>
    void allocate(const std::string & name_, const std::string & name)
    {
      if(buffer) { log::error_and_throw("[{}] An object named {} already exists on the datastore.", name_, name); }
      buffer.reset(reinterpret_cast<uint8_t *>(internal::Allocator<T>().allocate(1)));
    }
 
    template<typename T, typename... ArgsT>
    T & setup()
    {
      this->type = &type_name<T>;
      this->same = &internal::is_valid_hash<T, ArgsT...>;
      this->same_name = &internal::is_valid_name<T, ArgsT...>;
      this->destroy = [](Data & self)
      {
        T * p = reinterpret_cast<T *>(self.buffer.release());
        p->~T();
        internal::Allocator<T>().deallocate(p, 1);
      };
      return *(reinterpret_cast<T *>(buffer.get()));
    }
  };
 
  template<typename T>
  const T & safe_cast(const Data & data, const std::string & name) const
  {
    if(!data.same(typeid(T).hash_code()) && !data.same_name(type_name<T>()))
    {
      log::error_and_throw(
          "[{} Object for key \"{}\" does not have the same type as the stored type. Stored {} but requested {}.",
          name_, name, data.type(), type_name<T>());
    }
    return *(reinterpret_cast<T *>(data.buffer.get()));
  }
 
  template<typename RetT, typename... FuncArgsT, typename... ArgsT>
  RetT safe_call(const std::string & name, ArgsT &&... args) const
  {
    const auto & data = get_data(name);
    using fn_t = std::function<RetT(FuncArgsT...)>;
    if(!data.same(typeid(fn_t).hash_code()) && !data.same_name(type_name<fn_t>()))
    {
      log::error_and_throw("[{}] Function for key \"{}\" does not have the same signature as the "
                           "requested one. Stored {} but requested {}",
                           name_, name, data.type(), type_name<fn_t>());
    }
    auto & fn = *(reinterpret_cast<fn_t *>(data.buffer.get()));
    return fn(std::forward<ArgsT>(args)...);
  }
 
  template<typename T>
  const T & get_(const std::string & name) const
  {
    return safe_cast<T>(get_data(name), name);
  }
 
  inline const Data & get_data(const std::string & name) const
  {
    const auto it = datas_.find(name);
    if(it == datas_.end()) { log::error_and_throw("[{}] No key \"{}\"", name_, name); }
    return it->second;
  }
 
private:
  std::unordered_map<std::string, Data> datas_;
  std::string name_ = "DataStore";
};
 
} // namespace mc_rtc