TAMSVIZ
Visualization and annotation tool for ROS
mparser.h
1 // TAMSVIZ
2 // (c) 2020 Philipp Ruppel
3 
4 #pragma once
5 
6 #include "message.h"
7 #include "profiler.h"
8 
9 #include <typeindex>
10 #include <unordered_map>
11 #include <vector>
12 
13 class MessageParser {
14 
15  struct Node {
16  std::string hash;
17  virtual void index(const MessageParser &parser,
18  size_t &data_offset) const = 0;
19  virtual bool hasFixedSize() const = 0;
20  virtual size_t fixedSize() const {
21  throw std::runtime_error("Node does not have a fixed size");
22  }
23  virtual MessageParser elementAt(const MessageParser &parser,
24  size_t index) const {
25  return MessageParser();
26  }
27  virtual ssize_t fieldIndex(const MessageParser &parser,
28  const std::string &name) const {
29  return -1;
30  }
31  virtual void init(const MessageParser &parser) {}
32  virtual size_t elementCount(const MessageParser &parser) const { return 0; }
33  virtual std::string toString(const MessageParser &parser) const {
34  return "";
35  }
36  virtual double toDouble(const MessageParser &parser) const { return 0; }
37  virtual int64_t toInteger(const MessageParser &parser) const { return 0; }
38  };
39 
40  struct PrimitiveNode : MessageParser::Node {
41  std::string name;
42  };
43 
44  struct TimeNode : PrimitiveNode {
45  std::string name;
46  virtual bool hasFixedSize() const override { return true; }
47  virtual size_t fixedSize() const override { return 8; }
48  virtual void index(const MessageParser &parser,
49  size_t &start) const override {
50  start += sizeof(fixedSize());
51  }
52  ros::Time value(const MessageParser &parser) const {
53  uint32_t sec = parser.data<uint32_t>(parser._data_start + 0);
54  uint32_t nsec = parser.data<uint32_t>(parser._data_start + 4);
55  return ros::Time(sec, nsec);
56  }
57  virtual double toDouble(const MessageParser &parser) const override {
58  return value(parser).toSec();
59  }
60  virtual int64_t toInteger(const MessageParser &parser) const override {
61  return value(parser).toNSec();
62  }
63  /*virtual std::string toString(const MessageParser &parser) const override {
64  return std::to_string(toInteger(parser));
65  }*/
66  };
67 
68  struct DurationNode : PrimitiveNode {
69  std::string name;
70  virtual bool hasFixedSize() const override { return true; }
71  virtual size_t fixedSize() const override { return 8; }
72  virtual void index(const MessageParser &parser,
73  size_t &start) const override {
74  start += sizeof(fixedSize());
75  }
76  ros::Duration value(const MessageParser &parser) const {
77  uint32_t sec = parser.data<uint32_t>(parser._data_start + 0);
78  uint32_t nsec = parser.data<uint32_t>(parser._data_start + 4);
79  return ros::Duration(sec, nsec);
80  }
81  virtual double toDouble(const MessageParser &parser) const override {
82  return value(parser).toSec();
83  }
84  virtual int64_t toInteger(const MessageParser &parser) const override {
85  return value(parser).toNSec();
86  }
87  /*virtual std::string toString(const MessageParser &parser) const override {
88  return std::to_string(toInteger(parser));
89  }*/
90  };
91 
92  struct StringNode : PrimitiveNode {
93  std::string name;
94  virtual bool hasFixedSize() const override { return false; }
95  virtual void index(const MessageParser &parser,
96  size_t &data_offset) const override {
97  auto len = parser.data<uint32_t>(data_offset);
98  data_offset += 4 + len;
99  }
100  virtual std::string toString(const MessageParser &parser) const override {
101  size_t len = parser.data<uint32_t>(parser._data_start);
102  return std::string((const char *)parser.data(parser._data_start + 4, len),
103  len);
104  }
105  };
106 
107  struct ArrayNode : Node {
108  bool has_fixed_element_count = false;
109  size_t fixed_element_count = 0;
110  std::shared_ptr<Node> element_node;
111  virtual bool hasFixedSize() const override {
112  return has_fixed_element_count && element_node->hasFixedSize();
113  }
114  virtual size_t fixedSize() const override {
115  return fixed_element_count * element_node->fixedSize();
116  }
117  virtual void index(const MessageParser &parser,
118  size_t &data_offset) const override {
119  size_t element_count = fixed_element_count;
120  if (!has_fixed_element_count) {
121  element_count = parser.data<uint32_t>(data_offset);
122  data_offset += sizeof(uint32_t);
123  }
124  if (element_node->hasFixedSize()) {
125  data_offset += element_node->fixedSize() * element_count;
126  } else {
127  auto *index = parser._data->index.get();
128  size_t index_start = index->alloc(element_count * 2);
129  for (size_t i = 0; i < element_count; i++) {
130  index->at(index_start + i * 2 + 0) = index->size();
131  index->at(index_start + i * 2 + 1) = data_offset;
132  element_node->index(parser, data_offset);
133  }
134  }
135  }
136  virtual void init(const MessageParser &parser) override {
137  element_node->init(parser);
138  }
139  virtual size_t elementCount(const MessageParser &parser) const override {
140  if (has_fixed_element_count) {
141  return fixed_element_count;
142  } else {
143  return parser.data<uint32_t>(parser._data_start);
144  }
145  }
146  virtual MessageParser elementAt(const MessageParser &parser,
147  size_t index) const override {
148  MessageParser ret;
149  size_t element_count = fixed_element_count;
150  size_t data_start = parser._data_start;
151  if (!has_fixed_element_count) {
152  element_count = parser.data<uint32_t>(parser._data_start);
153  data_start += sizeof(uint32_t);
154  }
155  if (index < element_count) {
156  ret._data = parser._data;
157  ret._node = element_node;
158  if (element_node->hasFixedSize()) {
159  ret._index_start = parser._index_start;
160  ret._data_start = data_start + element_node->fixedSize() * index;
161  } else {
162  ret._index_start =
163  parser._data->index->at(parser._index_start + index * 2 + 0);
164  ret._data_start =
165  parser._data->index->at(parser._index_start + index * 2 + 1);
166  }
167  }
168  return ret;
169  }
170  };
171 
172  struct ConstNode {
173  std::string type_name;
174  std::string name;
175  std::string value_string;
176  };
177 
178  struct MessageNode : Node {
179  std::string name;
180  std::vector<std::shared_ptr<Node>> field_nodes;
181  std::vector<std::string> field_names;
182  std::unordered_map<std::string, size_t> field_map;
183  std::vector<std::shared_ptr<ConstNode>> const_nodes;
184  std::unordered_map<std::string, size_t> const_map;
185  bool fixed_size_initialized = false;
186  bool has_fixed_size = false;
187  size_t fixed_size = 0;
188  std::vector<size_t> fixed_offsets;
189  virtual void init(const MessageParser &parser) override {
190  for (auto &field : field_nodes) {
191  field->init(parser);
192  }
193  has_fixed_size = true;
194  for (auto &field : field_nodes) {
195  if (!field->hasFixedSize()) {
196  has_fixed_size = false;
197  break;
198  }
199  }
200  fixed_size = 0;
201  fixed_offsets.clear();
202  if (has_fixed_size) {
203  for (auto &field : field_nodes) {
204  fixed_offsets.push_back(fixed_size);
205  fixed_size += field->fixedSize();
206  }
207  }
208  fixed_size_initialized = true;
209  }
210  virtual bool hasFixedSize() const override {
211  if (!fixed_size_initialized) {
212  throw std::runtime_error("Fixed size not initialized");
213  }
214  return has_fixed_size;
215  }
216  virtual size_t fixedSize() const override {
217  if (!fixed_size_initialized) {
218  throw std::runtime_error("Fixed size not initialized");
219  }
220  return fixed_size;
221  }
222  virtual void index(const MessageParser &parser,
223  size_t &data_offset) const override {
224  if (!fixed_size_initialized) {
225  throw std::runtime_error("Fixed size not initialized");
226  }
227  if (has_fixed_size) {
228  data_offset += fixed_size;
229  } else {
230  auto *index = parser._data->index.get();
231  size_t index_start = index->alloc(field_nodes.size() * 2);
232  for (size_t i = 0; i < field_nodes.size(); i++) {
233  index->at(index_start + i * 2 + 0) = index->size();
234  index->at(index_start + i * 2 + 1) = data_offset;
235  field_nodes[i]->index(parser, data_offset);
236  }
237  }
238  }
239  virtual size_t elementCount(const MessageParser &parser) const override {
240  return field_nodes.size();
241  }
242  virtual ssize_t fieldIndex(const MessageParser &parser,
243  const std::string &name) const override {
244  auto it = field_map.find(name);
245  if (it != field_map.end()) {
246  return it->second;
247  } else {
248  return -1;
249  }
250  }
251  virtual MessageParser elementAt(const MessageParser &parser,
252  size_t index) const override {
253  if (!fixed_size_initialized) {
254  throw std::runtime_error("Fixed size not initialized");
255  }
256  MessageParser ret;
257  if (index < field_nodes.size()) {
258  ret._data = parser._data;
259  ret._node = field_nodes.at(index);
260  if (has_fixed_size) {
261  ret._index_start = parser._index_start;
262  ret._data_start = parser._data_start + fixed_offsets.at(index);
263  } else {
264  ret._index_start =
265  parser._data->index->at(parser._index_start + index * 2 + 0);
266  ret._data_start =
267  parser._data->index->at(parser._index_start + index * 2 + 1);
268  }
269  }
270  return ret;
271  }
272  };
273 
274  class Index {
275  std::vector<size_t> _data;
276 
277  public:
278  inline size_t alloc(size_t size) {
279  size_t ret = _data.size();
280  _data.resize(_data.size() + size, 0);
281  return ret;
282  }
283  inline void clear() { _data.clear(); }
284  inline size_t at(size_t index) const { return _data.at(index); }
285  inline size_t &at(size_t index) { return _data.at(index); }
286  // inline size_t at(size_t index) const { return _data[index]; }
287  // inline size_t &at(size_t index) { return _data[index]; }
288  inline void push(size_t v) { _data.push_back(v); }
289  inline size_t size() const { return _data.size(); }
290  };
291 
292  struct Data {
293  std::shared_ptr<const Message> message;
294  std::shared_ptr<Index> index;
295  std::unordered_map<std::string, std::shared_ptr<Node>> type_map;
296  };
297 
298  const void *data(size_t start, size_t size) const {
299  if (_data->message->size() < start + size) {
300  throw std::runtime_error("Index out of range");
301  }
302  return _data->message->data() + start;
303  }
304 
305  template <class T> const T &data(size_t start) const {
306  return *(const T *)data(start, sizeof(T));
307  }
308 
309  void parseDefinition(const std::shared_ptr<const MessageType> &type);
310 
311  template <class T>
312  void registerType(const std::string &name, const std::shared_ptr<T> &t) {
313  t->name = name;
314  _data->type_map[name] = t;
315  }
316 
317  template <class T, class... Args>
318  void registerType(const std::string &name, const Args &... args) {
319  registerType(name, std::make_shared<T>(args...));
320  }
321 
322  template <class T> void registerPrimitiveType(const std::string &name) {
323  struct NodeType : PrimitiveNode {
324  virtual bool hasFixedSize() const override { return true; }
325  virtual size_t fixedSize() const override { return sizeof(T); }
326  virtual void index(const MessageParser &parser,
327  size_t &data_offset) const override {
328  data_offset += sizeof(T);
329  }
330  T value(const MessageParser &parser) const {
331  return *(const T *)parser.data(parser._data_start, sizeof(T));
332  }
333  virtual std::string toString(const MessageParser &parser) const override {
334  return std::to_string(value(parser));
335  }
336  virtual double toDouble(const MessageParser &parser) const {
337  return value(parser);
338  }
339  virtual int64_t toInteger(const MessageParser &parser) const {
340  return value(parser);
341  }
342  };
343  auto node = std::make_shared<NodeType>();
344  registerType(name, node);
345  }
346 
347  std::shared_ptr<Node> findType(const std::string &name,
348  const std::string &ns = "");
349 
350  std::shared_ptr<Data> _data;
351  std::shared_ptr<const Node> _node;
352  size_t _data_start = 0;
353  size_t _index_start = 0;
354 
355 public:
356  MessageParser() {}
357  MessageParser(const std::shared_ptr<const Message> &message) {
358  parse(message);
359  }
360  void parse(const std::shared_ptr<const Message> &message);
361  ssize_t fieldIndex(const std::string &name) const {
362  if (isNull()) {
363  return -1;
364  } else {
365  return _node->fieldIndex(*this, name);
366  }
367  }
368  MessageParser at(size_t index) const {
369  if (isNull()) {
370  return MessageParser();
371  } else {
372  return _node->elementAt(*this, index);
373  }
374  }
375  MessageParser operator[](size_t index) const { return at(index); }
376  MessageParser operator[](const std::string &name) const {
377  return at(fieldIndex(name));
378  }
379  bool isNull() const { return _data == nullptr || _node == nullptr; }
380  bool isPrimitive() const {
381  return !isNull() &&
382  dynamic_cast<const PrimitiveNode *>(_node.get()) != nullptr;
383  }
384  bool isMessage() const {
385  return !isNull() &&
386  dynamic_cast<const MessageNode *>(_node.get()) != nullptr;
387  }
388  bool isArray() const {
389  return !isNull() && dynamic_cast<const ArrayNode *>(_node.get()) != nullptr;
390  }
391  bool isString() const {
392  return !isNull() &&
393  dynamic_cast<const StringNode *>(_node.get()) != nullptr;
394  }
395  bool isTime() const {
396  return !isNull() && dynamic_cast<const TimeNode *>(_node.get()) != nullptr;
397  }
398  size_t size() const { return _node->elementCount(*this); }
399  const std::string &fieldName(size_t i) const {
400  return dynamic_cast<const MessageNode *>(_node.get())->field_names.at(i);
401  }
402  void print(std::ostream &stream) const;
403  std::string print() const;
404  std::string toString() const {
405  if (isNull()) {
406  return "";
407  } else {
408  return _node->toString(*this);
409  }
410  }
411  double toDouble() const {
412  if (isNull()) {
413  return 0;
414  } else {
415  return _node->toDouble(*this);
416  }
417  }
418  int64_t toInteger() const {
419  if (isNull()) {
420  return 0;
421  } else {
422  return _node->toInteger(*this);
423  }
424  }
425  // operator bool() const { return !isNull(); }
426 };
Generated by   doxygen 1.8.11