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,
310  double roll)
311  {
312  // Abbreviations for the angular functions
313  double cy = std::cos(yaw * 0.5);
314  double sy = std::sin(yaw * 0.5);
315  double cp = std::cos(pitch * 0.5);
316  double sp = std::sin(pitch * 0.5);
317  double cr = std::cos(roll * 0.5);
318  double sr = std::sin(roll * 0.5);
319 
320  QuaternionMsg q;
321  q.w = cr * cp * cy + sr * sp * sy;
322  q.x = sr * cp * cy - cr * sp * sy;
323  q.y = cr * sp * cy + sr * cp * sy;
324  q.z = cr * cp * sy - sr * sp * cy;
325 
326  return q;
327  }
328 
329  uint32_t convertUserPeriodToRxCommand(uint32_t period_user)
330  {
331  if (period_user <= 500 && period_user >= 10)
332  return period_user;
333  else
334  {
335  return period_user / 1000;
336  }
337  }
338 
339  uint16_t getCrc(const uint8_t* buffer)
340  {
341  return parseUInt16(buffer + 2);
342  }
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)
355  {
356  return parseUInt16(buffer + 6);
357  }
358 
359  uint32_t getTow(const uint8_t* buffer)
360  {
361  return parseUInt32(buffer + 8);
362  }
363 
364  uint16_t getWnc(const uint8_t* buffer)
365  {
366  return parseUInt16(buffer + 12);
367  }
368 } // namespace parsing_utilities
uint32_t getTow(const uint8_t *buffer)
Get the time of week in ms of the SBF message.
bool toFloat(const std::string &string, float &value)
Interprets the contents of "string" as a floating point number of type float.
uint16_t getCrc(const uint8_t *buffer)
Get the CRC of the SBF 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...
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...
double wrapAngle180to180(double angle)
Wraps an angle between -180 and 180 degrees.
uint16_t getLength(const uint8_t *buffer)
Get the length of the SBF message.
uint16_t getWnc(const uint8_t *buffer)
Get the GPS week counter of the SBF message.
uint16_t parseUInt16(const uint8_t *buffer)
Converts a 2-byte-buffer into an unsigned 16-bit integer.
Declares lower-level string utility functions used when parsing messages.
uint32_t parseUInt32(const uint8_t *buffer)
Converts a 4-byte-buffer into an unsigned 32-bit integer.
double convertDMSToDegrees(double dms)
Converts latitude or longitude from the DMS notation (in the without-colon-delimiter format)...
Declares utility functions used when parsing messages.
geometry_msgs::Quaternion QuaternionMsg
Definition: typedefs.hpp:84
uint32_t convertUserPeriodToRxCommand(uint32_t period_user)
Transforms the input polling period [milliseconds] into a uint32_t number that can be appended to eit...
QuaternionMsg convertEulerToQuaternion(double yaw, double pitch, double roll)
Transforms Euler angles to a quaternion.
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...
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...
double parseDouble(const uint8_t *buffer)
Converts an 8-byte-buffer into a double.
int16_t parseInt16(const uint8_t *buffer)
Converts a 2-byte-buffer into a signed 16-bit integer.
uint16_t getId(const uint8_t *buffer)
Get the ID of the SBF message.
int32_t parseInt32(const uint8_t *buffer)
Converts a 4-byte-buffer into a signed 32-bit integer.
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. ...


septentrio_gnss_driver
Author(s): Tibor Dome
autogenerated on Thu Jun 23 2022 02:11:38