mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-10 10:00:48 +02:00
Adopted roscpp code style and naming convention
This commit is contained in:
@@ -1,178 +1,175 @@
|
||||
#pragma once
|
||||
|
||||
#include "ur_modern_driver/log.h"
|
||||
#include "ur_modern_driver/types.h"
|
||||
#include <assert.h>
|
||||
#include <cstddef>
|
||||
#include <cstring>
|
||||
#include <endian.h>
|
||||
#include <inttypes.h>
|
||||
#include <cstddef>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
#include "ur_modern_driver/log.h"
|
||||
#include "ur_modern_driver/types.h"
|
||||
|
||||
class BinParser {
|
||||
class BinParser
|
||||
{
|
||||
private:
|
||||
uint8_t *_buf_pos, *_buf_end;
|
||||
BinParser& _parent;
|
||||
uint8_t *buf_pos_, *buf_end_;
|
||||
BinParser& parent_;
|
||||
|
||||
public:
|
||||
BinParser(uint8_t* buffer, size_t buf_len)
|
||||
: _buf_pos(buffer)
|
||||
, _buf_end(buffer + buf_len)
|
||||
, _parent(*this)
|
||||
{
|
||||
assert(_buf_pos <= _buf_end);
|
||||
}
|
||||
BinParser(uint8_t* buffer, size_t buf_len) : buf_pos_(buffer), buf_end_(buffer + buf_len), parent_(*this)
|
||||
{
|
||||
assert(buf_pos_ <= buf_end_);
|
||||
}
|
||||
|
||||
BinParser(BinParser& parent, size_t sub_len)
|
||||
: _buf_pos(parent._buf_pos)
|
||||
, _buf_end(parent._buf_pos + sub_len)
|
||||
, _parent(parent)
|
||||
{
|
||||
assert(_buf_pos <= _buf_end);
|
||||
}
|
||||
BinParser(BinParser& parent, size_t sub_len)
|
||||
: buf_pos_(parent.buf_pos_), buf_end_(parent.buf_pos_ + sub_len), parent_(parent)
|
||||
{
|
||||
assert(buf_pos_ <= buf_end_);
|
||||
}
|
||||
|
||||
~BinParser()
|
||||
{
|
||||
_parent._buf_pos = _buf_pos;
|
||||
}
|
||||
~BinParser()
|
||||
{
|
||||
parent_.buf_pos_ = buf_pos_;
|
||||
}
|
||||
|
||||
//Decode from network encoding (big endian) to host encoding
|
||||
template <typename T>
|
||||
T decode(T val)
|
||||
{
|
||||
return val;
|
||||
}
|
||||
uint16_t decode(uint16_t val)
|
||||
{
|
||||
return be16toh(val);
|
||||
}
|
||||
uint32_t decode(uint32_t val)
|
||||
{
|
||||
return be32toh(val);
|
||||
}
|
||||
uint64_t decode(uint64_t val)
|
||||
{
|
||||
return be64toh(val);
|
||||
}
|
||||
int16_t decode(int16_t val)
|
||||
{
|
||||
return be16toh(val);
|
||||
}
|
||||
int32_t decode(int32_t val)
|
||||
{
|
||||
return be32toh(val);
|
||||
}
|
||||
int64_t decode(int64_t val)
|
||||
{
|
||||
return be64toh(val);
|
||||
}
|
||||
// Decode from network encoding (big endian) to host encoding
|
||||
template <typename T>
|
||||
T decode(T val)
|
||||
{
|
||||
return val;
|
||||
}
|
||||
uint16_t decode(uint16_t val)
|
||||
{
|
||||
return be16toh(val);
|
||||
}
|
||||
uint32_t decode(uint32_t val)
|
||||
{
|
||||
return be32toh(val);
|
||||
}
|
||||
uint64_t decode(uint64_t val)
|
||||
{
|
||||
return be64toh(val);
|
||||
}
|
||||
int16_t decode(int16_t val)
|
||||
{
|
||||
return be16toh(val);
|
||||
}
|
||||
int32_t decode(int32_t val)
|
||||
{
|
||||
return be32toh(val);
|
||||
}
|
||||
int64_t decode(int64_t val)
|
||||
{
|
||||
return be64toh(val);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
T peek()
|
||||
{
|
||||
assert(_buf_pos <= _buf_end);
|
||||
T val;
|
||||
std::memcpy(&val, _buf_pos, sizeof(T));
|
||||
return decode(val);
|
||||
}
|
||||
template <typename T>
|
||||
T peek()
|
||||
{
|
||||
assert(buf_pos_ <= buf_end_);
|
||||
T val;
|
||||
std::memcpy(&val, buf_pos_, sizeof(T));
|
||||
return decode(val);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void parse(T& val)
|
||||
{
|
||||
val = peek<T>();
|
||||
_buf_pos += sizeof(T);
|
||||
}
|
||||
template <typename T>
|
||||
void parse(T& val)
|
||||
{
|
||||
val = peek<T>();
|
||||
buf_pos_ += sizeof(T);
|
||||
}
|
||||
|
||||
void parse(double& val)
|
||||
{
|
||||
uint64_t inner;
|
||||
parse<uint64_t>(inner);
|
||||
std::memcpy(&val, &inner, sizeof(double));
|
||||
}
|
||||
void parse(float& val)
|
||||
{
|
||||
uint32_t inner;
|
||||
parse<uint32_t>(inner);
|
||||
std::memcpy(&val, &inner, sizeof(float));
|
||||
}
|
||||
void parse(double& val)
|
||||
{
|
||||
uint64_t inner;
|
||||
parse<uint64_t>(inner);
|
||||
std::memcpy(&val, &inner, sizeof(double));
|
||||
}
|
||||
void parse(float& val)
|
||||
{
|
||||
uint32_t inner;
|
||||
parse<uint32_t>(inner);
|
||||
std::memcpy(&val, &inner, sizeof(float));
|
||||
}
|
||||
|
||||
// UR uses 1 byte for boolean values but sizeof(bool) is implementation
|
||||
// defined so we must ensure they're parsed as uint8_t on all compilers
|
||||
void parse(bool& val)
|
||||
{
|
||||
uint8_t inner;
|
||||
parse<uint8_t>(inner);
|
||||
val = inner != 0;
|
||||
}
|
||||
// UR uses 1 byte for boolean values but sizeof(bool) is implementation
|
||||
// defined so we must ensure they're parsed as uint8_t on all compilers
|
||||
void parse(bool& val)
|
||||
{
|
||||
uint8_t inner;
|
||||
parse<uint8_t>(inner);
|
||||
val = inner != 0;
|
||||
}
|
||||
|
||||
// Explicit parsing order of fields to avoid issues with struct layout
|
||||
void parse(double3_t& val)
|
||||
{
|
||||
parse(val.x);
|
||||
parse(val.y);
|
||||
parse(val.z);
|
||||
}
|
||||
// Explicit parsing order of fields to avoid issues with struct layout
|
||||
void parse(double3_t& val)
|
||||
{
|
||||
parse(val.x);
|
||||
parse(val.y);
|
||||
parse(val.z);
|
||||
}
|
||||
|
||||
// Explicit parsing order of fields to avoid issues with struct layout
|
||||
void parse(cartesian_coord_t& val)
|
||||
{
|
||||
parse(val.position);
|
||||
parse(val.rotation);
|
||||
}
|
||||
// Explicit parsing order of fields to avoid issues with struct layout
|
||||
void parse(cartesian_coord_t& val)
|
||||
{
|
||||
parse(val.position);
|
||||
parse(val.rotation);
|
||||
}
|
||||
|
||||
void parse_remainder(std::string& val)
|
||||
{
|
||||
parse(val, size_t(_buf_end - _buf_pos));
|
||||
}
|
||||
void parse_remainder(std::string& val)
|
||||
{
|
||||
parse(val, size_t(buf_end_ - buf_pos_));
|
||||
}
|
||||
|
||||
void parse(std::string& val, size_t len)
|
||||
{
|
||||
val.assign(reinterpret_cast<char*>(_buf_pos), len);
|
||||
_buf_pos += len;
|
||||
}
|
||||
void parse(std::string& val, size_t len)
|
||||
{
|
||||
val.assign(reinterpret_cast<char*>(buf_pos_), len);
|
||||
buf_pos_ += len;
|
||||
}
|
||||
|
||||
// Special string parse function that assumes uint8_t len followed by chars
|
||||
void parse(std::string& val)
|
||||
{
|
||||
uint8_t len;
|
||||
parse(len);
|
||||
parse(val, size_t(len));
|
||||
}
|
||||
// Special string parse function that assumes uint8_t len followed by chars
|
||||
void parse(std::string& val)
|
||||
{
|
||||
uint8_t len;
|
||||
parse(len);
|
||||
parse(val, size_t(len));
|
||||
}
|
||||
|
||||
template <typename T, size_t N>
|
||||
void parse(T (&array)[N])
|
||||
template <typename T, size_t N>
|
||||
void parse(T (&array)[N])
|
||||
{
|
||||
for (size_t i = 0; i < N; i++)
|
||||
{
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
parse(array[i]);
|
||||
}
|
||||
parse(array[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void consume()
|
||||
{
|
||||
_buf_pos = _buf_end;
|
||||
}
|
||||
void consume(size_t bytes)
|
||||
{
|
||||
_buf_pos += bytes;
|
||||
}
|
||||
void consume()
|
||||
{
|
||||
buf_pos_ = buf_end_;
|
||||
}
|
||||
void consume(size_t bytes)
|
||||
{
|
||||
buf_pos_ += bytes;
|
||||
}
|
||||
|
||||
bool check_size(size_t bytes)
|
||||
{
|
||||
return bytes <= size_t(_buf_end - _buf_pos);
|
||||
}
|
||||
template <typename T>
|
||||
bool check_size(void)
|
||||
{
|
||||
return check_size(T::SIZE);
|
||||
}
|
||||
bool checkSize(size_t bytes)
|
||||
{
|
||||
return bytes <= size_t(buf_end_ - buf_pos_);
|
||||
}
|
||||
template <typename T>
|
||||
bool checkSize(void)
|
||||
{
|
||||
return checkSize(T::SIZE);
|
||||
}
|
||||
|
||||
bool empty()
|
||||
{
|
||||
return _buf_pos == _buf_end;
|
||||
}
|
||||
bool empty()
|
||||
{
|
||||
return buf_pos_ == buf_end_;
|
||||
}
|
||||
|
||||
void debug()
|
||||
{
|
||||
LOG_DEBUG("BinParser: %p - %p (%zu bytes)", _buf_pos, _buf_end, _buf_end - _buf_pos);
|
||||
}
|
||||
void debug()
|
||||
{
|
||||
LOG_DEBUG("BinParser: %p - %p (%zu bytes)", buf_pos_, buf_end_, buf_end_ - buf_pos_);
|
||||
}
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user