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