parsing_utilities.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 
31 // ROSaic includes
34 // C++ library includes
35 #include <limits>
36 // Boost
37 #include <boost/spirit/include/qi_binary.hpp>
38 
45 namespace parsing_utilities {
46 
47  namespace qi = boost::spirit::qi;
48 
49  double wrapAngle180to180(double angle)
50  {
51  while (angle > 180.0)
52  {
53  angle -= 360.0;
54  }
55  while (angle < -180.0)
56  {
57  angle += 360.0;
58  }
59  return angle;
60  }
61 
62  double parseDouble(const uint8_t* buffer)
63  {
64  double val;
65  qi::parse(buffer, buffer + 8, qi::little_bin_double, val);
66  return val;
67  }
68 
74  bool parseDouble(const std::string& string, double& value)
75  {
76  return string_utilities::toDouble(string, value) || string.empty();
77  }
78 
79  float parseFloat(const uint8_t* buffer)
80  {
81  float val;
82  qi::parse(buffer, buffer + 4, qi::little_bin_float, val);
83  return val;
84  }
85 
91  bool parseFloat(const std::string& string, float& value)
92  {
93  return string_utilities::toFloat(string, value) || string.empty();
94  }
95 
103  int16_t parseInt16(const uint8_t* buffer)
104  {
105  int16_t val;
106  qi::parse(buffer, buffer + 2, qi::little_word, val);
107  return val;
108  }
109 
115  bool parseInt16(const std::string& string, int16_t& value, int32_t base)
116  {
117  value = 0;
118  if (string.empty())
119  {
120  return true;
121  }
122 
123  int32_t intermd;
124  if (string_utilities::toInt32(string, intermd, base) &&
125  intermd <= std::numeric_limits<int16_t>::max() &&
126  intermd >= std::numeric_limits<int16_t>::min())
127  {
128  value = static_cast<int16_t>(intermd);
129  return true;
130  }
131 
132  return false;
133  }
134 
135  int32_t parseInt32(const uint8_t* buffer)
136  {
137  int32_t val;
138  qi::parse(buffer, buffer + 4, qi::little_dword, val);
139  return val;
140  }
141 
147  bool parseInt32(const std::string& string, int32_t& value, int32_t base)
148  {
149  return string_utilities::toInt32(string, value, base) || string.empty();
150  }
151 
157  bool parseUInt8(const std::string& string, uint8_t& value, int32_t base)
158  {
159  value = 0;
160  if (string.empty())
161  {
162  return true;
163  }
164 
165  uint32_t intermd;
166  if (string_utilities::toUInt32(string, intermd, base) &&
167  intermd <= std::numeric_limits<uint8_t>::max())
168  {
169  value = static_cast<uint8_t>(intermd);
170  return true;
171  }
172 
173  return false;
174  }
175 
176  uint16_t parseUInt16(const uint8_t* buffer)
177  {
178  uint16_t val;
179  qi::parse(buffer, buffer + 2, qi::little_word, val);
180  return val;
181  }
182 
188  bool parseUInt16(const std::string& string, uint16_t& value, int32_t base)
189  {
190  value = 0;
191  if (string.empty())
192  {
193  return true;
194  }
195 
196  uint32_t intermd;
197  if (string_utilities::toUInt32(string, intermd, base) &&
198  intermd <= std::numeric_limits<uint16_t>::max())
199  {
200  value = static_cast<uint16_t>(intermd);
201  return true;
202  }
203 
204  return false;
205  }
206 
207  uint32_t parseUInt32(const uint8_t* buffer)
208  {
209  uint32_t val;
210  qi::parse(buffer, buffer + 4, qi::little_dword, val);
211  return val;
212  }
213 
219  bool parseUInt32(const std::string& string, uint32_t& value, int32_t base)
220  {
221  return string_utilities::toUInt32(string, value, base) || string.empty();
222  }
223 
229  double convertUTCDoubleToSeconds(double utc_double)
230  {
231  uint32_t hours = static_cast<uint32_t>(utc_double) / 10000;
232  uint32_t minutes = (static_cast<uint32_t>(utc_double) - hours * 10000) / 100;
233  double seconds =
234  utc_double - static_cast<double>(hours * 10000 + minutes * 100);
235  seconds += static_cast<double>(hours * 3600 + minutes * 60);
236  return seconds;
237  }
238 
244  double convertDMSToDegrees(double dms)
245  {
246  uint32_t whole_degrees = static_cast<uint32_t>(dms) / 100;
247  double minutes = dms - static_cast<double>(whole_degrees * 100);
248  double degrees = static_cast<double>(whole_degrees) + minutes / 60.0;
249  return degrees;
250  }
251 
268  time_t convertUTCtoUnix(double utc_double)
269  {
270  time_t time_now = time(0);
271  struct tm* timeinfo;
272 
273  // The function gmtime uses the value at &time_now to fill a tm structure
274  // with the values that represent the corresponding time, expressed as a UTC
275  // time.
276  timeinfo = gmtime(&time_now);
277 
278  uint32_t hours = static_cast<uint32_t>(utc_double) / 10000;
279  uint32_t minutes = (static_cast<uint32_t>(utc_double) - hours * 10000) / 100;
280  uint32_t seconds =
281  (static_cast<uint32_t>(utc_double) - hours * 10000 - minutes * 100);
282 
283  // Overwriting timeinfo with UTC time as extracted from utc_double..
284  timeinfo->tm_hour = hours; // hours since midnight - [0,23]
285  timeinfo->tm_min = minutes; // minutes after the hour - [0,59]
286  timeinfo->tm_sec = seconds; // seconds after the minute - [0,59]
287 
288  /* // If you are doing a simulation, add year, month and day here:
289  uint32_t year; // year, starting from 1900
290  uint32_t month; // months since January - [0,11]
291  uint32_t day; //day of the month - [1,31]
292  timeinfo->tm_year = year;
293  timeinfo->tm_mon = month;
294  timeinfo->tm_mday = day;
295  */
296 
297  // Inverse of gmtime, the latter converts time_t (Unix time) to tm (UTC time)
298  time_t date = timegm(timeinfo);
299 
300  // ROS_DEBUG("Since 1970/01/01 %jd seconds have passed.\n", (intmax_t) date);
301  return date;
302  }
303 
309  QuaternionMsg convertEulerToQuaternion(double yaw, double pitch, double roll)
310  {
311  // Abbreviations for the angular functions
312  double cy = std::cos(yaw * 0.5);
313  double sy = std::sin(yaw * 0.5);
314  double cp = std::cos(pitch * 0.5);
315  double sp = std::sin(pitch * 0.5);
316  double cr = std::cos(roll * 0.5);
317  double sr = std::sin(roll * 0.5);
318 
319  QuaternionMsg q;
320  q.w = cr * cp * cy + sr * sp * sy;
321  q.x = sr * cp * cy - cr * sp * sy;
322  q.y = cr * sp * cy + sr * cp * sy;
323  q.z = cr * cp * sy - sr * sp * cy;
324 
325  return q;
326  }
327 
328  std::string convertUserPeriodToRxCommand(uint32_t period_user)
329  {
330  std::string cmd;
331 
332  if (period_user == 0)
333  return "OnChange";
334  else if (period_user < 1000)
335  return "msec" + std::to_string(period_user);
336  else if (period_user <= 60000)
337  return "sec" + std::to_string(period_user / 1000);
338  else
339  return "min" + std::to_string(period_user / 60000);
340  }
341 
342  uint16_t getCrc(const uint8_t* buffer) { return parseUInt16(buffer + 2); }
343 
344  uint16_t getId(const uint8_t* buffer)
345  {
346  // Defines bit mask..
347  // Highest three bits are for revision and rest for block number
348  static uint16_t mask = 8191;
349  // Bitwise AND gives us all but highest 3 bits set to zero, rest unchanged
350 
351  return parseUInt16(buffer + 4) & mask;
352  }
353 
354  uint16_t getLength(const uint8_t* buffer) { return parseUInt16(buffer + 6); }
355 
356  uint32_t getTow(const uint8_t* buffer) { return parseUInt32(buffer + 8); }
357 
358  uint16_t getWnc(const uint8_t* buffer) { return parseUInt16(buffer + 12); }
359 } // namespace parsing_utilities
string_utilities::toInt32
bool toInt32(const std::string &string, int32_t &value, int32_t base=10)
Interprets the contents of "string" as a floating point number of whatever integer type your system h...
Definition: string_utilities.cpp:104
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
parsing_utilities::getWnc
uint16_t getWnc(const uint8_t *buffer)
Get the GPS week counter of the SBF message.
Definition: parsing_utilities.cpp:358
parsing_utilities::getId
uint16_t getId(const uint8_t *buffer)
Get the ID of the SBF message.
Definition: parsing_utilities.cpp:344
parsing_utilities::getTow
uint32_t getTow(const uint8_t *buffer)
Get the time of week in ms of the SBF message.
Definition: parsing_utilities.cpp:356
parsing_utilities::wrapAngle180to180
double wrapAngle180to180(double angle)
Wraps an angle between -180 and 180 degrees.
Definition: parsing_utilities.cpp:49
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:268
parsing_utilities::convertUserPeriodToRxCommand
std::string convertUserPeriodToRxCommand(uint32_t period_user)
Transforms the input polling period [milliseconds] into a std::string number that can be appended to ...
Definition: parsing_utilities.cpp:328
parsing_utilities::parseInt32
int32_t parseInt32(const uint8_t *buffer)
Converts a 4-byte-buffer into a signed 32-bit integer.
Definition: parsing_utilities.cpp:135
parsing_utilities::parseFloat
float parseFloat(const uint8_t *buffer)
Converts a 4-byte-buffer into a float.
Definition: parsing_utilities.cpp:79
parsing_utilities::getLength
uint16_t getLength(const uint8_t *buffer)
Get the length of the SBF message.
Definition: parsing_utilities.cpp:354
parsing_utilities
Definition: parsing_utilities.hpp:53
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:229
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:207
parsing_utilities::parseUInt16
uint16_t parseUInt16(const uint8_t *buffer)
Converts a 2-byte-buffer into an unsigned 16-bit integer.
Definition: parsing_utilities.cpp:176
string_utilities.h
Declares lower-level string utility functions used when parsing messages.
parsing_utilities::convertEulerToQuaternion
QuaternionMsg convertEulerToQuaternion(double yaw, double pitch, double roll)
Transforms Euler angles to a quaternion.
Definition: parsing_utilities.cpp:309
parsing_utilities.hpp
Declares utility functions used when parsing messages.
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:244
string_utilities::toUInt32
bool toUInt32(const std::string &string, uint32_t &value, int32_t base=10)
Interprets the contents of "string" as a floating point number of whatever unsigned integer type your...
Definition: string_utilities.cpp:135
angle
TF2SIMD_FORCE_INLINE tf2Scalar angle(const Quaternion &q1, const Quaternion &q2)
QuaternionMsg
geometry_msgs::Quaternion QuaternionMsg
Definition: typedefs.hpp:95
parsing_utilities::getCrc
uint16_t getCrc(const uint8_t *buffer)
Get the CRC of the SBF message.
Definition: parsing_utilities.cpp:342
parsing_utilities::parseUInt8
bool parseUInt8(const std::string &string, uint8_t &value, int32_t base=10)
Interprets the contents of "string" as a unsigned integer number of type uint8_t.
Definition: parsing_utilities.cpp:157
parsing_utilities::parseInt16
int16_t parseInt16(const uint8_t *buffer)
Converts a 2-byte-buffer into a signed 16-bit integer.
Definition: parsing_utilities.cpp:103
parsing_utilities::parseDouble
double parseDouble(const uint8_t *buffer)
Converts an 8-byte-buffer into a double.
Definition: parsing_utilities.cpp:62
string_utilities::toFloat
bool toFloat(const std::string &string, float &value)
Interprets the contents of "string" as a floating point number of type float.
Definition: string_utilities.cpp:79


septentrio_gnss_driver
Author(s): Tibor Dome
autogenerated on Thu Nov 10 2022 04:02:01