Can search buffer for messages, read/parse them, and so on. More...

#include <message_handler.hpp>

Public Member Functions
	MessageHandler (ROSaicNodeBase *node)
	Constructor of the MessageHandler class. More...

void	parseNmea (const std::shared_ptr< Telegram > &telegram)
	Parse NMEA block. More...

void	parseSbf (const std::shared_ptr< Telegram > &telegram)
	Parse SBF block. More...

void	setLeapSeconds ()

Private Member Functions
void	assembleAimAndDiagnosticArray ()
	"Callback" function when constructing RFStatus DiagnosticArrayMsg messages More...

void	assembleDiagnosticArray (const std::shared_ptr< Telegram > &telegram)
	"Callback" function when constructing DiagnosticArrayMsg messages More...

void	assembleGpsFix ()
	"Callback" function when constructing GPSFix messages More...

template<typename T >
void	assembleHeader (const std::string &frameId, const std::shared_ptr< Telegram > &telegram, T &msg) const
	Header assembling. More...

void	assembleImu ()
	"Callback" function when constructing ImuMsg messages More...

void	assembleLocalizationEcef ()
	"Callback" function when constructing LocalizationMsg messages in ECEF More...

void	assembleLocalizationMsgTwist (double roll, double pitch, double yaw, LocalizationMsg &msg) const
	function to fill twist part of LocalizationMsg More...

void	assembleLocalizationUtm ()
	"Callback" function when constructing LocalizationMsg messages in UTM More...

void	assembleNavSatFix ()
	"Callback" function when constructing NavSatFix messages More...

void	assembleOsnmaDiagnosticArray ()
	"Callback" function when constructing OSNMA DiagnosticArrayMsg messages More...

void	assemblePoseWithCovarianceStamped ()
	"Callback" function when constructing PoseWithCovarianceStamped messages More...

void	assembleTimeReference (const std::shared_ptr< Telegram > &telegram)
	function when constructing TimeReferenceMsg messages More...

void	assembleTwist (bool fromIns=false)
	"Callback" function when constructing TwistWithCovarianceStampedMsg messages More...

template<typename M >
void	publish (const std::string &topic, const M &msg)
	Publishing function. More...

void	publishTf (const LocalizationMsg &msg)
	Publishing function. More...

Timestamp	timestampSBF (const std::vector< uint8_t > &message) const
	Calculates the timestamp, in the Unix Epoch time format This is either done using the TOW as transmitted with the SBF block (if "use_gnss" is true), or using the current time. More...

Timestamp	timestampSBF (uint32_t tow, uint16_t wnc) const
	Calculates the timestamp, in the Unix Epoch time format This is either done using the TOW as transmitted with the SBF block (if "use_gnss" is true), or using the current time. More...

void	wait (Timestamp time_obj)
	Waits according to time when reading from file. More...

Private Attributes
int32_t	current_leap_seconds_ = -128
	Current leap seconds as received, do not use value is -128. More...

std::shared_ptr< std::string >	fixedUtmZone_
	Fixed UTM zone. More...

AttCovEulerMsg	last_attcoveuler_
	Since GPSFix etc. need AttCovEuler, incoming AttCovEuler blocks need to be stored. More...

AttEulerMsg	last_atteuler_
	Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to be stored. More...

ChannelStatus	last_channelstatus_
	Since GPSFix needs ChannelStatus, incoming ChannelStatus blocks need to be stored. More...

Dop	last_dop_
	Since GPSFix needs DOP, incoming DOP blocks need to be stored. More...

ExtSensorMeasMsg	last_extsensmeas_
	Since Imu needs ExtSensorMeas, incoming ExtSensorMeas blocks need to be stored. More...

GalAuthStatusMsg	last_gal_auth_status_
	Stores incoming GALAuthStatus block. More...

INSNavCartMsg	last_insnavcart_
	Since LoclaizationEcef needs INSNavCart, incoming INSNavCart blocks need to be stored. More...

INSNavGeodMsg	last_insnavgeod_
	Since NavSatFix, GPSFix, Imu and Pose need INSNavGeod, incoming INSNavGeod blocks need to be stored. More...

MeasEpochMsg	last_measepoch_
	Since GPSFix needs MeasEpoch (for SNRs), incoming MeasEpoch blocks need to be stored. More...

