rx_message.hpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // © Copyright 2020, Septentrio NV/SA.
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // 1. Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // 2. Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // 3. Neither the name of the copyright holder nor the names of its
14 // contributors may be used to endorse or promote products derived
15 // from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 // *****************************************************************************
30 
31 // *****************************************************************************
32 //
33 // Boost Software License - Version 1.0 - August 17th, 2003
34 //
35 // Permission is hereby granted, free of charge, to any person or organization
36 // obtaining a copy of the software and accompanying documentation covered by
37 // this license (the "Software") to use, reproduce, display, distribute,
38 // execute, and transmit the Software, and to prepare derivative works of the
39 // Software, and to permit third-parties to whom the Software is furnished to
40 // do so, all subject to the following:
41 
42 // The copyright notices in the Software and this entire statement, including
43 // the above license grant, this restriction and the following disclaimer,
44 // must be included in all copies of the Software, in whole or in part, and
45 // all derivative works of the Software, unless such copies or derivative
46 // works are solely in the form of machine-executable object code generated by
47 // a source language processor.
48 //
49 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
50 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
51 // FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
52 // SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
53 // FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
54 // ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
55 // DEALINGS IN THE SOFTWARE.
56 //
57 // *****************************************************************************
58 
60 #ifndef NMEA_SYNC_BYTE_1
61 #define NMEA_SYNC_BYTE_1 0x24
62 #endif
63 #ifndef NMEA_SYNC_BYTE_2_1
65 #define NMEA_SYNC_BYTE_2_1 0x47
66 #endif
67 #ifndef NMEA_SYNC_BYTE_2_2
69 #define NMEA_SYNC_BYTE_2_2 0x50
70 #endif
71 #ifndef RESPONSE_SYNC_BYTE_1
73 #define RESPONSE_SYNC_BYTE_1 0x24
74 #endif
75 #ifndef RESPONSE_SYNC_BYTE_2
77 #define RESPONSE_SYNC_BYTE_2 0x52
78 #endif
79 #ifndef CARRIAGE_RETURN
81 #define CARRIAGE_RETURN 0x0D
82 #endif
83 #ifndef LINE_FEED
85 #define LINE_FEED 0x0A
86 #endif
87 #ifndef RESPONSE_SYNC_BYTE_3
90 #define RESPONSE_SYNC_BYTE_3 0x3F
91 #endif
92 #ifndef CONNECTION_DESCRIPTOR_BYTE_1
95 #define CONNECTION_DESCRIPTOR_BYTE_1 0x49
96 #endif
97 #ifndef CONNECTION_DESCRIPTOR_BYTE_2
100 #define CONNECTION_DESCRIPTOR_BYTE_2 0x50
101 #endif
102 
103 // C++ libraries
104 #include <cassert> // for assert
105 #include <cstddef>
106 #include <map>
107 #include <sstream>
108 // Boost includes
109 #include <boost/call_traits.hpp>
110 #include <boost/format.hpp>
111 #include <boost/math/constants/constants.hpp>
112 #include <boost/tokenizer.hpp>
113 // ROSaic includes
121 
122 #ifndef RX_MESSAGE_HPP
123 #define RX_MESSAGE_HPP
124 
131 extern bool g_read_cd;
132 extern uint32_t g_cd_count;
133 
137 {
148 };
149 
154 {
193 };
194 
195 namespace io_comm_rx {
196 
201  class RxMessage
202  {
203  public:
214  RxMessage(ROSaicNodeBase* node, Settings* settings) :
215  node_(node), settings_(settings), unix_time_(0)
216  {
217  found_ = false;
218  crc_check_ = false;
219  message_size_ = 0;
220 
222  std::pair<uint16_t, TypeOfPVT_Enum> type_of_pvt_pairs[] = {
223  std::make_pair(static_cast<uint16_t>(0), evNoPVT),
224  std::make_pair(static_cast<uint16_t>(1), evStandAlone),
225  std::make_pair(static_cast<uint16_t>(2), evDGPS),
226  std::make_pair(static_cast<uint16_t>(3), evFixed),
227  std::make_pair(static_cast<uint16_t>(4), evRTKFixed),
228  std::make_pair(static_cast<uint16_t>(5), evRTKFloat),
229  std::make_pair(static_cast<uint16_t>(6), evSBAS),
230  std::make_pair(static_cast<uint16_t>(7), evMovingBaseRTKFixed),
231  std::make_pair(static_cast<uint16_t>(8), evMovingBaseRTKFloat),
232  std::make_pair(static_cast<uint16_t>(10), evPPP)};
233 
235  TypeOfPVTMap(type_of_pvt_pairs, type_of_pvt_pairs + evPPP + 1);
236 
238  std::pair<std::string, RxID_Enum> rx_id_pairs[] = {
239  std::make_pair("NavSatFix", evNavSatFix),
240  std::make_pair("INSNavSatFix", evINSNavSatFix),
241  std::make_pair("GPSFix", evGPSFix),
242  std::make_pair("INSGPSFix", evINSGPSFix),
243  std::make_pair("PoseWithCovarianceStamped",
245  std::make_pair("INSPoseWithCovarianceStamped",
247  std::make_pair("$GPGGA", evGPGGA),
248  std::make_pair("$GPRMC", evGPRMC),
249  std::make_pair("$GPGSA", evGPGSA),
250  std::make_pair("$GPGSV", evGPGSV),
251  std::make_pair("$GLGSV", evGLGSV),
252  std::make_pair("$GAGSV", evGAGSV),
253  std::make_pair("4006", evPVTCartesian),
254  std::make_pair("4007", evPVTGeodetic),
255  std::make_pair("4043", evBaseVectorCart),
256  std::make_pair("4028", evBaseVectorGeod),
257  std::make_pair("5905", evPosCovCartesian),
258  std::make_pair("5906", evPosCovGeodetic),
259  std::make_pair("5938", evAttEuler),
260  std::make_pair("5939", evAttCovEuler),
261  std::make_pair("GPST", evGPST),
262  std::make_pair("4013", evChannelStatus),
263  std::make_pair("4027", evMeasEpoch),
264  std::make_pair("4001", evDOP),
265  std::make_pair("5908", evVelCovGeodetic),
266  std::make_pair("DiagnosticArray", evDiagnosticArray),
267  std::make_pair("Localization", evLocalization),
268  std::make_pair("4014", evReceiverStatus),
269  std::make_pair("4082", evQualityInd),
270  std::make_pair("5902", evReceiverSetup),
271  std::make_pair("4225", evINSNavCart),
272  std::make_pair("4226", evINSNavGeod),
273  std::make_pair("4230", evExtEventINSNavGeod),
274  std::make_pair("4229", evExtEventINSNavCart),
275  std::make_pair("4224", evIMUSetup),
276  std::make_pair("4244", evVelSensorSetup),
277  std::make_pair("4050", evExtSensorMeas),
278  std::make_pair("5914", evReceiverTime)};
279 
280  rx_id_map = RxIDMap(rx_id_pairs, rx_id_pairs + evReceiverSetup + 1);
281  }
282 
289  void newData(Timestamp recvTimestamp, const uint8_t* data, std::size_t& size)
290  {
291  recvTimestamp_ = recvTimestamp;
292  data_ = data;
293  count_ = size;
294  found_ = false;
295  crc_check_ = false;
296  message_size_ = 0;
297  }
298 
300  bool isMessage(const uint16_t ID);
303  bool isMessage(std::string ID);
305  bool isSBF();
307  bool isNMEA();
309  bool isResponse();
312  bool isConnectionDescriptor();
315  bool isErrorMessage();
318  std::size_t messageSize();
321  std::string messageID();
322 
327  std::size_t getCount() { return count_; };
333  const uint8_t* search();
334 
342  uint16_t getBlockLength();
343 
348  const uint8_t* getPosBuffer();
349 
355  const uint8_t* getEndBuffer();
356 
362  bool found();
363 
368  void next();
369 
375  template <typename M>
376  void publish(const std::string& topic, const M& msg);
377 
382  void publishTf(const LocalizationUtmMsg& msg);
383 
388  bool read(std::string message_key, bool search = false);
389 
393  bool found_;
394 
398  bool gnss_gpsfix_complete(uint32_t id);
399 
403  bool ins_gpsfix_complete(uint32_t id);
404 
408  bool gnss_navsatfix_complete(uint32_t id);
409 
413  bool ins_navsatfix_complete(uint32_t id);
414 
418  bool gnss_pose_complete(uint32_t id);
419 
423  bool ins_pose_complete(uint32_t id);
424 
428  bool diagnostics_complete(uint32_t id);
429 
433  bool ins_localization_complete(uint32_t id);
434 
435  private:
440 
445 
449  const uint8_t* data_;
450 
454  std::size_t count_;
455 
461 
466  std::size_t message_size_;
467 
471  uint32_t count_gpsfix_ = 0;
472 
478 
484 
490 
496 
502 
508 
514 
520 
525 
531 
537 
543 
549 
552  typedef std::unordered_map<uint16_t, TypeOfPVT_Enum> TypeOfPVTMap;
553 
558  TypeOfPVTMap type_of_pvt_map;
559 
562  typedef std::unordered_map<std::string, RxID_Enum> RxIDMap;
563 
572  RxIDMap rx_id_map;
573 
577 
579  int8_t current_leap_seconds_ = -128;
580 
584 
588 
591 
595 
599 
603 
607 
611 
615 
619 
623 
630 
634 
638 
642 
646 
650 
654 
658 
664 
670 
678 
686 
694 
702 
710  TwistWithCovarianceStampedMsg TwistCallback(bool fromIns = false);
711 
715  void wait(Timestamp time_obj);
716 
720  bool allTrue(std::vector<bool>& vec, uint32_t id);
721 
726 
730  std::shared_ptr<std::string> fixedUtmZone_;
731 
742  Timestamp timestampSBF(const uint8_t* data, bool use_gnss_time);
743 
757  Timestamp timestampSBF(uint32_t tow, uint16_t wnc, bool use_gnss_time);
758  };
759 } // namespace io_comm_rx
760 #endif // for RX_MESSAGE_HPP
septentrio_gnss_driver::VelCovGeodetic VelCovGeodeticMsg
Definition: typedefs.hpp:124
void publish(const std::string &topic, const M &msg)
Publishing function.
Derived class for parsing GSA messages.
Timestamp recvTimestamp_
Timestamp of receiving buffer.
Definition: rx_message.hpp:444
AttCovEulerMsg last_attcoveuler_
Since GPSFix etc. need AttCovEuler, incoming AttCovEuler blocks need to be stored.
Definition: rx_message.hpp:495
bool gnss_gpsfix_complete(uint32_t id)
Wether all blocks from GNSS have arrived for GpsFix Message.
bool read(std::string message_key, bool search=false)
Performs the CRC check (if SBF) and publishes ROS messages.
const uint8_t * data_
Pointer to the buffer of messages.
Definition: rx_message.hpp:449
bool isResponse()
Determines whether data_ currently points to an NMEA message.
bool poscovgeodetic_has_arrived_gpsfix_
Definition: rx_message.hpp:598
const uint8_t * search()
Searches the buffer for the beginning of the next message (NMEA or SBF)
PosCovGeodeticMsg last_poscovgeodetic_
Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic blocks need to be stored...
Definition: rx_message.hpp:483
septentrio_gnss_driver::AttCovEuler AttCovEulerMsg
Definition: typedefs.hpp:114
TwistWithCovarianceStampedMsg TwistCallback(bool fromIns=false)
"Callback" function when constructing TwistWithCovarianceStampedMsg messages
Definition: rx_message.cpp:408
bool insnavgeod_has_arrived_navsatfix_
Definition: rx_message.hpp:626
bool allTrue(std::vector< bool > &vec, uint32_t id)
Wether all elements are true.
bool gnss_pose_complete(uint32_t id)
Wether all blocks from GNSS have arrived for Pose Message.
std::string messageID()
void wait(Timestamp time_obj)
Waits according to time when reading from file.
PVTGeodeticMsg last_pvtgeodetic_
Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks need to be stored.
Definition: rx_message.hpp:477
bool crc_check_
Whether the CRC check as evaluated in the read() method was successful or not is stored here...
Definition: rx_message.hpp:460
RxMessage(ROSaicNodeBase *node, Settings *settings)
Constructor of the RxMessage class.
Definition: rx_message.hpp:214
NavSatFixMsg NavSatFixCallback()
"Callback" function when constructing NavSatFix messages
Definition: rx_message.cpp:906
PoseWithCovarianceStampedMsg PoseWithCovarianceStampedCallback()
"Callback" function when constructing PoseWithCovarianceStamped messages
Definition: rx_message.cpp:55
septentrio_gnss_driver::PVTGeodetic PVTGeodeticMsg
Definition: typedefs.hpp:117
bool ins_gpsfix_complete(uint32_t id)
Wether all blocks from INS have arrived for GpsFix Message.
Derived class for parsing GSV messages.
septentrio_gnss_driver::MeasEpoch MeasEpochMsg
Definition: typedefs.hpp:111
ReceiverSetup last_receiversetup_
Since DiagnosticArray needs ReceiverSetup, incoming ReceiverSetup blocks need to be stored...
Definition: rx_message.hpp:548
septentrio_gnss_driver::ExtSensorMeas ExtSensorMeasMsg
Definition: typedefs.hpp:137
Declares the functions to compute and validate the CRC of a buffer.
DOP last_dop_
Since GPSFix needs DOP, incoming DOP blocks need to be stored.
Definition: rx_message.hpp:524
Struct for the SBF block "DOP".
void next()
Goes to the start of the next message based on the calculated length of current message.
uint64_t Timestamp
Definition: typedefs.hpp:88
septentrio_gnss_driver::AttEuler AttEulerMsg
Definition: typedefs.hpp:115
uint32_t g_cd_count
bool receiverstatus_has_arrived_diagnostics_
Definition: rx_message.hpp:649
bool isNMEA()
Determines whether data_ currently points to an NMEA message.
bool insnavgeod_has_arrived_localization_
Definition: rx_message.hpp:657
const uint8_t * getPosBuffer()
Gets the current position in the read buffer.
Struct for the SBF block "QualityInd".
ImuMsg ImuCallback()
"Callback" function when constructing ImuMsg messages
Definition: rx_message.cpp:294
bool diagnostics_complete(uint32_t id)
Wether all blocks have arrived for Diagnostics Message.
INSNavGeodMsg last_insnavgeod_
Since NavSatFix, GPSFix, Imu and Pose. need INSNavGeod, incoming INSNavGeod blocks need to be stored...
Definition: rx_message.hpp:501
RxIDMap rx_id_map
All instances of the RxMessage class shall have access to the map without reinitializing it...
Definition: rx_message.hpp:572
Derived class for parsing GGA messages.
Timestamp timestampSBF(const uint8_t *data, bool use_gnss_time)
Calculates the timestamp, in the Unix Epoch time format This is either done using the TOW as transmit...
void newData(Timestamp recvTimestamp, const uint8_t *data, std::size_t &size)
Put new data.
Definition: rx_message.hpp:289
bool velcovgeodetic_has_arrived_gpsfix_
Definition: rx_message.hpp:602
bool gnss_navsatfix_complete(uint32_t id)
Wether all blocks from GNSS have arrived for NavSatFix Message.
ChannelStatus last_channelstatus_
Since GPSFix needs ChannelStatus, incoming ChannelStatus blocks need to be stored.
Definition: rx_message.hpp:513
bool isSBF()
Determines whether data_ currently points to an SBF block.
Settings * settings_
Settings struct.
Definition: rx_message.hpp:725
bool ins_pose_complete(uint32_t id)
Wether all blocks from INS have arrived for Pose Message.
bool ins_navsatfix_complete(uint32_t id)
Wether all blocks from INS have arrived for NavSatFix Message.
std::size_t count_
Number of unread bytes in the buffer.
Definition: rx_message.hpp:454
Declares lower-level string utility functions used when parsing messages.
Derived class for parsing RMC messages.
TypeOfPVTMap type_of_pvt_map
All instances of the RxMessage class shall have access to the map without reinitializing it...
Definition: rx_message.hpp:558
void publishTf(const LocalizationUtmMsg &msg)
Publishing function.
bool ins_localization_complete(uint32_t id)
Wether all blocks have arrived for Localization Message.
ROSaicNodeBase * node_
Pointer to the node.
Definition: rx_message.hpp:439
std::size_t messageSize()
Settings struct.
Definition: settings.h:103
uint32_t count_gpsfix_
Number of times the GPSFixMsg message has been published.
Definition: rx_message.hpp:471
std::size_t getCount()
Returns the count_ variable.
Definition: rx_message.hpp:327
RxID_Enum
Definition: rx_message.hpp:153
sensor_msgs::Imu ImuMsg
Definition: typedefs.hpp:104
septentrio_gnss_driver::INSNavGeod INSNavGeodMsg
Definition: typedefs.hpp:134
Struct for the SBF block "ChannelStatus".
bool qualityind_has_arrived_diagnostics_
Definition: rx_message.hpp:653
bool poscovgeodetic_has_arrived_pose_
Definition: rx_message.hpp:633
uint16_t getBlockLength()
Gets the length of the SBF block.
VelCovGeodeticMsg last_velcovgeodetic_
Since GPSFix needs VelCovGeodetic, incoming VelCovGeodetic blocks need to be stored.
Definition: rx_message.hpp:530
septentrio_gnss_driver::PosCovGeodetic PosCovGeodeticMsg
Definition: typedefs.hpp:119
std::size_t message_size_
Helps to determine size of response message / NMEA message / SBF block.
Definition: rx_message.hpp:466
TypeOfPVT_Enum
Definition: rx_message.hpp:136
ReceiverStatus last_receiverstatus_
Since DiagnosticArray needs ReceiverStatus, incoming ReceiverStatus blocks need to be stored...
Definition: rx_message.hpp:536
DiagnosticArrayMsg DiagnosticArrayCallback()
"Callback" function when constructing DiagnosticArrayMsg messages
Definition: rx_message.cpp:187
bool pvtgeodetic_has_arrived_navsatfix_
Definition: rx_message.hpp:618
bool dop_has_arrived_gpsfix_
For GPSFix: Whether the DOP block of the current epoch has arrived or not.
Definition: rx_message.hpp:590
const uint8_t * getEndBuffer()
Gets the end position in the read buffer.
diagnostic_msgs::DiagnosticArray DiagnosticArrayMsg
Definition: typedefs.hpp:93
LocalizationUtmMsg LocalizationUtmCallback()
"Callback" function when constructing LocalizationUtmMsg messages
Definition: rx_message.cpp:629
Struct for the SBF block "ReceiverStatus".
nav_msgs::Odometry LocalizationUtmMsg
Definition: typedefs.hpp:105
MeasEpochMsg last_measepoch_
Since GPSFix needs MeasEpoch (for SNRs), incoming MeasEpoch blocks need to be stored.
Definition: rx_message.hpp:519
bool pvtgeodetic_has_arrived_gpsfix_
Definition: rx_message.hpp:594
std::shared_ptr< std::string > fixedUtmZone_
Fixed UTM zone.
Definition: rx_message.hpp:730
This class is the base class for abstraction.
Definition: typedefs.hpp:174
GPSFixMsg GPSFixCallback()
"Callback" function when constructing GPSFix messages
Can search buffer for messages, read/parse them, and so on.
Definition: rx_message.hpp:201
std::unordered_map< std::string, RxID_Enum > RxIDMap
Definition: rx_message.hpp:562
AttEulerMsg last_atteuler_
Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to be stored.
Definition: rx_message.hpp:489
QualityInd last_qualityind_
Since DiagnosticArray needs QualityInd, incoming QualityInd blocks need to be stored.
Definition: rx_message.hpp:542
int8_t current_leap_seconds_
Current leap seconds as received, do not use value is -128.
Definition: rx_message.hpp:579
Struct for the SBF block "ReceiverSetup".
bool found()
Has an NMEA message, SBF block or command reply been found in the buffer?
bool attcoveuler_has_arrived_gpsfix_
Definition: rx_message.hpp:610
std::unordered_map< uint16_t, TypeOfPVT_Enum > TypeOfPVTMap
Definition: rx_message.hpp:552
sensor_msgs::NavSatFix NavSatFixMsg
Definition: typedefs.hpp:101
gps_common::GPSFix GPSFixMsg
Definition: typedefs.hpp:99
bool g_read_cd
bool channelstatus_has_arrived_gpsfix_
Definition: rx_message.hpp:583
geometry_msgs::PoseWithCovarianceStamped PoseWithCovarianceStampedMsg
Definition: typedefs.hpp:96
bool poscovgeodetic_has_arrived_navsatfix_
Definition: rx_message.hpp:622
ExtSensorMeasMsg last_extsensmeas_
Since Imu needs ExtSensorMeas, incoming ExtSensorMeas blocks need to be stored.
Definition: rx_message.hpp:507
bool found_
Whether or not a message has been found.
Definition: rx_message.hpp:393
geometry_msgs::TwistWithCovarianceStamped TwistWithCovarianceStampedMsg
Definition: typedefs.hpp:97
bool isMessage(const uint16_t ID)
Determines whether data_ points to the SBF block with ID "ID", e.g. 5003.


septentrio_gnss_driver
Author(s): Tibor Dome
autogenerated on Sat Mar 11 2023 03:12:56