novatel_message_extractor.h

Go to the documentation of this file.

// *****************************************************************************
//
// Copyright (c) 2017, Southwest Research Institute® (SwRI®)
// 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 Southwest Research Institute® (SwRI®) 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 SOUTHWEST RESEARCH INSTITUTE 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 NOVTEL_OEM628_NOVATEL_MESSAGE_PARSER_H_
#define NOVTEL_OEM628_NOVATEL_MESSAGE_PARSER_H_

#include <limits>
#include <sstream>
#include <string>
#include <vector>

#include <gps_common/GPSFix.h>
#include <novatel_gps_msgs/Gpgga.h>
#include <novatel_gps_msgs/Gpgsa.h>
#include <novatel_gps_msgs/Gpgsv.h>
#include <novatel_gps_msgs/Gprmc.h>
#include <novatel_gps_msgs/NovatelCorrectedImuData.h>
#include <novatel_gps_msgs/NovatelPosition.h>
#include <novatel_gps_msgs/NovatelMessageHeader.h>
#include <novatel_gps_msgs/NovatelReceiverStatus.h>
#include <novatel_gps_msgs/NovatelVelocity.h>
#include <novatel_gps_msgs/Range.h>
#include <novatel_gps_msgs/Time.h>
#include <novatel_gps_msgs/Trackstat.h>

#include <novatel_gps_driver/binary_message.h>
#include <novatel_gps_driver/nmea_sentence.h>
#include <novatel_gps_driver/novatel_sentence.h>

namespace novatel_gps_driver
{
  class NovatelMessageExtractor
  {
  public:
    bool ExtractCompleteMessages(
        const std::string& input,
        std::vector<NmeaSentence>& nmea_sentences,
        std::vector<NovatelSentence>& novatel_sentences,
        std::vector<BinaryMessage>& binary_messages,
        std::string& remaining,
        bool keep_nmea_container = false);

    double GetMostRecentUtcTime(const std::vector<NmeaSentence>& sentences);

    void GetGpsFixMessage(
        const novatel_gps_msgs::Gprmc& gprmc,
        const novatel_gps_msgs::Gpgga& gpgga,
        const gps_common::GPSFixPtr& gps_fix);

  private:
    // Constants for parsing message structures
    static const std::string CHECKSUM_FLAG;
    static const std::string FIELD_SEPARATOR;
    static const std::string HEADER_SEPARATOR;
    static const std::string NMEA_SENTENCE_FLAG;
    static const std::string NOVATEL_SENTENCE_FLAG;
    static const std::string NOVATEL_ASCII_FLAGS;
    static const std::string NOVATEL_BINARY_SYNC_BYTES;
    static const std::string NOVATEL_ENDLINE;

    static constexpr uint32_t NOVATEL_CRC32_POLYNOMIAL = 0xEDB88320L;

    // From Novatel OEMV® Family Firmware Reference Manual
    uint32_t CalculateBlockCRC32(
        uint32_t ulCount,
        const uint8_t* ucBuffer);

    uint32_t CRC32Value(int32_t i);


    void FindAsciiSentence(const std::string& sentence,
                           size_t current_idx,
                           size_t& start_idx,
                           size_t& end_idx,
                           size_t& invalid_char_idx);

    int32_t GetBinaryMessage(const std::string& str,
                             size_t start_idx,
                             BinaryMessage& msg);

    bool GetNovatelMessageParts(
        const std::string& sentence,
        std::string& message_id,
        std::vector<std::string>& header,
        std::vector<std::string>& body);

    int32_t GetNmeaSentence(
        const std::string& str,
        size_t start_idx,
        std::string& sentence,
        bool keep_container = false);

    int32_t GetNovatelSentence(
        const std::string& str,
        size_t start_idx,
        std::string& sentence);

    size_t GetSentenceChecksumStart(
        const std::string& str,
        size_t start_idx);

    uint8_t NmeaChecksum(const std::string& sentence);

    bool VectorizeNovatelSentence(
        const std::string& data,
        NovatelSentence& sentence);

    void VectorizeNmeaSentence(
        const std::string& sentence,
        NmeaSentence& vectorized_message);

    void VectorizeString(
        const std::string& str,
        std::vector<std::string>& vectorized_message,
        const std::string& delimiters);
  };
}

#endif  // NOVATEL_OEM628_NOVATEL_MESSAGE_PARSER_H_