PosCovGeodeticMsg	last_poscovgeodetic_
	Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic blocks need to be stored. More...

uint64_t	last_pvt_latency_ = 0
	Last reported PVT processing latency. More...

PVTGeodeticMsg	last_pvtgeodetic_
	Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks need to be stored. More...

QualityInd	last_qualityind_
	Since DiagnosticArray needs QualityInd, incoming QualityInd blocks need to be stored. More...

ReceiverSetup	last_receiversetup_
	Since DiagnosticArray needs ReceiverSetup, incoming ReceiverSetup blocks need to be stored. More...

ReceiverStatus	last_receiverstatus_
	Since DiagnosticArray needs ReceiverStatus, incoming ReceiverStatus blocks need to be stored. More...

RfStatusMsg	last_rf_status_
	Stores incoming RFStatus block. More...

VelCovGeodeticMsg	last_velcovgeodetic_
	Since GPSFix needs VelCovGeodetic, incoming VelCovGeodetic blocks need to be stored. More...

std::unordered_map< std::string, uint8_t >	nmeaMap_
	Map of NMEA messgae IDs and uint8_t. More...

ROSaicNodeBase *	node_
	Pointer to the node. More...

bool	osnma_info_available_ = false
	Wether OSNMA info has been received. More...

const Settings *	settings_
	Pointer to settings struct. More...

Timestamp	unix_time_

Detailed Description

Can search buffer for messages, read/parse them, and so on.

Definition at line 138 of file message_handler.hpp.

Constructor & Destructor Documentation

◆ MessageHandler()

io::MessageHandler::MessageHandler ( ROSaicNodeBase * node )

inline

Constructor of the MessageHandler class.

Parameters

[in] node Pointer to the node)

Definition at line 145 of file message_handler.hpp.

Member Function Documentation

◆ assembleAimAndDiagnosticArray()

void io::MessageHandler::assembleAimAndDiagnosticArray ( )

private

"Callback" function when constructing RFStatus DiagnosticArrayMsg messages

Definition at line 446 of file message_handler.cpp.

◆ assembleDiagnosticArray()

void io::MessageHandler::assembleDiagnosticArray ( const std::shared_ptr< Telegram > & telegram )

private

"Callback" function when constructing DiagnosticArrayMsg messages

Parameters

[in] telegram telegram from which the msg was assembled

Definition at line 216 of file message_handler.cpp.

◆ assembleGpsFix()

void io::MessageHandler::assembleGpsFix ( )

private

"Callback" function when constructing GPSFix messages

Note that the field "dip" denotes the local magnetic inclination in degrees (positive when the magnetic field points downwards (into the Earth)). This quantity cannot be calculated by most Septentrio receivers. We assume that for the ROS field "err_time", we are requested to provide the 2 sigma uncertainty on the clock bias estimate in square meters, not the clock drift estimate (latter would be "2*std::sqrt(last_velcovgeodetic_.Cov_DtDt)"). The "err_track" entry is calculated via the Gaussian error propagation formula from the eastward and the northward velocities. For the formula's usage we have to assume that the eastward and the northward velocities are independent variables. Note that elevations and azimuths of visible satellites are taken from the ChannelStatus block, which provides 1 degree precision, while the SatVisibility block could provide hundredths of degrees precision. Change if imperative for your application... Definition of "visible satellite" adopted here: We define a visible satellite as being !up to! "in sync" mode with the receiver, which corresponds to last_measepoch_.N (signal-to-noise ratios are thereby available for these), though not last_channelstatus_.N, which also includes those "in search". In case certain values appear unphysical, please consult the firmware, since those most likely refer to Do-Not-Use values.

Definition at line 1590 of file message_handler.cpp.

◆ assembleHeader()

template<typename T >

void io::MessageHandler::assembleHeader	(	const std::string &	frameId,
		const std::shared_ptr< Telegram > &	telegram,
		T &	msg
	)		const

private

Header assembling.

Parameters

[in]	frameId	String of frame ID
[in]	telegram	telegram from which the msg was assembled
[in]	msg	ROS msg for which the header is assembled

Definition at line 2069 of file message_handler.cpp.

◆ assembleImu()

void io::MessageHandler::assembleImu ( )

private

"Callback" function when constructing ImuMsg messages

Definition at line 548 of file message_handler.cpp.

◆ assembleLocalizationEcef()

