.. _program_listing_file__tmp_ws_src_ublox_ublox_serialization_include_ublox_serialization_serialization.hpp:

Program Listing for File serialization.hpp
==========================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_ublox_ublox_serialization_include_ublox_serialization_serialization.hpp>` (``/tmp/ws/src/ublox/ublox_serialization/include/ublox_serialization/serialization.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   //==============================================================================
   // 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