Program Listing for File serialization.hpp

↰ Return to documentation for file (/tmp/ws/src/ublox/ublox_serialization/include/ublox_serialization/serialization.hpp)

//==============================================================================
// Copyright (c) 2012, Johannes Meyer, TU Darmstadt
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the Flight Systems and Automatic Control group,
//       TU Darmstadt, nor the names of its contributors may be used to
//       endorse or promote products derived from this software without
//       specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//==============================================================================

#ifndef UBLOX_SERIALIZATION_SERIALIZATION_HPP
#define UBLOX_SERIALIZATION_SERIALIZATION_HPP

#include <algorithm>
#include <array>
#include <cstdint>
#include <cstring>
#include <string>
#include <utility>
#include <vector>

#include "checksum.hpp"



namespace ublox {

static const uint8_t DEFAULT_SYNC_A = 0xB5;
static const uint8_t DEFAULT_SYNC_B = 0x62;
static const uint8_t kHeaderLength = 6;
static const uint8_t kChecksumLength = 2;

template<typename T, typename Enabled = void>
struct UbloxSerializer
{
  template<typename Stream>
  inline static void write(Stream& stream, const T & t);

  template<typename Stream>
  inline static void read(Stream& stream, T & t);

  inline static uint32_t serializedLength(const T & t);
};

template <typename T>
struct UbloxSerializer<T, typename std::enable_if<std::is_same<T, uint8_t>::value ||
                                                  std::is_same<T, uint16_t>::value ||
                                                  std::is_same<T, uint32_t>::value ||
                                                  std::is_same<T, uint64_t>::value ||
                                                  std::is_same<T, int8_t>::value ||
                                                  std::is_same<T, int16_t>::value ||
                                                  std::is_same<T, int32_t>::value ||
                                                  std::is_same<T, int64_t>::value ||
                                                  std::is_same<T, float>::value ||
                                                  std::is_same<T, double>::value>::type>
{
  template<typename Stream>
  inline static void write(Stream& stream, const T v) {
    *reinterpret_cast<T*>(stream.advance(sizeof(v))) = v;
  }

  template<typename Stream>
  inline static void read(Stream& stream, T& v) {
    v = *reinterpret_cast<T*>(stream.advance(sizeof(v)));
  }

  inline static uint32_t serializedLength(const T& v) {
    (void)v;
    return sizeof(T);
  }
};

template<typename T, typename Stream>
inline void serialize(Stream& stream, const T& t) {
  UbloxSerializer<T>::write(stream, t);
}

template<typename T, typename Stream>
inline void deserialize(Stream& stream, T& t) {
  UbloxSerializer<T>::read(stream, t);
}

template<typename T>
inline uint32_t serializationLength(const T& t) {
  return UbloxSerializer<T>::serializedLength(t);
}

template<typename T, size_t N, class Enabled = void>
struct StdArrayUbloxSerializer
{};

template<typename T, size_t N>
struct StdArrayUbloxSerializer<T, N, typename std::enable_if<std::is_same<T, uint8_t>::value ||
                                                             std::is_same<T, uint16_t>::value ||
                                                             std::is_same<T, uint32_t>::value ||
                                                             std::is_same<T, uint64_t>::value ||
                                                             std::is_same<T, int8_t>::value ||
                                                             std::is_same<T, int16_t>::value ||
                                                             std::is_same<T, int32_t>::value ||
                                                             std::is_same<T, int64_t>::value ||
                                                             std::is_same<T, float>::value ||
                                                             std::is_same<T, double>::value>::type>
{
  template<typename Stream>
  inline static void write(Stream& stream, const std::array<T, N>& v) {
    const uint32_t data_len = N * sizeof(T);
    std::memcpy(stream.advance(data_len), &v.front(), data_len);
  }

  template<typename Stream>
  inline static void read(Stream& stream, std::array<T, N>& v) {
    const uint32_t data_len = N * sizeof(T);
    std::memcpy(&v.front(), stream.advance(data_len), data_len);
  }

  inline static uint32_t serializedLength(const std::array<T, N>& v) {
    (void)v;
    return N * sizeof(T);
  }
};

template<typename T, size_t N, typename Stream>
inline void serialize(Stream& stream, const std::array<T, N>& t)
{
  StdArrayUbloxSerializer<T, N>::write(stream, t);
}

template<typename T, size_t N, typename Stream>
inline void deserialize(Stream& stream, std::array<T, N>& t) {
  StdArrayUbloxSerializer<T, N>::read(stream, t);
}

template<typename T, size_t N>
inline uint32_t serializationLength(const std::array<T, N>& t)
{
  return StdArrayUbloxSerializer<T, N>::serializedLength(t);
}

struct UbloxStream
{
  /*
   * \brief Returns a pointer to the current position of the stream
   */
  inline uint8_t* getData() {
    return data_;
  }
  uint8_t* advance(uint32_t len) {
    uint8_t* old_data = data_;
    data_ += len;
    if (data_ > end_) {
      // Throwing directly here causes a significant speed hit due to the extra code generated
      // for the throw statement
      //throwStreamOverrun();
    }
    return old_data;
  }

