ROSaic
gpgga.cpp
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 
32 
39 const std::string GpggaParser::MESSAGE_ID = "$GPGGA";
40 
41 const std::string GpggaParser::getMessageID() const
42 {
44 }
45 
51 rosaic::GpggaPtr GpggaParser::parseASCII(const NMEASentence& sentence) noexcept(false)
52 {
53  //ROS_DEBUG("Just testing that first entry is indeed what we expect it to be: %s", sentence.get_body()[0].c_str());
54  // Check the length first, which should be 16 elements.
55  const size_t LEN = 16;
56  if (sentence.get_body().size() > LEN || sentence.get_body().size() < LEN)
57  {
58  std::stringstream error;
59  error << "GGA parsing failed: Expected GPGGA length is " << LEN << ", but actual length is " << sentence.get_body().size();
60  throw ParseException(error.str());
61  }
62 
63  rosaic::GpggaPtr msg = boost::make_shared<rosaic::Gpgga>();
64  msg->header.frame_id = g_frame_id;
65 
66  msg->message_id = sentence.get_body()[0];
67 
68  if (sentence.get_body()[1].empty() || sentence.get_body()[1] == "0")
69  {
70  msg->utc_seconds = 0;
71  }
72  else
73  {
74  double utc_double;
75  if (string_utilities::toDouble(sentence.get_body()[1], utc_double))
76  {
77  if(g_use_gnss_time)
78  {
79  //ROS_DEBUG("utc_double is %f", (float) utc_double);
80  msg->utc_seconds = parsing_utilities::convertUTCDoubleToSeconds(utc_double);
81 
82  // The Header's Unix Epoch time stamp
83  time_t unix_time_seconds = parsing_utilities::convertUTCtoUnix(utc_double);
84  // The following assumes that there are two digits after the decimal point in utc_double, i.e. in the NMEA UTC time.
85  uint32_t unix_time_nanoseconds = (static_cast<uint32_t>(utc_double*100)%100)*10000;
86  msg->header.stamp.sec = unix_time_seconds;
87  msg->header.stamp.nsec = unix_time_nanoseconds;
88  }
89  else
90  {
91  ros::Time time_obj;
92  time_obj = ros::Time::now();
93  msg->header.stamp.sec = time_obj.sec;
94  msg->header.stamp.nsec = time_obj.nsec;
95  }
96  }
97  else
98  {
99  throw ParseException("Error parsing UTC seconds in GPGGA"); // E.g. if one of the fields of the NMEA UTC string is empty
100  }
101  }
102 
103  bool valid = true;
104 
105  double latitude = 0.0;
106  valid = valid && parsing_utilities::parseDouble(sentence.get_body()[2], latitude);
107  msg->lat = parsing_utilities::convertDMSToDegrees(latitude);
108 
109  double longitude = 0.0;
110  valid = valid && parsing_utilities::parseDouble(sentence.get_body()[4], longitude);
111  msg->lon = parsing_utilities::convertDMSToDegrees(longitude);
112 
113  msg->lat_dir = sentence.get_body()[3];
114  msg->lon_dir = sentence.get_body()[5];
115  valid = valid && parsing_utilities::parseUInt32(sentence.get_body()[6], msg->gps_qual);
116  valid = valid && parsing_utilities::parseUInt32(sentence.get_body()[7], msg->num_sats);
117  //ROS_INFO("Valid is %s so far with number of satellites in use being %s", valid ? "true" : "false", sentence.get_body()[7].c_str());
118 
119  valid = valid && parsing_utilities::parseFloat(sentence.get_body()[8], msg->hdop);
120  valid = valid && parsing_utilities::parseFloat(sentence.get_body()[9], msg->alt);
121  msg->altitude_units = sentence.get_body()[10];
122  valid = valid && parsing_utilities::parseFloat(sentence.get_body()[11], msg->undulation);
123  msg->undulation_units = sentence.get_body()[12];
124  double diff_age_temp;
125  valid = valid && parsing_utilities::parseDouble(sentence.get_body()[13], diff_age_temp);
126  msg->diff_age = static_cast<uint32_t>(round(diff_age_temp));
127  msg->station_id = sentence.get_body()[14];
128 
129  if (!valid)
130  {
131  was_last_gpgga_valid_ = false;
132  throw ParseException("GPGGA message was invalid.");
133  }
134 
135  // If we made it this far, we successfully parsed the message and will consider it to be valid.
136  was_last_gpgga_valid_ = true;
137 
138  return msg;
139 }
140 
142 {
143  return was_last_gpgga_valid_;
144 }
std::string g_frame_id
The frame ID used in the header of every published ROS message.
rosaic::GpggaPtr parseASCII(const NMEASentence &sentence) noexcept(false) override
Parses one GGA message.
Definition: gpgga.cpp:51
bool toDouble(const std::string &string, double &value)
Interprets the contents of "string" as a floating point number of type double It stores the "string"&#39;...
float parseFloat(const uint8_t *buffer)
Converts a 4-byte-buffer into a float.
static const std::string MESSAGE_ID
Declares the string MESSAGE_ID.
Definition: gpgga.hpp:114
double convertUTCDoubleToSeconds(double utc_double)
Converts UTC time from the without-colon-delimiter format to the number-of-seconds-since-midnight for...
Derived class for parsing GGA messages.
bool wasLastGPGGAValid() const
Tells us whether the last GGA message was valid or not.
Definition: gpgga.cpp:141
uint32_t parseUInt32(const uint8_t *buffer)
Converts a 4-byte-buffer into an unsigned 32-bit integer.
double convertDMSToDegrees(double dms)
Converts latitude or longitude from the DMS notation (in the without-colon-delimiter format)...
time_t convertUTCtoUnix(double utc_double)
Converts UTC time from the without-colon-delimiter format to Unix Epoch time (a number-of-seconds-sin...
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...
bool g_use_gnss_time
const std::string getMessageID() const override
Returns the ASCII message ID, here "$GPGGA".
Definition: gpgga.cpp:41
double parseDouble(const uint8_t *buffer)
Converts an 8-byte-buffer into a double.
bool was_last_gpgga_valid_
Declares a boolean representing whether or not the last GPGGA message was valid.
Definition: gpgga.hpp:120