ROSaic
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 
44 {
45 
51  double parseDouble(const uint8_t* buffer)
52  {
53  double diff_loc;
54  std::copy(buffer, buffer + sizeof(double), reinterpret_cast<uint8_t*>(&diff_loc));
55  return diff_loc;
56  }
57 
62  bool parseDouble(const std::string& string, double& value)
63  {
64  return string_utilities::toDouble(string, value) || string.empty();
65  }
66 
72  float parseFloat(const uint8_t* buffer)
73  {
74  float diff_loc;
75  std::copy(buffer, buffer + sizeof(float), reinterpret_cast<uint8_t*>(&diff_loc));
76  return diff_loc;
77  }
78 
83  bool parseFloat(const std::string& string, float& value)
84  {
85  return string_utilities::toFloat(string, value) || string.empty();
86  }
87 
93  int16_t parseInt16(const uint8_t* buffer)
94  {
95  int16_t diff_loc;
96  std::copy(buffer, buffer+2, reinterpret_cast<uint8_t*>(&diff_loc));
97  return diff_loc;
98  }
99 
104  bool parseInt16(const std::string& string, int16_t& value, int32_t base)
105  {
106  value = 0;
107  if (string.empty())
108  {
109  return true;
110  }
111 
112  int32_t intermd;
113  if (string_utilities::toInt32(string, intermd, base) &&
114  intermd <= std::numeric_limits<int16_t>::max() &&
115  intermd >= std::numeric_limits<int16_t>::min())
116  {
117  value = static_cast<int16_t>(intermd);
118  return true;
119  }
120 
121  return false;
122  }
123 
129  int32_t parseInt32(const uint8_t* buffer)
130  {
131  int32_t diff_loc;
132  std::copy(buffer, buffer+4, reinterpret_cast<uint8_t*>(&diff_loc));
133  return diff_loc;
134  }
135 
140  bool parseInt32(const std::string& string, int32_t& value, int32_t base)
141  {
142  return string_utilities::toInt32(string, value, base) || string.empty();
143  }
144 
149  bool parseUInt8(const std::string& string, uint8_t& value, int32_t base)
150  {
151  value = 0;
152  if (string.empty())
153  {
154  return true;
155  }
156 
157  uint32_t intermd;
158  if (string_utilities::toUInt32(string, intermd, base) && intermd <= std::numeric_limits<uint8_t>::max())
159  {
160  value = static_cast<uint8_t>(intermd);
161  return true;
162  }
163 
164  return false;
165  }
166 
172  uint16_t parseUInt16(const uint8_t* buffer)
173  {
174  uint16_t number;
175  std::copy(buffer, buffer+2, reinterpret_cast<uint8_t*>(&number));
176  return number;
177  }
178 
183  bool parseUInt16(const std::string& string, uint16_t& value, int32_t base)
184  {
185  value = 0;
186  if (string.empty())
187  {
188  return true;
189  }
190 
191  uint32_t intermd;
192  if (string_utilities::toUInt32(string, intermd, base) && intermd <= std::numeric_limits<uint16_t>::max())
193  {
194  value = static_cast<uint16_t>(intermd);
195  return true;
196  }
197 
198  return false;
199  }
200 
206  uint32_t parseUInt32(const uint8_t* buffer)
207  {
208  uint32_t diff_loc;
209  std::copy(buffer, buffer+4, reinterpret_cast<uint8_t*>(&diff_loc));
210  return diff_loc;
211  }
212 
217  bool parseUInt32(const std::string& string, uint32_t& value, int32_t base)
218  {
219  return string_utilities::toUInt32(string, value, base) || string.empty();
220  }
221 
226  double convertUTCDoubleToSeconds(double utc_double)
227  {
228  uint32_t hours = static_cast<uint32_t>(utc_double) / 10000;
229  uint32_t minutes = (static_cast<uint32_t>(utc_double) - hours * 10000) / 100;
230  double seconds = utc_double - static_cast<double>(hours * 10000 + minutes * 100);
231  seconds += static_cast<double> (hours * 3600 + minutes * 60);
232  return seconds;
233  }
234 
239  double convertDMSToDegrees(double dms)
240  {
241  uint32_t whole_degrees = static_cast<uint32_t>(dms) / 100;
242  double minutes = dms - static_cast<double>(whole_degrees * 100);
243  double degrees = static_cast<double>(whole_degrees) + minutes / 60.0;
244  return degrees;
245  }
246 
259  time_t convertUTCtoUnix(double utc_double)
260  {
261  time_t time_now = time(0);
262  struct tm * timeinfo;
263 
264  // The function gmtime uses the value at &time_now to fill a tm structure with the values that represent the
265  // corresponding time, expressed as a UTC time.
266  timeinfo = gmtime(&time_now);
267 
268  uint32_t hours = static_cast<uint32_t>(utc_double) / 10000;
269  uint32_t minutes = (static_cast<uint32_t>(utc_double) - hours * 10000) / 100;
270  uint32_t seconds = (static_cast<uint32_t>(utc_double) - hours * 10000 - minutes * 100);
271 
272  // Overwriting timeinfo with UTC time as extracted from utc_double..
273  timeinfo->tm_hour = hours; // hours since midnight - [0,23]
274  timeinfo->tm_min = minutes; // minutes after the hour - [0,59]
275  timeinfo->tm_sec = seconds; // seconds after the minute - [0,59]
276 
277  /* // If you are doing a simulation, add year, month and day here:
278  uint32_t year; // year, starting from 1900
279  uint32_t month; // months since January - [0,11]
280  uint32_t day; //day of the month - [1,31]
281  timeinfo->tm_year = year;
282  timeinfo->tm_mon = month;
283  timeinfo->tm_mday = day;
284  */
285 
286  // Inverse of gmtime, the latter converts time_t (Unix time) to tm (UTC time)
287  time_t date = timegm(timeinfo);
288 
289  //ROS_DEBUG("Since 1970/01/01 %jd seconds have passed.\n", (intmax_t) date);
290  return date;
291  }
292 
296  geometry_msgs::Quaternion convertEulerToQuaternion(double yaw, double pitch, double roll)
297  {
298  // Abbreviations for the angular functions
299  double cy = cos(yaw * 0.5);
300  double sy = sin(yaw * 0.5);
301  double cp = cos(pitch * 0.5);
302  double sp = sin(pitch * 0.5);
303  double cr = cos(roll * 0.5);
304  double sr = sin(roll * 0.5);
305 
306  geometry_msgs::Quaternion q;
307  q.w = cr * cp * cy + sr * sp * sy;
308  q.x = sr * cp * cy - cr * sp * sy;
309  q.y = cr * sp * cy + sr * cp * sy;
310  q.z = cr * cp * sy - sr * sp * cy;
311 
312  return q;
313  }
314 
315  uint32_t convertUserPeriodToRxCommand(uint32_t period_user)
316  {
317  if (period_user <= 500 && period_user >= 10) return period_user;
318  else
319  {
320  return period_user/1000;
321  }
322  }
323 }
324 
325 
326 
bool toFloat(const std::string &string, float &value)
Interprets the contents of "string" as a floating point number of type float.
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)
Interprets the contents of "string" as a floating point number of whatever unsigned integer type your...
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.
uint32_t convertUserPeriodToRxCommand(uint32_t period_user)
Transforms the input polling period [milliseconds] into a uint32_t number that can be appended to eit...
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)
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.
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)
Interprets the contents of "string" as a unsigned integer number of type uint8_t. ...
geometry_msgs::Quaternion convertEulerToQuaternion(double yaw, double pitch, double roll)
Transforms Euler angles to a quaternion.