ROSaic
Public Member Functions | Data Fields | Private Member Functions | Private Attributes | Static Private Attributes
io_comm_mosaic::mosaicMessage Class Reference

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

#include <mosaicMessage.hpp>

Collaboration diagram for io_comm_mosaic::mosaicMessage:
Collaboration graph
[legend]

Public Member Functions

 mosaicMessage (const uint8_t *data, std::size_t &size)
 Constructor of the mosaicMessage class. 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 IsSBF ()
 Determines whether data_ currently points to an SBF block. 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 IsConnectionDescriptor ()
 Determines whether data_ currently points to a connection descriptor (right after initiating TCP connection) More...
 
bool IsErrorMessage ()
 Determines whether data_ currently points to an error message reply from mosaic. More...
 
std::size_t SegmentEnd ()
 Determines size of message that data_ is currently pointing at. More...
 
std::string MessageID ()
 Returns the MessageID of the message where data_ is pointing at at the moment, SBF identifiers embellished with inverted commas, e.g. "5003". More...
 
uint32_t GetCount ()
 Returns the count_ variable. More...
 
const uint8_t * Search ()
 Searches the buffer for the beginning of the next message (NMEA or SBF) More...
 
uint16_t BlockLength ()
 Gets the length of the SBF block. More...
 
const uint8_t * Pos ()
 Gets the current position in the read buffer. More...
 
const uint8_t * End ()
 Gets the end position in the read buffer. More...
 
bool Found ()
 Has an NMEA message or SBF block been found in the buffer? More...
 
const uint8_t * 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 block) and populates message with the necessary content (mapped 1-to-1 if SBF, parsed if NMEA), works for pure ROS message mapping. More...
 

Data Fields

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

Private Member Functions

rosaic::PVTCartesianPtr PVTCartesianCallback (PVTCartesian &data)
 Callback function when reading PVTCartesian blocks. More...
 
rosaic::PVTGeodeticPtr PVTGeodeticCallback (PVTGeodetic &data)
 Callback function when reading PVTGeodetic blocks. 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...
 
rosaic::AttEulerPtr AttEulerCallback (AttEuler &data)
 Callback function when reading AttEuler blocks. More...
 
rosaic::AttCovEulerPtr AttCovEulerCallback (AttCovEuler &data)
 Callback function when reading AttCovEuler blocks. More...
 
sensor_msgs::NavSatFixPtr NavSatFixCallback ()
 "Callback" function when constructing NavSatFix messages More...
 
gps_common::GPSFixPtr GPSFixCallback ()
 "Callback" function when constructing GPSFix messages More...
 
geometry_msgs::PoseWithCovarianceStampedPtr PoseWithCovarianceStampedCallback ()
 "Callback" function when constructing PoseWithCovarianceStamped messages More...
 

Private Attributes

const uint8_t * data_
 The pointer to the buffer of messages. More...
 
