TAMSVIZ
Visualization and annotation tool for ROS
snapshot.h
1 // TAMSVIZ
2 // (c) 2020 Philipp Ruppel
3 
4 #pragma once
5 
6 #include "object.h"
7 
8 #include <unordered_map>
9 
10 struct SnapshotBase {
11  SnapshotBase();
12  SnapshotBase(const SnapshotBase &other);
13  virtual ~SnapshotBase();
14  static size_t instanceCount();
15  mutable uint64_t counter = 0;
16 };
17 
18 template <class T>
19 struct Snapshot : std::enable_if<fromStringSupported((T *)nullptr) ||
20  propertiesSupported((T *)nullptr),
21  SnapshotBase>::type {
22  T value;
23  static std::shared_ptr<const Snapshot<T>>
24  save(const T &v, const std::shared_ptr<const Snapshot<T>> &s,
25  const SnapshotFilter &filter) {
26  if (s && s->value == v) {
27  return s;
28  } else {
29  auto ret = std::make_shared<Snapshot>();
30  ret->value = v;
31  return ret;
32  }
33  }
34  void apply(T &v) const { v = value; }
35 };
36 
37 template <class T> struct Snapshot<std::shared_ptr<T>> : SnapshotBase {
38  uint64_t id = 0;
39  std::shared_ptr<const Type> type;
40  bool nonzero = false;
41  std::vector<std::shared_ptr<const SnapshotBase>> property_snapshots;
42  static std::shared_ptr<const Type> findType(const std::shared_ptr<T> &p) {
43  if (p == nullptr) {
44  return nullptr;
45  }
46  return Type::find(typeid(*p));
47  }
48  static std::shared_ptr<const Snapshot<std::shared_ptr<T>>>
49  save(const std::shared_ptr<T> &v,
50  const std::shared_ptr<const Snapshot<std::shared_ptr<T>>> &s0,
51  const SnapshotFilter &filter) {
52  std::shared_ptr<Snapshot<std::shared_ptr<T>>> ret;
53  if (s0 == nullptr || s0->nonzero != (bool)v || (v && (s0->id != v->id())) ||
54  (v && s0->type->typeId() != std::type_index(typeid(*v)))) {
55  ret = std::make_shared<Snapshot<std::shared_ptr<T>>>();
56  if (v != nullptr) {
57  ret->property_snapshots.resize(v->properties().size());
58  }
59  ret->id = (v ? v->id() : 0);
60  ret->type = findType(v);
61  ret->nonzero = (bool)v;
62  }
63  if (v != nullptr) {
64  auto properties = v->properties();
65  for (size_t property_index = 0; property_index < properties.size();
66  property_index++) {
67  auto &property = properties[property_index];
68  auto previous_snapshot =
69  ((s0 != nullptr && property_index < s0->property_snapshots.size() &&
70  s0->nonzero && s0->type->typeId() == std::type_index(typeid(*v)))
71  ? s0->property_snapshots.at(property_index)
72  : nullptr);
73  auto property_snapshot =
74  ((property.canContainObjects() || filter == nullptr ||
75  previous_snapshot == nullptr || filter(v))
76  ? property.save(previous_snapshot, filter)
77  : previous_snapshot);
78  // auto property_snapshot =
79  // property.save(previous_snapshot, (filter != v ? filter :
80  // nullptr));
81  if (property_snapshot == nullptr) {
82  throw std::runtime_error("failed to save property");
83  }
84  if (property_snapshot != previous_snapshot && ret == nullptr) {
85  ret = std::make_shared<Snapshot<std::shared_ptr<T>>>(*s0);
86  ret->property_snapshots.resize(v->properties().size());
87  }
88  if (ret) {
89  ret->property_snapshots.at(property_index) = property_snapshot;
90  }
91  }
92  } else {
93  if (ret) {
94  ret->property_snapshots.clear();
95  }
96  }
97  return ret ? ret : s0;
98  }
99  void apply(std::shared_ptr<T> &v) const {
100  if (!nonzero) {
101  v = nullptr;
102  return;
103  } else {
104  if (v == nullptr || type != Type::find(typeid(*v))) {
105  v = type->instantiate<T>();
106  }
107  v->setId(id);
108  {
109  auto properties = v->properties();
110  for (size_t property_index = 0; property_index < properties.size();
111  property_index++) {
112  auto &property = properties[property_index];
113  property.applySnapshot(property_snapshots.at(property_index));
114  }
115  }
116  }
117  }
118 };
119 
120 template <class T> struct Snapshot<std::vector<T>> : SnapshotBase {
121  std::vector<std::shared_ptr<const Snapshot<T>>> snapshots;
122  static std::shared_ptr<const Snapshot>
123  save(const std::vector<T> &v, const std::shared_ptr<const Snapshot> &s0,
124  const SnapshotFilter &filter) {
125  std::shared_ptr<Snapshot<std::vector<T>>> ret;
126  if (s0 == nullptr || (s0->snapshots.size() != v.size())) {
127  ret = std::make_shared<Snapshot<std::vector<T>>>();
128  ret->snapshots.resize(v.size());
129  }
130  for (size_t i = 0; i < v.size(); i++) {
131  auto shot = Snapshot<T>::save(v[i],
132  (s0 != nullptr && i < s0->snapshots.size())
133  ? s0->snapshots[i]
134  : nullptr,
135  filter);
136  if (ret != nullptr || i >= s0->snapshots.size() ||
137  shot != s0->snapshots[i]) {
138  if (ret == nullptr) {
139  if (s0) {
140  ret = std::make_shared<Snapshot<std::vector<T>>>(*s0);
141  } else {
142  ret = std::make_shared<Snapshot<std::vector<T>>>();
143  }
144  ret->snapshots.resize(v.size());
145  }
146  ret->snapshots[i] = shot;
147  }
148  }
149  return ret ? ret : s0;
150  }
151  inline void shuffle(...) const {}
152  template <class X> void shuffle(std::vector<std::shared_ptr<X>> &v) const {
153  // if (v.size() != snapshots.size()) {
154  std::unordered_map<uint64_t, std::shared_ptr<X>> id_map;
155  id_map.reserve(v.size());
156  for (auto &x : v) {
157  if (x) {
158  id_map[x->id()] = x;
159  }
160  }
161  v.resize(snapshots.size());
162  for (size_t i = 0; i < snapshots.size(); i++) {
163  auto iter = id_map.find(snapshots[i]->id);
164  if (iter != id_map.end()) {
165  v[i] = iter->second;
166  } else {
167  v[i] = nullptr;
168  }
169  }
170  //}
171  }
172  void apply(std::vector<T> &v) const {
173  shuffle(v);
174  v.resize(snapshots.size());
175  for (size_t i = 0; i < snapshots.size(); i++) {
176  snapshots[i]->apply(v[i]);
177  }
178  }
179 };
180 
181 template <class T> struct Snapshot<Handle<T>> : SnapshotBase {
182  uint64_t id = 0;
183  static std::shared_ptr<const Snapshot>
184  save(const Handle<T> &v, const std::shared_ptr<const Snapshot> &s0,
185  const SnapshotFilter &filter) {
186  if (s0 && s0->id == v.id()) {
187  return s0;
188  } else {
189  auto ret = std::make_shared<Snapshot>();
190  ret->id = v.id();
191  return ret;
192  }
193  }
194  void apply(Handle<T> &v) const { v.reset(id); }
195 };
196 
197 template <class T>
198 std::shared_ptr<const SnapshotBase>
199 PropertyInfoImpl<T>::save(const void *value,
200  const std::shared_ptr<const SnapshotBase> &x,
201  const SnapshotFilter &filter) const {
202  auto snapshot = std::dynamic_pointer_cast<const Snapshot<T>>(x);
203  if (x && !snapshot) {
204  throw std::runtime_error("snapshot type mismatch");
205  }
206  return Snapshot<T>::save(*(const T *)value, snapshot, filter);
207 }
208 
209 template <class T>
211  void *value, const std::shared_ptr<const SnapshotBase> &x) const {
212  std::dynamic_pointer_cast<const Snapshot<T>>(x)->apply(*(T *)value);
213 }
Definition: type.h:117