Public Member Functions | Public Attributes | Private Types | Private Member Functions | Private Attributes | Static Private Attributes | List of all members
io_comm_rx::RxMessage Class Reference

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

#include <rx_message.hpp>

Public Member Functions

bool found ()
 Has an NMEA message, SBF block or command reply been found in the buffer? More...
 
uint16_t getBlockLength ()
 Gets the length of the SBF block. More...
 
std::size_t getCount ()
 Returns the count_ variable. More...
 
const uint8_t * getEndBuffer ()
 Gets the end position in the read buffer. More...
 
const uint8_t * getPosBuffer ()
 Gets the current position in the read buffer. More...
 
bool isConnectionDescriptor ()
 
bool isErrorMessage ()
 Determines whether data_ currently points to an error message reply from the Rx. More...
 
bool isMessage (const uint16_t ID)
 Determines whether data_ points to the SBF block with ID "ID", e.g. 5003. More...
 
bool isMessage (std::string ID)
 Determines whether data_ points to the NMEA message with ID "ID", e.g. "$GPGGA". More...
 
bool isNMEA ()
 Determines whether data_ currently points to an NMEA message. More...
 
bool isResponse ()
 Determines whether data_ currently points to an NMEA message. More...
 
bool isSBF ()
 Determines whether data_ currently points to an SBF block. More...
 
std::string messageID ()
 
std::size_t messageSize ()
 Determines size of the message (also command reply) that data_ is currently pointing at. More...
 
void next ()
 Goes to the start of the next message based on the calculated length of current message. More...
 
template<typename T >
bool read (typename boost::call_traits< T >::reference message, std::string message_key, bool search=false)
 Performs the CRC check (if SBF) and populates ROS message "message" with the necessary content. More...
 
 RxMessage (const uint8_t *data, std::size_t &size)
 Constructor of the RxMessage class. More...
 
const uint8_t * search ()
 Searches the buffer for the beginning of the next message (NMEA or SBF) More...
 

Public Attributes

bool found_
 Whether or not a message has been found. More...
 

Private Types

typedef std::map< std::string, RxID_EnumRxIDMap
 Shorthand for the map responsible for matching ROS message identifiers to an enum value. More...
 
typedef std::map< uint16_t, TypeOfPVT_EnumTypeOfPVTMap
 Shorthand for the map responsible for matching PVTGeodetic's Mode field to an enum value. More...
 

Private Member Functions

rosaic::AttCovEulerPtr AttCovEulerCallback (AttCovEuler &data)
 Callback function when reading AttCovEuler blocks. More...
 
rosaic::AttEulerPtr AttEulerCallback (AttEuler &data)
 Callback function when reading AttEuler blocks. More...
 
diagnostic_msgs::DiagnosticArrayPtr DiagnosticArrayCallback ()
 "Callback" function when constructing diagnostic_msgs::DiagnosticArray messages More...
 
gps_common::GPSFixPtr GPSFixCallback ()
 "Callback" function when constructing GPSFix messages More...
 
sensor_msgs::NavSatFixPtr NavSatFixCallback ()
 "Callback" function when constructing NavSatFix messages More...
 
rosaic::PosCovCartesianPtr PosCovCartesianCallback (PosCovCartesian &data)
 Callback function when reading PosCovCartesian blocks. More...
 
rosaic::PosCovGeodeticPtr PosCovGeodeticCallback (PosCovGeodetic &data)
 Callback function when reading PosCovGeodetic blocks. More...
 
geometry_msgs::PoseWithCovarianceStampedPtr PoseWithCovarianceStampedCallback ()
 "Callback" function when constructing PoseWithCovarianceStamped messages More...
 
rosaic::PVTCartesianPtr PVTCartesianCallback (PVTCartesian &data)
 Callback function when reading PVTCartesian blocks. More...
 
rosaic::PVTGeodeticPtr PVTGeodeticCallback (PVTGeodetic &data)
 Callback function when reading PVTGeodetic blocks. More...
 

Private Attributes

std::size_t count_
 Number of unread bytes in the buffer. More...
 
bool crc_check_
 Whether the CRC check as evaluated in the read() method was successful or not is stored here. More...
 
const uint8_t * data_
 Pointer to the buffer of messages. More...
 
std::size_t message_size_
 Helps to determine size of response message / NMEA message / SBF block. More...
 

Static Private Attributes

