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


septentrio_gnss_driver
Author(s): Tibor Dome
autogenerated on Thu Jun 23 2022 02:11:38