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;

struct Settings
{
    bool activate_debug_log;
    std::string device;
    std::string login_user;
    std::string login_password;
    float reconnect_delay_s;
    uint32_t baudrate;
    std::string hw_flow_control;
    std::string rx_serial_port;
    std::string datum;
    uint32_t polling_period_pvt;
    uint32_t polling_period_rest;
    float delta_e;
    float delta_n;
    float delta_u;
    std::string ant_type;
    std::string ant_aux1_type;
    std::string ant_serial_nr;
    std::string ant_aux1_serial_nr;
    bool use_ros_axis_orientation;
    double theta_x;
    double theta_y;
    double theta_z;
    double ant_lever_x;
    double ant_lever_y;
    double ant_lever_z;
    double poi_x;
    double poi_y;
    double poi_z;
    double vsm_x;
    double vsm_y;
    double vsm_z;
    double heading_offset;
    double pitch_offset;
    bool multi_antenna;
    bool ins_use_poi;
    std::string ins_initial_heading;
    float att_std_dev;
    float pos_std_dev;
    std::string ntrip_mode;
    std::string caster;
    uint32_t caster_port;
    std::string ntrip_username;
    std::string ntrip_password;
    std::string mountpoint;
    std::string ntrip_version;
    bool rx_has_internet;
    std::string rtcm_version;
    uint32_t rx_input_corrections_tcp;
    std::string rx_input_corrections_serial;
    std::string send_gga;
    bool publish_gpgga;
    bool publish_gprmc;
    bool publish_gpgsa;
    bool publish_gpgsv;
    bool publish_measepoch;
    bool publish_pvtcartesian;
    bool publish_pvtgeodetic;
    bool publish_poscovcartesian;
    bool publish_poscovgeodetic;
    bool publish_velcovgeodetic;
    bool publish_atteuler;
    bool publish_attcoveuler;
    bool publish_insnavcart;
    bool publish_insnavgeod;
    bool publish_imusetup;
    bool publish_velsensorsetup;
    bool publish_exteventinsnavgeod;
    bool publish_exteventinsnavcart;
    bool publish_extsensormeas;
    bool publish_gpst;
    bool publish_navsatfix;
    bool publish_gpsfix;
    bool publish_pose;
    bool publish_diagnostics;
    bool publish_imu;
    bool publish_localization;
    bool publish_tf;
    std::string septentrio_receiver_type;
    bool use_gnss_time;
    std::string frame_id;
    std::string imu_frame_id;
    std::string poi_frame_id;
    std::string vsm_frame_id;
    std::string aux1_frame_id;
    std::string vehicle_frame_id;
    bool lock_utm_zone;
    uint32_t leap_seconds;
    bool read_from_sbf_log;
    bool read_from_pcap;
};

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,
    evPosCovCartesian,
    evPosCovGeodetic,
    evAttEuler,
    evAttCovEuler,
    evINSNavCart,
    evINSNavGeod,
    evIMUSetup,
    evVelSensorSetup,
    evExtEventINSNavGeod,
    evExtEventINSNavCart,
    evExtSensorMeas,
    evGPST,
    evChannelStatus,
    evMeasEpoch,
    evDOP,
    evVelCovGeodetic,
    evDiagnosticArray,
    evLocalization,
    evReceiverStatus,
    evQualityInd,
    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("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)
            };

            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();

        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_;

        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();

        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