-
Notifications
You must be signed in to change notification settings - Fork 34
/
DataStore.h
463 lines (420 loc) · 13.8 KB
/
DataStore.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
/*
* 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);
}
/** Extract return type and argument types from a lambda by accessing ::operator() */
template<typename T>
struct lambda_traits : public lambda_traits<decltype(&T::operator())>
{
};
/** Specialization that matches non-mutable lambda */
template<typename C, typename RetT, typename... Args>
struct lambda_traits<RetT (C::*)(Args...) const>
{
using fn_t = std::function<RetT(Args...)>;
};
/** Specialization that matches mutable lambda */
template<typename C, typename RetT, typename... Args>
struct lambda_traits<RetT (C::*)(Args...)>
{
using fn_t = std::function<RetT(Args...)>;
};
/** Bend usual C++ rules to deduct the argument types in call
*
* Normally when passing an lvalue to a template function, T && will be T &. In
* our case, for arithmetic types (typically double) we far more often want T
* so this traits changes the deduction rule for those types
*/
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;
};
/** Allocator<T> is std::allocator<T> for usual types */
template<typename T, typename = void>
struct Allocator : public std::allocator<T>
{
};
/** Allocator<T> is Eigen::aligned_allocator for EIGEN_MAKE_ALIGNED_OPERATOR_NEW types */
template<typename T>
struct Allocator<T, typename T::eigen_aligned_operator_new_marker_type> : public Eigen::aligned_allocator<T>
{
};
} // namespace internal
/**
* @brief Generic data store
*
* This class allows to store and retrieve C++ objects. It is intended to be
* used as a way to share arbitrary data between various parts of the framework
* (states, plugins, observers, etc...)
*
* \code{cpp}
* DataStore store;
* // Creates a vector of 4 double with value 42 on the datastore
* store.make<std::vector<double>>("Data", 4, 42);
* ...
* auto & data = store.get<std::vector<double>>("Data");
* // Use the object
* data[3] = 0;
* ...
* // Get another reference to the object
* auto & data2 = store.get<std::vector<double>>("Data");
* data2.push_back(0);
* // vector now has size 5
* log::info(data.size());
* \endcode
*
* When retrieving an object using get<Type>, checks are performed to ensure that
* the stored type and Type are compatible.
*
* To handle inheritance, you need to explicitely provide the class hierarchy:
* \code{cpp}
* struct A {};
* struct B : public A {};
* // Creating an inherited object and checking virtual inheritance
* store.make<B, A>("data");
* auto & base = store.get<A>("data");
* auto & derived = store.get<B>("data");
* \endcode
*/
struct DataStore
{
DataStore() = default;
DataStore(const DataStore &) = delete;
DataStore & operator=(const DataStore &) = delete;
DataStore(DataStore &&) = default;
DataStore & operator=(DataStore &&) = default;
/**
* @brief Checks whether an object is in the datastore
*
* @param name Name of the stored object
*
* @return true when the object is in the datastore
*/
inline bool has(const std::string & name) const noexcept { return datas_.find(name) != datas_.end(); }
/**
* @brief Returns all objects in the datastore
*/
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;
}
/**
* @brief Get a reference to an object on the datastore
* @param name Name of the stored oject
* @return Reference to the stored object
*
* @throws std::runtime_error when the type of T does not match the one
* defined upon creation.
*
* @ref make
*/
template<typename T>
T & get(const std::string & name)
{
return const_cast<T &>(get_<T>(name));
}
/** @brief const variant of \ref get */
template<typename T>
const T & get(const std::string & name) const
{
return get_<T>(name);
}
/**
* @brief Assign value from the datastore if it exists, leave value unchanged
* otherwise
*
* @param name Name of the stored data
* @param data Reference to the external object to assign with the stored data
*/
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); }
}
/**
* @brief Gets a value from the datastore if it exists or a default value
* otherwise
*
* @param name Name of the stored data
* @param defaultValue Default value used if the data is not in the datastore
*
* @return Stored data if it exists, defaultValue otherwise.
*/
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;
}
/**
* @brief Copies an object to an existing datastore object
*
* @param name Name of the stored object
* @param data Data to copy on the datastore
*
* \see get(std::string)
*/
template<typename T>
void assign(const std::string & name, const T & data)
{
get<T>(name) = data;
}
/**
* @brief Creates an object on the datastore and returns a reference to it
*
* @param name Name of the stored object.
* @param args Parameters to be passed to the object's constructor
*
* @return A reference to the constructed object
*
* @throws std::runtime_error if an object with the same name already exists
* in the datastore.
*
* \anchor make
*/
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...>();
}
/**
* @brief Creates an object on the datastore and returns a reference to it
*
* This overload should only work with lambda-like objects and transform the lambda into an equivalent std::function
*
* @param name Name of the stored object
* @param fn Function that will be stored in the datastore
*
* @return A reference to the constructed object
*/
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));
}
/**
* @brief Creates an object on the datastore using list initialization and returns a reference to it
*
* @param name Name of the stored object.
* @param args Parameters to be passed to the object using list initialization
*
* @return A reference to the constructed object
*/
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...>();
}
/** @brief Calls a function that was registered in the datastore and returns
* this call result
*
* This is syntactic sugar for getting the function object and calling it
*
* In this version you must specify the argument types for the function you
* are calling. This is especially useful when the types deduced from the
* arguments do not match the function type (typically passing a non-const
* reference to a function expecting a const reference.
*
* @param name Name of the stored function
*
* @tparam FunArgsT Types of arguments expected by the function
*
* @param args Arguments passed to the function
*
*/
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)...);
}
/** @brief Calls a function that was registered in the datastore and returns
* this call result
*
* This is syntaxic sugar for getting the function object and calling it
*
* This version deduces the functions' arguments from the arguments passed to
* the call. This follows C++ rules for deducing types from universal
* reference collapsing *except* for arithmetic types where the type is
* deduced to be of value type.
*
* If you need to call a callback that takes an arithmetic type by reference
* then you must specify the arguments' type explicitely
*
* @param name Name of the stored object
*
* @params args Arguments passed to the function
*
*/
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)...);
}
/**
* @brief Removes an object from the datastore
* @param name Name of the stored object
*/
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);
}
/**
* @brief Remove all entries in the datastore
*/
inline void clear() noexcept { datas_.clear(); }
/**
* @brief Name of this datastore
*/
inline const std::string & name() const noexcept { return name_; }
/**
* @brief Sets this datastore's name
*
* @param name Name for the datastore
*/
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;
/** Hold the data */
std::unique_ptr<uint8_t[]> buffer;
/** Return the stored type name */
std::string (*type)();
/** Check requested type */
bool (*same)(std::size_t);
/** Fallback on checking the demangled name */
bool (*same_name)(const std::string &);
/** Call destructor and delete the buffer */
void (*destroy)(Data &);
/** Destructor */
~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