Program Listing for File ByteOrderConversion.h
↰ Return to documentation for file (/tmp/ws/src/schunk_svh_library/include/schunk_svh_library/serial/ByteOrderConversion.h)
//
// © Copyright 2022 SCHUNK Mobile Greifsysteme GmbH, Lauffen/Neckar Germany
// © Copyright 2022 FZI Forschungszentrum Informatik, Karlsruhe, Germany
//
// This file is part of the Schunk SVH Library.
//
// The Schunk SVH Library is free software: you can redistribute it and/or
// modify it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or (at your
// option) any later version.
//
// The Schunk SVH Library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
// Public License for more details.
//
// You should have received a copy of the GNU General Public License along with
// the Schunk SVH Library. If not, see <https://www.gnu.org/licenses/>.
//
//----------------------------------------------------------------------
//----------------------------------------------------------------------
#ifndef DRIVER_SVH_BYTE_ORDER_CONVERISON_H_INCLUDED
#define DRIVER_SVH_BYTE_ORDER_CONVERISON_H_INCLUDED
#include "schunk_svh_library/ImportExport.h"
#include <assert.h>
#include <iomanip>
#include <iostream>
#include <vector>
namespace driver_svh {
template <typename T>
size_t toLittleEndian(const T& data, std::vector<uint8_t>& array, size_t& write_pos)
{
// Resize the target array in case it it to small to avoid out of bounds acces
if (write_pos + sizeof(T) > array.size())
{
// TODO: Remove Debug
// std::cout << "To Little Endian has to extend the Array. Current array Size: "<<
// (int)array.size() << " Pos: " << (int)pos << " Size_T: "<< sizeof(T) << " New array Size: "<<
// (int)(pos + sizeof(T)) << std::endl;
array.resize(write_pos + sizeof(T));
}
// Endianess Conversion
for (size_t i = 0; i < sizeof(T); ++i)
{
// Copy each byte into the bytearray, always convert byte order to little endian regardles of
// source architecture
array[write_pos + i] = static_cast<uint8_t>((data >> (i * 8)) & 0xFF);
}
return write_pos + sizeof(T);
}
template <>
DRIVER_SVH_IMPORT_EXPORT size_t toLittleEndian<float>(const float& data,
std::vector<uint8_t>& array,
size_t& write_pos);
template <>
DRIVER_SVH_IMPORT_EXPORT size_t toLittleEndian<double>(const double& data,
std::vector<uint8_t>& array,
size_t& write_pos);
template <typename T>
size_t fromLittleEndian(T& data, std::vector<uint8_t>& array, size_t& read_pos)
{
// TODO: Remove once everything is tested :)
// std::cout << "From Little Endian Called with: "<<" Size_T: "<< sizeof(T) << " Pos: " <<
// (int)read_pos << "Current Array Size: "<< (int)array.size() << std::endl;
// Reset data as we only write with or
data = 0;
// Check if ArrayBuilder has enough data
if (read_pos + sizeof(T) > array.size())
{
// TODO: better error handling?
return read_pos;
}
// Endianess Conversion
for (size_t i = 0; i < sizeof(T); ++i)
{
// Copy each byte into the bytearray, always convert byte order back from little endian
data |= (array[read_pos + i] & 0xFF) << (i * 8);
// std::cout << "Converting Value: 0x" << std::setw(2) << std::setfill('0') << std::hex <<
// static_cast<int>((array[read_pos+i]& 0xFF) <<(i*8)) << " At the position
// i="<<static_cast<int>(i) << "resulting in the variable data: " << static_cast<int>(data) <<
// std::dec <<" Or on DEC :" << static_cast<int>(data) << std::endl;
}
// Note: The Vector still contains the elements at this point maybe we would like to delete that?
// But its expensive
return read_pos + sizeof(T);
}
template <>
DRIVER_SVH_IMPORT_EXPORT size_t fromLittleEndian<float>(float& data,
std::vector<uint8_t>& array,
size_t& read_pos);
template <>
DRIVER_SVH_IMPORT_EXPORT size_t fromLittleEndian<double>(double& data,
std::vector<uint8_t>& array,
size_t& read_pos);
class ArrayBuilder
{
public:
ArrayBuilder(size_t array_size = 1)
: write_pos(0)
, read_pos(0)
, array(array_size, 0)
{
}
size_t write_pos;
size_t read_pos;
std::vector<uint8_t> array;
void reset(size_t array_size = 1);
template <typename T>
void appendWithoutConversion(const T& data)
{
// Resize the target array in case it it to small to avoid out of bounds acces
if (write_pos + sizeof(T) > array.size())
{
array.resize(write_pos + sizeof(T));
}
// write data to array without conversion
*(reinterpret_cast<T*>(&array[write_pos])) = data;
write_pos += sizeof(T);
}
template <typename T>
void appendWithoutConversion(const std::vector<T>& data)
{
// Just insert every element of the Vector individually
for (typename std::vector<T>::const_iterator it = data.begin(); it != data.end(); ++it)
{
appendWithoutConversion(*it);
}
}
template <typename T>
ArrayBuilder& operator<<(const T& data);
template <typename T>
ArrayBuilder& operator<<(const std::vector<T>& data);
template <typename T>
ArrayBuilder& operator>>(T& data);
template <typename T>
ArrayBuilder& operator>>(std::vector<T>& data);
template <typename T>
T readBack();
};
template <typename T>
ArrayBuilder& ArrayBuilder::operator<<(const T& data)
{
// Convert the type to correct encoding and poit it into the Array
write_pos = toLittleEndian<T>(data, array, write_pos);
// TODO: Remove debug output
// std::cout << "Arraybuilder got a generic type of length: "<< sizeof(data) << " and With data
// Packet: "<< (int)data << std::endl;
return *this;
}
template <typename T>
ArrayBuilder& ArrayBuilder::operator<<(const std::vector<T>& data)
{
// Just insert every element of the Vector individually
for (typename std::vector<T>::const_iterator it = data.begin(); it != data.end(); ++it)
{
*this << *it;
}
// TODO: Remove Debug output
// std::cout << "ArrayBuilder got a vector of length: "<< data.size() << std::endl;
return *this;
}
template <typename T>
ArrayBuilder& ArrayBuilder::operator>>(T& data)
{
read_pos = fromLittleEndian<T>(data, array, read_pos);
return *this;
}
template <typename T>
ArrayBuilder& ArrayBuilder::operator>>(std::vector<T>& data)
{
// Todo: For u_int8 Vectors this could also just be handled by an insert -> faster :)
// Just insert every element of the Vector individually --> Do it in reverse order as we read from
// the end of the list :)
for (typename std::vector<T>::iterator it = data.begin(); it != data.end(); ++it)
{
*this >> *it;
}
return *this;
}
template <typename T>
T ArrayBuilder::readBack()
{
T data;
size_t read_back_pos = write_pos - sizeof(T);
fromLittleEndian<T>(data, array, read_back_pos);
return data;
}
std::ostream& operator<<(std::ostream& o, const ArrayBuilder& ab);
} // namespace driver_svh
#endif