ROSaic
mosaicMessage.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 <cmath>
114 #include <vector>
115 // Boost includes
116 #include <boost/tokenizer.hpp>
117 #include <boost/call_traits.hpp>
118 #include <boost/format.hpp>
119 #include <boost/math/constants/constants.hpp>
120 // ROS includes
121 #include <ros/ros.h>
122 #include <rosaic/Gpgga.h>
123 #include <rosaic/Gprmc.h>
124 #include <rosaic/Gpgsa.h>
125 #include <rosaic/Gpgsv.h>
126 #include <sensor_msgs/NavSatFix.h>
127 #include <sensor_msgs/TimeReference.h>
128 #include <geometry_msgs/PoseWithCovarianceStamped.h>
129 #include <gps_common/GPSFix.h>
130 // ROSaic includes
135 #include <rosaic/crc/crc.h>
138 #include <rosaic/PVTCartesian.h>
139 #include <rosaic/PVTGeodetic.h>
140 #include <rosaic/PosCovCartesian.h>
141 #include <rosaic/PosCovGeodetic.h>
142 #include <rosaic/AttEuler.h>
143 #include <rosaic/AttCovEuler.h>
145 
146 
147 #ifndef MOSAIC_MESSAGE_HPP
148 #define MOSAIC_MESSAGE_HPP
149 
156 extern bool use_GNSS_time;
157 extern bool read_cd;
158 extern uint32_t cd_count;
159 extern uint32_t leap_seconds;
160 extern std::vector<int32_t> svid_pvt;
166 static std::map<std::string, NMEA_ID_Enum> StringValues;
168 {
169  StringValues.insert(std::make_pair("NavSatFix", evNavSatFix));
170  StringValues.insert(std::make_pair("GPSFix", evGPSFix));
171  StringValues.insert(std::make_pair("PoseWithCovarianceStamped", evPoseWithCovarianceStamped));
172  StringValues.insert(std::make_pair("$GPGGA", evGPGGA));
173  StringValues.insert(std::make_pair("$GPRMC", evGPRMC));
174  StringValues.insert(std::make_pair("$GPGSA", evGPGSA));
175  StringValues.insert(std::make_pair("$GPGSV", evGPGSV));
176  StringValues.insert(std::make_pair("$GLGSV", evGLGSV));
177  StringValues.insert(std::make_pair("$GAGSV", evGAGSV));
178  StringValues.insert(std::make_pair("4006", evPVTCartesian));
179  StringValues.insert(std::make_pair("4007", evPVTGeodetic));
180  StringValues.insert(std::make_pair("5905", evPosCovCartesian));
181  StringValues.insert(std::make_pair("5906", evPosCovGeodetic));
182  StringValues.insert(std::make_pair("5938", evAttEuler));
183  StringValues.insert(std::make_pair("5939", evAttCovEuler));
184  StringValues.insert(std::make_pair("GPST", evGPST));
185  StringValues.insert(std::make_pair("4013", evChannelStatus));
186  StringValues.insert(std::make_pair("4027", evMeasEpoch));
187  StringValues.insert(std::make_pair("4001", evDOP));
188  StringValues.insert(std::make_pair("5908", evVelCovGeodetic));
189 }
190 
191 namespace io_comm_mosaic
192 {
199  ros::Time TimestampSBF(uint32_t TOW, bool use_GNSS);
200 
206  {
207  public:
216  mosaicMessage(const uint8_t* data, std::size_t& size): data_(data), count_(size) {found_ = false; CRCcheck_ = false; segment_size_ = 0;}
217 
219  bool IsMessage(const uint16_t ID);
221  bool IsMessage(std::string ID);
223  bool IsSBF();
225  bool IsNMEA();
227  bool IsResponse();
229  bool IsConnectionDescriptor();
231  bool IsErrorMessage();
233  std::size_t SegmentEnd();
235  std::string MessageID();
236 
241  uint32_t GetCount() {return count_;};
246  const uint8_t* Search();
247 
254  uint16_t BlockLength();
255 
260  const uint8_t* Pos();
261 
266  const uint8_t* End();
267 
272  bool Found();
273 
277  const uint8_t* Next();
278 
283  template <typename T>
284  bool Read(typename boost::call_traits<T>::reference message, std::string message_key, bool search = false);
285 
289  bool found_;
290 
291  private:
292 
296  const uint8_t* data_;
297 
301  uint32_t count_;
302 
306  bool CRCcheck_;
307 
311  std::size_t segment_size_;
312 
316  static uint32_t count_gpsfix_;
317 
322 
327 
332 
337 
342 
347 
351  static DOP last_dop_;
352 
357 
363  rosaic::PVTCartesianPtr PVTCartesianCallback(PVTCartesian& data);
364 
370  rosaic::PVTGeodeticPtr PVTGeodeticCallback(PVTGeodetic& data);
371 
377  rosaic::PosCovCartesianPtr PosCovCartesianCallback(PosCovCartesian& data);
378 
384  rosaic::PosCovGeodeticPtr PosCovGeodeticCallback(PosCovGeodetic& data);
385 
391  rosaic::AttEulerPtr AttEulerCallback(AttEuler& data);
392 
398  rosaic::AttCovEulerPtr AttCovEulerCallback(AttCovEuler& data);
399 
404  sensor_msgs::NavSatFixPtr NavSatFixCallback();
405 
410  gps_common::GPSFixPtr GPSFixCallback();
411 
417  geometry_msgs::PoseWithCovarianceStampedPtr PoseWithCovarianceStampedCallback();
418 
419  };
420 
421 
431  template <typename T>
432  bool mosaicMessage::Read(typename boost::call_traits<T>::reference message, std::string message_key, bool search)
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  }
795 }
796 
797 #endif // for MOSAIC_MESSAGE_HPP
798 
799 
static PosCovGeodetic last_poscovgeodetic_
Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic blocks need to be stored...
Struct for the SBF block "AttCovEuler".
Derived class for parsing GSA messages.
const uint8_t * Next()
Goes to the start of the next message based on the calculated length of current message.
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.
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)
mosaicMessage(const uint8_t *data, std::size_t &size)
Constructor of the mosaicMessage class.
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
static void StringValues_Initialize()
Derived class for parsing GGA messages.
Definition: gpgga.hpp:85
const uint8_t * End()
Gets the end position in the read buffer.
Derived class for parsing GSV messages.
uint16_t BlockLength()
Gets the length of the SBF block.
Struct for the SBF block "VelCovGeodetic".
rosaic::PVTGeodeticPtr PVTGeodeticCallback(PVTGeodetic &data)
Callback function when reading PVTGeodetic blocks.
bool IsErrorMessage()
Determines whether data_ currently points to an error message reply from mosaic.
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...
NMEA_ID_Enum
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...
Struct for the SBF block "PosCovGeodetic".
Declares the functions to compute and validate the CRC of a buffer.
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.
std::vector< int32_t > svid_pvt
uint32_t leap_seconds
The number of leap seconds that have been inserted into the UTC time.
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?
uint32_t GetCount()
Returns the count_ variable.
bool IsResponse()
Determines whether data_ currently points to an NMEA message.
Struct for the SBF block "PVTGeodetic".
Declares and defines structs into which SBF blocks are unpacked then shipped to handler functions...
static AttEuler last_atteuler_
Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to be stored.
Struct for the SBF block "AttEuler".
const uint8_t * Pos()
Gets the current position in the read buffer.
Derived class for parsing GGA messages.
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...
Declares lower-level string utility functions used when parsing messages.
uint32_t cd_count
Since after SSSSSSSSSSS we need to wait for second connection descriptor, we have to count the connec...
Struct for the SBF block "PVTCartesian".
Derived class for parsing RMC messages.
bool IsNMEA()
Determines whether data_ currently points to an NMEA message.
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 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...
bool IsSBF()
Determines whether data_ currently points to an SBF block.
Struct for the SBF block "ChannelStatus".
const uint8_t * data_
The pointer to the buffer of messages.
Struct for the SBF block "MeasEpoch".
std::string frame_id
The frame ID used in the header of every published ROS message.
bool IsConnectionDescriptor()
Determines whether data_ currently points to a connection descriptor (right after initiating TCP conn...
Struct for the SBF block "DOP".
Can search buffer for messages, read/parse them, and so on.
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
bool read_cd
Whether or not we still want to read the connection descriptor, which we only want in the very beginn...
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.
Defines a struct NMEASentence, into which NMEA sentences - both standardized and proprietary ones - s...
rosaic::GprmcPtr ParseASCII(const NMEASentence &sentence) noexcept(false) override
Parses one RMC message.
Definition: gprmc.cpp:54
rosaic::GpgsvPtr ParseASCII(const NMEASentence &sentence) noexcept(false) override
Parses one GSV message.
Definition: gpgsv.cpp:51
bool IsMessage(const uint16_t ID)
Determines whether data_ points to the SBF block with ID "ID", e.g. 5003.