uint32_t count_
 The number of bytes in the buffer, decremented as the buffer is read (so it is the buffer's size in the beginning) More...
 
bool CRCcheck_
 Whether the CRC check as evaluated in the read() method was successful or not is stored here. More...
 
std::size_t segment_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 PVTGeodetic last_pvtgeodetic_ = PVTGeodetic()
 Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic 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 AttEuler last_atteuler_ = AttEuler()
 Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to be stored. More...
 
static AttCovEuler last_attcoveuler_ = AttCovEuler()
 Since GPSFix etc. need AttCovEuler, incoming AttCovEuler blocks need to be stored. More...
 
static ChannelStatus last_channelstatus_ = ChannelStatus()
 Since GPSFix etc. need ChannelStatus, incoming ChannelStatus blocks need to be stored. More...
 
static MeasEpoch last_measepoch_ = MeasEpoch()
 Since GPSFix etc. need MeasEpoch (for signal-to-noise ratios), incoming MeasEpoch blocks need to be stored. More...
 
static DOP last_dop_ = DOP()
 Since GPSFix etc. need DOP, incoming DOP blocks need to be stored. More...
 
static VelCovGeodetic last_velcovgeodetic_ = VelCovGeodetic()
 Since GPSFix etc. need VelCovGeodetic (for signal-to-noise ratios), incoming VelCovGeodetic blocks need to be stored. More...
 

Detailed Description

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

Definition at line 205 of file mosaicMessage.hpp.

Constructor & Destructor Documentation

◆ mosaicMessage()

io_comm_mosaic::mosaicMessage::mosaicMessage ( const uint8_t *  data,
std::size_t &  size 
)
inline

Constructor of the mosaicMessage 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 216 of file mosaicMessage.hpp.

References CRCcheck_, found_, IsConnectionDescriptor(), IsErrorMessage(), IsMessage(), IsNMEA(), IsResponse(), IsSBF(), MessageID(), segment_size_, and SegmentEnd().

216 : data_(data), count_(size) {found_ = false; CRCcheck_ = false; segment_size_ = 0;}
std::size_t segment_size_
Helps to determine size of response message / NMEA message / SBF block.
bool found_
Whether or not a message has been found.
uint32_t count_
The number of bytes in the buffer, decremented as the buffer is read (so it is the buffer&#39;s size in t...
bool CRCcheck_
Whether the CRC check as evaluated in the read() method was successful or not is stored here...
const uint8_t * data_
The pointer to the buffer of messages.
Here is the call graph for this function:

Member Function Documentation

◆ AttCovEulerCallback()

rosaic::AttCovEulerPtr io_comm_mosaic::mosaicMessage::AttCovEulerCallback ( AttCovEuler data)
private

Callback function when reading AttCovEuler blocks.

Parameters
[in]dataThe (packed and aligned) struct instance that we use to populate our ROS message rosaic::AttCovEuler
Returns
A smart pointer to the ROS message rosaic::AttCovEuler just created

Definition at line 202 of file mosaicMessage.cpp.

References AttCovEuler::Block_Header, AttCovEuler::Cov_HeadHead, AttCovEuler::Cov_HeadPitch, AttCovEuler::Cov_HeadRoll, AttCovEuler::Cov_PitchPitch, AttCovEuler::Cov_PitchRoll, AttCovEuler::Cov_RollRoll, BlockHeader_t::CRC, AttCovEuler::Error, BlockHeader_t::ID, BlockHeader_t::Length, BlockHeader_t::SYNC1, BlockHeader_t::SYNC2, AttCovEuler::TOW, and AttCovEuler::WNc.

Referenced by Read().

203 {
204  rosaic::AttCovEulerPtr msg = boost::make_shared<rosaic::AttCovEuler>();
205  msg->Block_Header.SYNC1 = data.Block_Header.SYNC1;
206  msg->Block_Header.SYNC2 = data.Block_Header.SYNC2;
207  msg->Block_Header.CRC = data.Block_Header.CRC;
208  msg->Block_Header.ID = data.Block_Header.ID;
209  msg->Block_Header.Length = data.Block_Header.Length;
210  msg->Block_Header.TOW = data.TOW;
211  msg->Block_Header.WNc = data.WNc;
212  msg->Error = data.Error;
213  msg->Cov_HeadHead = data.Cov_HeadHead;
214  msg->Cov_PitchPitch = data.Cov_PitchPitch;
215  msg->Cov_RollRoll = data.Cov_RollRoll;
216  msg->Cov_HeadPitch = data.Cov_HeadPitch;
217  msg->Cov_HeadRoll = data.Cov_HeadRoll;
218  msg->Cov_PitchRoll = data.Cov_PitchRoll;
219  return msg;
220 };
float Cov_PitchRoll
float Cov_HeadPitch
uint8_t Error
uint16_t CRC
the Check Sum !
float Cov_HeadRoll
uint16_t Length
length of the entire message including the header. A multiple of 4 between 8 and 4096 ...
float Cov_HeadHead
uint32_t TOW
uint16_t ID
The ID is the "Block ID".
uint8_t SYNC1
first sync byte is $ or 0x24
uint16_t WNc
float Cov_RollRoll
float Cov_PitchPitch
uint8_t SYNC2
2nd sync byte is @ or 0x40
BlockHeader_t Block_Header
Here is the caller graph for this function:

◆ AttEulerCallback()

rosaic::AttEulerPtr io_comm_mosaic::mosaicMessage::AttEulerCallback ( AttEuler data)
private

Callback function when reading AttEuler blocks.

Parameters
[in]dataThe (packed and aligned) struct instance that we use to populate our ROS message rosaic::AttEuler
Returns
A smart pointer to the ROS message rosaic::AttEuler just created

Definition at line 180 of file mosaicMessage.cpp.

References AttEuler::Block_Header, BlockHeader_t::CRC, AttEuler::Error, AttEuler::Heading, AttEuler::HeadingDot, BlockHeader_t::ID, BlockHeader_t::Length, AttEuler::Mode, AttEuler::NrSV, AttEuler::Pitch, AttEuler::PitchDot, AttEuler::Roll, AttEuler::RollDot, BlockHeader_t::SYNC1, BlockHeader_t::SYNC2, AttEuler::TOW, and AttEuler::WNc.

Referenced by Read().

181 {
182  rosaic::AttEulerPtr msg = boost::make_shared<rosaic::AttEuler>();
183  msg->Block_Header.SYNC1 = data.Block_Header.SYNC1;
184  msg->Block_Header.SYNC2 = data.Block_Header.SYNC2;
185  msg->Block_Header.CRC = data.Block_Header.CRC;
186  msg->Block_Header.ID = data.Block_Header.ID;
187  msg->Block_Header.Length = data.Block_Header.Length;
188  msg->Block_Header.TOW = data.TOW;
189  msg->Block_Header.WNc = data.WNc;
190  msg->NrSV = data.NrSV;
191  msg->Error = data.Error;
192  msg->Mode = data.Mode;
193  msg->Heading = data.Heading;
194  msg->Pitch = data.Pitch;
195  msg->Roll = data.Roll;
196  msg->PitchDot = data.PitchDot;
197  msg->RollDot = data.RollDot;
198  msg->HeadingDot = data.HeadingDot;
199  return msg;
200 };
uint32_t TOW
BlockHeader_t Block_Header
uint8_t Error
float RollDot
uint16_t CRC
the Check Sum !
uint16_t WNc
uint16_t Length
length of the entire message including the header. A multiple of 4 between 8 and 4096 ...
float Roll
float Heading
uint8_t NrSV
float Pitch
uint16_t ID
The ID is the "Block ID".
float HeadingDot
uint8_t SYNC1
first sync byte is $ or 0x24
float PitchDot
uint16_t Mode
uint8_t SYNC2
2nd sync byte is @ or 0x40
Here is the caller graph for this function:

◆ BlockLength()

uint16_t io_comm_mosaic::mosaicMessage::BlockLength ( )

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 853 of file mosaicMessage.cpp.

References data_, and IsSBF().

Referenced by GetCount(), Next(), Read(), and io_comm_mosaic::CallbackHandlers::ReadCallback().

854 {
855  if (this->IsSBF())
856  {
857  uint16_t block_length;
858  // Note that static_cast<uint16_t>(data_[6]) would just take the one byte "data_[6]" and cast it as requested, !neglecting! the byte "data_[7]".
859  block_length = *(reinterpret_cast<const uint16_t*>(data_ + 6));
860  return block_length;
861  }
862  else
863  {
864  return 0;
865  }
866 }
bool IsSBF()
Determines whether data_ currently points to an SBF block.
const uint8_t * data_
The pointer to the buffer of messages.
Here is the call graph for this function:
Here is the caller graph for this function:

◆ End()

const uint8_t * io_comm_mosaic::mosaicMessage::End ( )

Gets the end position in the read buffer.

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

Definition at line 848 of file mosaicMessage.cpp.

References count_, and data_.

Referenced by GetCount(), and io_comm_mosaic::CallbackHandlers::ReadCallback().

849 {
850  return data_ + count_;
851 }
uint32_t count_
The number of bytes in the buffer, decremented as the buffer is read (so it is the buffer&#39;s size in t...
const uint8_t * data_
The pointer to the buffer of messages.
Here is the caller graph for this function:

◆ Found()

bool io_comm_mosaic::mosaicMessage::Found ( )

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

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

Definition at line 652 of file mosaicMessage.cpp.

References found_, IsConnectionDescriptor(), IsNMEA(), IsResponse(), IsSBF(), and read_cd.

Referenced by GetCount(), Next(), Read(), and io_comm_mosaic::CallbackHandlers::ReadCallback().

653 {
654  if (found_) return true;
655 
656  // Verify header bytes
657  if (!this->IsSBF() && !this->IsNMEA() && !this->IsResponse() && !(read_cd && this->IsConnectionDescriptor()))
658  {
659  return false;
660  }
661 
662  found_ = true;
663  return true;
664 }
bool found_
Whether or not a message has been found.
bool read_cd
Whether or not we still want to read the connection descriptor, which we only want in the very beginn...
bool IsResponse()
Determines whether data_ currently points to an NMEA message.
bool IsNMEA()
Determines whether data_ currently points to an NMEA message.
bool IsSBF()
Determines whether data_ currently points to an SBF block.
bool IsConnectionDescriptor()
Determines whether data_ currently points to a connection descriptor (right after initiating TCP conn...
Here is the call graph for this function:
Here is the caller graph for this function:

◆ GetCount()

uint32_t io_comm_mosaic::mosaicMessage::GetCount ( )
inline

Returns the count_ variable.

Returns
The variable count_

Definition at line 241 of file mosaicMessage.hpp.

References BlockLength(), count_, End(), Found(), Next(), Pos(), Read(), and Search().

241 {return count_;};
uint32_t count_
The number of bytes in the buffer, decremented as the buffer is read (so it is the buffer&#39;s size in t...
Here is the call graph for this function:

◆ GPSFixCallback()

gps_common::GPSFixPtr io_comm_mosaic::mosaicMessage::GPSFixCallback ( )
private

"Callback" function when constructing GPSFix messages

Returns
A smart pointer to the ROS message gps_common::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 to understand the meaning of the satellite 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 mosaic 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 its 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".

Definition at line 365 of file mosaicMessage.cpp.

References ChannelSatInfo::Az_RiseSet, ChannelStatus::Block_Header, MeasEpoch::Block_Header, MeasEpochChannelType1::CN0, PVTGeodetic::COG, PosCovGeodetic::Cov_bb, PosCovGeodetic::Cov_hgthgt, PosCovGeodetic::Cov_lathgt, PosCovGeodetic::Cov_latlat, PosCovGeodetic::Cov_latlon, PosCovGeodetic::Cov_lonhgt, PosCovGeodetic::Cov_lonlon, AttCovEuler::Cov_PitchPitch, AttCovEuler::Cov_RollRoll, VelCovGeodetic::Cov_VeVe, VelCovGeodetic::Cov_VnVn, VelCovGeodetic::Cov_VuVu, ChannelStatus::Data, MeasEpoch::Data, ChannelSatInfo::Elev, DOP::HDOP, PVTGeodetic::Height, last_attcoveuler_, last_atteuler_, last_channelstatus_, last_dop_, last_measepoch_, last_poscovgeodetic_, last_pvtgeodetic_, last_velcovgeodetic_, PVTGeodetic::Latitude, PVTGeodetic::Longitude, PVTGeodetic::Mode, ChannelStatus::N, MeasEpoch::N, ChannelSatInfo::N2, MeasEpochChannelType1::N_Type2, PVTGeodetic::NrSV, DOP::PDOP, AttEuler::Pitch, ChannelStateInfo::PVTStatus, PVTGeodetic::ReferenceID, AttEuler::Roll, ChannelStatus::SB1Size, MeasEpoch::SB1Size, ChannelStatus::SB2Size, MeasEpoch::SB2Size, ChannelSatInfo::SVID, MeasEpochChannelType1::SVID, svid_pvt, BlockHeader_t::SYNC1, DOP::TDOP, PVTGeodetic::TOW, MeasEpochChannelType1::Type, DOP::VDOP, PVTGeodetic::Ve, PVTGeodetic::Vn, PVTGeodetic::Vu, and PVTGeodetic::WNc.

Referenced by Read().

366 {
367  gps_common::GPSFixPtr msg = boost::make_shared<gps_common::GPSFix>();
368 
369  msg->status.satellites_used = static_cast<uint16_t>(last_pvtgeodetic_.NrSV);
370 
371  // MeasEpoch Processing
372  std::vector<int32_t> cno_tracked;
373  std::vector<int32_t> svid_in_sync;
374  {
375  uint8_t sb1_size = last_measepoch_.SB1Size;
376  uint8_t sb2_size = last_measepoch_.SB2Size;
377  uint8_t *sb_start = &last_measepoch_.Data[0];
378  int32_t index = sb_start - &last_measepoch_.Block_Header.SYNC1;
379  for (int32_t i = 0; i < static_cast<int32_t>(last_measepoch_.N); ++i)
380  {
381  // Define MeasEpochChannelType1 struct for the corresponding sub-block
382  MeasEpochChannelType1 *measepoch_channel_type1 = reinterpret_cast<MeasEpochChannelType1*>(&last_measepoch_.Block_Header.SYNC1 + index);
383  svid_in_sync.push_back(static_cast<int32_t>(measepoch_channel_type1->SVID));
384  uint8_t type_mask = 15; // We extract the first four bits using this mask.
385  if (measepoch_channel_type1->Type & type_mask == static_cast<uint8_t>(1) || measepoch_channel_type1->Type & type_mask == static_cast<uint8_t>(2))
386  {
387  cno_tracked.push_back(static_cast<int32_t>(measepoch_channel_type1->CN0)/4);
388  }
389  else
390  {
391  cno_tracked.push_back(static_cast<int32_t>(measepoch_channel_type1->CN0)/4+static_cast<int32_t>(10));
392  }
393  //ROS_DEBUG("static_cast<int32_t>(measepoch_channel_type1->SVID)) is %i", static_cast<int32_t>(measepoch_channel_type1->SVID));
394  index += sb1_size;
395  for (int32_t j = 0; j < static_cast<int32_t>(measepoch_channel_type1->N_Type2); j++)
396  {
397  index += sb2_size;
398  }
399  }
400  }
401 
402  // ChannelStatus Processing
403  std::vector<int32_t> svid_in_sync_2;
404  std::vector<int32_t> elevation_tracked;
405  std::vector<int32_t> azimuth_tracked;
406  std::vector<int32_t> svid_pvt;
407  svid_pvt.clear();
408  std::vector<int32_t> ordering;
409  {
410  uint8_t sb1_size = last_channelstatus_.SB1Size;
411  uint8_t sb2_size = last_channelstatus_.SB2Size;
412  uint8_t *sb_start = &last_channelstatus_.Data[0];
413  int32_t index = sb_start - &last_channelstatus_.Block_Header.SYNC1;
414  //ROS_DEBUG("index is %i", index); // yields 20, as expected
415 
416  uint16_t azimuth_mask = 511;
417  for (int32_t i = 0; i < static_cast<int32_t>(last_channelstatus_.N); i++)
418  {
419  // Define ChannelSatInfo struct for the corresponding sub-block
420  ChannelSatInfo *channel_sat_info = reinterpret_cast<ChannelSatInfo*>(&last_channelstatus_.Block_Header.SYNC1 + index);
421  bool to_be_added = false;
422  for (int32_t j = 0; j < static_cast<int32_t>(svid_in_sync.size()); ++j)
423  {
424  if (svid_in_sync[j] == static_cast<int32_t>(channel_sat_info->SVID))
425  {
426  ordering.push_back(j);
427  to_be_added = true;
428  break;
429  }
430  }
431  if (to_be_added)
432  {
433  svid_in_sync_2.push_back(static_cast<int32_t>(channel_sat_info->SVID));
434  elevation_tracked.push_back(static_cast<int32_t>(channel_sat_info->Elev));
435  azimuth_tracked.push_back(static_cast<int32_t>((channel_sat_info->Az_RiseSet & azimuth_mask)));
436  }
437  index += sb1_size;
438  for (int32_t j = 0; j < static_cast<int32_t>(channel_sat_info->N2); j++)
439  {
440  // Define ChannelStateInfo struct for the corresponding sub-block
441  ChannelStateInfo *channel_state_info = reinterpret_cast<ChannelStateInfo*>(&last_channelstatus_.Block_Header.SYNC1 + index);
442  bool pvt_status = false;
443  uint16_t pvt_status_mask = std::pow(2,15)+std::pow(2,14);
444  for(int k = 15; k != -1; k -= 2)
445  {
446  uint16_t pvt_status_value = (channel_state_info->PVTStatus & pvt_status_mask) >> k-1;
447  //ROS_DEBUG("value is %u and channel_state_info->PVTStatus is %u", pvt_status_value, channel_state_info->PVTStatus);
448  if (pvt_status_value == 2)
449  {
450  pvt_status = true;
451  }
452  if (k > 1)
453  {
454  pvt_status_mask = pvt_status_mask - std::pow(2,k) - std::pow(2,k-1) + std::pow(2,k-2) + std::pow(2,k-3);
455  }
456  }
457  if (pvt_status)
458  {
459  svid_pvt.push_back(static_cast<int32_t>(channel_sat_info->SVID));
460  }
461  index += sb2_size;
462  }
463  }
464  }
465  msg->status.satellite_used_prn = svid_pvt; // Entries such as int32[] in ROS messages are to be treated as std::vectors.
466  msg->status.satellites_visible = static_cast<uint16_t>(svid_in_sync.size());
467  msg->status.satellite_visible_prn = svid_in_sync_2;
468  msg->status.satellite_visible_z = elevation_tracked;
469  msg->status.satellite_visible_azimuth = azimuth_tracked;
470 
471  // Reordering CNO vector to that of all previous arrays
472  std::vector<int32_t> cno_tracked_reordered;
473  //ROS_DEBUG("size svid_in_sync is %i, size of cno_tracked is %i, size of svid_in_sync_2 is %i", static_cast<int32_t>(svid_in_sync.size()), static_cast<int32_t>(cno_tracked.size()), static_cast<int32_t>(svid_in_sync_2.size()));
474  if (static_cast<int32_t>(last_channelstatus_.N) != 0)
475  {
476  for (int32_t k = 0; k < static_cast<int32_t>(ordering.size()); ++k)
477  {
478  cno_tracked_reordered.push_back(cno_tracked[ordering[k]]);
479  }
480  }
481  msg->status.satellite_visible_snr = cno_tracked_reordered;
482 
483  // PVT Status Analysis
484  uint16_t status_mask = 15; // We extract the first four bits using this mask.
485  uint16_t type_of_pvt = ((uint16_t) (last_pvtgeodetic_.Mode)) & status_mask;
486  switch(type_of_pvt)
487  {
488  case 0:
489  {
490  msg->status.status = gps_common::GPSStatus::STATUS_NO_FIX;
491  break;
492  }
493  case 1: case 3:
494  {
495  msg->status.status = gps_common::GPSStatus::STATUS_FIX;
496  break;
497  }
498  case 2: case 4: case 5: case 7: case 8: case 10:
499  {
500  msg->status.status = gps_common::GPSStatus::STATUS_GBAS_FIX;
501  break;
502  }
503  case 6:
504  {
505  uint16_t reference_id = last_pvtgeodetic_.ReferenceID;
506  if (reference_id == 131 || reference_id == 133 || reference_id == 135 || reference_id == 135) // PRNs of the 4 WAAS satellites
507  {
508  msg->status.status = gps_common::GPSStatus::STATUS_WAAS_FIX;
509  }
510  else
511  {
512  msg->status.status = gps_common::GPSStatus::STATUS_SBAS_FIX;
513  }
514  break;
515  }
516  default:
517  {
518  throw std::runtime_error("PVTGeodetic's Mode field contains an invalid type of PVT solution.");
519  }
520  }
521  msg->status.motion_source = gps_common::GPSStatus::SOURCE_POINTS; // Doppler is not used when calculating the velocities of mosaic.
522  msg->status.orientation_source = gps_common::GPSStatus::SOURCE_POINTS; // Doppler is not used when calculating the orientation of mosaic.
523  msg->status.position_source = gps_common::GPSStatus::SOURCE_GPS;
524  msg->latitude = static_cast<double>(last_pvtgeodetic_.Latitude)*360/(2*boost::math::constants::pi<double>());
525  msg->longitude = static_cast<double>(last_pvtgeodetic_.Longitude)*360/(2*boost::math::constants::pi<double>());
526  msg->altitude = static_cast<double>(last_pvtgeodetic_.Height);
527  msg->track = static_cast<double>(last_pvtgeodetic_.COG); // Note that COG is of type float32 while track is of type float64.
528  msg->speed = std::sqrt(std::pow(static_cast<double>(last_pvtgeodetic_.Vn), 2) + std::pow(static_cast<double>(last_pvtgeodetic_.Ve), 2));
529  msg->climb = static_cast<double>(last_pvtgeodetic_.Vu);
530  msg->pitch = static_cast<double>(last_atteuler_.Pitch);
531  msg->roll = static_cast<double>(last_atteuler_.Roll);
532  if (last_dop_.PDOP == static_cast<uint16_t>(0) || last_dop_.TDOP == static_cast<uint16_t>(0))
533  {
534  msg->gdop = static_cast<double>(-1);
535  }
536  else
537  {
538  msg->gdop = std::sqrt(std::pow(static_cast<double>(last_dop_.PDOP)/100, 2) + std::pow(static_cast<double>(last_dop_.TDOP)/100, 2));
539  }
540  if (last_dop_.PDOP == static_cast<uint16_t>(0))
541  {
542  msg->pdop = static_cast<double>(-1);
543  }
544  else
545  {
546  msg->pdop = static_cast<double>(last_dop_.PDOP)/100;
547  }
548  if (last_dop_.HDOP == static_cast<uint16_t>(0))
549  {
550  msg->hdop = static_cast<double>(-1);
551  }
552  else
553  {
554  msg->hdop = static_cast<double>(last_dop_.HDOP)/100;
555  }
556  if (last_dop_.VDOP == static_cast<uint16_t>(0))
557  {
558  msg->vdop = static_cast<double>(-1);
559  }
560  else
561  {
562  msg->vdop = static_cast<double>(last_dop_.VDOP)/100;
563  }
564  if (last_dop_.TDOP == static_cast<uint16_t>(0))
565  {
566  msg->tdop = static_cast<double>(-1);
567  }
568  else
569  {
570  msg->tdop = static_cast<double>(last_dop_.TDOP)/100;
571  }
572  msg->time = static_cast<double>(last_pvtgeodetic_.TOW)/1000 + static_cast<double>(last_pvtgeodetic_.WNc*7*24*60*60);
573  msg->err = 2*(std::sqrt(static_cast<double>(last_poscovgeodetic_.Cov_latlat) + static_cast<double>(last_poscovgeodetic_.Cov_lonlon) + static_cast<double>(last_poscovgeodetic_.Cov_hgthgt)));
574  msg->err_horz = 2*(std::sqrt(static_cast<double>(last_poscovgeodetic_.Cov_latlat) + static_cast<double>(last_poscovgeodetic_.Cov_lonlon)));
575  msg->err_vert = 2*std::sqrt(static_cast<double>(last_poscovgeodetic_.Cov_hgthgt));
576  msg->err_track = 2*(std::sqrt(std::pow(static_cast<double>(1)/(static_cast<double>(last_pvtgeodetic_.Vn)+std::pow(static_cast<double>(last_pvtgeodetic_.Ve),2)/static_cast<double>(last_pvtgeodetic_.Vn)),2)*static_cast<double>(last_poscovgeodetic_.Cov_lonlon)+std::pow((static_cast<double>(last_pvtgeodetic_.Ve))/(std::pow(static_cast<double>(last_pvtgeodetic_.Vn),2)+std::pow(static_cast<double>(last_pvtgeodetic_.Ve),2)),2)*static_cast<double>(last_poscovgeodetic_.Cov_latlat)));
577  msg->err_speed = 2*(std::sqrt(static_cast<double>(last_velcovgeodetic_.Cov_VnVn) + static_cast<double>(last_velcovgeodetic_.Cov_VeVe)));
578  msg->err_climb = 2*std::sqrt(static_cast<double>(last_velcovgeodetic_.Cov_VuVu));
579  msg->err_time = 2*std::sqrt(static_cast<double>(last_poscovgeodetic_.Cov_bb));
580  msg->err_pitch = 2*std::sqrt(static_cast<double>(last_attcoveuler_.Cov_PitchPitch));
581  msg->err_roll = 2*std::sqrt(static_cast<double>(last_attcoveuler_.Cov_RollRoll));
582  msg->position_covariance[0] = static_cast<double>(last_poscovgeodetic_.Cov_lonlon);
583  msg->position_covariance[1] = static_cast<double>(last_poscovgeodetic_.Cov_latlon);
584  msg->position_covariance[2] = static_cast<double>(last_poscovgeodetic_.Cov_lonhgt);
585  msg->position_covariance[3] = static_cast<double>(last_poscovgeodetic_.Cov_latlon);
586  msg->position_covariance[4] = static_cast<double>(last_poscovgeodetic_.Cov_latlat);
587  msg->position_covariance[5] = static_cast<double>(last_poscovgeodetic_.Cov_lathgt);
588  msg->position_covariance[6] = static_cast<double>(last_poscovgeodetic_.Cov_lonhgt);
589  msg->position_covariance[7] = static_cast<double>(last_poscovgeodetic_.Cov_lathgt);
590  msg->position_covariance[8] = static_cast<double>(last_poscovgeodetic_.Cov_hgthgt);
591  msg->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN;
592 
593  return msg;
594 }
static PosCovGeodetic last_poscovgeodetic_
Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic blocks need to be stored...
uint16_t TDOP
uint8_t N
uint16_t Az_RiseSet
static DOP last_dop_
Since GPSFix etc. need DOP, incoming DOP blocks need to be stored.
uint32_t TOW
Struct for the SBF sub-block "MeasEpochChannelType1".
static VelCovGeodetic last_velcovgeodetic_
Since GPSFix etc. need VelCovGeodetic (for signal-to-noise ratios), incoming VelCovGeodetic blocks ne...
static AttCovEuler last_attcoveuler_
Since GPSFix etc. need AttCovEuler, incoming AttCovEuler blocks need to be stored.
static PVTGeodetic last_pvtgeodetic_
Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks need to be stored.
std::vector< int32_t > svid_pvt
float Roll
uint16_t PDOP
static AttEuler last_atteuler_
Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to be stored.
uint16_t VDOP
uint8_t Data[(80+1) *sizeof(ChannelSatInfo)+((80+1) *4) *sizeof(ChannelStateInfo)]
double Longitude
uint16_t ReferenceID
float Pitch
static ChannelStatus last_channelstatus_
Since GPSFix etc. need ChannelStatus, incoming ChannelStatus blocks need to be stored.
double Latitude
Struct for the SBF sub-block "ChannelStateInfo".
BlockHeader_t Block_Header
uint8_t SYNC1
first sync byte is $ or 0x24
uint8_t NrSV
uint8_t SB1Size
Struct for the SBF sub-block "ChannelSatInfo".
float Cov_RollRoll
BlockHeader_t Block_Header
uint16_t HDOP
uint16_t WNc
uint8_t SB2Size
static MeasEpoch last_measepoch_
Since GPSFix etc. need MeasEpoch (for signal-to-noise ratios), incoming MeasEpoch blocks need to be s...
uint8_t Data[((80+1) *sizeof(MeasEpochChannelType1)+(((80+1)) *(((7) *(3)) -1)) *sizeof(MeasEpochChannelType2))]
float Cov_PitchPitch
uint8_t Mode
Here is the caller graph for this function:

◆ IsConnectionDescriptor()

bool io_comm_mosaic::mosaicMessage::IsConnectionDescriptor ( )

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

Definition at line 794 of file mosaicMessage.cpp.

References CONNECTION_DESCRIPTOR_BYTE_1, CONNECTION_DESCRIPTOR_BYTE_2, and data_.

Referenced by Found(), mosaicMessage(), Next(), io_comm_mosaic::CallbackHandlers::ReadCallback(), and Search().

795 {
797  {
798  return true;
799  }
800  else
801  {
802  return false;
803  }
804 }
#define CONNECTION_DESCRIPTOR_BYTE_2
0x50 is ASCII for P - 2nd character of connection descriptor sent by mosaic after initiating TCP conn...
#define CONNECTION_DESCRIPTOR_BYTE_1
0x49 is ASCII for I - 1st character of connection descriptor sent by mosaic after initiating TCP conn...
const uint8_t * data_
The pointer to the buffer of messages.
Here is the caller graph for this function:

◆ IsErrorMessage()

bool io_comm_mosaic::mosaicMessage::IsErrorMessage ( )

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

Definition at line 806 of file mosaicMessage.cpp.

References data_, RESPONSE_SYNC_BYTE_1, RESPONSE_SYNC_BYTE_2, and RESPONSE_SYNC_BYTE_3.

Referenced by mosaicMessage(), and io_comm_mosaic::CallbackHandlers::ReadCallback().

807 {
809  {
810  return true;
811  }
812  else
813  {
814  return false;
815  }
816 }
#define RESPONSE_SYNC_BYTE_2
0x52 is ASCII for R (for "Response") - 2nd byte in each response from mosaic
#define RESPONSE_SYNC_BYTE_3
0x3F is ASCII for ? - 3rd byte in the response message from mosaic in case the command was invalid ...
#define RESPONSE_SYNC_BYTE_1
0x24 is ASCII for $ - 1st byte in each response from mosaic
const uint8_t * data_
The pointer to the buffer of messages.
Here is the caller graph for this function:

◆ IsMessage() [1/2]

bool io_comm_mosaic::mosaicMessage::IsMessage ( const uint16_t  ID)

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

Definition at line 712 of file mosaicMessage.cpp.

References data_, and IsSBF().

Referenced by mosaicMessage().

713 {
714  if (this->IsSBF())
715  {
716  uint16_t mask = 8191;
717  if (*(reinterpret_cast<const uint16_t *>(data_ + 4)) & mask == static_cast<const uint16_t>(ID))
718  // Caution: reinterpret_cast is the most dangerous cast, It's used primarily for particularly weird conversions and bit manipulations, like turning a raw data stream into actual data
719  {
720  return true;
721  }
722  else
723  {
724  return false;
725  }
726  }
727  else
728  {
729  return false;
730  }
731 }
bool IsSBF()
Determines whether data_ currently points to an SBF block.
const uint8_t * data_
The pointer to the buffer of messages.
Here is the call graph for this function:
Here is the caller graph for this function:

◆ IsMessage() [2/2]

bool io_comm_mosaic::mosaicMessage::IsMessage ( std::string  ID)

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

Definition at line 734 of file mosaicMessage.cpp.

References data_, IsNMEA(), and SegmentEnd().

735 {
736  if (this->IsNMEA())
737  {
738  boost::char_separator<char> sep(",");
739  typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
740  std::size_t nmea_size = std::min(this->SegmentEnd(), static_cast<std::size_t>(89));
741  std::string block_in_string(reinterpret_cast<const char*>(data_), nmea_size);
742  tokenizer tokens(block_in_string,sep);
743  if (*tokens.begin() == ID)
744  {
745  return true;
746  }
747  else
748  {
749  return false;
750  }
751  }
752  else
753  {
754  return false;
755  }
756 }
bool IsNMEA()
Determines whether data_ currently points to an NMEA message.
const uint8_t * data_
The pointer to the buffer of messages.
std::size_t SegmentEnd()
Determines size of message that data_ is currently pointing at.
Here is the call graph for this function:

◆ IsNMEA()

bool io_comm_mosaic::mosaicMessage::IsNMEA ( )

Determines whether data_ currently points to an NMEA message.

Definition at line 770 of file mosaicMessage.cpp.

References data_, NMEA_SYNC_BYTE_1, NMEA_SYNC_BYTE_2_1, and NMEA_SYNC_BYTE_2_2.

Referenced by Found(), IsMessage(), MessageID(), mosaicMessage(), Next(), io_comm_mosaic::CallbackHandlers::ReadCallback(), and Search().

771 {
773  {
774  return true;
775  }
776  else
777  {
778  return false;
779  }
780 }
#define NMEA_SYNC_BYTE_1
0x24 is ASCII for $ - 1st byte in each message
#define NMEA_SYNC_BYTE_2_1
0x47 is ASCII for G - 2nd byte to indicate NMEA-type ASCII message
const uint8_t * data_
The pointer to the buffer of messages.
#define NMEA_SYNC_BYTE_2_2
0x50 is ASCII for P - 2nd byte to indicate proprietary ASCII message
Here is the caller graph for this function:

◆ IsResponse()

bool io_comm_mosaic::mosaicMessage::IsResponse ( )

Determines whether data_ currently points to an NMEA message.

Definition at line 782 of file mosaicMessage.cpp.

References data_, RESPONSE_SYNC_BYTE_1, and RESPONSE_SYNC_BYTE_2.

Referenced by Found(), mosaicMessage(), Next(), io_comm_mosaic::CallbackHandlers::ReadCallback(), Search(), and SegmentEnd().

783 {
785  {
786  return true;
787  }
788  else
789  {
790  return false;
791  }
792 }
#define RESPONSE_SYNC_BYTE_2
0x52 is ASCII for R (for "Response") - 2nd byte in each response from mosaic
#define RESPONSE_SYNC_BYTE_1
0x24 is ASCII for $ - 1st byte in each response from mosaic
const uint8_t * data_
The pointer to the buffer of messages.
Here is the caller graph for this function:

◆ IsSBF()

bool io_comm_mosaic::mosaicMessage::IsSBF ( )

Determines whether data_ currently points to an SBF block.

Definition at line 758 of file mosaicMessage.cpp.

References data_, SBF_SYNC_BYTE_1, and SBF_SYNC_BYTE_2.

Referenced by BlockLength(), Found(), IsMessage(), MessageID(), mosaicMessage(), Next(), Read(), io_comm_mosaic::CallbackHandlers::ReadCallback(), and Search().

759 {
760  if (data_[0] == SBF_SYNC_BYTE_1 && data_[1] == SBF_SYNC_BYTE_2)
761  {
762  return true;
763  }
764  else
765  {
766  return false;
767  }
768 }
#define SBF_SYNC_BYTE_1
0x24 is ASCII for $ - 1st byte in each message
#define SBF_SYNC_BYTE_2
0x40 is ASCII for @ - 2nd byte to indicate SBF block
const uint8_t * data_
The pointer to the buffer of messages.
Here is the caller graph for this function:

◆ MessageID()

std::string io_comm_mosaic::mosaicMessage::MessageID ( )

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

Definition at line 818 of file mosaicMessage.cpp.

References data_, IsNMEA(), IsSBF(), and SegmentEnd().

Referenced by io_comm_mosaic::CallbackHandler< T >::Handle(), io_comm_mosaic::CallbackHandlers::Handle(), mosaicMessage(), Read(), and io_comm_mosaic::CallbackHandlers::ReadCallback().

819 {
820  if (this->IsSBF())
821  {
822  // Define bit mask:
823  uint16_t mask = 8191; // It is not as stated in the firmware: !first! three bits are for revision (not last 3), and rest for block number
824  uint16_t value = (*(reinterpret_cast<const uint16_t*>(data_+4))) & mask; // Bitwise AND gives us all but first 3 bits set to zero, rest unchanged
825  std::stringstream ss;
826  ss << value;
827  return ss.str();
828  }
829  if (this->IsNMEA())
830  {
831  boost::char_separator<char> sep(",");
832  typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
833  std::size_t nmea_size = std::min(this->SegmentEnd(), static_cast<std::size_t>(89));
834  std::string block_in_string(reinterpret_cast<const char*>(data_), nmea_size);
835  tokenizer tokens(block_in_string,sep);
836  return *tokens.begin();
837  }
838  return std::string(); // less CPU work than return "";
839 }
bool IsNMEA()
Determines whether data_ currently points to an NMEA message.
bool IsSBF()
Determines whether data_ currently points to an SBF block.
const uint8_t * data_
The pointer to the buffer of messages.
std::size_t SegmentEnd()
Determines size of message that data_ is currently pointing at.
Here is the call graph for this function:
Here is the caller graph for this function:

◆ NavSatFixCallback()

sensor_msgs::NavSatFixPtr io_comm_mosaic::mosaicMessage::NavSatFixCallback ( )
private

"Callback" function when constructing NavSatFix messages

Returns
A smart pointer to the ROS message sensor_msgs::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 283 of file mosaicMessage.cpp.

References PosCovGeodetic::Cov_hgthgt, PosCovGeodetic::Cov_lathgt, PosCovGeodetic::Cov_latlat, PosCovGeodetic::Cov_latlon, PosCovGeodetic::Cov_lonhgt, PosCovGeodetic::Cov_lonlon, PVTGeodetic::Height, last_poscovgeodetic_, last_pvtgeodetic_, PVTGeodetic::Latitude, PVTGeodetic::Longitude, PVTGeodetic::Mode, and PVTGeodetic::SignalInfo.

Referenced by Read().

284 {
285  sensor_msgs::NavSatFixPtr msg = boost::make_shared<sensor_msgs::NavSatFix>();
286  uint16_t mask = 15; // We extract the first four bits using this mask.
287  uint16_t type_of_pvt = ((uint16_t) (last_pvtgeodetic_.Mode)) & mask;
288  switch(type_of_pvt)
289  {
290  case 0:
291  {
292  msg->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
293  break;
294  }
295  case 1: case 3:
296  {
297  msg->status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
298  break;
299  }
300  case 2: case 4: case 5: case 7: case 8: case 10:
301  {
302  msg->status.status = sensor_msgs::NavSatStatus::STATUS_GBAS_FIX;
303  break;
304  }
305  case 6:
306  {
307  msg->status.status = sensor_msgs::NavSatStatus::STATUS_SBAS_FIX;
308  break;
309  }
310  default:
311  {
312  throw std::runtime_error("PVTGeodetic's Mode field contains an invalid type of PVT solution.");
313  }
314  }
315  bool gps_in_pvt = false;
316  bool glo_in_pvt = false;
317  bool com_in_pvt = false;
318  bool gal_in_pvt = false;
319  uint32_t mask_2 = 1;
320  for(int bit = 0; bit != 31; ++bit)
321  {
322  bool in_use = last_pvtgeodetic_.SignalInfo & mask_2;
323  if (bit <= 5 && in_use)
324  {
325  gps_in_pvt = true;
326  }
327  if (8 <= bit && bit <= 12 && in_use) glo_in_pvt = true;
328  if (((13 <= bit && bit <= 14) || (28 <= bit && bit <= 30)) && in_use) com_in_pvt = true;
329  if ((bit == 17 || (19 <= bit && bit <= 22)) && in_use) gal_in_pvt = true;
330  mask_2 *= 2;
331  }
332  //ROS_DEBUG("GPS is in use: %s, GLO is in use: %s, COM is in use: %s, GAL is in use: %s", gps_in_pvt ? "true" : "false", glo_in_pvt ? "true" : "false", com_in_pvt ? "true" : "false", gal_in_pvt ? "true" : "false");
333  uint16_t service = gps_in_pvt*1+glo_in_pvt*2+com_in_pvt*4+gal_in_pvt*8; // booleans will be promoted to integers automatically
334  msg->status.service = service;
335  msg->latitude = last_pvtgeodetic_.Latitude*360/(2*boost::math::constants::pi<double>());
336  msg->longitude = last_pvtgeodetic_.Longitude*360/(2*boost::math::constants::pi<double>());
337  msg->altitude = last_pvtgeodetic_.Height;
338  msg->position_covariance[0] = static_cast<double>(last_poscovgeodetic_.Cov_lonlon);
339  msg->position_covariance[1] = static_cast<double>(last_poscovgeodetic_.Cov_latlon);
340  msg->position_covariance[2] = static_cast<double>(last_poscovgeodetic_.Cov_lonhgt);
341  msg->position_covariance[3] = static_cast<double>(last_poscovgeodetic_.Cov_latlon);
342  msg->position_covariance[4] = static_cast<double>(last_poscovgeodetic_.Cov_latlat);
343  msg->position_covariance[5] = static_cast<double>(last_poscovgeodetic_.Cov_lathgt);
344  msg->position_covariance[6] = static_cast<double>(last_poscovgeodetic_.Cov_lonhgt);
345  msg->position_covariance[7] = static_cast<double>(last_poscovgeodetic_.Cov_lathgt);
346  msg->position_covariance[8] = static_cast<double>(last_poscovgeodetic_.Cov_hgthgt);
347  msg->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN;
348  return msg;
349 }
static PosCovGeodetic last_poscovgeodetic_
Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic blocks need to be stored...
static PVTGeodetic last_pvtgeodetic_
Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks need to be stored.
double Longitude
double Latitude
uint32_t SignalInfo
uint8_t Mode
Here is the caller graph for this function:

◆ Next()

const uint8_t * io_comm_mosaic::mosaicMessage::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 872 of file mosaicMessage.cpp.

References BlockLength(), cd_count, count_, CRCcheck_, data_, Found(), found_, IsConnectionDescriptor(), IsNMEA(), IsResponse(), IsSBF(), and read_cd.

Referenced by GetCount(), and Search().

873 {
874  if (Found())
875  {
876  if (this->IsNMEA() || this->IsResponse() || (read_cd && this->IsConnectionDescriptor()))
877  {
878  if (read_cd && this->IsConnectionDescriptor() && cd_count == 2)
879  {
880  read_cd = false;
881  }
882  --count_;
883  ++data_;
884  found_ = false;
885  return data_;
886  }
887  if (this->IsSBF())
888  {
889  uint32_t jump_size;
890  if (CRCcheck_)
891  {
892  jump_size = this->BlockLength();
893  if (jump_size == 0) jump_size = 1; // Some corrupted messages that survive the CRC check (this happened) could tell ROSaic their size is 0,
894  // which would lead to an endless while loop in the ReadCallback() method of the CallbackHandlers class.
895  }
896  else
897  {
898  jump_size = 1;
899  }
900  //ROS_DEBUG("Jump about to happen with jump size %u", jump_size);
901  data_ += jump_size; count_ -= jump_size;
902  found_ = false;
903  return data_;
904  }
905  }
906  found_ = false;
907  --count_;
908  ++data_;
909  return data_;
910 }
bool found_
Whether or not a message has been found.
uint16_t BlockLength()
Gets the length of the SBF block.
uint32_t count_
The number of bytes in the buffer, decremented as the buffer is read (so it is the buffer&#39;s size in t...
bool Found()
Has an NMEA message or SBF block been found in the buffer?
uint32_t cd_count
Since after SSSSSSSSSSS we need to wait for second connection descriptor, we have to count the connec...
bool read_cd
Whether or not we still want to read the connection descriptor, which we only want in the very beginn...
bool IsResponse()
Determines whether data_ currently points to an NMEA message.
bool CRCcheck_
Whether the CRC check as evaluated in the read() method was successful or not is stored here...
bool IsNMEA()
Determines whether data_ currently points to an NMEA message.
bool IsSBF()
Determines whether data_ currently points to an SBF block.
const uint8_t * data_
The pointer to the buffer of messages.
bool IsConnectionDescriptor()
Determines whether data_ currently points to a connection descriptor (right after initiating TCP conn...
Here is the call graph for this function:
Here is the caller graph for this function:

◆ Pos()

const uint8_t * io_comm_mosaic::mosaicMessage::Pos ( )

Gets the current position in the read buffer.

Returns
The current position of the read buffer

Definition at line 843 of file mosaicMessage.cpp.

References data_.

Referenced by GetCount(), and io_comm_mosaic::CallbackHandlers::ReadCallback().

844 {
845  return data_;
846 }
const uint8_t * data_
The pointer to the buffer of messages.
Here is the caller graph for this function:

◆ PosCovCartesianCallback()

rosaic::PosCovCartesianPtr io_comm_mosaic::mosaicMessage::PosCovCartesianCallback ( PosCovCartesian data)
private

Callback function when reading PosCovCartesian blocks.

Parameters
[in]dataThe (packed and aligned) struct instance that we use to populate our ROS message rosaic::PosCovCartesian
Returns
A smart pointer to the ROS message rosaic::PosCovCartesian just created

Definition at line 129 of file mosaicMessage.cpp.

References PosCovCartesian::Block_Header, PosCovCartesian::Cov_bb, PosCovCartesian::Cov_xb, PosCovCartesian::Cov_xx, PosCovCartesian::Cov_xy, PosCovCartesian::Cov_xz, PosCovCartesian::Cov_yb, PosCovCartesian::Cov_yy, PosCovCartesian::Cov_yz, PosCovCartesian::Cov_zb, PosCovCartesian::Cov_zz, BlockHeader_t::CRC, PosCovCartesian::Error, BlockHeader_t::ID, BlockHeader_t::Length, PosCovCartesian::Mode, BlockHeader_t::SYNC1, BlockHeader_t::SYNC2, PosCovCartesian::TOW, and PosCovCartesian::WNc.

Referenced by Read().

130 {
131  rosaic::PosCovCartesianPtr msg = boost::make_shared<rosaic::PosCovCartesian>();
132  msg->Block_Header.SYNC1 = data.Block_Header.SYNC1;
133  msg->Block_Header.SYNC2 = data.Block_Header.SYNC2;
134  msg->Block_Header.CRC = data.Block_Header.CRC;
135  msg->Block_Header.ID = data.Block_Header.ID;
136  msg->Block_Header.Length = data.Block_Header.Length;
137  msg->Block_Header.TOW = data.TOW;
138  msg->Block_Header.WNc = data.WNc;
139  msg->Mode = data.Mode;
140  msg->Error = data.Error;
141  msg->Cov_xx = data.Cov_xx;
142  msg->Cov_yy = data.Cov_yy;
143  msg->Cov_zz = data.Cov_zz;
144  msg->Cov_bb = data.Cov_bb;
145  msg->Cov_xy = data.Cov_xy;
146  msg->Cov_xz = data.Cov_xz;
147  msg->Cov_xb = data.Cov_xb;
148  msg->Cov_yz = data.Cov_yz;
149  msg->Cov_yb = data.Cov_yb;
150  msg->Cov_zb = data.Cov_zb;
151  return msg;
152 }
uint16_t CRC
the Check Sum !
uint16_t Length
length of the entire message including the header. A multiple of 4 between 8 and 4096 ...
uint16_t ID
The ID is the "Block ID".
uint8_t SYNC1
first sync byte is $ or 0x24
uint8_t SYNC2
2nd sync byte is @ or 0x40
BlockHeader_t Block_Header
Here is the caller graph for this function:

◆ PosCovGeodeticCallback()

rosaic::PosCovGeodeticPtr io_comm_mosaic::mosaicMessage::PosCovGeodeticCallback ( PosCovGeodetic data)
private

Callback function when reading PosCovGeodetic blocks.

Parameters
[in]dataThe (packed and aligned) struct instance that we use to populate our ROS message rosaic::PosCovGeodetic
Returns
A smart pointer to the ROS message rosaic::PosCovGeodetic just created

Definition at line 155 of file mosaicMessage.cpp.

References PosCovGeodetic::Block_Header, PosCovGeodetic::Cov_bb, PosCovGeodetic::Cov_hb, PosCovGeodetic::Cov_hgthgt, PosCovGeodetic::Cov_latb, PosCovGeodetic::Cov_lathgt, PosCovGeodetic::Cov_latlat, PosCovGeodetic::Cov_latlon, PosCovGeodetic::Cov_lonb, PosCovGeodetic::Cov_lonhgt, PosCovGeodetic::Cov_lonlon, BlockHeader_t::CRC, PosCovGeodetic::Error, BlockHeader_t::ID, BlockHeader_t::Length, PosCovGeodetic::Mode, BlockHeader_t::SYNC1, BlockHeader_t::SYNC2, PosCovGeodetic::TOW, and PosCovGeodetic::WNc.

Referenced by Read().

156 {
157  rosaic::PosCovGeodeticPtr msg = boost::make_shared<rosaic::PosCovGeodetic>();
158  msg->Block_Header.SYNC1 = data.Block_Header.SYNC1;
159  msg->Block_Header.SYNC2 = data.Block_Header.SYNC2;
160  msg->Block_Header.CRC = data.Block_Header.CRC;
161  msg->Block_Header.ID = data.Block_Header.ID;
162  msg->Block_Header.Length = data.Block_Header.Length;
163  msg->Block_Header.TOW = data.TOW;
164  msg->Block_Header.WNc = data.WNc;
165  msg->Mode = data.Mode;
166  msg->Error = data.Error;
167  msg->Cov_latlat = data.Cov_latlat;
168  msg->Cov_lonlon = data.Cov_lonlon;
169  msg->Cov_hgthgt = data.Cov_hgthgt;
170  msg->Cov_bb = data.Cov_bb;
171  msg->Cov_latlon = data.Cov_latlon;
172  msg->Cov_lathgt = data.Cov_lathgt;
173  msg->Cov_latb = data.Cov_latb;
174  msg->Cov_lonhgt = data.Cov_lonhgt;
175  msg->Cov_lonb = data.Cov_lonb;
176  msg->Cov_hb = data.Cov_hb;
177  return msg;
178 }
BlockHeader_t Block_Header
uint16_t CRC
the Check Sum !
uint16_t Length
length of the entire message including the header. A multiple of 4 between 8 and 4096 ...
uint16_t ID
The ID is the "Block ID".
uint8_t SYNC1
first sync byte is $ or 0x24
uint8_t SYNC2
2nd sync byte is @ or 0x40
Here is the caller graph for this function:

◆ PoseWithCovarianceStampedCallback()

geometry_msgs::PoseWithCovarianceStampedPtr io_comm_mosaic::mosaicMessage::PoseWithCovarianceStampedCallback ( )
private

"Callback" function when constructing PoseWithCovarianceStamped messages

Returns
A smart pointer to the ROS message geometry_msgs::PoseWithCovarianceStamped just created
Todo:
Replaces the zeros in the covariance fields with proper estimations of those quantities

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 229 of file mosaicMessage.cpp.

References AttCovEuler::Cov_HeadHead, AttCovEuler::Cov_HeadPitch, AttCovEuler::Cov_HeadRoll, PosCovGeodetic::Cov_hgthgt, PosCovGeodetic::Cov_lathgt, PosCovGeodetic::Cov_latlat, PosCovGeodetic::Cov_latlon, PosCovGeodetic::Cov_lonhgt, PosCovGeodetic::Cov_lonlon, AttCovEuler::Cov_PitchPitch, AttCovEuler::Cov_PitchRoll, AttCovEuler::Cov_RollRoll, AttEuler::Heading, PVTGeodetic::Height, last_attcoveuler_, last_atteuler_, last_poscovgeodetic_, last_pvtgeodetic_, PVTGeodetic::Latitude, PVTGeodetic::Longitude, AttEuler::Pitch, AttEuler::Roll, and parsing_utilities::ToQuaternion().

Referenced by Read().

230 {
231  geometry_msgs::PoseWithCovarianceStampedPtr msg = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>();
232  // Filling in the pose data
233  msg->pose.pose.orientation = parsing_utilities::ToQuaternion(static_cast<double>(last_atteuler_.Heading), static_cast<double>(last_atteuler_.Pitch), static_cast<double>(last_atteuler_.Roll));
234  msg->pose.pose.position.x = static_cast<double>(last_pvtgeodetic_.Longitude)*360/(2*boost::math::constants::pi<double>());
235  msg->pose.pose.position.y = static_cast<double>(last_pvtgeodetic_.Latitude)*360/(2*boost::math::constants::pi<double>());
236  msg->pose.pose.position.z = static_cast<double>(last_pvtgeodetic_.Height);
237  // Filling in the covariance data in row-major order
238  msg->pose.covariance[0] = static_cast<double>(last_poscovgeodetic_.Cov_lonlon);
239  msg->pose.covariance[1] = static_cast<double>(last_poscovgeodetic_.Cov_latlon);
240  msg->pose.covariance[2] = static_cast<double>(last_poscovgeodetic_.Cov_lonhgt);
241  msg->pose.covariance[3] = 0;
242  msg->pose.covariance[4] = 0;
243  msg->pose.covariance[5] = 0;
244  msg->pose.covariance[6] = static_cast<double>(last_poscovgeodetic_.Cov_latlon);
245  msg->pose.covariance[7] = static_cast<double>(last_poscovgeodetic_.Cov_latlat);
246  msg->pose.covariance[8] = static_cast<double>(last_poscovgeodetic_.Cov_lathgt);
247  msg->pose.covariance[9] = 0;
248  msg->pose.covariance[10] = 0;
249  msg->pose.covariance[11] = 0;
250  msg->pose.covariance[12] = static_cast<double>(last_poscovgeodetic_.Cov_lonhgt);
251  msg->pose.covariance[13] = static_cast<double>(last_poscovgeodetic_.Cov_lathgt);
252  msg->pose.covariance[14] = static_cast<double>(last_poscovgeodetic_.Cov_hgthgt);
253  msg->pose.covariance[15] = 0;
254  msg->pose.covariance[16] = 0;
255  msg->pose.covariance[17] = 0;
256  msg->pose.covariance[18] = 0;
257  msg->pose.covariance[19] = 0;
258  msg->pose.covariance[20] = 0;
259  msg->pose.covariance[21] = static_cast<double>(last_attcoveuler_.Cov_RollRoll);
260  msg->pose.covariance[22] = static_cast<double>(last_attcoveuler_.Cov_PitchRoll);
261  msg->pose.covariance[23] = static_cast<double>(last_attcoveuler_.Cov_HeadRoll);
262  msg->pose.covariance[24] = 0;
263  msg->pose.covariance[25] = 0;
264  msg->pose.covariance[26] = 0;
265  msg->pose.covariance[27] = static_cast<double>(last_attcoveuler_.Cov_PitchRoll);
266  msg->pose.covariance[28] = static_cast<double>(last_attcoveuler_.Cov_PitchPitch);
267  msg->pose.covariance[29] = static_cast<double>(last_attcoveuler_.Cov_HeadPitch);
268  msg->pose.covariance[30] = 0;
269  msg->pose.covariance[31] = 0;
270  msg->pose.covariance[32] = 0;
271  msg->pose.covariance[33] = static_cast<double>(last_attcoveuler_.Cov_HeadRoll);
272  msg->pose.covariance[34] = static_cast<double>(last_attcoveuler_.Cov_PitchRoll);
273  msg->pose.covariance[35] = static_cast<double>(last_attcoveuler_.Cov_HeadHead);
274 
275  return msg;
276 }
float Cov_PitchRoll
static PosCovGeodetic last_poscovgeodetic_
Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic blocks need to be stored...
float Cov_HeadPitch
float Cov_HeadRoll
static AttCovEuler last_attcoveuler_
Since GPSFix etc. need AttCovEuler, incoming AttCovEuler blocks need to be stored.
static PVTGeodetic last_pvtgeodetic_
Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks need to be stored.
float Roll
geometry_msgs::Quaternion ToQuaternion(double yaw, double pitch, double roll)
Transforms Euler angles to a quaternion.
float Cov_HeadHead
float Heading
static AttEuler last_atteuler_
Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to be stored.
double Longitude
float Pitch
double Latitude
float Cov_RollRoll
float Cov_PitchPitch
Here is the call graph for this function:
Here is the caller graph for this function:

◆ PVTCartesianCallback()

rosaic::PVTCartesianPtr io_comm_mosaic::mosaicMessage::PVTCartesianCallback ( PVTCartesian data)
private

Callback function when reading PVTCartesian blocks.

Parameters
[in]dataThe (packed and aligned) struct instance that we use to populate our ROS message rosaic::PVTCartesian
Returns
A smart pointer to the ROS message rosaic::PVTCartesian just created

Definition at line 90 of file mosaicMessage.cpp.

References PVTCartesian::AlertFlag, PVTCartesian::Block_Header, PVTCartesian::COG, BlockHeader_t::CRC, PVTCartesian::Datum, PVTCartesian::Error, PVTCartesian::HAccuracy, BlockHeader_t::ID, PVTCartesian::Latency, BlockHeader_t::Length, PVTCartesian::MeanCorrAge, PVTCartesian::Misc, PVTCartesian::Mode, PVTCartesian::NrBases, PVTCartesian::NrSV, PVTCartesian::PPPInfo, PVTCartesian::ReferenceID, PVTCartesian::RxClkBias, PVTCartesian::RxClkDrift, PVTCartesian::SignalInfo, BlockHeader_t::SYNC1, BlockHeader_t::SYNC2, PVTCartesian::TimeSystem, PVTCartesian::TOW, PVTCartesian::Undulation, PVTCartesian::VAccuracy, PVTCartesian::Vx, PVTCartesian::Vy, PVTCartesian::Vz, PVTCartesian::WACorrInfo, PVTCartesian::WNc, PVTCartesian::X, PVTCartesian::Y, and PVTCartesian::Z.

Referenced by Read().

91 {
92  rosaic::PVTCartesianPtr msg = boost::make_shared<rosaic::PVTCartesian>();
93  msg->Block_Header.SYNC1 = data.Block_Header.SYNC1;
94  msg->Block_Header.SYNC2 = data.Block_Header.SYNC2;
95  msg->Block_Header.CRC = data.Block_Header.CRC;
96  msg->Block_Header.ID = data.Block_Header.ID;
97  msg->Block_Header.Length = data.Block_Header.Length;
98  msg->Block_Header.TOW = data.TOW;
99  msg->Block_Header.WNc = data.WNc;
100  msg->Mode = data.Mode;
101  msg->Error = data.Error;
102  msg->X = data.X;
103  msg->Y = data.Y;
104  msg->Z = data.Z;
105  msg->Undulation = data.Undulation;
106  msg->Vx = data.Vx;
107  msg->Vy = data.Vy;
108  msg->Vz = data.Vz;
109  msg->COG = data.COG;
110  msg->RxClkBias = data.RxClkBias;
111  msg->RxClkDrift = data.RxClkDrift;
112  msg->TimeSystem = data.TimeSystem;
113  msg->Datum = data.Datum;
114  msg->NrSV = data.NrSV;
115  msg->WACorrInfo = data.WACorrInfo;
116  msg->ReferenceID = data.ReferenceID;
117  msg->MeanCorrAge = data.MeanCorrAge;
118  msg->SignalInfo = data.SignalInfo;
119  msg->AlertFlag = data.AlertFlag;
120  msg->NrBases = data.NrBases;
121  msg->PPPInfo = data.PPPInfo;
122  msg->Latency = data.Latency;
123  msg->HAccuracy = data.HAccuracy;
124  msg->VAccuracy = data.VAccuracy;
125  msg->Misc = data.Misc;
126  return msg;
127 }
uint8_t WACorrInfo
uint16_t WNc
uint16_t MeanCorrAge
double RxClkBias
uint8_t AlertFlag
uint16_t CRC
the Check Sum !
uint16_t HAccuracy
uint16_t Length
length of the entire message including the header. A multiple of 4 between 8 and 4096 ...
uint16_t PPPInfo
uint8_t NrBases
uint32_t SignalInfo
uint16_t ID
The ID is the "Block ID".
uint16_t Latency
uint8_t SYNC1
first sync byte is $ or 0x24
uint8_t TimeSystem
uint16_t ReferenceID
uint32_t TOW
uint8_t SYNC2
2nd sync byte is @ or 0x40
BlockHeader_t Block_Header
uint16_t VAccuracy
Here is the caller graph for this function:

◆ PVTGeodeticCallback()

rosaic::PVTGeodeticPtr io_comm_mosaic::mosaicMessage::PVTGeodeticCallback ( PVTGeodetic data)
private

Callback function when reading PVTGeodetic blocks.

Parameters
[in]dataThe (packed and aligned) struct instance that we use to populate our ROS message rosaic::PVTGeodetic
Returns
A smart pointer to the ROS message rosaic::PVTGeodetic just created

Definition at line 50 of file mosaicMessage.cpp.

References PVTGeodetic::AlertFlag, PVTGeodetic::Block_Header, PVTGeodetic::COG, BlockHeader_t::CRC, PVTGeodetic::Datum, PVTGeodetic::Error, PVTGeodetic::HAccuracy, PVTGeodetic::Height, BlockHeader_t::ID, PVTGeodetic::Latency, PVTGeodetic::Latitude, BlockHeader_t::Length, PVTGeodetic::Longitude, PVTGeodetic::MeanCorrAge, PVTGeodetic::Misc, PVTGeodetic::Mode, PVTGeodetic::NrBases, PVTGeodetic::NrSV, PVTGeodetic::PPPInfo, PVTGeodetic::ReferenceID, PVTGeodetic::RxClkBias, PVTGeodetic::RxClkDrift, PVTGeodetic::SignalInfo, BlockHeader_t::SYNC1, BlockHeader_t::SYNC2, PVTGeodetic::TimeSystem, PVTGeodetic::TOW, PVTGeodetic::Undulation, PVTGeodetic::VAccuracy, PVTGeodetic::Ve, PVTGeodetic::Vn, PVTGeodetic::Vu, PVTGeodetic::WACorrInfo, and PVTGeodetic::WNc.

Referenced by Read().

51 {
52  rosaic::PVTGeodeticPtr msg = boost::make_shared<rosaic::PVTGeodetic>();
53  msg->Block_Header.SYNC1 = data.Block_Header.SYNC1;
54  msg->Block_Header.SYNC2 = data.Block_Header.SYNC2;
55  msg->Block_Header.CRC = data.Block_Header.CRC;
56  msg->Block_Header.ID = data.Block_Header.ID;
57  msg->Block_Header.Length = data.Block_Header.Length;
58  msg->Block_Header.TOW = data.TOW;
59  msg->Block_Header.WNc = data.WNc;
60  msg->Mode = data.Mode;
61  msg->Error = data.Error;
62  msg->Latitude = data.Latitude;
63  msg->Longitude = data.Longitude;
64  msg->Height = data.Height;
65  msg->Undulation = data.Undulation;
66  msg->Vn = data.Vn;
67  msg->Ve = data.Ve;
68  msg->Vu = data.Vu;
69  msg->COG = data.COG;
70  msg->RxClkBias = data.RxClkBias;
71  msg->RxClkDrift = data.RxClkDrift;
72  msg->TimeSystem = data.TimeSystem;
73  msg->Datum = data.Datum;
74  msg->NrSV = data.NrSV;
75  msg->WACorrInfo = data.WACorrInfo;
76  msg->ReferenceID = data.ReferenceID;
77  msg->MeanCorrAge = data.MeanCorrAge;
78  msg->SignalInfo = data.SignalInfo;
79  msg->AlertFlag = data.AlertFlag;
80  msg->NrBases = data.NrBases;
81  msg->PPPInfo = data.PPPInfo;
82  msg->Latency = data.Latency;
83  msg->HAccuracy = data.HAccuracy;
84  msg->VAccuracy = data.VAccuracy;
85  msg->Misc = data.Misc;
86  return msg;
87 }
float Undulation
uint8_t WACorrInfo
uint8_t AlertFlag
uint8_t TimeSystem
uint32_t TOW
uint16_t CRC
the Check Sum !
uint16_t Length
length of the entire message including the header. A multiple of 4 between 8 and 4096 ...
uint16_t VAccuracy
uint8_t NrBases
double Longitude
uint16_t ReferenceID
uint16_t ID
The ID is the "Block ID".
double Latitude
float RxClkDrift
uint8_t SYNC1
first sync byte is $ or 0x24
uint16_t MeanCorrAge
uint8_t NrSV
uint16_t HAccuracy
uint8_t Misc
uint8_t Datum
uint32_t SignalInfo
uint16_t WNc
uint16_t PPPInfo
uint8_t Error
uint8_t SYNC2
2nd sync byte is @ or 0x40
BlockHeader_t Block_Header
uint16_t Latency
double RxClkBias
uint8_t Mode
Here is the caller graph for this function:

◆ Read()

template<typename T >
bool io_comm_mosaic::mosaicMessage::Read ( typename boost::call_traits< T >::reference  message,
std::string  message_key,
bool  search = false 
)

Performs the CRC check (if SBF block) and populates message with the necessary content (mapped 1-to-1 if SBF, parsed if NMEA), works for pure ROS message mapping.

Returns
True if read was successful, false otherwise

Note that boost::call_traits<T>::reference is more robust than traditional T&. Note that this function also assigns 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. Finally 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 mosaic by default increases precision on lat/lon s.t. the maximum allowed e.g. for GGA seems to be 89. Luckily, when parsing we do not care since we just search for <LF><CR>.

Definition at line 432 of file mosaicMessage.hpp.

References AttCovEulerCallback(), AttEulerCallback(), BlockLength(), count_, count_gpsfix_, CRCcheck_, CRCIsValid(), data_, evAttCovEuler, evAttEuler, evChannelStatus, evDOP, evGAGSV, evGLGSV, evGPGGA, evGPGSA, evGPGSV, evGPRMC, evGPSFix, evGPST, evMeasEpoch, evNavSatFix, evPosCovCartesian, evPosCovGeodetic, evPoseWithCovarianceStamped, evPVTCartesian, evPVTGeodetic, evVelCovGeodetic, Found(), frame_id, GPSFixCallback(), IsSBF(), last_attcoveuler_, last_atteuler_, last_channelstatus_, last_dop_, last_measepoch_, last_poscovgeodetic_, last_pvtgeodetic_, last_velcovgeodetic_, MessageID(), NavSatFixCallback(), GpggaParser::ParseASCII(), GpgsvParser::ParseASCII(), GprmcParser::ParseASCII(), GpgsaParser::ParseASCII(), PosCovCartesianCallback(), PosCovGeodeticCallback(), PoseWithCovarianceStampedCallback(), PVTCartesianCallback(), PVTGeodeticCallback(), Search(), SegmentEnd(), io_comm_mosaic::TimestampSBF(), PVTGeodetic::TOW, and use_GNSS_time.

Referenced by GetCount(), and io_comm_mosaic::CallbackHandler< T >::Handle().

433  {
434  //ROS_DEBUG("message key is %s", message_key.c_str());
435  if (search) this->Search();
436  if (!Found()) return false;
437  if (this->IsSBF())
438  {
439  // If the CRC check is unsuccessful, throw an error message.
441  if (!CRCcheck_)
442  {
443  throw std::runtime_error("CRC Check returned False. Not a valid data block, perhaps noisy. Ignore..");
444  }
445  // If full message did not yet arrive, throw an error message.
446  if (this->BlockLength() > count_)
447  {
448  throw std::runtime_error("Not a valid data block, parts of the SBF block are not yet received. Ignore..");
449  }
450  }
451  switch(StringValues[message_key])
452  {
453  case evPVTCartesian: // Position and velocity in XYZ
454  { // The curly bracket here is crucial: Declarations inside a block remain inside, and will die at the end of the block. Otherwise variable overloading etc.
455  rosaic::PVTCartesianPtr msg = boost::make_shared<rosaic::PVTCartesian>();
456  PVTCartesian pvtcartesian;
457  memcpy(&pvtcartesian, data_, sizeof(pvtcartesian));
458  msg = PVTCartesianCallback(pvtcartesian);
459  msg->header.frame_id = frame_id;
460  uint32_t TOW = *(reinterpret_cast<const uint32_t *>(data_ + 8));
461  ros::Time time_obj;
462  time_obj = TimestampSBF(TOW, use_GNSS_time);
463  msg->header.stamp.sec = time_obj.sec;
464  msg->header.stamp.nsec = time_obj.nsec;
465  msg->Block_Header.ID = 4006;
466  memcpy(&message, msg.get(), sizeof(*msg));
467  break;
468  }
469  case evPVTGeodetic: // Position and velocity in geodetic coordinate frame (ENU frame)
470  {
471  rosaic::PVTGeodeticPtr msg = boost::make_shared<rosaic::PVTGeodetic>();
472  memcpy(&last_pvtgeodetic_, data_, sizeof(last_pvtgeodetic_));
474  msg->header.frame_id = frame_id;
475  uint32_t TOW = *(reinterpret_cast<const uint32_t *>(data_ + 8));
476  ros::Time time_obj;
477  time_obj = TimestampSBF(TOW, use_GNSS_time);
478  msg->header.stamp.sec = time_obj.sec;
479  msg->header.stamp.nsec = time_obj.nsec;
480  msg->Block_Header.ID = 4007;
481  memcpy(&message, msg.get(), sizeof(*msg));
482  break;
483  }
484  case evPosCovCartesian:
485  {
486  rosaic::PosCovCartesianPtr msg = boost::make_shared<rosaic::PosCovCartesian>();
487  PosCovCartesian poscovcartesian;
488  memcpy(&poscovcartesian, data_, sizeof(poscovcartesian));
489  msg = PosCovCartesianCallback(poscovcartesian);
490  msg->header.frame_id = frame_id;
491  uint32_t TOW = *(reinterpret_cast<const uint32_t *>(data_ + 8));
492  ros::Time time_obj;
493  time_obj = TimestampSBF(TOW, use_GNSS_time);
494  msg->header.stamp.sec = time_obj.sec;
495  msg->header.stamp.nsec = time_obj.nsec;
496  msg->Block_Header.ID = 5905;
497  memcpy(&message, msg.get(), sizeof(*msg));
498  break;
499  }
500  case evPosCovGeodetic:
501  {
502  rosaic::PosCovGeodeticPtr msg = boost::make_shared<rosaic::PosCovGeodetic>();
505  msg->header.frame_id = frame_id;
506  uint32_t TOW = *(reinterpret_cast<const uint32_t *>(data_ + 8));
507  ros::Time time_obj;
508  time_obj = TimestampSBF(TOW, use_GNSS_time);
509  msg->header.stamp.sec = time_obj.sec;
510  msg->header.stamp.nsec = time_obj.nsec;
511  msg->Block_Header.ID = 5906;
512  memcpy(&message, msg.get(), sizeof(*msg));
513  break;
514  }
515  case evAttEuler:
516  {
517  rosaic::AttEulerPtr msg = boost::make_shared<rosaic::AttEuler>();
518  memcpy(&last_atteuler_, data_, sizeof(last_atteuler_));
520  msg->header.frame_id = frame_id;
521  uint32_t TOW = *(reinterpret_cast<const uint32_t *>(data_ + 8));
522  ros::Time time_obj;
523  time_obj = TimestampSBF(TOW, use_GNSS_time);
524  msg->header.stamp.sec = time_obj.sec;
525  msg->header.stamp.nsec = time_obj.nsec;
526  msg->Block_Header.ID = 5938;
527  memcpy(&message, msg.get(), sizeof(*msg));
528  break;
529  }
530  case evAttCovEuler:
531  {
532  rosaic::AttCovEulerPtr msg = boost::make_shared<rosaic::AttCovEuler>();
533  memcpy(&last_attcoveuler_, data_, sizeof(last_attcoveuler_));
535  msg->header.frame_id = frame_id;
536  uint32_t TOW = *(reinterpret_cast<const uint32_t *>(data_ + 8));
537  ros::Time time_obj;
538  time_obj = TimestampSBF(TOW, use_GNSS_time);
539  msg->header.stamp.sec = time_obj.sec;
540  msg->header.stamp.nsec = time_obj.nsec;
541  msg->Block_Header.ID = 5939;
542  memcpy(&message, msg.get(), sizeof(*msg));
543  break;
544  }
545  case evGPST:
546  {
547  sensor_msgs::TimeReferencePtr msg = boost::make_shared<sensor_msgs::TimeReference>();
548  uint32_t TOW = *(reinterpret_cast<const uint32_t *>(data_ + 8));
549  ros::Time time_obj;
550  time_obj = TimestampSBF(TOW, true); // We need the GPS time, hence true
551  msg->time_ref.sec = time_obj.sec;
552  msg->time_ref.nsec = time_obj.nsec;
553  msg->source = "GPST";
554  memcpy(&message, msg.get(), sizeof(*msg));
555  break;
556  }
557  case evGPGGA:
558  {
559  boost::char_separator<char> sep("\r");
560  typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
561  std::size_t nmea_size = std::min(this->SegmentEnd(), static_cast<std::size_t>(89));
562  std::string block_in_string(reinterpret_cast<const char*>(data_), nmea_size);
563  tokenizer tokens(block_in_string, sep);
564 
565  std::string id = this->MessageID();
566  std::string one_message = *tokens.begin();
567  boost::char_separator<char> sep_2(",*", "", boost::keep_empty_tokens); // No kept delimiters, hence "". Also, we specify that empty tokens should show up in the output when two delimiters are next to each other.
568  // Hence we also append the checksum part of the GGA message to "body" below, though it is not parsed.
569  tokenizer tokens_2(one_message, sep_2);
570  std::vector<std::string> body;
571  for (tokenizer::iterator tok_iter = tokens_2.begin(); tok_iter != tokens_2.end(); ++tok_iter) // perhaps str.erase from <string.h> would be faster, but i would not know what exactly to do..
572  {
573  body.push_back(*tok_iter);
574  }
575  // Create NmeaSentence struct to pass to GpggaParser::ParseASCII
576  NMEASentence gga_message(id, body);
577  rosaic::GpggaPtr gpgga_ros_message_ptr;
578  GpggaParser parser_obj;
579  try
580  {
581  gpgga_ros_message_ptr = parser_obj.ParseASCII(gga_message);
582  }
583  catch (ParseException& e)
584  {
585  throw std::runtime_error(e.what());
586  }
587  //ROS_DEBUG("ID is %s, size of message is %lu and sizeof ptr is %lu", gpgga_ros_message_ptr->message_id.c_str(), sizeof(message), sizeof(*gpgga_ros_message_ptr));
588  memcpy(&message, gpgga_ros_message_ptr.get(), sizeof(*gpgga_ros_message_ptr));
589  break;
590  }
591  case evGPRMC:
592  {
593  boost::char_separator<char> sep("\r");
594  typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
595  std::size_t nmea_size = std::min(this->SegmentEnd(), static_cast<std::size_t>(89));
596  std::string block_in_string(reinterpret_cast<const char*>(data_), nmea_size);
597  tokenizer tokens(block_in_string, sep);
598 
599  std::string id = this->MessageID();
600  std::string one_message = *tokens.begin();
601  boost::char_separator<char> sep_2(",*", "", boost::keep_empty_tokens); // No kept delimiters, hence "". Also, we specify that empty tokens should show up in the output when two delimiters are next to each other.
602  // Hence we also append the checksum part of the GGA message to "body" below, though it is not parsed.
603  tokenizer tokens_2(one_message, sep_2);
604  std::vector<std::string> body;
605  for (tokenizer::iterator tok_iter = tokens_2.begin(); tok_iter != tokens_2.end(); ++tok_iter) // perhaps str.erase from <string.h> would be faster, but i would not know what exactly to do..
606  {
607  body.push_back(*tok_iter);
608  }
609  // Create NmeaSentence struct to pass to GprmcParser::ParseASCII
610  NMEASentence rmc_message(id, body);
611  rosaic::GprmcPtr msg = boost::make_shared<rosaic::Gprmc>();
612  GprmcParser parser_obj;
613  try
614  {
615  msg = parser_obj.ParseASCII(rmc_message);
616  }
617  catch (ParseException& e)
618  {
619  throw std::runtime_error(e.what());
620  }
621  //ROS_DEBUG("ID is %s, size of message is %lu and sizeof ptr is %lu", gprmc_ros_message_ptr->message_id.c_str(), sizeof(message), sizeof(*gprmc_ros_message_ptr));
622  //ROS_DEBUG("size of message is currently %li and to be copied is %li", sizeof(message), sizeof(*msg));
623  memcpy(&message, msg.get(), sizeof(*msg));
624  break;
625  }
626  case evGPGSA:
627  {
628  boost::char_separator<char> sep("\r");
629  typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
630  std::size_t nmea_size = std::min(this->SegmentEnd(), static_cast<std::size_t>(89));
631  std::string block_in_string(reinterpret_cast<const char*>(data_), nmea_size);
632  tokenizer tokens(block_in_string, sep);
633 
634  std::string id = this->MessageID();
635  std::string one_message = *tokens.begin();
636  boost::char_separator<char> sep_2(",*", "", boost::keep_empty_tokens); // No kept delimiters, hence "". Also, we specify that empty tokens should show up in the output when two delimiters are next to each other.
637  // Hence we also append the checksum part of the GGA message to "body" below, though it is not parsed.
638  tokenizer tokens_2(one_message, sep_2);
639  std::vector<std::string> body;
640  for (tokenizer::iterator tok_iter = tokens_2.begin(); tok_iter != tokens_2.end(); ++tok_iter) // perhaps str.erase from <string.h> would be faster, but i would not know what exactly to do..
641  {
642  body.push_back(*tok_iter);
643  }
644  // Create NmeaSentence struct to pass to GpgsaParser::ParseASCII
645  NMEASentence gsa_message(id, body);
646  rosaic::GpgsaPtr msg = boost::make_shared<rosaic::Gpgsa>();
647  GpgsaParser parser_obj;
648  try
649  {
650  msg = parser_obj.ParseASCII(gsa_message);
651  }
652  catch (ParseException& e)
653  {
654  throw std::runtime_error(e.what());
655  }
656  uint32_t TOW = last_pvtgeodetic_.TOW;
657  ros::Time time_obj;
658  time_obj = TimestampSBF(TOW, use_GNSS_time);
659  msg->header.stamp.sec = time_obj.sec;
660  msg->header.stamp.nsec = time_obj.nsec;
661  //ROS_DEBUG("ID is %s, size of message is %lu and sizeof ptr is %lu", gprmc_ros_message_ptr->message_id.c_str(), sizeof(message), sizeof(*gprmc_ros_message_ptr));
662  //ROS_DEBUG("size of message is currently %li and to be copied is %li", sizeof(message), sizeof(*msg));
663  memcpy(&message, msg.get(), sizeof(*msg));
664  break;
665  }
666  case evGPGSV: case evGLGSV: case evGAGSV:
667  {
668  boost::char_separator<char> sep("\r");
669  typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
670  std::size_t nmea_size = std::min(this->SegmentEnd(), static_cast<std::size_t>(89));
671  std::string block_in_string(reinterpret_cast<const char*>(data_), nmea_size);
672  tokenizer tokens(block_in_string, sep);
673 
674  std::string id = this->MessageID();
675  std::string one_message = *tokens.begin();
676  boost::char_separator<char> sep_2(",*", "", boost::keep_empty_tokens); // No kept delimiters, hence "". Also, we specify that empty tokens should show up in the output when two delimiters are next to each other.
677  // Hence we also append the checksum part of the GGA message to "body" below, though it is not parsed.
678  tokenizer tokens_2(one_message, sep_2);
679  std::vector<std::string> body;
680  for (tokenizer::iterator tok_iter = tokens_2.begin(); tok_iter != tokens_2.end(); ++tok_iter) // perhaps str.erase from <string.h> would be faster, but i would not know what exactly to do..
681  {
682  body.push_back(*tok_iter);
683  }
684  // Create NmeaSentence struct to pass to GpgsvParser::ParseASCII
685  NMEASentence gsv_message(id, body);
686  rosaic::GpgsvPtr msg = boost::make_shared<rosaic::Gpgsv>();
687  GpgsvParser parser_obj;
688  try
689  {
690  msg = parser_obj.ParseASCII(gsv_message);
691  }
692  catch (ParseException& e)
693  {
694  throw std::runtime_error(e.what());
695  }
696  uint32_t TOW = last_pvtgeodetic_.TOW;
697  ros::Time time_obj;
698  time_obj = TimestampSBF(TOW, use_GNSS_time);
699  msg->header.stamp.sec = time_obj.sec;
700  msg->header.stamp.nsec = time_obj.nsec;
701  //ROS_DEBUG("ID is %s, size of message is %lu and sizeof ptr is %lu", gprmc_ros_message_ptr->message_id.c_str(), sizeof(message), sizeof(*gprmc_ros_message_ptr));
702  //ROS_DEBUG("size of message is currently %li and to be copied is %li", sizeof(message), sizeof(*msg));
703  memcpy(&message, msg.get(), sizeof(*msg));
704  break;
705  }
706  case evNavSatFix:
707  {
708  sensor_msgs::NavSatFixPtr msg = boost::make_shared<sensor_msgs::NavSatFix>();
709  try
710  {
711  msg = NavSatFixCallback();
712  }
713  catch (std::runtime_error& e)
714  {
715  throw std::runtime_error(e.what());
716  }
717  msg->header.frame_id = frame_id;
718  uint32_t TOW = last_pvtgeodetic_.TOW;
719  ros::Time time_obj;
720  time_obj = TimestampSBF(TOW, use_GNSS_time);
721  msg->header.stamp.sec = time_obj.sec;
722  msg->header.stamp.nsec = time_obj.nsec;
723  memcpy(&message, msg.get(), sizeof(*msg));
724  break;
725  }
726  case evGPSFix:
727  {
728  gps_common::GPSFixPtr msg = boost::make_shared<gps_common::GPSFix>();
729  try
730  {
731  msg = GPSFixCallback();
732  }
733  catch (std::runtime_error& e)
734  {
735  throw std::runtime_error(e.what());
736  }
737  msg->status.header.seq = count_gpsfix_;
738  msg->header.frame_id = frame_id;
739  msg->status.header.frame_id = frame_id;
740  uint32_t TOW = last_pvtgeodetic_.TOW;
741  ros::Time time_obj;
742  time_obj = TimestampSBF(TOW, use_GNSS_time);
743  msg->header.stamp.sec = time_obj.sec;
744  msg->status.header.stamp.sec = time_obj.sec;
745  msg->header.stamp.nsec = time_obj.nsec;
746  msg->status.header.stamp.nsec = time_obj.nsec;
747  memcpy(&message, msg.get(), sizeof(*msg));
748  ++count_gpsfix_;
749  break;
750  }
752  {
753  geometry_msgs::PoseWithCovarianceStampedPtr msg = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>();
754  try
755  {
757  }
758  catch (std::runtime_error& e)
759  {
760  throw std::runtime_error(e.what());
761  }
762  msg->header.frame_id = frame_id;
763  uint32_t TOW = last_pvtgeodetic_.TOW;
764  ros::Time time_obj;
765  time_obj = TimestampSBF(TOW, use_GNSS_time);
766  msg->header.stamp.sec = time_obj.sec;
767  msg->header.stamp.nsec = time_obj.nsec;
768  memcpy(&message, msg.get(), sizeof(*msg));
769  break;
770  }
771  case evChannelStatus:
772  {
773  memcpy(&last_channelstatus_, data_, sizeof(last_channelstatus_));
774  break;
775  }
776  case evMeasEpoch:
777  {
778  memcpy(&last_measepoch_, data_, sizeof(last_measepoch_));
779  break;
780  }
781  case evDOP:
782  {
783  memcpy(&last_dop_, data_, sizeof(last_dop_));
784  break;
785  }
786  case evVelCovGeodetic:
787  {
789  break;
790  }
791  // Many more to be implemented...
792  }
793  return true;
794  }
static PosCovGeodetic last_poscovgeodetic_
Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic blocks need to be stored...
std::string MessageID()
Returns the MessageID of the message where data_ is pointing at at the moment, SBF identifiers embell...
const uint8_t * Search()
Searches the buffer for the beginning of the next message (NMEA or SBF)
rosaic::AttCovEulerPtr AttCovEulerCallback(AttCovEuler &data)
Callback function when reading AttCovEuler blocks.
static DOP last_dop_
Since GPSFix etc. need DOP, incoming DOP blocks need to be stored.
uint32_t TOW
Derived class for parsing GSA messages.
Definition: gpgsa.hpp:87
Derived class for parsing GGA messages.
Definition: gpgga.hpp:85
uint16_t BlockLength()
Gets the length of the SBF block.
rosaic::PVTGeodeticPtr PVTGeodeticCallback(PVTGeodetic &data)
Callback function when reading PVTGeodetic blocks.
uint32_t count_
The number of bytes in the buffer, decremented as the buffer is read (so it is the buffer&#39;s size in t...
rosaic::AttEulerPtr AttEulerCallback(AttEuler &data)
Callback function when reading AttEuler blocks.
bool use_GNSS_time
static VelCovGeodetic last_velcovgeodetic_
Since GPSFix etc. need VelCovGeodetic (for signal-to-noise ratios), incoming VelCovGeodetic blocks ne...
static AttCovEuler last_attcoveuler_
Since GPSFix etc. need AttCovEuler, incoming AttCovEuler blocks need to be stored.
bool CRCIsValid(const void *Block)
This function validates whether the calculated CRC of the SBF block at hand matches the CRC field tha...
Definition: crc.c:63
static PVTGeodetic last_pvtgeodetic_
Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks need to be stored.
rosaic::PosCovCartesianPtr PosCovCartesianCallback(PosCovCartesian &data)
Callback function when reading PosCovCartesian blocks.
gps_common::GPSFixPtr GPSFixCallback()
"Callback" function when constructing GPSFix messages
bool Found()
Has an NMEA message or SBF block been found in the buffer?
static AttEuler last_atteuler_
Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to be stored.
geometry_msgs::PoseWithCovarianceStampedPtr PoseWithCovarianceStampedCallback()
"Callback" function when constructing PoseWithCovarianceStamped messages
static ChannelStatus last_channelstatus_
Since GPSFix etc. need ChannelStatus, incoming ChannelStatus blocks need to be stored.
bool CRCcheck_
Whether the CRC check as evaluated in the read() method was successful or not is stored here...
Struct for the SBF block "PVTCartesian".
static std::map< std::string, NMEA_ID_Enum > StringValues
Struct for the SBF block "PosCovCartesian".
sensor_msgs::NavSatFixPtr NavSatFixCallback()
"Callback" function when constructing NavSatFix messages
rosaic::PVTCartesianPtr PVTCartesianCallback(PVTCartesian &data)
Callback function when reading PVTCartesian blocks.
ros::Time TimestampSBF(uint32_t TOW, bool use_GNSS)
Calculates the timestamp, in the Unix Epoch time format, either using the TOW as transmitted with the...
bool IsSBF()
Determines whether data_ currently points to an SBF block.
const uint8_t * data_
The pointer to the buffer of messages.
std::string frame_id
The frame ID used in the header of every published ROS message.
Derived class for parsing RMC messages.
Definition: gprmc.hpp:85
rosaic::GpggaPtr ParseASCII(const NMEASentence &sentence) noexcept(false) override
Parses one GGA message.
Definition: gpgga.cpp:51
Struct to split an NMEA sentence into its ID (e.g. the standardized "$GPGGA" or proprietary "$PSSN...
Ćlass to declare error message format when parsing, derived from the public class "std::runtime_error...
Derived class for parsing GSV messages.
Definition: gpgsv.hpp:85
static MeasEpoch last_measepoch_
Since GPSFix etc. need MeasEpoch (for signal-to-noise ratios), incoming MeasEpoch blocks need to be s...
rosaic::PosCovGeodeticPtr PosCovGeodeticCallback(PosCovGeodetic &data)
Callback function when reading PosCovGeodetic blocks.
static uint32_t count_gpsfix_
Number of times the gps_common::GPSFix message has been published.
rosaic::GpgsaPtr ParseASCII(const NMEASentence &sentence) noexcept(false) override
Parses one GSA message.
Definition: gpgsa.cpp:51
std::size_t SegmentEnd()
Determines size of message that data_ is currently pointing at.
rosaic::GpgsvPtr ParseASCII(const NMEASentence &sentence) noexcept(false) override
Parses one GSV message.
Definition: gpgsv.cpp:51
Here is the call graph for this function:
Here is the caller graph for this function:

◆ Search()

const uint8_t * io_comm_mosaic::mosaicMessage::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 666 of file mosaicMessage.cpp.

References count_, data_, found_, IsConnectionDescriptor(), IsNMEA(), IsResponse(), IsSBF(), Next(), and read_cd.

Referenced by GetCount(), Read(), and io_comm_mosaic::CallbackHandlers::ReadCallback().

667 {
668  if (found_)
669  {
670  Next();
671  }
672  // Search for message or a response header
673  for( ; count_ > 0; --count_, ++data_)
674  {
675  if (this->IsSBF() || this->IsNMEA() || this->IsResponse() || (read_cd && this->IsConnectionDescriptor()))
676  {
677  break;
678  }
679  }
680  found_ = true;
681  return data_;
682 }
const uint8_t * Next()
Goes to the start of the next message based on the calculated length of current message.
bool found_
Whether or not a message has been found.
uint32_t count_
The number of bytes in the buffer, decremented as the buffer is read (so it is the buffer&#39;s size in t...
bool read_cd
Whether or not we still want to read the connection descriptor, which we only want in the very beginn...
bool IsResponse()
Determines whether data_ currently points to an NMEA message.
bool IsNMEA()
Determines whether data_ currently points to an NMEA message.
bool IsSBF()
Determines whether data_ currently points to an SBF block.
const uint8_t * data_
The pointer to the buffer of messages.
bool IsConnectionDescriptor()
Determines whether data_ currently points to a connection descriptor (right after initiating TCP conn...
Here is the call graph for this function:
Here is the caller graph for this function:

◆ SegmentEnd()

std::size_t io_comm_mosaic::mosaicMessage::SegmentEnd ( )

Determines size of message that data_ is currently pointing at.

Definition at line 684 of file mosaicMessage.cpp.

References CARRIAGE_RETURN, count_, data_, IsResponse(), LINE_FEED, and segment_size_.

Referenced by IsMessage(), MessageID(), mosaicMessage(), Read(), and io_comm_mosaic::CallbackHandlers::ReadCallback().

685 {
686  uint16_t pos = 0;
687  segment_size_ = 0;
688  uint32_t count_copy = count_;
689  if (this->IsResponse())
690  {
691  do
692  {
693  ++segment_size_;
694  ++pos;
695  --count_copy;
696  if (count_copy == 0) break;
697  } while(!((data_[pos] == CARRIAGE_RETURN && data_[pos+1] == LINE_FEED)) || (data_[pos] == CARRIAGE_RETURN && data_[pos+1] == LINE_FEED && data_[pos+2] == 0x20 && data_[pos+3] == 0x20 && data_[pos+4] == 0x4E) || (data_[pos] == CARRIAGE_RETURN && data_[pos+1] == LINE_FEED && data_[pos+2] == 0x20 && data_[pos+3] == 0x20 && data_[pos+4] == 0x53) || (data_[pos] == CARRIAGE_RETURN && data_[pos+1] == LINE_FEED && data_[pos+2] == 0x20 && data_[pos+3] == 0x20 && data_[pos+4] == 0x52));
698  }
699  else
700  {
701  do
702  {
703  ++segment_size_;
704  ++pos;
705  --count_copy;
706  if (count_copy == 0) break;
707  } while(!((data_[pos] == CARRIAGE_RETURN && data_[pos+1] == LINE_FEED) || data_[pos] == CARRIAGE_RETURN || data_[pos] == LINE_FEED));
708  }
709  return segment_size_;
710 }
std::size_t segment_size_
Helps to determine size of response message / NMEA message / SBF block.
uint32_t count_
The number of bytes in the buffer, decremented as the buffer is read (so it is the buffer&#39;s size in t...
bool IsResponse()
Determines whether data_ currently points to an NMEA message.
#define CARRIAGE_RETURN
0x0D is ASCII for "Carriage Return", i.e. "Enter"
const uint8_t * data_
The pointer to the buffer of messages.
#define LINE_FEED
0x0A is ASCII for "Line Feed", i.e. "New Line"
Here is the call graph for this function:
Here is the caller graph for this function:

Field Documentation

◆ count_

uint32_t io_comm_mosaic::mosaicMessage::count_
private

The number of bytes in the buffer, decremented as the buffer is read (so it is the buffer's size in the beginning)

Definition at line 301 of file mosaicMessage.hpp.

Referenced by End(), GetCount(), Next(), Read(), Search(), and SegmentEnd().

◆ count_gpsfix_

uint32_t io_comm_mosaic::mosaicMessage::count_gpsfix_ = 0
staticprivate

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

Number of times the "read" method of the mosaicMessage class has been called.

Definition at line 316 of file mosaicMessage.hpp.

Referenced by Read().

◆ CRCcheck_

bool io_comm_mosaic::mosaicMessage::CRCcheck_
private

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

Definition at line 306 of file mosaicMessage.hpp.

Referenced by mosaicMessage(), Next(), and Read().

◆ data_

const uint8_t* io_comm_mosaic::mosaicMessage::data_
private

The pointer to the buffer of messages.

Definition at line 296 of file mosaicMessage.hpp.

Referenced by BlockLength(), End(), IsConnectionDescriptor(), IsErrorMessage(), IsMessage(), IsNMEA(), IsResponse(), IsSBF(), MessageID(), Next(), Pos(), Read(), Search(), and SegmentEnd().

◆ found_

bool io_comm_mosaic::mosaicMessage::found_

Whether or not a message has been found.

Definition at line 289 of file mosaicMessage.hpp.

Referenced by Found(), mosaicMessage(), Next(), and Search().

◆ last_attcoveuler_

AttCovEuler io_comm_mosaic::mosaicMessage::last_attcoveuler_ = AttCovEuler()
staticprivate

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

Definition at line 336 of file mosaicMessage.hpp.

Referenced by GPSFixCallback(), PoseWithCovarianceStampedCallback(), and Read().

◆ last_atteuler_

AttEuler io_comm_mosaic::mosaicMessage::last_atteuler_ = AttEuler()
staticprivate

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

Definition at line 331 of file mosaicMessage.hpp.

Referenced by GPSFixCallback(), PoseWithCovarianceStampedCallback(), and Read().

◆ last_channelstatus_

ChannelStatus io_comm_mosaic::mosaicMessage::last_channelstatus_ = ChannelStatus()
staticprivate

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

Definition at line 341 of file mosaicMessage.hpp.

Referenced by GPSFixCallback(), and Read().

◆ last_dop_

DOP io_comm_mosaic::mosaicMessage::last_dop_ = DOP()
staticprivate

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

Definition at line 351 of file mosaicMessage.hpp.

Referenced by GPSFixCallback(), and Read().

◆ last_measepoch_

MeasEpoch io_comm_mosaic::mosaicMessage::last_measepoch_ = MeasEpoch()
staticprivate

Since GPSFix etc. need MeasEpoch (for signal-to-noise ratios), incoming MeasEpoch blocks need to be stored.

Definition at line 346 of file mosaicMessage.hpp.

Referenced by GPSFixCallback(), and Read().

◆ last_poscovgeodetic_

PosCovGeodetic io_comm_mosaic::mosaicMessage::last_poscovgeodetic_ = PosCovGeodetic()
staticprivate

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

Definition at line 326 of file mosaicMessage.hpp.

Referenced by GPSFixCallback(), NavSatFixCallback(), PoseWithCovarianceStampedCallback(), and Read().

◆ last_pvtgeodetic_

PVTGeodetic io_comm_mosaic::mosaicMessage::last_pvtgeodetic_ = PVTGeodetic()
staticprivate

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

Definition at line 321 of file mosaicMessage.hpp.

Referenced by GPSFixCallback(), NavSatFixCallback(), PoseWithCovarianceStampedCallback(), and Read().

◆ last_velcovgeodetic_

VelCovGeodetic io_comm_mosaic::mosaicMessage::last_velcovgeodetic_ = VelCovGeodetic()
staticprivate

Since GPSFix etc. need VelCovGeodetic (for signal-to-noise ratios), incoming VelCovGeodetic blocks need to be stored.

Definition at line 356 of file mosaicMessage.hpp.

Referenced by GPSFixCallback(), and Read().

◆ segment_size_

std::size_t io_comm_mosaic::mosaicMessage::segment_size_
private

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

Definition at line 311 of file mosaicMessage.hpp.

Referenced by mosaicMessage(), and SegmentEnd().


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