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


septentrio_gnss_driver
Author(s): Tibor Dome
autogenerated on Wed Nov 22 2023 04:04:27