rx_message.hpp

Go to the documentation of this file.

// *****************************************************************************
//
// © Copyright 2020, Septentrio NV/SA.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//    1. Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//    2. 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.
//    3. Neither the name of the copyright holder 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 OR CONTRIBUTORS 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.
//
// *****************************************************************************

// *****************************************************************************
//
// Boost Software License - Version 1.0 - August 17th, 2003
//
// Permission is hereby granted, free of charge, to any person or organization
// obtaining a copy of the software and accompanying documentation covered by
// this license (the "Software") to use, reproduce, display, distribute,
// execute, and transmit the Software, and to prepare derivative works of the
// Software, and to permit third-parties to whom the Software is furnished to
// do so, all subject to the following:

// The copyright notices in the Software and this entire statement, including
// the above license grant, this restriction and the following disclaimer,
// must be included in all copies of the Software, in whole or in part, and
// all derivative works of the Software, unless such copies or derivative
// works are solely in the form of machine-executable object code generated by
// a source language processor.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
//
// *****************************************************************************

#ifndef NMEA_SYNC_BYTE_1
#define NMEA_SYNC_BYTE_1 0x24
#endif
#ifndef NMEA_SYNC_BYTE_2_1
#define NMEA_SYNC_BYTE_2_1 0x47
#endif
#ifndef NMEA_SYNC_BYTE_2_2
#define NMEA_SYNC_BYTE_2_2 0x50
#endif
#ifndef RESPONSE_SYNC_BYTE_1
#define RESPONSE_SYNC_BYTE_1 0x24
#endif
#ifndef RESPONSE_SYNC_BYTE_2
#define RESPONSE_SYNC_BYTE_2 0x52
#endif
#ifndef CARRIAGE_RETURN
#define CARRIAGE_RETURN 0x0D
#endif
#ifndef LINE_FEED
#define LINE_FEED 0x0A
#endif
#ifndef RESPONSE_SYNC_BYTE_3
#define RESPONSE_SYNC_BYTE_3 0x3F
#endif
#ifndef CONNECTION_DESCRIPTOR_BYTE_1
#define CONNECTION_DESCRIPTOR_BYTE_1 0x49
#endif
#ifndef CONNECTION_DESCRIPTOR_BYTE_2
#define CONNECTION_DESCRIPTOR_BYTE_2 0x50
#endif

// C++ libraries
#include <cassert> // for assert
#include <cstddef>
#include <map>
#include <sstream>
// Boost includes
#include <boost/call_traits.hpp>
#include <boost/format.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/tokenizer.hpp>
// ROSaic includes
#include <septentrio_gnss_driver/abstraction/typedefs.hpp>
#include <septentrio_gnss_driver/crc/crc.h>
#include <septentrio_gnss_driver/parsers/nmea_parsers/gpgga.hpp>
#include <septentrio_gnss_driver/parsers/nmea_parsers/gpgsa.hpp>
#include <septentrio_gnss_driver/parsers/nmea_parsers/gpgsv.hpp>
#include <septentrio_gnss_driver/parsers/nmea_parsers/gprmc.hpp>
#include <septentrio_gnss_driver/parsers/string_utilities.h>

#ifndef RX_MESSAGE_HPP
#define RX_MESSAGE_HPP

extern bool g_read_cd;
extern uint32_t g_cd_count;

enum TypeOfPVT_Enum
{
    evNoPVT,
    evStandAlone,
    evDGPS,
    evFixed,
    evRTKFixed,
    evRTKFloat,
    evSBAS,
    evMovingBaseRTKFixed,
    evMovingBaseRTKFloat,
    evPPP
};

enum RxID_Enum
{
    evNavSatFix,
    evINSNavSatFix,
    evGPSFix,
    evINSGPSFix,
    evPoseWithCovarianceStamped,
    evINSPoseWithCovarianceStamped,
    evGPGGA,
    evGPRMC,
    evGPGSA,
    evGPGSV,
    evGLGSV,
    evGAGSV,
    evPVTCartesian,
    evPVTGeodetic,
    evBaseVectorCart,
    evBaseVectorGeod,
    evPosCovCartesian,
    evPosCovGeodetic,
    evAttEuler,
    evAttCovEuler,
    evINSNavCart,
    evINSNavGeod,
    evIMUSetup,
    evVelSensorSetup,
    evExtEventINSNavGeod,
    evExtEventINSNavCart,
    evExtSensorMeas,
    evGPST,
    evChannelStatus,
    evMeasEpoch,
    evDOP,
    evVelCovGeodetic,
    evDiagnosticArray,
    evLocalization,
    evReceiverStatus,
    evQualityInd,
    evReceiverTime,
    evReceiverSetup
};