  inline uint32_t getLength() { return static_cast<uint32_t>(end_ - data_); }

protected:
  UbloxStream(uint8_t* _data, uint32_t _count)
  : data_(_data)
  , end_(_data + _count)
  {}

private:
  uint8_t* data_;
  uint8_t* end_;
};

struct UbloxIStream : public UbloxStream
{
  UbloxIStream(uint8_t* data, uint32_t count)
  : UbloxStream(data, count)
  {}

  template<typename T>
  void next(T& t) {
    deserialize(*this, t);
  }
};

struct UbloxOStream : public UbloxStream
{
  UbloxOStream(uint8_t* data, uint32_t count)
  : UbloxStream(data, count)
  {}

  template<typename T>
  void next(const T& t) {
    serialize(*this, t);
  }
};

template <typename T>
class Message {
 public:
  static bool canDecode(uint8_t class_id, uint8_t message_id) {
    return std::find(keys_.begin(), keys_.end(),
                     std::make_pair(class_id, message_id)) != keys_.end();
  }

  static void addKey(uint8_t class_id, uint8_t message_id) {
    keys_.emplace_back(std::make_pair(class_id, message_id));
  }

  struct StaticKeyInitializer
  {
    StaticKeyInitializer(uint8_t class_id, uint8_t message_id) {
      Message<T>::addKey(class_id, message_id);
    }
  };

 private:
  static std::vector<std::pair<uint8_t,uint8_t> > keys_;
};

struct Options {
  Options() : sync_a(DEFAULT_SYNC_A), sync_b(DEFAULT_SYNC_B),
              header_length(kHeaderLength), checksum_length(kChecksumLength) {}
  uint8_t sync_a;
  uint8_t sync_b;
  uint8_t header_length;
  uint8_t checksum_length;

  uint32_t wrapper_length() {
    return header_length + checksum_length;
  }
};

class Reader {
 public:
  Reader(const uint8_t *data, uint32_t count,
         const Options &options = Options()) :
      data_(data), count_(count), found_(false), options_(options)
  {
    extra_data_.reserve(1024);
  }

  using iterator = const uint8_t *;

  iterator search()
  {
    if (found_) {
      next();
    }

    // Search for a message header
    for( ; count_ > 0; --count_, ++data_) {
      if (data_[0] == options_.sync_a &&
          (count_ == 1 || data_[1] == options_.sync_b)) {
        break;
      } else {
        extra_data_.push_back(data_[0]);
      }
    }

    return data_;
  }

  bool found()
  {
    if (found_) {
      return true;
    }
    // Verify message is long enough to have sync chars, id, length & checksum
    if (count_ < options_.wrapper_length()) {
      return false;
    }
    // Verify the header bits
    if (data_[0] != options_.sync_a || data_[1] != options_.sync_b) {
      return false;
    }
    // Verify that the buffer length is long enough based on the received
    // message length
    if (count_ < length() + options_.wrapper_length()) {
      return false;
    }

    found_ = true;
    return true;
  }

  iterator next() {
    if (found()) {
      uint32_t size = length() + options_.wrapper_length();
      data_ += size; count_ -= size;
    }
    found_ = false;
    return data_;
  }

  iterator pos() {
    return data_;
  }

  iterator end() {
    return data_ + count_;
  }

  uint8_t classId() { return data_[2]; }
  uint8_t messageId() { return data_[3]; }

  uint32_t length() { return (data_[5] << 8) + data_[4]; }
  const uint8_t *data() { return data_ + options_.header_length; }

