Program Listing for File novatel_message_extractor.h

↰ Return to documentation for file (/tmp/ws/src/novatel_gps_driver/novatel_gps_driver/include/novatel_gps_driver/novatel_message_extractor.h)

// *****************************************************************************
//
// Copyright (c) 2019, 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_msgs/msg/gps_fix.hpp>
#include <novatel_gps_msgs/msg/gpgga.hpp>
#include <novatel_gps_msgs/msg/gpgsa.hpp>
#include <novatel_gps_msgs/msg/gpgsv.hpp>
#include <novatel_gps_msgs/msg/gprmc.hpp>
#include <novatel_gps_msgs/msg/novatel_corrected_imu_data.hpp>
#include <novatel_gps_msgs/msg/novatel_position.hpp>
#include <novatel_gps_msgs/msg/novatel_message_header.hpp>
#include <novatel_gps_msgs/msg/novatel_receiver_status.hpp>
#include <novatel_gps_msgs/msg/novatel_velocity.hpp>
#include <novatel_gps_msgs/msg/range.hpp>
#include <novatel_gps_msgs/msg/time.hpp>
#include <novatel_gps_msgs/msg/trackstat.hpp>

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

#include <rclcpp/logger.hpp>

namespace novatel_gps_driver
{
  class NovatelMessageExtractor
  {
  public:
    explicit NovatelMessageExtractor(rclcpp::Logger logger);
    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::msg::Gprmc& gprmc,
        const novatel_gps_msgs::msg::Gpgga& gpgga,
        const gps_msgs::msg::GPSFix::UniquePtr& 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;

    rclcpp::Logger logger_;

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