void io::MessageHandler::assembleLocalizationEcef ( )

private

"Callback" function when constructing LocalizationMsg messages in ECEF

Localization in ECEF coordinates. Attitude is converted from local to ECEF Linear velocity of twist in body frame as per msg definition. Angular velocity not available, thus according autocovariances are set to -1.0.

Definition at line 1091 of file message_handler.cpp.

◆ assembleLocalizationMsgTwist()

void io::MessageHandler::assembleLocalizationMsgTwist	(	double	roll,
		double	pitch,
		double	yaw,
		LocalizationMsg &	msg
	)		const

private

function to fill twist part of LocalizationMsg

Parameters

[in]	roll	roll [rad]
[in]	pitch	pitch [rad]
[in]	yaw	yaw [rad]
[in,out]	msg	LocalizationMsg to be filled

Definition at line 1255 of file message_handler.cpp.

◆ assembleLocalizationUtm()

void io::MessageHandler::assembleLocalizationUtm ( )

private

"Callback" function when constructing LocalizationMsg messages in UTM

Localization in UTM coordinates. Yaw angle is converted from true north to grid north. Linear velocity of twist in body frame as per msg definition. Angular velocity not available, thus according autocovariances are set to -1.0.

Definition at line 887 of file message_handler.cpp.

◆ assembleNavSatFix()

void io::MessageHandler::assembleNavSatFix ( )

private

"Callback" function when constructing NavSatFix messages

The position_covariance array is populated in row-major order, where the basis of the corresponding matrix is ENU (so Cov_lonlon is in location 11 of the matrix). The B2b signal type of BeiDou is not checked for usage, since the SignalInfo field of the PVTGeodetic block does not disclose it. For that, one would need to go to the ObsInfo field of the MeasEpochChannelType1 sub-block.

Definition at line 1378 of file message_handler.cpp.

◆ assembleOsnmaDiagnosticArray()

void io::MessageHandler::assembleOsnmaDiagnosticArray ( )

private

"Callback" function when constructing OSNMA DiagnosticArrayMsg messages

Definition at line 353 of file message_handler.cpp.

◆ assemblePoseWithCovarianceStamped()

void io::MessageHandler::assemblePoseWithCovarianceStamped ( )

private

"Callback" function when constructing PoseWithCovarianceStamped messages

Definition at line 57 of file message_handler.cpp.

◆ assembleTimeReference()

void io::MessageHandler::assembleTimeReference ( const std::shared_ptr< Telegram > & telegram )

private

function when constructing TimeReferenceMsg messages

Parameters

[in] telegram telegram from which the msg was assembled

Definition at line 2058 of file message_handler.cpp.

◆ assembleTwist()

void io::MessageHandler::assembleTwist ( bool fromIns = false )

private

"Callback" function when constructing TwistWithCovarianceStampedMsg messages

Parameters

[in] fromIns Wether to contruct message from INS data

Definition at line 659 of file message_handler.cpp.

◆ parseNmea()

void io::MessageHandler::parseNmea ( const std::shared_ptr< Telegram > & telegram )

Parse NMEA block.

Parameters

[in] telegram Telegram to be parsed

Definition at line 2754 of file message_handler.cpp.

◆ parseSbf()

void io::MessageHandler::parseSbf ( const std::shared_ptr< Telegram > & telegram )

Parse SBF block.

Parameters

[in] telegram Telegram to be parsed

Definition at line 2203 of file message_handler.cpp.

◆ publish()

template<typename M >

void io::MessageHandler::publish	(	const std::string &	topic,
		const M &	msg
	)

private

Publishing function.

Parameters

[in]	topic	String of topic
[in]	msg	ROS message to be published

If GNSS time is used, Publishing is only done with valid leap seconds

Definition at line 2148 of file message_handler.cpp.

◆ publishTf()

void io::MessageHandler::publishTf ( const LocalizationMsg & msg )

private

Publishing function.

Parameters

[in] msg Localization message

If GNSS time is used, Publishing is only done with valid leap seconds

Definition at line 2177 of file message_handler.cpp.

◆ setLeapSeconds()

void io::MessageHandler::setLeapSeconds ( )

inline

Definition at line 150 of file message_handler.hpp.

◆ timestampSBF() [1/2]

Timestamp io::MessageHandler::timestampSBF ( const std::vector< uint8_t > & message ) const

private

