gprmc.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 GprmcParser::MESSAGE_ID = "$GPRMC";
40 
41 const std::string GprmcParser::getMessageID() const
42 {
44 }
45 
58  const std::string& frame_id, bool use_gnss_time,
59  Timestamp time_obj) noexcept(false)
60 {
61 
62  // Checking the length first, it should be between 13 and 14 elements
63  const size_t LEN_MIN = 13;
64  const size_t LEN_MAX = 14;
65 
66  if (sentence.get_body().size() > LEN_MAX || sentence.get_body().size() < LEN_MIN)
67  {
68  std::stringstream error;
69  error << "Expected GPRMC length is between " << LEN_MIN << " and " << LEN_MAX
70  << ". The actual length is " << sentence.get_body().size();
71  throw ParseException(error.str());
72  }
73 
74  GprmcMsg msg;
75 
76  msg.header.frame_id = frame_id;
77 
78  msg.message_id = sentence.get_body()[0];
79 
80  if (sentence.get_body()[1].empty() || sentence.get_body()[1] == "0")
81  {
82  msg.utc_seconds = 0;
83  } else
84  {
85  double utc_double;
86  if (string_utilities::toDouble(sentence.get_body()[1], utc_double))
87  {
88  msg.utc_seconds =
90  if (use_gnss_time)
91  {
92  // The Header's Unix Epoch time stamp
93  time_t unix_time_seconds =
95  // The following assumes that there are two digits after the decimal
96  // point in utc_double, i.e. in the NMEA UTC time.
97  Timestamp unix_time_nanoseconds =
98  unix_time_seconds * 1000000000 +
99  (static_cast<Timestamp>(utc_double * 100) % 100) * 10000;
100  msg.header.stamp = timestampToRos(unix_time_nanoseconds);
101  } else
102  {
103  msg.header.stamp = timestampToRos(time_obj);
104  }
105  } else
106  {
107  throw ParseException(
108  "Error parsing UTC seconds in GPRMC"); // E.g. if one of the fields
109  // of the NMEA UTC string is
110  // empty
111  }
112  }
113  bool valid = true;
114  bool to_be_ignored = false;
115 
116  msg.position_status = sentence.get_body()[2];
117  // Check to see whether this message should be ignored
118  to_be_ignored &= !(sentence.get_body()[2].compare("A") ==
119  0); // 0 : if both strings are equal.
120  to_be_ignored &=
121  (sentence.get_body()[3].empty() || sentence.get_body()[5].empty());
122 
123  double latitude = 0.0;
124  valid =
125  valid && parsing_utilities::parseDouble(sentence.get_body()[3], latitude);
126  msg.lat = parsing_utilities::convertDMSToDegrees(latitude);
127 
128  double longitude = 0.0;
129  valid =
130  valid && parsing_utilities::parseDouble(sentence.get_body()[5], longitude);
131  msg.lon = parsing_utilities::convertDMSToDegrees(longitude);
132 
133  msg.lat_dir = sentence.get_body()[4];
134  msg.lon_dir = sentence.get_body()[6];
135 
136  valid =
137  valid && parsing_utilities::parseFloat(sentence.get_body()[7], msg.speed);
138  msg.speed *= KNOTS_TO_MPS;
139 
140  valid =
141  valid && parsing_utilities::parseFloat(sentence.get_body()[8], msg.track);
142 
143  std::string date_str = sentence.get_body()[9];
144  if (!date_str.empty())
145  {
146  msg.date = std::string("20") + date_str.substr(4, 2) + std::string("-") +
147  date_str.substr(2, 2) + std::string("-") + date_str.substr(0, 2);
148  }
149  valid =
150  valid && parsing_utilities::parseFloat(sentence.get_body()[10], msg.mag_var);
151  msg.mag_var_direction = sentence.get_body()[11];
152  if (sentence.get_body().size() == LEN_MAX)
153  {
154  msg.mode_indicator = sentence.get_body()[12];
155  }
156 
157  if (!valid)
158  {
159  was_last_gprmc_valid_ = false;
160  throw ParseException("Error parsing GPRMC message.");
161  }
162 
163  was_last_gprmc_valid_ = !to_be_ignored;
164 
165  return msg;
166 }
167 
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...
uint64_t Timestamp
Definition: typedefs.hpp:88
nmea_msgs::Gprmc GprmcMsg
Definition: typedefs.hpp:130
static constexpr double KNOTS_TO_MPS
Definition: gprmc.hpp:118
static const std::string MESSAGE_ID
Declares the string MESSAGE_ID.
Definition: gprmc.hpp:113
bool wasLastGPRMCValid() const
Tells us whether the last RMC message was valid/usable or not.
Definition: gprmc.cpp:168
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)...
Derived class for parsing RMC messages.
const std::string getMessageID() const override
Returns the ASCII message ID, here "$GPRMC".
Definition: gprmc.cpp:41
std::string frame_id
TimestampRos timestampToRos(Timestamp ts)
Convert nsec timestamp to ROS timestamp.
Definition: typedefs.hpp:144
GprmcMsg parseASCII(const NMEASentence &sentence, const std::string &frame_id, bool use_gnss_time, Timestamp time_obj) noexcept(false) override
Parses one RMC message.
Definition: gprmc.cpp:57
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_gprmc_valid_
Declares a boolean representing whether or not the last GPRMC message was valid.
Definition: gprmc.hpp:125


septentrio_gnss_driver
Author(s): Tibor Dome
autogenerated on Sat Mar 11 2023 03:12:55