static uint32_t count_gpsfix_ = 0
 Number of times the gps_common::GPSFix message has been published. More...
 
static AttCovEuler last_attcoveuler_ = AttCovEuler()
 Since GPSFix etc. need AttCovEuler, incoming AttCovEuler blocks need to be stored. More...
 
static AttEuler last_atteuler_ = AttEuler()
 Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to be stored. More...
 
static ChannelStatus last_channelstatus_ = ChannelStatus()
 Since GPSFix needs ChannelStatus, incoming ChannelStatus blocks need to be stored. More...
 
static DOP last_dop_ = DOP()
 Since GPSFix needs DOP, incoming DOP blocks need to be stored. More...
 
static MeasEpoch last_measepoch_ = MeasEpoch()
 Since GPSFix needs MeasEpoch (for SNRs), incoming MeasEpoch blocks need to be stored. More...
 
static PosCovGeodetic last_poscovgeodetic_ = PosCovGeodetic()
 Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic blocks need to be stored. More...
 
static PVTGeodetic last_pvtgeodetic_ = PVTGeodetic()
 Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks need to be stored. More...
 
static QualityInd last_qualityind_ = QualityInd()
 Since DiagnosticArray needs QualityInd, incoming QualityInd blocks need to be stored. More...
 
static ReceiverSetup last_receiversetup_ = ReceiverSetup()
 Since DiagnosticArray needs ReceiverSetup, incoming ReceiverSetup blocks need to be stored. More...
 
static ReceiverStatus last_receiverstatus_ = ReceiverStatus()
 Since DiagnosticArray needs ReceiverStatus, incoming ReceiverStatus blocks need to be stored. More...
 
static VelCovGeodetic last_velcovgeodetic_ = VelCovGeodetic()
 Since GPSFix needs VelCovGeodetic, incoming VelCovGeodetic blocks need to be stored. More...
 
static RxIDMap rx_id_map
 All instances of the RxMessage class shall have access to the map without reinitializing it, hence static. More...
 
static TypeOfPVTMap type_of_pvt_map
 All instances of the RxMessage class shall have access to the map without reinitializing it, hence static. More...
 

Detailed Description

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

Definition at line 200 of file rx_message.hpp.

Member Typedef Documentation

◆ RxIDMap

typedef std::map<std::string, RxID_Enum> io_comm_rx::RxMessage::RxIDMap
private

Shorthand for the map responsible for matching ROS message identifiers to an enum value.

Definition at line 384 of file rx_message.hpp.

◆ TypeOfPVTMap

typedef std::map<uint16_t, TypeOfPVT_Enum> io_comm_rx::RxMessage::TypeOfPVTMap
private

Shorthand for the map responsible for matching PVTGeodetic's Mode field to an enum value.

Definition at line 376 of file rx_message.hpp.

Constructor & Destructor Documentation

◆ RxMessage()

io_comm_rx::RxMessage::RxMessage ( const uint8_t *  data,
std::size_t &  size 
)
inline

Constructor of the RxMessage class.

One can always provide a non-const value where a const one was expected. The const-ness of the argument just means the function promises not to change it.. Recall: static_cast by the way can remove or add const-ness, no other C++ cast is capable of removing it (not even reinterpret_cast)

Parameters
[in]dataPointer to the buffer that is about to be analyzed
[in]sizeSize of the buffer (as handed over by async_read_some)

Definition at line 213 of file rx_message.hpp.

Member Function Documentation

◆ AttCovEulerCallback()

rosaic::AttCovEulerPtr io_comm_rx::RxMessage::AttCovEulerCallback ( AttCovEuler data)
private

Callback function when reading AttCovEuler blocks.

Parameters
[in]dataThe (packed and aligned) struct instance used to populate the ROS message AttCovEuler
Returns
A smart pointer to the ROS message AttCovEuler just created

Definition at line 252 of file rx_message.cpp.

◆ AttEulerCallback()

rosaic::AttEulerPtr io_comm_rx::RxMessage::AttEulerCallback ( AttEuler data)
private

Callback function when reading AttEuler blocks.

Parameters
[in]dataThe (packed and aligned) struct instance used to populate the ROS message AttEuler
Returns
A smart pointer to the ROS message AttEuler just created

Definition at line 230 of file rx_message.cpp.

◆ DiagnosticArrayCallback()

diagnostic_msgs::DiagnosticArrayPtr io_comm_rx::RxMessage::DiagnosticArrayCallback ( )
private

