bin_parser.h
Go to the documentation of this file.
1 /*
2  * Copyright 2019, FZI Forschungszentrum Informatik (refactor)
3  *
4  * Copyright 2017, 2018 Simon Rasmussen (refactor)
5  *
6  * Copyright 2015, 2016 Thomas Timm Andersen (original version)
7  *
8  * Licensed under the Apache License, Version 2.0 (the "License");
9  * you may not use this file except in compliance with the License.
10  * You may obtain a copy of the License at
11  *
12  * http://www.apache.org/licenses/LICENSE-2.0
13  *
14  * Unless required by applicable law or agreed to in writing, software
15  * distributed under the License is distributed on an "AS IS" BASIS,
16  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
17  * See the License for the specific language governing permissions and
18  * limitations under the License.
19  */
20 
21 #pragma once
22 
23 #include <assert.h>
24 #include <endian.h>
25 #include <inttypes.h>
26 #include <array>
27 #include <bitset>
28 #include <cstddef>
29 #include <cstring>
30 #include <string>
31 #include <memory>
32 #include "ur_client_library/log.h"
35 
36 namespace urcl
37 {
38 namespace comm
39 {
44 class BinParser
45 {
46 private:
47  uint8_t *buf_pos_, *buf_end_;
49 
50  // Decode from network encoding (big endian) to host encoding
51  template <typename T>
52  T decode(T val)
53  {
54  return val;
55  }
56  uint16_t decode(uint16_t val)
57  {
58  return be16toh(val);
59  }
60  uint32_t decode(uint32_t val)
61  {
62  return be32toh(val);
63  }
64  uint64_t decode(uint64_t val)
65  {
66  return be64toh(val);
67  }
68  int16_t decode(int16_t val)
69  {
70  return be16toh(val);
71  }
72  int32_t decode(int32_t val)
73  {
74  return be32toh(val);
75  }
76  int64_t decode(int64_t val)
77  {
78  return be64toh(val);
79  }
80 
81 public:
88  BinParser(uint8_t* buffer, size_t buf_len) : buf_pos_(buffer), buf_end_(buffer + buf_len), parent_(*this)
89  {
90  assert(buf_pos_ <= buf_end_);
91  }
92 
99  BinParser(BinParser& parent, size_t sub_len)
100  : buf_pos_(parent.buf_pos_), buf_end_(parent.buf_pos_ + sub_len), parent_(parent)
101  {
102  assert(buf_pos_ <= buf_end_);
103  }
104 
109  {
110  parent_.buf_pos_ = buf_pos_;
111  }
112 
120  template <typename T>
121  T peek()
122  {
123  if (buf_pos_ + sizeof(T) > buf_end_)
124  throw UrException("Could not parse received package. This can occur if the driver is started while the robot is "
125  "booting - please restart the driver once the robot has finished booting. "
126  "If the problem persists after the robot has booted, please contact the package maintainer.");
127  T val;
128  std::memcpy(&val, buf_pos_, sizeof(T));
129  return decode(val);
130  }
131 
138  template <typename T>
139  void parse(T& val)
140  {
141  val = peek<T>();
142  buf_pos_ += sizeof(T);
143  }
144 
150  void parse(double& val)
151  {
152  uint64_t inner;
153  parse<uint64_t>(inner);
154  std::memcpy(&val, &inner, sizeof(double));
155  }
161  void parse(float& val)
162  {
163  uint32_t inner;
164  parse<uint32_t>(inner);
165  std::memcpy(&val, &inner, sizeof(float));
166  }
167 
177  void parse(bool& val)
178  {
179  uint8_t inner;
180  parse<uint8_t>(inner);
181  val = inner != 0;
182  }
183 
189  void parse(vector3d_t& val)
190  {
191  for (size_t i = 0; i < val.size(); ++i)
192  {
193  parse(val[i]);
194  }
195  }
196 
202  void parse(vector6d_t& val)
203  {
204  for (size_t i = 0; i < val.size(); ++i)
205  {
206  parse(val[i]);
207  }
208  }
209 
216  {
217  for (size_t i = 0; i < val.size(); ++i)
218  {
219  parse(val[i]);
220  }
221  }
222 
229  {
230  for (size_t i = 0; i < val.size(); ++i)
231  {
232  parse(val[i]);
233  }
234  }
235 
242  void rawData(std::unique_ptr<uint8_t>& buffer, size_t& buffer_length)
243  {
244  buffer_length = buf_end_ - buf_pos_;
245  buffer.reset(new uint8_t[buffer_length]);
246  memcpy(buffer.get(), buf_pos_, buffer_length);
247  consume();
248  }
249 
255  void parseRemainder(std::string& val)
256  {
257  parse(val, size_t(buf_end_ - buf_pos_));
258  }
259 
266  void parse(std::string& val, size_t len)
267  {
268  val.assign(reinterpret_cast<char*>(buf_pos_), len);
269  buf_pos_ += len;
270  }
271 
277  void parse(std::string& val)
278  {
279  uint8_t len;
280  parse(len);
281  parse(val, size_t(len));
282  }
283 
291  template <typename T, size_t N>
292  void parse(std::array<T, N>& array)
293  {
294  for (size_t i = 0; i < N; i++)
295  {
296  parse(array[i]);
297  }
298  }
299 
307  template <typename T, size_t N>
308  void parse(std::bitset<N>& set)
309  {
310  T val;
311  parse(val);
312  set = std::bitset<N>(val);
313  }
314 
318  void consume()
319  {
320  buf_pos_ = buf_end_;
321  }
327  void consume(size_t bytes)
328  {
329  buf_pos_ += bytes;
330  }
331 
339  bool checkSize(size_t bytes)
340  {
341  return bytes <= size_t(buf_end_ - buf_pos_);
342  }
350  template <typename T>
351  bool checkSize(void)
352  {
353  return checkSize(T::SIZE);
354  }
355 
361  bool empty()
362  {
363  return buf_pos_ == buf_end_;
364  }
365 
369  void debug()
370  {
371  URCL_LOG_DEBUG("BinParser: %p - %p (%zu bytes)", buf_pos_, buf_end_, buf_end_ - buf_pos_);
372  }
373 };
374 
375 } // namespace comm
376 } // namespace urcl
void parse(float &val)
Parses the next bytes as a float.
Definition: bin_parser.h:161
void consume(size_t bytes)
Moves the current buffer position ahead by a given amount.
Definition: bin_parser.h:327
void debug()
Logs debugging information about the BinParser object.
Definition: bin_parser.h:369
void parse(vector6d_t &val)
Parses the next bytes as a vector of 6 doubles.
Definition: bin_parser.h:202
void parse(T &val)
Parses the next bytes as given type.
Definition: bin_parser.h:139
void parseRemainder(std::string &val)
Parses the remaining bytes as a string.
Definition: bin_parser.h:255
bool empty()
Checks if no unparsed bytes remain in the buffer.
Definition: bin_parser.h:361
BinParser & parent_
Definition: bin_parser.h:48
BinParser(BinParser &parent, size_t sub_len)
Creates a new BinParser object for part of a buffer from a parent BinParser.
Definition: bin_parser.h:99
The BinParser class handles a byte buffer and functionality to iteratively parse the content...
Definition: bin_parser.h:44
BinParser(uint8_t *buffer, size_t buf_len)
Creates a new BinParser object from a given buffer.
Definition: bin_parser.h:88
void parse(bool &val)
Parses the next byte as a bool.
Definition: bin_parser.h:177
void parse(double &val)
Parses the next bytes as a double.
Definition: bin_parser.h:150
uint16_t decode(uint16_t val)
Definition: bin_parser.h:56
bool checkSize(void)
Checks if enough bytes for a given type remain unparsed in the buffer.
Definition: bin_parser.h:351
uint64_t decode(uint64_t val)
Definition: bin_parser.h:64
void parse(vector3d_t &val)
Parses the next bytes as a vector of 3 doubles.
Definition: bin_parser.h:189
int32_t decode(int32_t val)
Definition: bin_parser.h:72
void rawData(std::unique_ptr< uint8_t > &buffer, size_t &buffer_length)
Writes the remaining bytes into a given buffer without parsing them.
Definition: bin_parser.h:242
std::array< double, 3 > vector3d_t
Definition: types.h:29
void parse(vector6uint32_t &val)
Parses the next bytes as a vector of 6 unsigned 32 bit integers.
Definition: bin_parser.h:228
void parse(std::string &val)
Special string parse function that assumes uint8_t len followed by chars.
Definition: bin_parser.h:277
void parse(std::bitset< N > &set)
Parses the next bytes as a value of a given type, but also copies it to a bitset. ...
Definition: bin_parser.h:308
void consume()
Sets the current buffer position to the end of the buffer, finishing parsing.
Definition: bin_parser.h:318
bool checkSize(size_t bytes)
Checks if at least a given number of bytes is still remaining unparsed in the buffer.
Definition: bin_parser.h:339
#define URCL_LOG_DEBUG(...)
Definition: log.h:34
~BinParser()
Deconstructor for the BinParser.
Definition: bin_parser.h:108
int64_t decode(int64_t val)
Definition: bin_parser.h:76
std::array< int32_t, 6 > vector6int32_t
Definition: types.h:31
void parse(std::array< T, N > &array)
Parses the next bytes as an array of a given type and size.
Definition: bin_parser.h:292
uint32_t decode(uint32_t val)
Definition: bin_parser.h:60
std::array< uint32_t, 6 > vector6uint32_t
Definition: types.h:32
std::array< double, 6 > vector6d_t
Definition: types.h:30
int16_t decode(int16_t val)
Definition: bin_parser.h:68
T peek()
Parses the next bytes as given type without moving the buffer pointer.
Definition: bin_parser.h:121
void parse(vector6int32_t &val)
Parses the next bytes as a vector of 6 32 bit integers.
Definition: bin_parser.h:215
Our base class for exceptions. Specialized exceptions should inherit from those.
Definition: exceptions.h:40
void parse(std::string &val, size_t len)
Parses a given number of bytes as a string.
Definition: bin_parser.h:266


ur_client_library
Author(s): Thomas Timm Andersen, Simon Rasmussen, Felix Exner, Lea Steffen, Tristan Schnell
autogenerated on Sun May 9 2021 02:16:26