Program Listing for File message_handler.hpp
↰ Return to documentation for file (/tmp/ws/src/septentrio_gnss_driver/include/septentrio_gnss_driver/communication/message_handler.hpp
)
// *****************************************************************************
//
// © 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.
//
// *****************************************************************************
#pragma once
// 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/communication/telegram.hpp>
#include <septentrio_gnss_driver/crc/crc.hpp>
#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.hpp>
enum TypeOfPVT_Enum
{
evNoPVT,
evStandAlone,
evDGPS,
evFixed,
evRTKFixed,
evRTKFloat,
evSBAS,
evMovingBaseRTKFixed,
evMovingBaseRTKFloat,
evPPP
};
enum SbfId
{
PVT_CARTESIAN = 4006,
PVT_GEODETIC = 4007,
BASE_VECTOR_CART = 4043,
BASE_VECTOR_GEOD = 4028,
POS_COV_CARTESIAN = 5905,
POS_COV_GEODETIC = 5906,
ATT_EULER = 5938,
ATT_COV_EULER = 5939,
CHANNEL_STATUS = 4013,
MEAS_EPOCH = 4027,
DOP = 4001,
VEL_COV_CARTESIAN = 5907,
VEL_COV_GEODETIC = 5908,
RECEIVER_STATUS = 4014,
QUALITY_IND = 4082,
RECEIVER_SETUP = 5902,
INS_NAV_CART = 4225,
INS_NAV_GEOD = 4226,
EXT_EVENT_INS_NAV_GEOD = 4230,
EXT_EVENT_INS_NAV_CART = 4229,
IMU_SETUP = 4224,
VEL_SENSOR_SETUP = 4244,
EXT_SENSOR_MEAS = 4050,
RECEIVER_TIME = 5914,
GAL_AUTH_STATUS = 4245,
RF_STATUS = 4092
};
namespace io {
class MessageHandler
{
public:
MessageHandler(ROSaicNodeBase* node) :
node_(node), settings_(node->settings()), unix_time_(0)
{
}
void setLeapSeconds()
{
// set leap seconds to paramter if reading from file
if (settings_->read_from_sbf_log || settings_->read_from_pcap)
current_leap_seconds_ = settings_->leap_seconds;
}
void parseSbf(const std::shared_ptr<Telegram>& telegram);
void parseNmea(const std::shared_ptr<Telegram>& telegram);
private:
template <typename T>
void assembleHeader(const std::string& frameId,
const std::shared_ptr<Telegram>& telegram, T& msg) const;
template <typename M>
void publish(const std::string& topic, const M& msg);
void publishTf(const LocalizationMsg& msg);
ROSaicNodeBase* node_;
const Settings* settings_;
std::unordered_map<std::string, uint8_t> nmeaMap_{
{"$GPGGA", 0}, {"$INGGA", 0}, {"$GPRMC", 1}, {"$INRMC", 1},
{"$GPGSA", 2}, {"$INGSA", 2}, {"$GAGSV", 3}, {"$INGSV", 3}};
PVTGeodeticMsg last_pvtgeodetic_;
PosCovGeodeticMsg last_poscovgeodetic_;
AttEulerMsg last_atteuler_;
AttCovEulerMsg last_attcoveuler_;
INSNavGeodMsg last_insnavgeod_;
INSNavCartMsg last_insnavcart_;
ExtSensorMeasMsg last_extsensmeas_;
ChannelStatus last_channelstatus_;
MeasEpochMsg last_measepoch_;
Dop last_dop_;
VelCovGeodeticMsg last_velcovgeodetic_;
ReceiverStatus last_receiverstatus_;
QualityInd last_qualityind_;
ReceiverSetup last_receiversetup_;
GalAuthStatusMsg last_gal_auth_status_;
bool osnma_info_available_ = false;
RfStatusMsg last_rf_status_;
Timestamp unix_time_;
mutable uint64_t last_pvt_latency_ = 0;
int32_t current_leap_seconds_ = -128;
void assembleNavSatFix();
void assembleGpsFix();
void assemblePoseWithCovarianceStamped();
void assembleDiagnosticArray(const std::shared_ptr<Telegram>& telegram);
void assembleOsnmaDiagnosticArray();
void assembleAimAndDiagnosticArray();
void assembleImu();
void assembleLocalizationUtm();
void assembleLocalizationEcef();
void assembleLocalizationMsgTwist(double roll, double pitch, double yaw,
LocalizationMsg& msg) const;
void assembleTwist(bool fromIns = false);
void assembleTimeReference(const std::shared_ptr<Telegram>& telegram);
void wait(Timestamp time_obj);
std::shared_ptr<std::string> fixedUtmZone_;
Timestamp timestampSBF(const std::vector<uint8_t>& message) const;
Timestamp timestampSBF(uint32_t tow, uint16_t wnc) const;
};
} // namespace io