"Callback" function when constructing diagnostic_msgs::DiagnosticArray messages

Returns
A smart pointer to the ROS message diagnostic_msgs::DiagnosticArray just created

Definition at line 332 of file rx_message.cpp.

◆ found()

bool io_comm_rx::RxMessage::found ( )

Has an NMEA message, SBF block or command reply been found in the buffer?

Returns
True if a message with the correct header & length has been found

Definition at line 820 of file rx_message.cpp.

◆ getBlockLength()

uint16_t io_comm_rx::RxMessage::getBlockLength ( )

Gets the length of the SBF block.

It determines the length from the header of the SBF block. The block length thus includes the header length.

Returns
The length of the SBF block

Definition at line 1066 of file rx_message.cpp.

◆ getCount()

std::size_t io_comm_rx::RxMessage::getCount ( )
inline

Returns the count_ variable.

Returns
The variable count_

Definition at line 241 of file rx_message.hpp.

◆ getEndBuffer()

const uint8_t * io_comm_rx::RxMessage::getEndBuffer ( )

Gets the end position in the read buffer.

Returns
A pointer pointing to just after the read buffer (matches search()'s final pointer in case no valid message is found)

Definition at line 1061 of file rx_message.cpp.

◆ getPosBuffer()

const uint8_t * io_comm_rx::RxMessage::getPosBuffer ( )

Gets the current position in the read buffer.

Returns
The current position of the read buffer

Definition at line 1056 of file rx_message.cpp.

◆ GPSFixCallback()

gps_common::GPSFixPtr io_comm_rx::RxMessage::GPSFixCallback ( )
private

"Callback" function when constructing GPSFix messages

Returns
A smart pointer to the ROS message GPSFix just created

For some unknown reason, the first 2 entries of the GPSStatus field's arrays are not shown properly when published. Please consult section 4.1.9 of the firmware (at least for mosaic-x5) to understand the meaning of the SV identifiers used in the arrays of the GPSStatus field. 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(static_cast<double>(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 519 of file rx_message.cpp.

◆ isConnectionDescriptor()

bool io_comm_rx::RxMessage::isConnectionDescriptor ( )

Determines whether data_ currently points to a connection descriptor (right after initiating a TCP connection)

Definition at line 991 of file rx_message.cpp.

◆ isErrorMessage()

bool io_comm_rx::RxMessage::isErrorMessage ( )

Determines whether data_ currently points to an error message reply from the Rx.

Definition at line 1010 of file rx_message.cpp.

◆ isMessage() [1/2]

bool io_comm_rx::RxMessage::isMessage ( const uint16_t  ID)

Determines whether data_ points to the SBF block with ID "ID", e.g. 5003.

Definition at line 886 of file rx_message.cpp.

◆ isMessage() [2/2]

bool io_comm_rx::RxMessage::isMessage ( std::string  ID)

Determines whether data_ points to the NMEA message with ID "ID", e.g. "$GPGGA".

Definition at line 909 of file rx_message.cpp.

◆ isNMEA()

bool io_comm_rx::RxMessage::isNMEA ( )

Determines whether data_ currently points to an NMEA message.

Definition at line 952 of file rx_message.cpp.

◆ isResponse()

bool io_comm_rx::RxMessage::isResponse ( )

Determines whether data_ currently points to an NMEA message.

Definition at line 972 of file rx_message.cpp.

◆ isSBF()

bool io_comm_rx::RxMessage::isSBF ( )

Determines whether data_ currently points to an SBF block.

Definition at line 933 of file rx_message.cpp.

◆ messageID()

std::string io_comm_rx::RxMessage::messageID ( )

Returns the message ID of the message where data_ is pointing at at the moment, SBF identifiers embellished with inverted commas, e.g. "5003"

Definition at line 1029 of file rx_message.cpp.

◆ messageSize()

std::size_t io_comm_rx::RxMessage::messageSize ( )

Determines size of the message (also command reply) that data_ is currently pointing at.

Definition at line 852 of file rx_message.cpp.

◆ NavSatFixCallback()

sensor_msgs::NavSatFixPtr io_comm_rx::RxMessage::NavSatFixCallback ( )
private

"Callback" function when constructing NavSatFix messages

Returns
A smart pointer to the ROS message NavSatFix just created

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 430 of file rx_message.cpp.

◆ next()

void io_comm_rx::RxMessage::next ( )

Goes to the start of the next message based on the calculated length of current message.

This method won't make data_ jump to the next message if the current one is an NMEA message or a command reply. In that case, search() will check the bytes one by one for the new message's sync bytes ($P, $G or $R).

Definition at line 1086 of file rx_message.cpp.

◆ PosCovCartesianCallback()

rosaic::PosCovCartesianPtr io_comm_rx::RxMessage::PosCovCartesianCallback ( PosCovCartesian data)
private

Callback function when reading PosCovCartesian blocks.

Parameters
[in]dataThe (packed and aligned) struct instance used to populate the ROS message PosCovCartesian
Returns
A smart pointer to the ROS message PosCovCartesian just created

Definition at line 179 of file rx_message.cpp.

◆ PosCovGeodeticCallback()

rosaic::PosCovGeodeticPtr io_comm_rx::RxMessage::PosCovGeodeticCallback ( PosCovGeodetic data)
private

Callback function when reading PosCovGeodetic blocks.

Parameters
[in]dataThe (packed and aligned) struct instance used to populate the ROS message PosCovGeodetic
Returns
A smart pointer to the ROS message PosCovGeodetic just created

Definition at line 205 of file rx_message.cpp.

◆ PoseWithCovarianceStampedCallback()

geometry_msgs::PoseWithCovarianceStampedPtr io_comm_rx::RxMessage::PoseWithCovarianceStampedCallback ( )
private

"Callback" function when constructing PoseWithCovarianceStamped messages

Returns
A smart pointer to the ROS message PoseWithCovarianceStamped just created

The position_covariance array is populated in row-major order, where the basis of the correspond matrix is (E, N, U, Roll, Pitch, Heading). Important: The Euler angles (Roll, Pitch, Heading) are with respect to a vehicle-fixed (e.g. for mosaic-x5 in moving base mode via the command setAntennaLocation, ...) !local! NED frame. Thus the orientation is !not! given with respect to the same frame as the position is given in. The cross-covariances are hence (apart from the fact that e.g. mosaic receivers do not calculate these quantities) set to zero. The position and the partial (with 2 antennas) or full (for INS receivers) orientation have covariances matrices available e.g. in the PosCovGeodetic or AttCovEuler blocks, yet those are separate computations.

Definition at line 282 of file rx_message.cpp.

◆ PVTCartesianCallback()

rosaic::PVTCartesianPtr io_comm_rx::RxMessage::PVTCartesianCallback ( PVTCartesian data)
private

Callback function when reading PVTCartesian blocks.

Parameters
[in]dataThe (packed and aligned) struct instance used to populate the ROS message PVTCartesian
Returns
A smart pointer to the ROS message PVTCartesian just created

Definition at line 140 of file rx_message.cpp.

◆ PVTGeodeticCallback()

rosaic::PVTGeodeticPtr io_comm_rx::RxMessage::PVTGeodeticCallback ( PVTGeodetic data)
private

Callback function when reading PVTGeodetic blocks.

Parameters
[in]dataThe (packed and aligned) struct instance used to populate the ROS message PVTGeodetic
Returns
A smart pointer to the ROS message PVTGeodetic just created

Definition at line 100 of file rx_message.cpp.

◆ read()

template<typename T >
bool io_comm_rx::RxMessage::read ( typename boost::call_traits< T >::reference  message,
std::string  message_key,
bool  search = false 
)

Performs the CRC check (if SBF) and populates ROS message "message" with the necessary content.

Returns
True if read was successful, false otherwise

Note that the boost::call_traits<T>::reference is more robust than the traditional T&. Note that this function can also deal with the appropriate !derived! parser in case T is an NMEA message. Note that putting the default in the definition's argument list instead of the declaration's is an added extra that is not available for function templates, hence no search = false here. Also note that it is bad practice (one gets undefined reference to .. error) to separate the definition of template functions into the source file and declarations into header file. Also note that the SBF block header part of the SBF-echoing ROS messages have ID fields that only show the block number as found in the firmware (e.g. 4007 for PVTGeodetic), without the revision number. NMEA 0183 messages are at most 82 characters long in principle, but most Septentrio Rxs by default increase precision on lat/lon s.t. the maximum allowed e.g. for GGA seems to be 89 on a mosaic-x5. Luckily, when parsing we do not care since we just search for <LF><CR>.

Definition at line 477 of file rx_message.hpp.

◆ search()

const uint8_t * io_comm_rx::RxMessage::search ( )

Searches the buffer for the beginning of the next message (NMEA or SBF)

Returns
A pointer to the start of the next message.

Definition at line 834 of file rx_message.cpp.

Member Data Documentation

◆ count_

std::size_t io_comm_rx::RxMessage::count_
private

Number of unread bytes in the buffer.

Definition at line 303 of file rx_message.hpp.

◆ count_gpsfix_

uint32_t io_comm_rx::RxMessage::count_gpsfix_ = 0
staticprivate

Number of times the gps_common::GPSFix message has been published.

Definition at line 318 of file rx_message.hpp.

◆ crc_check_

bool io_comm_rx::RxMessage::crc_check_
private

Whether the CRC check as evaluated in the read() method was successful or not is stored here.

Definition at line 308 of file rx_message.hpp.

◆ data_

const uint8_t* io_comm_rx::RxMessage::data_
private

Pointer to the buffer of messages.

Definition at line 298 of file rx_message.hpp.

◆ found_

bool io_comm_rx::RxMessage::found_

Whether or not a message has been found.

Definition at line 291 of file rx_message.hpp.

◆ last_attcoveuler_

AttCovEuler io_comm_rx::RxMessage::last_attcoveuler_ = AttCovEuler()
staticprivate

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

Definition at line 338 of file rx_message.hpp.

◆ last_atteuler_

AttEuler io_comm_rx::RxMessage::last_atteuler_ = AttEuler()
staticprivate

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

Definition at line 333 of file rx_message.hpp.

◆ last_channelstatus_

ChannelStatus io_comm_rx::RxMessage::last_channelstatus_ = ChannelStatus()
staticprivate

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

Definition at line 343 of file rx_message.hpp.

◆ last_dop_

DOP io_comm_rx::RxMessage::last_dop_ = DOP()
staticprivate

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

Definition at line 353 of file rx_message.hpp.

◆ last_measepoch_

MeasEpoch io_comm_rx::RxMessage::last_measepoch_ = MeasEpoch()
staticprivate

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

Definition at line 348 of file rx_message.hpp.

◆ last_poscovgeodetic_

PosCovGeodetic io_comm_rx::RxMessage::last_poscovgeodetic_ = PosCovGeodetic()
staticprivate

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

Definition at line 328 of file rx_message.hpp.

◆ last_pvtgeodetic_

PVTGeodetic io_comm_rx::RxMessage::last_pvtgeodetic_ = PVTGeodetic()
staticprivate

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

Definition at line 323 of file rx_message.hpp.

◆ last_qualityind_

QualityInd io_comm_rx::RxMessage::last_qualityind_ = QualityInd()
staticprivate

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

Definition at line 368 of file rx_message.hpp.

◆ last_receiversetup_

ReceiverSetup io_comm_rx::RxMessage::last_receiversetup_ = ReceiverSetup()
staticprivate

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

Definition at line 373 of file rx_message.hpp.

◆ last_receiverstatus_

ReceiverStatus io_comm_rx::RxMessage::last_receiverstatus_ = ReceiverStatus()
staticprivate

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

Definition at line 363 of file rx_message.hpp.

◆ last_velcovgeodetic_

VelCovGeodetic io_comm_rx::RxMessage::last_velcovgeodetic_ = VelCovGeodetic()
staticprivate

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

Definition at line 358 of file rx_message.hpp.

◆ message_size_

std::size_t io_comm_rx::RxMessage::message_size_
private

Helps to determine size of response message / NMEA message / SBF block.

Definition at line 313 of file rx_message.hpp.

◆ rx_id_map

io_comm_rx::RxMessage::RxIDMap io_comm_rx::RxMessage::rx_id_map
staticprivate

All instances of the RxMessage class shall have access to the map without reinitializing it, hence static.

This map is for mapping ROS message, SBF and NMEA identifiers to an enumeration and makes the switch-case formalism in rx_message.hpp more explicit.

Definition at line 392 of file rx_message.hpp.

◆ type_of_pvt_map

io_comm_rx::RxMessage::TypeOfPVTMap io_comm_rx::RxMessage::type_of_pvt_map
staticprivate

All instances of the RxMessage class shall have access to the map without reinitializing it, hence static.

Definition at line 381 of file rx_message.hpp.


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


rosaic
Author(s): Tibor Dome
autogenerated on Wed Oct 14 2020 03:43:50