serialization.h
Go to the documentation of this file.
00001 //==============================================================================
00002 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
00018 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
00019 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
00020 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //==============================================================================
00028 
00029 #ifndef UBLOX_SERIALIZATION_H
00030 #define UBLOX_SERIALIZATION_H
00031 
00032 #include <ros/console.h>
00033 #include <stdint.h>
00034 #include <boost/call_traits.hpp>
00035 #include <vector>
00036 #include <algorithm>
00037 
00038 #include "checksum.h"
00039 
00048 
00049 
00054 namespace ublox {
00055 
00057 static const uint8_t DEFAULT_SYNC_A = 0xB5; 
00059 static const uint8_t DEFAULT_SYNC_B = 0x62; 
00061 static const uint8_t kHeaderLength = 6; 
00063 static const uint8_t kChecksumLength = 2; 
00064 
00068 template <typename T>
00069 struct Serializer {
00076   static void read(const uint8_t *data, uint32_t count, 
00077                    typename boost::call_traits<T>::reference message);
00085   static uint32_t serializedLength(
00086       typename boost::call_traits<T>::param_type message);
00087   
00094   static void write(uint8_t *data, uint32_t size, 
00095                     typename boost::call_traits<T>::param_type message);
00096 };
00097 
00102 template <typename T>
00103 class Message {
00104  public:
00111   static bool canDecode(uint8_t class_id, uint8_t message_id) {
00112     return std::find(keys_.begin(), keys_.end(), 
00113                      std::make_pair(class_id, message_id)) != keys_.end();
00114   }
00115   
00122   static void addKey(uint8_t class_id, uint8_t message_id) {
00123     keys_.push_back(std::make_pair(class_id, message_id));
00124   }
00125 
00126   struct StaticKeyInitializer
00127   {
00128     StaticKeyInitializer(uint8_t class_id, uint8_t message_id) { 
00129       Message<T>::addKey(class_id, message_id); 
00130     }
00131   };
00132 
00133  private:
00134   static std::vector<std::pair<uint8_t,uint8_t> > keys_;
00135 };
00136 
00140 struct Options {
00144   Options() : sync_a(DEFAULT_SYNC_A), sync_b(DEFAULT_SYNC_B), 
00145               header_length(kHeaderLength), checksum_length(kChecksumLength) {}
00147   uint8_t sync_a; 
00149   uint8_t sync_b; 
00151   uint8_t header_length; 
00153   uint8_t checksum_length; 
00154   
00159   int wrapper_length() {
00160     return header_length + checksum_length; 
00161   }
00162 };
00163 
00167 class Reader {
00168  public:
00175   Reader(const uint8_t *data, uint32_t count, 
00176          const Options &options = Options()) : 
00177       data_(data), count_(count), found_(false), options_(options) {}
00178 
00179   typedef const uint8_t *iterator;
00180 
00185   iterator search()
00186   {
00187     if (found_) next();
00188 
00189     // Search for a message header
00190     for( ; count_ > 0; --count_, ++data_) {
00191       if (data_[0] == options_.sync_a && 
00192           (count_ == 1 || data_[1] == options_.sync_b)) 
00193         break;
00194     }
00195 
00196     return data_;
00197   }
00198 
00203   bool found()
00204   {
00205     if (found_) return true;
00206     // Verify message is long enough to have sync chars, id, length & checksum
00207     if (count_ < options_.wrapper_length()) return false;
00208     // Verify the header bits
00209     if (data_[0] != options_.sync_a || data_[1] != options_.sync_b) 
00210       return false;
00211     // Verify that the buffer length is long enough based on the received
00212     // message length
00213     if (count_ < length() + options_.wrapper_length()) return false;
00214 
00215     found_ = true;
00216     return true;
00217   }
00218 
00226   iterator next() {
00227     if (found()) {
00228       uint32_t size = length() + options_.wrapper_length();
00229       data_ += size; count_ -= size;
00230     }
00231     found_ = false;
00232     return data_;
00233   }
00234 
00239   iterator pos() {
00240     return data_;
00241   }
00242 
00243   iterator end() {
00244     return data_ + count_;
00245   }
00246 
00247   uint8_t classId() { return data_[2]; }
00248   uint8_t messageId() { return data_[3]; }
00249 
00257   uint32_t length() { return (data_[5] << 8) + data_[4]; }
00258   const uint8_t *data() { return data_ + options_.header_length; }
00259   
00265   uint16_t checksum() { 
00266     return *reinterpret_cast<const uint16_t *>(data_ + options_.header_length +
00267                                                length()); 
00268   }
00269 
00275   template <typename T>
00276   bool read(typename boost::call_traits<T>::reference message, 
00277             bool search = false) {
00278     if (search) this->search();
00279     if (!found()) return false; 
00280     if (!Message<T>::canDecode(classId(), messageId())) return false;
00281 
00282     uint16_t chk;
00283     if (calculateChecksum(data_ + 2, length() + 4, chk) != this->checksum()) {
00284       // checksum error
00285       ROS_DEBUG("U-Blox read checksum error: 0x%02x / 0x%02x", classId(), 
00286                 messageId());
00287       return false;
00288     }
00289 
00290     Serializer<T>::read(data_ + options_.header_length, length(), message);
00291     return true;
00292   }
00293 
00299   template <typename T> 
00300   bool hasType() {
00301     if (!found()) return false;
00302     return Message<T>::canDecode(classId(), messageId());
00303   }
00304 
00310   bool isMessage(uint8_t class_id, uint8_t message_id) {
00311     if (!found()) return false;
00312     return (classId() == class_id && messageId() == message_id);
00313   }
00314 
00315  private:
00317   const uint8_t *data_; 
00319   uint32_t count_; 
00321   bool found_; 
00323   Options options_; 
00324 };
00325 
00329 class Writer {
00330  public:
00331   typedef uint8_t *iterator;
00332 
00339   Writer(uint8_t *data, uint32_t size, const Options &options = Options()) : 
00340       data_(data), size_(size), options_(options) {}
00341 
00349   template <typename T> bool write(const T& message, 
00350                                    uint8_t class_id = T::CLASS_ID, 
00351                                    uint8_t message_id = T::MESSAGE_ID) {
00352     // Check for buffer overflow
00353     uint32_t length = Serializer<T>::serializedLength(message);
00354     if (size_ < length + options_.wrapper_length()) {
00355       ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", 
00356                 class_id, message_id);
00357       return false;
00358     }
00359     // Encode the message and add it to the buffer
00360     Serializer<T>::write(data_ + options_.header_length, 
00361                          size_ - options_.header_length, message);
00362     return write(0, length, class_id, message_id);
00363   }
00364 
00374   bool write(const uint8_t* message, uint32_t length, uint8_t class_id, 
00375              uint8_t message_id) {
00376     if (size_ < length + options_.wrapper_length()) {
00377       ROS_ERROR("u-blox write buffer overflow. Message %u / %u not written", 
00378                 class_id, message_id);
00379       return false;
00380     }
00381     iterator start = data_;
00382 
00383     // write header
00384     *data_++ = options_.sync_a;
00385     *data_++ = options_.sync_b;
00386     *data_++ = class_id;
00387     *data_++ = message_id;
00388     *data_++ = length & 0xFF;
00389     *data_++ = (length >> 8) & 0xFF;
00390     size_ -= options_.header_length;
00391 
00392     // write message
00393     if (message) std::copy(message, message + length, data_);
00394     data_ += length;
00395     size_ -= length;
00396 
00397     // write checksum
00398     uint8_t ck_a, ck_b;
00399     calculateChecksum(start + 2, length + 4, ck_a, ck_b);
00400     *data_++ = ck_a;
00401     *data_++ = ck_b;
00402     size_ -= options_.checksum_length;
00403 
00404     return true;
00405   }
00406 
00407   iterator end() {
00408     return data_;
00409   }
00410 
00411  private:
00413   iterator data_; 
00415 
00416   uint32_t size_; 
00418   Options options_; 
00419 };
00420 
00421 } // namespace ublox
00422 
00423 // Use to declare u-blox messages and message serializers
00424 #define DECLARE_UBLOX_MESSAGE(class_id, message_id, package, message) \
00425   template class ublox::Serializer<package::message>; \
00426   template class ublox::Message<package::message>; \
00427   namespace package { namespace { \
00428     static const ublox::Message<message>::StaticKeyInitializer static_key_initializer_##message(class_id, message_id); \
00429   } } \
00430 
00431 // Use for messages which have the same structure but different IDs, e.g. INF
00432 // Call DECLARE_UBLOX_MESSAGE for the first message and DECLARE_UBLOX_MESSAGE_ID
00433 // for following declarations
00434 #define DECLARE_UBLOX_MESSAGE_ID(class_id, message_id, package, message, name) \
00435   namespace package { namespace { \
00436     static const ublox::Message<message>::StaticKeyInitializer static_key_initializer_##name(class_id, message_id); \
00437   } } \
00438 
00439 
00440 // use implementation of class Serializer in "serialization_ros.h"
00441 #include "serialization_ros.h"
00442 
00443 #endif // UBLOX_SERIALIZATION_H


ublox_serialization
Author(s): Johannes Meyer
autogenerated on Fri Jun 14 2019 19:26:11