ROSaic
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 SBF_SYNC_BYTE_1
61 #define SBF_SYNC_BYTE_1 0x24
62 #endif
63 #ifndef SBF_SYNC_BYTE_2
65 #define SBF_SYNC_BYTE_2 0x40
66 #endif
67 #ifndef NMEA_SYNC_BYTE_1
69 #define NMEA_SYNC_BYTE_1 0x24
70 #endif
71 #ifndef NMEA_SYNC_BYTE_2_1
73 #define NMEA_SYNC_BYTE_2_1 0x47
74 #endif
75 #ifndef NMEA_SYNC_BYTE_2_2
77 #define NMEA_SYNC_BYTE_2_2 0x50
78 #endif
79 #ifndef RESPONSE_SYNC_BYTE_1
81 #define RESPONSE_SYNC_BYTE_1 0x24
82 #endif
83 #ifndef RESPONSE_SYNC_BYTE_2
85 #define RESPONSE_SYNC_BYTE_2 0x52
86 #endif
87 #ifndef CARRIAGE_RETURN
89 #define CARRIAGE_RETURN 0x0D
90 #endif
91 #ifndef LINE_FEED
93 #define LINE_FEED 0x0A
94 #endif
95 #ifndef RESPONSE_SYNC_BYTE_3
97 #define RESPONSE_SYNC_BYTE_3 0x3F
98 #endif
99 #ifndef CONNECTION_DESCRIPTOR_BYTE_1
101 #define CONNECTION_DESCRIPTOR_BYTE_1 0x49
102 #endif
103 #ifndef CONNECTION_DESCRIPTOR_BYTE_2
105 #define CONNECTION_DESCRIPTOR_BYTE_2 0x50
106 #endif
107 
108 
109 // C++ libraries
110 #include <cstddef>
111 #include <sstream>
112 #include <map>
113 #include <cassert> // for assert
114 // Boost includes
115 #include <boost/tokenizer.hpp>
116 #include <boost/call_traits.hpp>
117 #include <boost/format.hpp>
118 #include <boost/math/constants/constants.hpp>
119 // ROS includes
120 #include <sensor_msgs/NavSatFix.h>
121 #include <sensor_msgs/TimeReference.h>
122 #include <geometry_msgs/PoseWithCovarianceStamped.h>
123 #include <diagnostic_msgs/DiagnosticArray.h>
124 #include <diagnostic_msgs/DiagnosticStatus.h>
125 #include <gps_common/GPSFix.h>
126 // ROSaic includes
133 #include <septentrio_gnss_driver/PVTCartesian.h>
134 #include <septentrio_gnss_driver/PVTGeodetic.h>
135 #include <septentrio_gnss_driver/PosCovCartesian.h>
136 #include <septentrio_gnss_driver/PosCovGeodetic.h>
137 #include <septentrio_gnss_driver/AttEuler.h>
138 #include <septentrio_gnss_driver/AttCovEuler.h>
139 
140 #ifndef RX_MESSAGE_HPP
141 #define RX_MESSAGE_HPP
142 
149 extern bool g_use_gnss_time;
150 extern bool g_read_cd;
151 extern uint32_t g_cd_count;
152 extern uint32_t g_leap_seconds;
155 extern bool g_dop_has_arrived_gpsfix;
164 extern bool g_atteuler_has_arrived_pose;
169 extern boost::shared_ptr<ros::NodeHandle> g_nh;
170 extern const uint32_t g_ROS_QUEUE_SIZE;
171 
175 
182 
183 namespace io_comm_rx
184 {
194  ros::Time timestampSBF(uint32_t tow, bool use_gnss);
195 
200  class RxMessage
201  {
202  public:
213  RxMessage(const uint8_t* data, std::size_t& size): data_(data), count_(size)
214  {found_ = false; crc_check_ = false; message_size_ = 0;}
215 
217  bool isMessage(const uint16_t ID);
219  bool isMessage(std::string ID);
221  bool isSBF();
223  bool isNMEA();
225  bool isResponse();
228  bool isConnectionDescriptor();
230  bool isErrorMessage();
232  std::size_t messageSize();
235  std::string messageID();
236 
241  std::size_t getCount() {return count_;};
246  const uint8_t* search();
247 
255  uint16_t getBlockLength();
256 
261  const uint8_t* getPosBuffer();
262 
268  const uint8_t* getEndBuffer();
269 
274  bool found();
275 
279  void next();
280 
285  template <typename T>
286  bool read(typename boost::call_traits<T>::reference message, std::string message_key, bool search = false);
287 
291  bool found_;
292 
293  private:
294 
298  const uint8_t* data_;
299 
303  std::size_t count_;
304 
309 
313  std::size_t message_size_;
314 
318  static uint32_t count_gpsfix_;
319 
324 
329 
334 
339 
344 
349 
353  static DOP last_dop_;
354 
359 
364 
369 
374 
376  typedef std::map<uint16_t, TypeOfPVT_Enum> TypeOfPVTMap;
377 
381  static TypeOfPVTMap type_of_pvt_map;
382 
384  typedef std::map<std::string, RxID_Enum> RxIDMap;
385 
392  static RxIDMap rx_id_map;
393 
399  septentrio_gnss_driver::PVTCartesianPtr PVTCartesianCallback(PVTCartesian& data);
400 
406  septentrio_gnss_driver::PVTGeodeticPtr PVTGeodeticCallback(PVTGeodetic& data);
407 
413  septentrio_gnss_driver::PosCovCartesianPtr PosCovCartesianCallback(PosCovCartesian& data);
414 
420  septentrio_gnss_driver::PosCovGeodeticPtr PosCovGeodeticCallback(PosCovGeodetic& data);
421 
427  septentrio_gnss_driver::AttEulerPtr AttEulerCallback(AttEuler& data);
428 
434  septentrio_gnss_driver::AttCovEulerPtr AttCovEulerCallback(AttCovEuler& data);
435 
440  sensor_msgs::NavSatFixPtr NavSatFixCallback();
441 
446  gps_common::GPSFixPtr GPSFixCallback();
447 
452  geometry_msgs::PoseWithCovarianceStampedPtr PoseWithCovarianceStampedCallback();
453 
458  diagnostic_msgs::DiagnosticArrayPtr DiagnosticArrayCallback();
459 
460  };
461 
462 
476  template <typename T>
477  bool RxMessage::read(typename boost::call_traits<T>::reference message, std::string message_key, bool search)
478  {
479  if (search) this->search();
480  if (!found()) return false;
481  if (this->isSBF())
482  {
483  // If the CRC check is unsuccessful, throw an error message.
485  if (!crc_check_)
486  {
487  throw std::runtime_error("CRC Check returned False. Not a valid data block, perhaps noisy. Ignore..");
488  }
489  }
490  switch(rx_id_map[message_key])
491  {
492  case evPVTCartesian: // Position and velocity in XYZ
493  { // The curly bracket here is crucial: Declarations inside a block remain inside, and will die at
494  // the end of the block. Otherwise variable overloading etc.
495  septentrio_gnss_driver::PVTCartesianPtr msg = boost::make_shared<septentrio_gnss_driver::PVTCartesian>();
496  PVTCartesian pvtcartesian;
497  memcpy(&pvtcartesian, data_, sizeof(pvtcartesian));
498  msg = PVTCartesianCallback(pvtcartesian);
499  msg->header.frame_id = g_frame_id;
500  uint32_t tow = *(reinterpret_cast<const uint32_t *>(data_ + 8));
501  ros::Time time_obj;
502  time_obj = timestampSBF(tow, g_use_gnss_time);
503  msg->header.stamp.sec = time_obj.sec;
504  msg->header.stamp.nsec = time_obj.nsec;
505  msg->block_header.id = 4006;
506  memcpy(&message, msg.get(), sizeof(*msg));
507  static ros::Publisher publisher = g_nh->advertise<septentrio_gnss_driver::PVTCartesian>("/pvtcartesian", g_ROS_QUEUE_SIZE);
508  publisher.publish(*msg);
509  break;
510  }
511  case evPVTGeodetic: // Position and velocity in geodetic coordinate frame (ENU frame)
512  {
513  septentrio_gnss_driver::PVTGeodeticPtr msg = boost::make_shared<septentrio_gnss_driver::PVTGeodetic>();
514  memcpy(&last_pvtgeodetic_, data_, sizeof(last_pvtgeodetic_));
516  msg->header.frame_id = g_frame_id;
517  uint32_t tow = *(reinterpret_cast<const uint32_t *>(data_ + 8));
518  ros::Time time_obj;
519  time_obj = timestampSBF(tow, g_use_gnss_time);
520  msg->header.stamp.sec = time_obj.sec;
521  msg->header.stamp.nsec = time_obj.nsec;
522  msg->block_header.id = 4007;
523  memcpy(&message, msg.get(), sizeof(*msg));
527  static ros::Publisher publisher = g_nh->advertise<septentrio_gnss_driver::PVTGeodetic>("/pvtgeodetic", g_ROS_QUEUE_SIZE);
528  publisher.publish(*msg);
529  break;
530  }
531  case evPosCovCartesian:
532  {
533  septentrio_gnss_driver::PosCovCartesianPtr msg = boost::make_shared<septentrio_gnss_driver::PosCovCartesian>();
534  PosCovCartesian poscovcartesian;
535  memcpy(&poscovcartesian, data_, sizeof(poscovcartesian));
536  msg = PosCovCartesianCallback(poscovcartesian);
537  msg->header.frame_id = g_frame_id;
538  uint32_t tow = *(reinterpret_cast<const uint32_t *>(data_ + 8));
539  ros::Time time_obj;
540  time_obj = timestampSBF(tow, g_use_gnss_time);
541  msg->header.stamp.sec = time_obj.sec;
542  msg->header.stamp.nsec = time_obj.nsec;
543  msg->block_header.id = 5905;
544  memcpy(&message, msg.get(), sizeof(*msg));
545  static ros::Publisher publisher = g_nh->advertise<septentrio_gnss_driver::PosCovCartesian>("/poscovcartesian", g_ROS_QUEUE_SIZE);
546  publisher.publish(*msg);
547  break;
548  }
549  case evPosCovGeodetic:
550  {
551  septentrio_gnss_driver::PosCovGeodeticPtr msg = boost::make_shared<septentrio_gnss_driver::PosCovGeodetic>();
554  msg->header.frame_id = g_frame_id;
555  uint32_t tow = *(reinterpret_cast<const uint32_t *>(data_ + 8));
556  ros::Time time_obj;
557  time_obj = timestampSBF(tow, g_use_gnss_time);
558  msg->header.stamp.sec = time_obj.sec;
559  msg->header.stamp.nsec = time_obj.nsec;
560  msg->block_header.id = 5906;
561  memcpy(&message, msg.get(), sizeof(*msg));
565  static ros::Publisher publisher = g_nh->advertise<septentrio_gnss_driver::PosCovGeodetic>("/poscovgeodetic", g_ROS_QUEUE_SIZE);
566  publisher.publish(*msg);
567  break;
568  }
569  case evAttEuler:
570  {
571  septentrio_gnss_driver::AttEulerPtr msg = boost::make_shared<septentrio_gnss_driver::AttEuler>();
572  memcpy(&last_atteuler_, data_, sizeof(last_atteuler_));
574  msg->header.frame_id = g_frame_id;
575  uint32_t tow = *(reinterpret_cast<const uint32_t *>(data_ + 8));
576  ros::Time time_obj;
577  time_obj = timestampSBF(tow, g_use_gnss_time);
578  msg->header.stamp.sec = time_obj.sec;
579  msg->header.stamp.nsec = time_obj.nsec;
580  msg->block_header.id = 5938;
581  memcpy(&message, msg.get(), sizeof(*msg));
584  static ros::Publisher publisher = g_nh->advertise<septentrio_gnss_driver::AttEuler>("/atteuler", g_ROS_QUEUE_SIZE);
585  publisher.publish(*msg);
586  break;
587  }
588  case evAttCovEuler:
589  {
590  septentrio_gnss_driver::AttCovEulerPtr msg = boost::make_shared<septentrio_gnss_driver::AttCovEuler>();
591  memcpy(&last_attcoveuler_, data_, sizeof(last_attcoveuler_));
593  msg->header.frame_id = g_frame_id;
594  uint32_t tow = *(reinterpret_cast<const uint32_t *>(data_ + 8));
595  ros::Time time_obj;
596  time_obj = timestampSBF(tow, g_use_gnss_time);
597  msg->header.stamp.sec = time_obj.sec;
598  msg->header.stamp.nsec = time_obj.nsec;
599  msg->block_header.id = 5939;
600  memcpy(&message, msg.get(), sizeof(*msg));
603  static ros::Publisher publisher = g_nh->advertise<septentrio_gnss_driver::AttCovEuler>("/attcoveuler", g_ROS_QUEUE_SIZE);
604  publisher.publish(*msg);
605  break;
606  }
607  case evGPST:
608  {
609  sensor_msgs::TimeReferencePtr msg = boost::make_shared<sensor_msgs::TimeReference>();
610  uint32_t tow = *(reinterpret_cast<const uint32_t *>(data_ + 8));
611  ros::Time time_obj;
612  time_obj = timestampSBF(tow, true); // We need the GPS time, hence true
613  msg->time_ref.sec = time_obj.sec;
614  msg->time_ref.nsec = time_obj.nsec;
615  msg->source = "GPST";
616  memcpy(&message, msg.get(), sizeof(*msg));
617  static ros::Publisher publisher = g_nh->advertise<sensor_msgs::TimeReference>("/gpst", g_ROS_QUEUE_SIZE);
618  publisher.publish(*msg);
619  break;
620  }
621  case evGPGGA:
622  {
623  boost::char_separator<char> sep("\r");
624  typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
625  std::size_t nmea_size = this->messageSize();
626  std::string block_in_string(reinterpret_cast<const char*>(data_), nmea_size);
627  tokenizer tokens(block_in_string, sep);
628 
629  std::string id = this->messageID();
630  std::string one_message = *tokens.begin();
631  // No kept delimiters, hence "". Also, we specify that empty tokens should show up in the output
632  // when two delimiters are next to each other.
633  // Hence we also append the checksum part of the GGA message to "body" below, though it is not parsed.
634  boost::char_separator<char> sep_2(",*", "", boost::keep_empty_tokens);
635  tokenizer tokens_2(one_message, sep_2);
636  std::vector<std::string> body;
637  for (tokenizer::iterator tok_iter = tokens_2.begin(); tok_iter != tokens_2.end(); ++tok_iter)
638  {
639  body.push_back(*tok_iter);
640  }
641  // Create NmeaSentence struct to pass to GpggaParser::parseASCII
642  NMEASentence gga_message(id, body);
643  septentrio_gnss_driver::GpggaPtr msg = boost::make_shared<septentrio_gnss_driver::Gpgga>();
644  GpggaParser parser_obj;
645  try
646  {
647  msg = parser_obj.parseASCII(gga_message);
648  }
649  catch (ParseException& e)
650  {
651  throw std::runtime_error(e.what());
652  }
653  memcpy(&message, msg.get(), sizeof(*msg));
654  static ros::Publisher publisher = g_nh->advertise<septentrio_gnss_driver::Gpgga>("/gpgga", g_ROS_QUEUE_SIZE);
655  publisher.publish(*msg);
656  break;
657  }
658  case evGPRMC:
659  {
660  boost::char_separator<char> sep("\r");
661  typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
662  std::size_t nmea_size = this->messageSize();
663  std::string block_in_string(reinterpret_cast<const char*>(data_), nmea_size);
664  tokenizer tokens(block_in_string, sep);
665 
666  std::string id = this->messageID();
667  std::string one_message = *tokens.begin();
668  boost::char_separator<char> sep_2(",*", "", boost::keep_empty_tokens);
669  tokenizer tokens_2(one_message, sep_2);
670  std::vector<std::string> body;
671  for (tokenizer::iterator tok_iter = tokens_2.begin(); tok_iter != tokens_2.end(); ++tok_iter)
672  {
673  body.push_back(*tok_iter);
674  }
675  // Create NmeaSentence struct to pass to GprmcParser::parseASCII
676  NMEASentence rmc_message(id, body);
677  septentrio_gnss_driver::GprmcPtr msg = boost::make_shared<septentrio_gnss_driver::Gprmc>();
678  GprmcParser parser_obj;
679  try
680  {
681  msg = parser_obj.parseASCII(rmc_message);
682  }
683  catch (ParseException& e)
684  {
685  throw std::runtime_error(e.what());
686  }
687  memcpy(&message, msg.get(), sizeof(*msg));
688  static ros::Publisher publisher = g_nh->advertise<septentrio_gnss_driver::Gprmc>("/gprmc", g_ROS_QUEUE_SIZE);
689  publisher.publish(*msg);
690  break;
691  }
692  case evGPGSA:
693  {
694  boost::char_separator<char> sep("\r");
695  typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
696  std::size_t nmea_size = this->messageSize();
697  std::string block_in_string(reinterpret_cast<const char*>(data_), nmea_size);
698  tokenizer tokens(block_in_string, sep);
699 
700  std::string id = this->messageID();
701  std::string one_message = *tokens.begin();
702  boost::char_separator<char> sep_2(",*", "", boost::keep_empty_tokens);
703  tokenizer tokens_2(one_message, sep_2);
704  std::vector<std::string> body;
705  for (tokenizer::iterator tok_iter = tokens_2.begin(); tok_iter != tokens_2.end(); ++tok_iter)
706  {
707  body.push_back(*tok_iter);
708  }
709  // Create NmeaSentence struct to pass to GpgsaParser::parseASCII
710  NMEASentence gsa_message(id, body);
711  septentrio_gnss_driver::GpgsaPtr msg = boost::make_shared<septentrio_gnss_driver::Gpgsa>();
712  GpgsaParser parser_obj;
713  try
714  {
715  msg = parser_obj.parseASCII(gsa_message);
716  }
717  catch (ParseException& e)
718  {
719  throw std::runtime_error(e.what());
720  }
721  uint32_t tow = last_pvtgeodetic_.tow;
722  ros::Time time_obj;
723  time_obj = timestampSBF(tow, g_use_gnss_time);
724  msg->header.stamp.sec = time_obj.sec;
725  msg->header.stamp.nsec = time_obj.nsec;
726  memcpy(&message, msg.get(), sizeof(*msg));
727  static ros::Publisher publisher = g_nh->advertise<septentrio_gnss_driver::Gpgsa>("/gpgsa", g_ROS_QUEUE_SIZE);
728  publisher.publish(*msg);
729  break;
730  }
731  case evGPGSV: case evGLGSV: case evGAGSV:
732  {
733  boost::char_separator<char> sep("\r");
734  typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
735  std::size_t nmea_size = this->messageSize();
736  std::string block_in_string(reinterpret_cast<const char*>(data_), nmea_size);
737  tokenizer tokens(block_in_string, sep);
738 
739  std::string id = this->messageID();
740  std::string one_message = *tokens.begin();
741  boost::char_separator<char> sep_2(",*", "", boost::keep_empty_tokens);
742  tokenizer tokens_2(one_message, sep_2);
743  std::vector<std::string> body;
744  for (tokenizer::iterator tok_iter = tokens_2.begin(); tok_iter != tokens_2.end(); ++tok_iter)
745  {
746  body.push_back(*tok_iter);
747  }
748  // Create NmeaSentence struct to pass to GpgsvParser::parseASCII
749  NMEASentence gsv_message(id, body);
750  septentrio_gnss_driver::GpgsvPtr msg = boost::make_shared<septentrio_gnss_driver::Gpgsv>();
751  GpgsvParser parser_obj;
752  try
753  {
754  msg = parser_obj.parseASCII(gsv_message);
755  }
756  catch (ParseException& e)
757  {
758  throw std::runtime_error(e.what());
759  }
760  uint32_t tow = last_pvtgeodetic_.tow;
761  ros::Time time_obj;
762  time_obj = timestampSBF(tow, g_use_gnss_time);
763  msg->header.stamp.sec = time_obj.sec;
764  msg->header.stamp.nsec = time_obj.nsec;
765  memcpy(&message, msg.get(), sizeof(*msg));
766  static ros::Publisher publisher = g_nh->advertise<septentrio_gnss_driver::Gpgsv>("/gpgsv", g_ROS_QUEUE_SIZE);
767  publisher.publish(*msg);
768  break;
769  }
770  case evNavSatFix:
771  {
772  sensor_msgs::NavSatFixPtr msg = boost::make_shared<sensor_msgs::NavSatFix>();
773  try
774  {
775  msg = NavSatFixCallback();
776  }
777  catch (std::runtime_error& e)
778  {
779  throw std::runtime_error(e.what());
780  }
781  msg->header.frame_id = g_frame_id;
782  uint32_t tow = *(reinterpret_cast<const uint32_t *>(data_ + 8));
783  ros::Time time_obj;
784  time_obj = timestampSBF(tow, g_use_gnss_time);
785  msg->header.stamp.sec = time_obj.sec;
786  msg->header.stamp.nsec = time_obj.nsec;
787  memcpy(&message, msg.get(), sizeof(*msg));
790  static ros::Publisher publisher = g_nh->advertise<sensor_msgs::NavSatFix>("/navsatfix", g_ROS_QUEUE_SIZE);
791  publisher.publish(*msg);
792  break;
793  }
794  case evGPSFix:
795  {
796  gps_common::GPSFixPtr msg = boost::make_shared<gps_common::GPSFix>();
797  try
798  {
799  msg = GPSFixCallback();
800  }
801  catch (std::runtime_error& e)
802  {
803  throw std::runtime_error(e.what());
804  }
805  msg->status.header.seq = count_gpsfix_;
806  msg->header.frame_id = g_frame_id;
807  msg->status.header.frame_id = g_frame_id;
808  uint32_t tow = *(reinterpret_cast<const uint32_t *>(data_ + 8));
809  ros::Time time_obj;
810  time_obj = timestampSBF(tow, g_use_gnss_time);
811  msg->header.stamp.sec = time_obj.sec;
812  msg->status.header.stamp.sec = time_obj.sec;
813  msg->header.stamp.nsec = time_obj.nsec;
814  msg->status.header.stamp.nsec = time_obj.nsec;
815  memcpy(&message, msg.get(), sizeof(*msg));
816  ++count_gpsfix_;
819  g_dop_has_arrived_gpsfix = false;
825  static ros::Publisher publisher = g_nh->advertise<gps_common::GPSFix>("/gpsfix", g_ROS_QUEUE_SIZE);
826  publisher.publish(*msg);
827  break;
828  }
830  {
831  geometry_msgs::PoseWithCovarianceStampedPtr msg = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>();
832  try
833  {
835  }
836  catch (std::runtime_error& e)
837  {
838  throw std::runtime_error(e.what());
839  }
840  msg->header.frame_id = g_frame_id;
841  uint32_t tow = *(reinterpret_cast<const uint32_t *>(data_ + 8));
842  ros::Time time_obj;
843  time_obj = timestampSBF(tow, g_use_gnss_time);
844  msg->header.stamp.sec = time_obj.sec;
845  msg->header.stamp.nsec = time_obj.nsec;
846  memcpy(&message, msg.get(), sizeof(*msg));
851  static ros::Publisher publisher = g_nh->advertise<geometry_msgs::PoseWithCovarianceStamped>("/pose", g_ROS_QUEUE_SIZE);
852  publisher.publish(*msg);
853  break;
854  }
855  case evChannelStatus:
856  {
857  memcpy(&last_channelstatus_, data_, sizeof(last_channelstatus_));
859  break;
860  }
861  case evMeasEpoch:
862  {
863  memcpy(&last_measepoch_, data_, sizeof(last_measepoch_));
865  break;
866  }
867  case evDOP:
868  {
869  memcpy(&last_dop_, data_, sizeof(last_dop_));
871  break;
872  }
873  case evVelCovGeodetic:
874  {
877  break;
878  }
879  case evDiagnosticArray:
880  {
881  diagnostic_msgs::DiagnosticArrayPtr msg = boost::make_shared<diagnostic_msgs::DiagnosticArray>();
882  try
883  {
884  msg = DiagnosticArrayCallback();
885  }
886  catch (std::runtime_error& e)
887  {
888  throw std::runtime_error(e.what());
889  }
890  msg->header.frame_id = g_frame_id;
891  uint32_t tow = *(reinterpret_cast<const uint32_t *>(data_ + 8));
892  ros::Time time_obj;
893  time_obj = timestampSBF(tow, g_use_gnss_time);
894  msg->header.stamp.sec = time_obj.sec;
895  msg->header.stamp.nsec = time_obj.nsec;
896  memcpy(&message, msg.get(), sizeof(*msg));
899  static ros::Publisher publisher = g_nh->advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", g_ROS_QUEUE_SIZE);
900  publisher.publish(*msg);
901  break;
902  }
903  case evReceiverStatus:
904  {
907  break;
908  }
909  case evQualityInd:
910  {
911  memcpy(&last_qualityind_, data_, sizeof(last_qualityind_));
913  break;
914  }
915  case evReceiverSetup:
916  {
917  memcpy(&last_receiversetup_, data_, sizeof(last_receiversetup_));
918  break;
919  }
920  // Many more to be implemented...
921  }
922  return true;
923  }
924 }
925 
926 #endif // for RX_MESSAGE_HPP
927 
928 
bool found()
Has an NMEA message, SBF block or command reply been found in the buffer?
Definition: rx_message.cpp:820
static PVTGeodetic last_pvtgeodetic_
Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks need to be stored.
Definition: rx_message.hpp:323
Struct for the SBF block "AttCovEuler".
Derived class for parsing GSA messages.
std::string g_frame_id
The frame ID used in the header of every published ROS message.
std::map< uint16_t, TypeOfPVT_Enum > TypeOfPVTMap
Shorthand for the map responsible for matching PVTGeodetic&#39;s Mode field to an enum value...
Definition: rx_message.hpp:376
sensor_msgs::NavSatFixPtr NavSatFixCallback()
"Callback" function when constructing NavSatFix messages
Definition: rx_message.cpp:430
std::size_t getCount()
Returns the count_ variable.
Definition: rx_message.hpp:241
static PosCovGeodetic last_poscovgeodetic_
Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic blocks need to be stored...
Definition: rx_message.hpp:328
RxMessage(const uint8_t *data, std::size_t &size)
Constructor of the RxMessage class.
Definition: rx_message.hpp:213
bool g_dop_has_arrived_gpsfix
For GPSFix: Whether the DOP block of the current epoch has arrived or not.
static DOP last_dop_
Since GPSFix needs DOP, incoming DOP blocks need to be stored.
Definition: rx_message.hpp:353
bool g_measepoch_has_arrived_gpsfix
For GPSFix: Whether the MeasEpoch block of the current epoch has arrived or not.
Derived class for parsing GSA messages.
Definition: gpgsa.hpp:82
static MeasEpoch last_measepoch_
Since GPSFix needs MeasEpoch (for SNRs), incoming MeasEpoch blocks need to be stored.
Definition: rx_message.hpp:348
Derived class for parsing GGA messages.
Definition: gpgga.hpp:83
bool isResponse()
Determines whether data_ currently points to an NMEA message.
Definition: rx_message.cpp:972
bool g_attcoveuler_has_arrived_pose
For PoseWithCovarianceStamped: Whether the AttCovEuler block of the current epoch has arrived or not...
bool g_poscovgeodetic_has_arrived_gpsfix
For GPSFix: Whether the PosCovGeodetic block of the current epoch has arrived or not.
Derived class for parsing GSV messages.
Struct for the SBF block "VelCovGeodetic".
bool g_pvtgeodetic_has_arrived_navsatfix
For NavSatFix: Whether the PVTGeodetic block of the current epoch has arrived or not.
Struct for the SBF block "ReceiverSetup".
gps_common::GPSFixPtr GPSFixCallback()
"Callback" function when constructing GPSFix messages
Definition: rx_message.cpp:519
Struct for the SBF block "PosCovGeodetic".
Declares the functions to compute and validate the CRC of a buffer.
std::string messageID()
septentrio_gnss_driver::AttCovEulerPtr AttCovEulerCallback(AttCovEuler &data)
Callback function when reading AttCovEuler blocks.
Definition: rx_message.cpp:252
bool g_atteuler_has_arrived_gpsfix
For GPSFix: Whether the AttEuler block of the current epoch has arrived or not.
static ChannelStatus last_channelstatus_
Since GPSFix needs ChannelStatus, incoming ChannelStatus blocks need to be stored.
Definition: rx_message.hpp:343
void next()
Goes to the start of the next message based on the calculated length of current message.
uint32_t g_cd_count
Since after SSSSSSSSSSS we need to wait for second connection descriptor, we have to count the connec...
bool g_receiverstatus_has_arrived_diagnostics
For DiagnosticArray: Whether the ReceiverStatus block of the current epoch has arrived or not...
Struct for the SBF block "PVTGeodetic".
static AttEuler last_atteuler_
Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to be stored.
Definition: rx_message.hpp:333
std::map< std::string, RxID_Enum > RxIDMap
Shorthand for the map responsible for matching ROS message identifiers to an enum value...
Definition: rx_message.hpp:384
Struct for the SBF block "AttEuler".
Can search buffer for messages, read/parse them, and so on.
Definition: rx_message.hpp:200
Derived class for parsing GGA messages.
bool g_attcoveuler_has_arrived_gpsfix
For GPSFix: Whether the AttCovEuler block of the current epoch has arrived or not.
bool isErrorMessage()
Determines whether data_ currently points to an error message reply from the Rx.
bool g_poscovgeodetic_has_arrived_navsatfix
For NavSatFix: Whether the PosCovGeodetic block of the current epoch has arrived or not...
std::size_t count_
Number of unread bytes in the buffer.
Definition: rx_message.hpp:303
septentrio_gnss_driver::GpgsaPtr parseASCII(const NMEASentence &sentence) noexcept(false) override
Parses one GSA message.
Definition: gpgsa.cpp:51
bool isNMEA()
Determines whether data_ currently points to an NMEA message.
Definition: rx_message.cpp:952
Struct for the SBF block "QualityInd".
static VelCovGeodetic last_velcovgeodetic_
Since GPSFix needs VelCovGeodetic, incoming VelCovGeodetic blocks need to be stored.
Definition: rx_message.hpp:358
bool isMessage(const uint16_t ID)
Determines whether data_ points to the SBF block with ID "ID", e.g. 5003.
Definition: rx_message.cpp:886
septentrio_gnss_driver::PosCovGeodeticPtr PosCovGeodeticCallback(PosCovGeodetic &data)
Callback function when reading PosCovGeodetic blocks.
Definition: rx_message.cpp:205
static TypeOfPVTMap type_of_pvt_map
All instances of the RxMessage class shall have access to the map without reinitializing it...
Definition: rx_message.hpp:381
static ReceiverStatus last_receiverstatus_
Since DiagnosticArray needs ReceiverStatus, incoming ReceiverStatus blocks need to be stored...
Definition: rx_message.hpp:363
Declares lower-level string utility functions used when parsing messages.
Struct for the SBF block "PVTCartesian".
uint16_t getBlockLength()
Gets the length of the SBF block.
Derived class for parsing RMC messages.
ros::Time timestampSBF(uint32_t tow, bool use_gnss)
Calculates the timestamp, in the Unix Epoch time format This is either done using the TOW as transmit...
Definition: rx_message.cpp:768
uint32_t g_leap_seconds
The number of leap seconds that have been inserted into the UTC time.
Struct for the SBF block "PosCovCartesian".
septentrio_gnss_driver::PVTCartesianPtr PVTCartesianCallback(PVTCartesian &data)
Callback function when reading PVTCartesian blocks.
Definition: rx_message.cpp:140
RxID_Enum
Definition: rx_message.hpp:178
std::size_t messageSize()
Determines size of the message (also command reply) that data_ is currently pointing at...
Definition: rx_message.cpp:852
boost::shared_ptr< ros::NodeHandle > g_nh
bool g_atteuler_has_arrived_pose
For PoseWithCovarianceStamped: Whether the AttEuler block of the current epoch has arrived or not...
const uint8_t * getEndBuffer()
Gets the end position in the read buffer.
bool g_use_gnss_time
bool g_channelstatus_has_arrived_gpsfix
For GPSFix: Whether the ChannelStatus block of the current epoch has arrived or not.
uint32_t tow
static uint32_t count_gpsfix_
Number of times the gps_common::GPSFix message has been published.
Definition: rx_message.hpp:318
Struct for the SBF block "ChannelStatus".
TypeOfPVT_Enum
Enum for NavSatFix&#39;s status.status field, which is obtained from PVTGeodetic&#39;s Mode field...
Definition: rx_message.hpp:173
bool g_velcovgeodetic_has_arrived_gpsfix
For GPSFix: Whether the VelCovGeodetic block of the current epoch has arrived or not.
Struct for the SBF block "MeasEpoch".
diagnostic_msgs::DiagnosticArrayPtr DiagnosticArrayCallback()
"Callback" function when constructing diagnostic_msgs::DiagnosticArray messages
Definition: rx_message.cpp:332
Struct for the SBF block "DOP".
bool g_pvtgeodetic_has_arrived_pose
For PoseWithCovarianceStamped: Whether the PVTGeodetic block of the current epoch has arrived or not...
bool g_poscovgeodetic_has_arrived_pose
For PoseWithCovarianceStamped: Whether the PosCovGeodetic block of the current epoch has arrived or n...
Derived class for parsing RMC messages.
Definition: gprmc.hpp:83
geometry_msgs::PoseWithCovarianceStampedPtr PoseWithCovarianceStampedCallback()
"Callback" function when constructing PoseWithCovarianceStamped messages
Definition: rx_message.cpp:282
Struct to split an NMEA sentence into its ID and its body, the latter tokenized into a vector of stri...
Class to declare error message format when parsing, derived from the public class "std::runtime_error...
static ReceiverSetup last_receiversetup_
Since DiagnosticArray needs ReceiverSetup, incoming ReceiverSetup blocks need to be stored...
Definition: rx_message.hpp:373
bool isSBF()
Determines whether data_ currently points to an SBF block.
Definition: rx_message.cpp:933
const uint32_t g_ROS_QUEUE_SIZE
Queue size for ROS publishers.
septentrio_gnss_driver::PosCovCartesianPtr PosCovCartesianCallback(PosCovCartesian &data)
Callback function when reading PosCovCartesian blocks.
Definition: rx_message.cpp:179
bool found_
Whether or not a message has been found.
Definition: rx_message.hpp:291
Derived class for parsing GSV messages.
Definition: gpgsv.hpp:82
const uint8_t * getPosBuffer()
Gets the current position in the read buffer.
std::size_t message_size_
Helps to determine size of response message / NMEA message / SBF block.
Definition: rx_message.hpp:313
const uint8_t * search()
Searches the buffer for the beginning of the next message (NMEA or SBF)
Definition: rx_message.cpp:834
bool g_pvtgeodetic_has_arrived_gpsfix
For GPSFix: Whether the PVTGeodetic block of the current epoch has arrived or not.
bool read(typename boost::call_traits< T >::reference message, std::string message_key, bool search=false)
Performs the CRC check (if SBF) and populates ROS message "message" with the necessary content...
Definition: rx_message.hpp:477
septentrio_gnss_driver::GprmcPtr parseASCII(const NMEASentence &sentence) noexcept(false) override
Parses one RMC message.
Definition: gprmc.cpp:54
septentrio_gnss_driver::AttEulerPtr AttEulerCallback(AttEuler &data)
Callback function when reading AttEuler blocks.
Definition: rx_message.cpp:230
static QualityInd last_qualityind_
Since DiagnosticArray needs QualityInd, incoming QualityInd blocks need to be stored.
Definition: rx_message.hpp:368
const uint8_t * data_
Pointer to the buffer of messages.
Definition: rx_message.hpp:298
static RxIDMap rx_id_map
All instances of the RxMessage class shall have access to the map without reinitializing it...
Definition: rx_message.hpp:392
static AttCovEuler last_attcoveuler_
Since GPSFix etc. need AttCovEuler, incoming AttCovEuler blocks need to be stored.
Definition: rx_message.hpp:338
bool g_qualityind_has_arrived_diagnostics
For DiagnosticArray: Whether the QualityInd block of the current epoch has arrived or not...
bool g_read_cd
bool isValid(const void *block)
Validates whether the calculated CRC of the SBF block at hand matches the CRC field of the streamed S...
Definition: crc.c:68
septentrio_gnss_driver::PVTGeodeticPtr PVTGeodeticCallback(PVTGeodetic &data)
Callback function when reading PVTGeodetic blocks.
Definition: rx_message.cpp:100
bool crc_check_
Whether the CRC check as evaluated in the read() method was successful or not is stored here...
Definition: rx_message.hpp:308
septentrio_gnss_driver::GpggaPtr parseASCII(const NMEASentence &sentence) noexcept(false) override
Parses one GGA message.
Definition: gpgga.cpp:51
Struct for the SBF block "ReceiverStatus".
septentrio_gnss_driver::GpgsvPtr parseASCII(const NMEASentence &sentence) noexcept(false) override
Parses one GSV message.
Definition: gpgsv.cpp:52