56 const size_t MIN_LENGTH = 4;
58 if (sentence.get_body().size() < MIN_LENGTH)
60 std::stringstream error;
61 error <<
"Expected GSV length is at least " << MIN_LENGTH
62 <<
". The actual length is " << sentence.get_body().size();
65 rosaic::GpgsvPtr msg = boost::make_shared<rosaic::Gpgsv>();
67 msg->message_id = sentence.get_body()[0];
74 std::stringstream error;
75 error <<
"n_msgs in GSV is too large: " << msg->n_msgs <<
".";
83 if (msg->msg_number > msg->n_msgs)
85 std::stringstream error;
86 error <<
"msg_number in GSV is larger than n_msgs: " << msg->msg_number <<
" > " << msg->n_msgs <<
".";
94 size_t n_sats_in_sentence = 4;
95 if (msg->msg_number == msg->n_msgs)
97 n_sats_in_sentence = msg->n_satellites %
static_cast<uint8_t
>(4);
98 if (msg->n_satellites % static_cast<uint8_t>(4) == 0)
100 n_sats_in_sentence = 4;
102 if (msg->n_satellites == 0)
104 n_sats_in_sentence = 0;
106 if (msg->msg_number == 1)
108 n_sats_in_sentence = msg->n_satellites;
112 size_t expected_length = MIN_LENGTH + 4 * n_sats_in_sentence + 1;
114 if (n_sats_in_sentence == 0)
118 expected_length += 4;
121 if (sentence.get_body().size() != expected_length && sentence.get_body().size() != expected_length - 1)
123 std::stringstream ss;
124 for (
size_t i = 0; i < sentence.get_body().size(); ++i)
126 ss << sentence.get_body()[i];
127 if ((i+1) < sentence.get_body().size())
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();
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)
145 std::stringstream error;
146 error <<
"Error parsing PRN for satellite " << sat <<
" in GSV.";
152 std::stringstream error;
153 error <<
"Error parsing elevation for satellite " << sat <<
" in GSV.";
156 msg->satellites[sat].elevation =
static_cast<uint8_t
>(elevation);
161 std::stringstream error;
162 error <<
"Error parsing azimuth for satellite " << sat <<
" in GSV.";
165 msg->satellites[sat].azimuth =
static_cast<uint16_t
>(azimuth);
167 if ((index + 3) >= sentence.get_body().size() || sentence.get_body()[index + 3].empty())
169 msg->satellites[sat].snr = -1;
176 std::stringstream error;
177 error <<
"Error parsing snr for satellite " << sat <<
" in GSV.";
180 msg->satellites[sat].snr =
static_cast<int8_t
>(snr);
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.
const std::string getMessageID() const override
Returns the ASCII message ID, here "$GPGSV".
rosaic::GpgsvPtr parseASCII(const NMEASentence &sentence) noexcept(false) override
Parses one GSV message.
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. ...