namespace io_comm_rx {

    class RxMessage
    {
    public:
        RxMessage(ROSaicNodeBase* node, Settings* settings) :
            node_(node), settings_(settings), unix_time_(0)
        {
            found_ = false;
            crc_check_ = false;
            message_size_ = 0;

            std::pair<uint16_t, TypeOfPVT_Enum> type_of_pvt_pairs[] = {
                std::make_pair(static_cast<uint16_t>(0), evNoPVT),
                std::make_pair(static_cast<uint16_t>(1), evStandAlone),
                std::make_pair(static_cast<uint16_t>(2), evDGPS),
                std::make_pair(static_cast<uint16_t>(3), evFixed),
                std::make_pair(static_cast<uint16_t>(4), evRTKFixed),
                std::make_pair(static_cast<uint16_t>(5), evRTKFloat),
                std::make_pair(static_cast<uint16_t>(6), evSBAS),
                std::make_pair(static_cast<uint16_t>(7), evMovingBaseRTKFixed),
                std::make_pair(static_cast<uint16_t>(8), evMovingBaseRTKFloat),
                std::make_pair(static_cast<uint16_t>(10), evPPP)};

            type_of_pvt_map =
                TypeOfPVTMap(type_of_pvt_pairs, type_of_pvt_pairs + evPPP + 1);

            std::pair<std::string, RxID_Enum> rx_id_pairs[] = {
                std::make_pair("NavSatFix", evNavSatFix),
                std::make_pair("INSNavSatFix", evINSNavSatFix),
                std::make_pair("GPSFix", evGPSFix),
                std::make_pair("INSGPSFix", evINSGPSFix),
                std::make_pair("PoseWithCovarianceStamped",
                               evPoseWithCovarianceStamped),
                std::make_pair("INSPoseWithCovarianceStamped",
                               evINSPoseWithCovarianceStamped),
                std::make_pair("$GPGGA", evGPGGA),
                std::make_pair("$GPRMC", evGPRMC),
                std::make_pair("$GPGSA", evGPGSA),
                std::make_pair("$GPGSV", evGPGSV),
                std::make_pair("$GLGSV", evGLGSV),
                std::make_pair("$GAGSV", evGAGSV),
                std::make_pair("4006", evPVTCartesian),
                std::make_pair("4007", evPVTGeodetic),
                std::make_pair("4043", evBaseVectorCart),
                std::make_pair("4028", evBaseVectorGeod),
                std::make_pair("5905", evPosCovCartesian),
                std::make_pair("5906", evPosCovGeodetic),
                std::make_pair("5938", evAttEuler),
                std::make_pair("5939", evAttCovEuler),
                std::make_pair("GPST", evGPST),
                std::make_pair("4013", evChannelStatus),
                std::make_pair("4027", evMeasEpoch),
                std::make_pair("4001", evDOP),
                std::make_pair("5908", evVelCovGeodetic),
                std::make_pair("DiagnosticArray", evDiagnosticArray),
                std::make_pair("Localization", evLocalization),
                std::make_pair("4014", evReceiverStatus),
                std::make_pair("4082", evQualityInd),
                std::make_pair("5902", evReceiverSetup),
                std::make_pair("4225", evINSNavCart),
                std::make_pair("4226", evINSNavGeod),
                std::make_pair("4230", evExtEventINSNavGeod),
                std::make_pair("4229", evExtEventINSNavCart),
                std::make_pair("4224", evIMUSetup),
                std::make_pair("4244", evVelSensorSetup),
                std::make_pair("4050", evExtSensorMeas),
                std::make_pair("5914", evReceiverTime)};

            rx_id_map = RxIDMap(rx_id_pairs, rx_id_pairs + evReceiverSetup + 1);
        }

        void newData(Timestamp recvTimestamp, const uint8_t* data, std::size_t& size)
        {
            recvTimestamp_ = recvTimestamp;
            data_ = data;
            count_ = size;
            found_ = false;
            crc_check_ = false;
            message_size_ = 0;
        }

        bool isMessage(const uint16_t ID);
        bool isMessage(std::string ID);
        bool isSBF();
        bool isNMEA();
        bool isResponse();
        bool isConnectionDescriptor();
        bool isErrorMessage();
        std::size_t messageSize();
        std::string messageID();