Calculates the timestamp, in the Unix Epoch time format This is either done using the TOW as transmitted with the SBF block (if "use_gnss" is true), or using the current time.

Parameters

[in] data Pointer to the buffer

Returns: Timestamp object containing seconds and nanoseconds since last epoch

Definition at line 2105 of file message_handler.cpp.

◆ timestampSBF() [2/2]

Timestamp io::MessageHandler::timestampSBF	(	uint32_t	tow,
		uint16_t	wnc
	)		const

private

Calculates the timestamp, in the Unix Epoch time format This is either done using the TOW as transmitted with the SBF block (if "use_gnss" is true), or using the current time.

Parameters

[in]	tow	(Time of Week) Number of milliseconds that elapsed since the beginning of the current GPS week as transmitted by the SBF block
[in]	wnc	(Week Number Counter) counts the number of complete weeks elapsed since January 6, 1980

Returns: Timestamp object containing seconds and nanoseconds since last epoch

If the current time shall be employed, it is calculated via the time(NULL) function found in the <ctime> library At the time of writing the code (2020), the GPS time was ahead of UTC time by 18 (leap) seconds. Adapt the settings_->leap_seconds ROSaic parameter accordingly as soon as the next leap second is inserted into the UTC time.

Definition at line 2121 of file message_handler.cpp.

◆ wait()

void io::MessageHandler::wait ( Timestamp time_obj )

private

Waits according to time when reading from file.

Parameters

[in] time_obj wait until time

Definition at line 2738 of file message_handler.cpp.

Member Data Documentation

◆ current_leap_seconds_

int32_t io::MessageHandler::current_leap_seconds_ = -128

private

Current leap seconds as received, do not use value is -128.

Definition at line 317 of file message_handler.hpp.

◆ fixedUtmZone_

std::shared_ptr<std::string> io::MessageHandler::fixedUtmZone_

private

Fixed UTM zone.

Definition at line 405 of file message_handler.hpp.

◆ last_attcoveuler_

AttCovEulerMsg io::MessageHandler::last_attcoveuler_

private

Since GPSFix etc. need AttCovEuler, incoming AttCovEuler blocks need to be stored.

Definition at line 233 of file message_handler.hpp.

◆ last_atteuler_

AttEulerMsg io::MessageHandler::last_atteuler_

private

Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to be stored.

Definition at line 227 of file message_handler.hpp.

◆ last_channelstatus_

ChannelStatus io::MessageHandler::last_channelstatus_

private

Since GPSFix needs ChannelStatus, incoming ChannelStatus blocks need to be stored.

Definition at line 257 of file message_handler.hpp.

◆ last_dop_

Dop io::MessageHandler::last_dop_

private

Since GPSFix needs DOP, incoming DOP blocks need to be stored.

Definition at line 268 of file message_handler.hpp.

◆ last_extsensmeas_

ExtSensorMeasMsg io::MessageHandler::last_extsensmeas_

private

Since Imu needs ExtSensorMeas, incoming ExtSensorMeas blocks need to be stored.

Definition at line 251 of file message_handler.hpp.

◆ last_gal_auth_status_

GalAuthStatusMsg io::MessageHandler::last_gal_auth_status_

private

Stores incoming GALAuthStatus block.

Definition at line 297 of file message_handler.hpp.

◆ last_insnavcart_

INSNavCartMsg io::MessageHandler::last_insnavcart_

private

Since LoclaizationEcef needs INSNavCart, incoming INSNavCart blocks need to be stored.

Definition at line 245 of file message_handler.hpp.

◆ last_insnavgeod_

INSNavGeodMsg io::MessageHandler::last_insnavgeod_

private

Since NavSatFix, GPSFix, Imu and Pose need INSNavGeod, incoming INSNavGeod blocks need to be stored.

Definition at line 239 of file message_handler.hpp.

◆ last_measepoch_

MeasEpochMsg io::MessageHandler::last_measepoch_

private

Since GPSFix needs MeasEpoch (for SNRs), incoming MeasEpoch blocks need to be stored.

Definition at line 263 of file message_handler.hpp.

◆ last_poscovgeodetic_

PosCovGeodeticMsg io::MessageHandler::last_poscovgeodetic_

private

Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic blocks need to be stored.

Definition at line 221 of file message_handler.hpp.

◆ last_pvt_latency_

