ROSaic
gpgsv.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 GpgsvParser::MESSAGE_ID = "$GPGSV";
40 
41 const std::string GpgsvParser::getMessageID() const
42 {
44 }
45 
52 septentrio_gnss_driver::GpgsvPtr GpgsvParser::parseASCII(const NMEASentence& sentence) noexcept(false)
53 {
54 
55 
56  const size_t MIN_LENGTH = 4;
57  // Checking that the message is at least as long as a GPGSV with no satellites
58  if (sentence.get_body().size() < MIN_LENGTH)
59  {
60  std::stringstream error;
61  error << "Expected GSV length is at least " << MIN_LENGTH
62  << ". The actual length is " << sentence.get_body().size();
63  throw ParseException(error.str());
64  }
65  septentrio_gnss_driver::GpgsvPtr msg = boost::make_shared<septentrio_gnss_driver::Gpgsv>();
66  msg->header.frame_id = g_frame_id;
67  msg->message_id = sentence.get_body()[0];
68  if (!parsing_utilities::parseUInt8(sentence.get_body()[1], msg->n_msgs))
69  {
70  throw ParseException("Error parsing n_msgs in GSV.");
71  }
72  if (msg->n_msgs > 9) // Checking that the number of messages is smaller or equal to 9
73  {
74  std::stringstream error;
75  error << "n_msgs in GSV is too large: " << msg->n_msgs << ".";
76  throw ParseException(error.str());
77  }
78 
79  if (!parsing_utilities::parseUInt8(sentence.get_body()[2], msg->msg_number))
80  {
81  throw ParseException("Error parsing msg_number in GSV.");
82  }
83  if (msg->msg_number > msg->n_msgs) // Checking that this message is within the sequence range
84  {
85  std::stringstream error;
86  error << "msg_number in GSV is larger than n_msgs: " << msg->msg_number << " > " << msg->n_msgs << ".";
87  throw ParseException(error.str());
88  }
89  if (!parsing_utilities::parseUInt8(sentence.get_body()[3], msg->n_satellites))
90  {
91  throw ParseException("Error parsing n_satellites in GSV.");
92  }
93  // Figuring out how many satellites should be described in this sentence
94  size_t n_sats_in_sentence = 4;
95  if (msg->msg_number == msg->n_msgs)
96  {
97  n_sats_in_sentence = msg->n_satellites % static_cast<uint8_t>(4);
98  if (msg->n_satellites % static_cast<uint8_t>(4) == 0)
99  {
100  n_sats_in_sentence = 4;
101  }
102  if (msg->n_satellites == 0)
103  {
104  n_sats_in_sentence = 0;
105  }
106  if (msg->msg_number == 1)
107  {
108  n_sats_in_sentence = msg->n_satellites;
109  }
110  }
111  // Checking that the sentence is the right length for the number of satellites
112  size_t expected_length = MIN_LENGTH + 4 * n_sats_in_sentence + 1;
113  // Note that we add +1 due to the checksum data being part of the argument "sentence".
114  if (n_sats_in_sentence == 0)
115  {
116  // Even if the number of sats is 0, the message will still have enough
117  // blank fields for 1 satellite.
118  expected_length += 4;
119  }
120  //ROS_DEBUG("number of sats is %u but nsats in sentence if msg_number = max is %u and msg->msg_number == msg->n_msgs is %s and nsats in sentence is %li", msg->n_satellites, msg->n_satellites % static_cast<uint8_t>(4), msg->msg_number == msg->n_msgs ? "true" : "false", n_sats_in_sentence);
121  if (sentence.get_body().size() != expected_length && sentence.get_body().size() != expected_length - 1)
122  {
123  std::stringstream ss;
124  for (size_t i = 0; i < sentence.get_body().size(); ++i)
125  {
126  ss << sentence.get_body()[i];
127  if ((i+1) < sentence.get_body().size())
128  {
129  ss << ",";
130  }
131  }
132  std::stringstream error;
133  error << "Expected GSV length is " << expected_length << " for message with "
134  << n_sats_in_sentence << " satellites. The actual length is "
135  << sentence.get_body().size() << ".\n" << ss.str().c_str();
136  throw ParseException(error.str());
137  }
138 
139  // Parsing information about n_sats_in_sentence SVs..
140  msg->satellites.resize(n_sats_in_sentence);
141  for (size_t sat = 0, index=MIN_LENGTH; sat < n_sats_in_sentence; ++sat, index += 4)
142  {
143  if (!parsing_utilities::parseUInt8(sentence.get_body()[index], msg->satellites[sat].prn))
144  {
145  std::stringstream error;
146  error << "Error parsing PRN for satellite " << sat << " in GSV.";
147  throw ParseException(error.str());
148  }
149  float elevation;
150  if (!parsing_utilities::parseFloat(sentence.get_body()[index + 1], elevation))
151  {
152  std::stringstream error;
153  error << "Error parsing elevation for satellite " << sat << " in GSV.";
154  throw ParseException(error.str());
155  }
156  msg->satellites[sat].elevation = static_cast<uint8_t>(elevation);
157 
158  float azimuth;
159  if (!parsing_utilities::parseFloat(sentence.get_body()[index + 2], azimuth))
160  {
161  std::stringstream error;
162  error << "Error parsing azimuth for satellite " << sat << " in GSV.";
163  throw ParseException(error.str());
164  }
165  msg->satellites[sat].azimuth = static_cast<uint16_t>(azimuth);
166 
167  if ((index + 3) >= sentence.get_body().size() || sentence.get_body()[index + 3].empty())
168  {
169  msg->satellites[sat].snr = -1;
170  }
171  else
172  {
173  uint8_t snr;
174  if (!parsing_utilities::parseUInt8(sentence.get_body()[index + 3], snr))
175  {
176  std::stringstream error;
177  error << "Error parsing snr for satellite " << sat << " in GSV.";
178  throw ParseException(error.str());
179  }
180  msg->satellites[sat].snr = static_cast<int8_t>(snr);
181  }
182  }
183  return msg;
184 }
std::string g_frame_id
The frame ID used in the header of every published ROS message.
float parseFloat(const uint8_t *buffer)
Converts a 4-byte-buffer into a float.
Derived class for parsing GSV messages.
static const std::string MESSAGE_ID
Declares the string MESSAGE_ID.
Definition: gpgsv.hpp:107
const std::string getMessageID() const override
Returns the ASCII message ID, here "$GPGSV".
Definition: gpgsv.cpp:41
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 parseUInt8(const std::string &string, uint8_t &value, int32_t base)
Interprets the contents of "string" as a unsigned integer number of type uint8_t. ...
septentrio_gnss_driver::GpgsvPtr parseASCII(const NMEASentence &sentence) noexcept(false) override
Parses one GSV message.
Definition: gpgsv.cpp:52