  uint16_t checksum() {
    return *reinterpret_cast<const uint16_t *>(data_ + options_.header_length +
                                               length());
  }

  template <typename T>
  bool read(T &message,
            bool search = false) {
    if (search) {
      this->search();
    }
    if (!found()) {
      return false;
    }
    if (!Message<T>::canDecode(classId(), messageId())) {
      return false;
    }

    uint16_t chk{0};
    if (calculateChecksum(data_ + 2, length() + 4, chk) != this->checksum()) {
      // checksum error
      // Note that it is possible (and even likely) that we get here without
      // having the entire packet available.  This happens when there are both
      // NMEA and UBlox messages configured on the serial wire, and the packet is
      // laid out like:
      //
      // <NMEA_message><UBlox_message_missing_two_bytes>
      //
      // Therefore, we do not print errors in this case and instead just don't
      // do any additional work.
      return false;
    }

    UbloxSerializer<T>::read(data_ + options_.header_length, length(), message);
    return true;
  }

  template <typename T>
  bool hasType() {
    if (!found()) {
      return false;
    }
    return Message<T>::canDecode(classId(), messageId());
  }

  bool isMessage(uint8_t class_id, uint8_t message_id) {
    if (!found()) {
      return false;
    }
    return (classId() == class_id && messageId() == message_id);
  }

  const std::string &getExtraData() const {
    return extra_data_;
  }

private:
  const uint8_t *data_;
  std::string extra_data_;
  uint32_t count_;
  bool found_;
  Options options_;
};

class Writer {
 public:
  using iterator = uint8_t *;

  Writer(uint8_t *data, uint32_t size, const Options &options = Options()) :
      data_(data), size_(size), options_(options) {}

  template <typename T> bool write(const T& message,
                                   uint8_t class_id = T::CLASS_ID,
                                   uint8_t message_id = T::MESSAGE_ID) {
    // Check for buffer overflow
    uint32_t length = UbloxSerializer<T>::serializedLength(message);
    if (size_ < length + options_.wrapper_length()) {
      // ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written",
      //           class_id, message_id);
      return false;
    }
    // Encode the message and add it to the buffer
    UbloxSerializer<T>::write(data_ + options_.header_length,
                         size_ - options_.header_length, message);
    return write(nullptr, length, class_id, message_id);
  }

  bool write(const uint8_t* message, uint32_t length, uint8_t class_id,
             uint8_t message_id) {
    if (size_ < length + options_.wrapper_length()) {
      // ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written",
      //           class_id, message_id);
      return false;
    }
    iterator start = data_;

    // write header
    *data_++ = options_.sync_a;
    *data_++ = options_.sync_b;
    *data_++ = class_id;
    *data_++ = message_id;
    *data_++ = length & 0xFF;
    *data_++ = (length >> 8) & 0xFF;
    size_ -= options_.header_length;

    // write message
    if (message) {
      std::copy(message, message + length, data_);
    }
    data_ += length;
    size_ -= length;

    // write checksum
    uint8_t ck_a, ck_b;
    calculateChecksum(start + 2, length + 4, ck_a, ck_b);
    *data_++ = ck_a;
    *data_++ = ck_b;
    size_ -= options_.checksum_length;

    return true;
  }

  iterator end() {
    return data_;
  }

 private:
  iterator data_;

  uint32_t size_;
  Options options_;
};

}  // namespace ublox

// Use to declare u-blox messages and message serializers
#define DECLARE_UBLOX_MESSAGE(class_id, message_id, package, message) \
  template class ublox::UbloxSerializer<package::msg::message>; \
  template class ublox::Message<package::msg::message>; \
  namespace package { namespace { \
    static const ublox::Message<package::msg::message>::StaticKeyInitializer static_key_initializer_##message(class_id, message_id); \
  } } \

// Use for messages which have the same structure but different IDs, e.g. INF
// Call DECLARE_UBLOX_MESSAGE for the first message and DECLARE_UBLOX_MESSAGE_ID
// for following declarations
#define DECLARE_UBLOX_MESSAGE_ID(class_id, message_id, package, message, name) \
  namespace package { namespace { \
    static const ublox::Message<package::msg::message>::StaticKeyInitializer static_key_initializer_##name(class_id, message_id); \
  } } \

#endif  // UBLOX_SERIALIZATION_SERIALIZATION_HPP