uint64_t io::MessageHandler::last_pvt_latency_ = 0

mutableprivate

Last reported PVT processing latency.

Definition at line 314 of file message_handler.hpp.

◆ last_pvtgeodetic_

PVTGeodeticMsg io::MessageHandler::last_pvtgeodetic_

private

Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks need to be stored.

Definition at line 215 of file message_handler.hpp.

◆ last_qualityind_

QualityInd io::MessageHandler::last_qualityind_

private

Since DiagnosticArray needs QualityInd, incoming QualityInd blocks need to be stored.

Definition at line 286 of file message_handler.hpp.

◆ last_receiversetup_

ReceiverSetup io::MessageHandler::last_receiversetup_

private

Since DiagnosticArray needs ReceiverSetup, incoming ReceiverSetup blocks need to be stored.

Definition at line 292 of file message_handler.hpp.

◆ last_receiverstatus_

ReceiverStatus io::MessageHandler::last_receiverstatus_

private

Since DiagnosticArray needs ReceiverStatus, incoming ReceiverStatus blocks need to be stored.

Definition at line 280 of file message_handler.hpp.

◆ last_rf_status_

RfStatusMsg io::MessageHandler::last_rf_status_

private

Stores incoming RFStatus block.

Definition at line 307 of file message_handler.hpp.

◆ last_velcovgeodetic_

VelCovGeodeticMsg io::MessageHandler::last_velcovgeodetic_

private

Since GPSFix needs VelCovGeodetic, incoming VelCovGeodetic blocks need to be stored.

Definition at line 274 of file message_handler.hpp.

◆ nmeaMap_

std::unordered_map<std::string, uint8_t> io::MessageHandler::nmeaMap_

private

Initial value:

{
            {"$GPGGA", 0}, {"$INGGA", 0}, {"$GPRMC", 1}, {"$INRMC", 1},
            {"$GPGSA", 2}, {"$INGSA", 2}, {"$GAGSV", 3}, {"$INGSV", 3}}

Map of NMEA messgae IDs and uint8_t.

Definition at line 207 of file message_handler.hpp.

◆ node_

ROSaicNodeBase* io::MessageHandler::node_

private

Pointer to the node.

Definition at line 197 of file message_handler.hpp.

◆ osnma_info_available_

bool io::MessageHandler::osnma_info_available_ = false

private

Wether OSNMA info has been received.

Definition at line 302 of file message_handler.hpp.

◆ settings_

const Settings* io::MessageHandler::settings_

private

Pointer to settings struct.

Definition at line 202 of file message_handler.hpp.

◆ unix_time_

Timestamp io::MessageHandler::unix_time_

private

When reading from an SBF file, the ROS publishing frequency is governed by the time stamps found in the SBF blocks therein.

Definition at line 311 of file message_handler.hpp.

The documentation for this class was generated from the following files:

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ MessageHandler()

Member Function Documentation

◆ assembleAimAndDiagnosticArray()

◆ assembleDiagnosticArray()

◆ assembleGpsFix()

◆ assembleHeader()

◆ assembleImu()

◆ assembleLocalizationEcef()

◆ assembleLocalizationMsgTwist()

◆ assembleLocalizationUtm()

◆ assembleNavSatFix()

◆ assembleOsnmaDiagnosticArray()

◆ assemblePoseWithCovarianceStamped()

◆ assembleTimeReference()

◆ assembleTwist()

◆ parseNmea()

◆ parseSbf()

◆ publish()

◆ publishTf()

◆ setLeapSeconds()

◆ timestampSBF() [1/2]

◆ timestampSBF() [2/2]

◆ wait()

Member Data Documentation

◆ current_leap_seconds_

◆ fixedUtmZone_

◆ last_attcoveuler_

◆ last_atteuler_

◆ last_channelstatus_

◆ last_dop_

◆ last_extsensmeas_

◆ last_gal_auth_status_

◆ last_insnavcart_

◆ last_insnavgeod_

◆ last_measepoch_

◆ last_poscovgeodetic_

◆ last_pvt_latency_

◆ last_pvtgeodetic_

◆ last_qualityind_

◆ last_receiversetup_

◆ last_receiverstatus_

◆ last_rf_status_

◆ last_velcovgeodetic_

◆ nmeaMap_

◆ node_

◆ osnma_info_available_

◆ settings_

◆ unix_time_