        std::size_t getCount() { return count_; };
        const uint8_t* search();

        uint16_t getBlockLength();

        const uint8_t* getPosBuffer();

        const uint8_t* getEndBuffer();

        bool found();

        void next();

        template <typename M>
        void publish(const std::string& topic, const M& msg);

        void publishTf(const LocalizationUtmMsg& msg);

        bool read(std::string message_key, bool search = false);

        bool found_;

        bool gnss_gpsfix_complete(uint32_t id);

        bool ins_gpsfix_complete(uint32_t id);

        bool gnss_navsatfix_complete(uint32_t id);

        bool ins_navsatfix_complete(uint32_t id);

        bool gnss_pose_complete(uint32_t id);

        bool ins_pose_complete(uint32_t id);

        bool diagnostics_complete(uint32_t id);

        bool ins_localization_complete(uint32_t id);

    private:
        ROSaicNodeBase* node_;

        Timestamp recvTimestamp_;

        const uint8_t* data_;

        std::size_t count_;

        bool crc_check_;

        std::size_t message_size_;

        uint32_t count_gpsfix_ = 0;

        PVTGeodeticMsg last_pvtgeodetic_;

        PosCovGeodeticMsg last_poscovgeodetic_;

        AttEulerMsg last_atteuler_;

        AttCovEulerMsg last_attcoveuler_;

        INSNavGeodMsg last_insnavgeod_;

        ExtSensorMeasMsg last_extsensmeas_;

        ChannelStatus last_channelstatus_;

        MeasEpochMsg last_measepoch_;

        DOP last_dop_;

        VelCovGeodeticMsg last_velcovgeodetic_;

        ReceiverStatus last_receiverstatus_;

        QualityInd last_qualityind_;

        ReceiverSetup last_receiversetup_;

        typedef std::unordered_map<uint16_t, TypeOfPVT_Enum> TypeOfPVTMap;

        TypeOfPVTMap type_of_pvt_map;

        typedef std::unordered_map<std::string, RxID_Enum> RxIDMap;

        RxIDMap rx_id_map;

        Timestamp unix_time_;

        int8_t current_leap_seconds_ = -128;

        bool channelstatus_has_arrived_gpsfix_ = false;

        bool measepoch_has_arrived_gpsfix_ = false;

        bool dop_has_arrived_gpsfix_ = false;

        bool pvtgeodetic_has_arrived_gpsfix_ = false;

        bool poscovgeodetic_has_arrived_gpsfix_ = false;

        bool velcovgeodetic_has_arrived_gpsfix_ = false;

        bool atteuler_has_arrived_gpsfix_ = false;

        bool attcoveuler_has_arrived_gpsfix_ = false;

        bool insnavgeod_has_arrived_gpsfix_ = false;

        bool pvtgeodetic_has_arrived_navsatfix_ = false;

        bool poscovgeodetic_has_arrived_navsatfix_ = false;

        bool insnavgeod_has_arrived_navsatfix_ = false;
        bool pvtgeodetic_has_arrived_pose_ = false;

        bool poscovgeodetic_has_arrived_pose_ = false;

        bool atteuler_has_arrived_pose_ = false;

        bool attcoveuler_has_arrived_pose_ = false;

        bool insnavgeod_has_arrived_pose_ = false;

        bool receiverstatus_has_arrived_diagnostics_ = false;

        bool qualityind_has_arrived_diagnostics_ = false;

        bool insnavgeod_has_arrived_localization_ = false;

        NavSatFixMsg NavSatFixCallback();

        GPSFixMsg GPSFixCallback();

        PoseWithCovarianceStampedMsg PoseWithCovarianceStampedCallback();

        DiagnosticArrayMsg DiagnosticArrayCallback();

        ImuMsg ImuCallback();

        LocalizationUtmMsg LocalizationUtmCallback();

        TwistWithCovarianceStampedMsg TwistCallback(bool fromIns = false);

        void wait(Timestamp time_obj);

        bool allTrue(std::vector<bool>& vec, uint32_t id);

        Settings* settings_;

        std::shared_ptr<std::string> fixedUtmZone_;

        Timestamp timestampSBF(const uint8_t* data, bool use_gnss_time);

        Timestamp timestampSBF(uint32_t tow, uint16_t wnc, bool use_gnss_time);
    };
} // namespace io_comm_rx
#endif // for RX_MESSAGE_HPP