parsing_utilities.hpp
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 #ifndef PARSING_UTILITIES_HPP
32 #define PARSING_UTILITIES_HPP
33 
34 // C++ library includes
35 #include <cmath> // C++ header, corresponds to <math.h> in C
36 #include <cstdint> // C++ header, corresponds to <stdint.h> in C
37 #include <ctime> // C++ header, corresponds to <time.h> in C
38 #include <string> // C++ header, corresponds to <string.h> in C
39 // Eigen Includes
40 #include <Eigen/Core>
41 #include <Eigen/LU>
42 // Boost includes
43 #include <boost/math/constants/constants.hpp>
44 // ROS includes
46 
53 namespace parsing_utilities {
54 
55  constexpr double pi_half = boost::math::constants::pi<double>() / 2.0;
56 
57  /***********************************************************************
58  * Square value
59  **********************************************************************/
60  template<class T>
61  inline T square(T val)
62  {
63  return val * val;
64  }
65 
66  /***********************************************************************
67  * Convert degrees to radians
68  **********************************************************************/
69  template<class T>
70  inline T deg2rad(T deg)
71  {
72  return deg * boost::math::constants::degree<T>();
73  }
74 
75  /***********************************************************************
76  * Convert degrees^2 to radians^2
77  **********************************************************************/
78  template<class T>
79  inline T deg2radSq(T deg)
80  {
81  return deg * boost::math::constants::degree<T>() * boost::math::constants::degree<T>();
82  }
83 
84  /***********************************************************************
85  * Convert radians to degree
86  **********************************************************************/
87  template<class T>
88  inline T rad2deg(T rad)
89  {
90  return rad * boost::math::constants::radian<T>();
91  }
92 
93  /***********************************************************************
94  * Convert Euler angles to rotation matrix
95  **********************************************************************/
96  inline Eigen::Matrix3d rpyToRot(double roll, double pitch, double yaw)
97  {
98  Eigen::Matrix3d M;
99  double sa, ca, sb, cb, sc, cc;
100  sa = std::sin(roll);
101  ca = std::cos(roll);
102  sb = std::sin(pitch);
103  cb = std::cos(pitch);
104  sc = std::sin(yaw);
105  cc = std::cos(yaw);
106 
107  M << cb * cc, -ca * sc + sa * sb * cc, sc* sa + ca * sb * cc,
108  cb* sc, ca* cc + sa * sb * sc, -sa * cc + ca * sb * sc,
109  -sb, sa* cb, ca* cb;
110  return M;
111  }
112 
117  double wrapAngle180to180(double angle);
118 
124  double parseDouble(const uint8_t* buffer);
125 
138  bool parseDouble(const std::string& string, double& value);
139 
145  float parseFloat(const uint8_t* buffer);
146 
159  bool parseFloat(const std::string& string, float& value);
160 
166  int16_t parseInt16(const uint8_t* buffer);
167 
182  bool parseInt16(const std::string& string, int16_t& value, int32_t base = 10);
183 
189  int32_t parseInt32(const uint8_t* buffer);
190 
205  bool parseInt32(const std::string& string, int32_t& value, int32_t base = 10);
206 
221  bool parseUInt8(const std::string& string, uint8_t& value, int32_t base = 10);
222 
228  uint16_t parseUInt16(const uint8_t* buffer);
229 
244  bool parseUInt16(const std::string& string, uint16_t& value, int32_t base = 10);
245 
251  uint32_t parseUInt32(const uint8_t* buffer);
252 
267  bool parseUInt32(const std::string& string, uint32_t& value, int32_t base = 10);
268 
276  double convertUTCDoubleToSeconds(double utc_double);
277 
288  std::time_t convertUTCtoUnix(double utc_double);
289 
299  double convertDMSToDegrees(double dms);
300 
308  QuaternionMsg convertEulerToQuaternion(double yaw, double pitch,
309  double roll);
310 
319  uint32_t convertUserPeriodToRxCommand(uint32_t period_user);
320 
327  uint16_t getCrc(const uint8_t* buffer);
328 
335  uint16_t getId(const uint8_t* buffer);
336 
343  uint16_t getLength(const uint8_t* buffer);
344 
351  uint32_t getTow(const uint8_t* buffer);
352 
359  uint16_t getWnc(const uint8_t* buffer);
360 } // namespace parsing_utilities
361 
362 #endif // PARSING_UTILITIES_HPP
uint32_t getTow(const uint8_t *buffer)
Get the time of week in ms of the SBF message.
TF2SIMD_FORCE_INLINE tf2Scalar angle(const Quaternion &q1, const Quaternion &q2)
uint16_t getCrc(const uint8_t *buffer)
Get the CRC of the SBF message.
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...
double wrapAngle180to180(double angle)
Wraps an angle between -180 and 180 degrees.
Eigen::Matrix3d rpyToRot(double roll, double pitch, double yaw)
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.
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)...
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.
constexpr double pi_half
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...
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