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 #pragma once
32 
33 // C++ library includes
34 #include <cmath> // C++ header, corresponds to <math.h> in C
35 #include <cstdint> // C++ header, corresponds to <stdint.h> in C
36 #include <ctime> // C++ header, corresponds to <time.h> in C
37 #include <string> // C++ header, corresponds to <string.h> in C
38 // Eigen Includes
39 #include <Eigen/Core>
40 #include <Eigen/LU>
41 // Boost includes
42 #include <boost/math/constants/constants.hpp>
43 // ROS includes
45 
52 namespace parsing_utilities {
53 
54  constexpr double pi_half = boost::math::constants::pi<double>() / 2.0;
55 
56  /***********************************************************************
57  * Square value
58  **********************************************************************/
59  template <class T>
60  [[nodiscard]] inline T square(T val)
61  {
62  return val * val;
63  }
64 
65  /***********************************************************************
66  * Convert degrees to radians
67  **********************************************************************/
68  template <class T>
69  [[nodiscard]] inline T deg2rad(T deg)
70  {
71  return deg * boost::math::constants::degree<T>();
72  }
73 
74  /***********************************************************************
75  * Convert degrees^2 to radians^2
76  **********************************************************************/
77  template <class T>
78  [[nodiscard]] inline T deg2radSq(T deg)
79  {
80  return deg * boost::math::constants::degree<T>() *
81  boost::math::constants::degree<T>();
82  }
83 
84  /***********************************************************************
85  * Convert radians to degree
86  **********************************************************************/
87  template <class T>
88  [[nodiscard]] 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  [[nodiscard]] 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, cb * sc,
108  ca * cc + sa * sb * sc, -sa * cc + ca * sb * sc, -sb, sa * cb, ca * cb;
109  return M;
110  }
111 
116  [[nodiscard]] double wrapAngle180to180(double angle);
117 
123  [[nodiscard]] double parseDouble(const uint8_t* buffer);
124 
137  [[nodiscard]] bool parseDouble(const std::string& string, double& value);
138 
144  [[nodiscard]] float parseFloat(const uint8_t* buffer);
145 
158  [[nodiscard]] bool parseFloat(const std::string& string, float& value);
159 
165  [[nodiscard]] int16_t parseInt16(const uint8_t* buffer);
166 
181  [[nodiscard]] bool parseInt16(const std::string& string, int16_t& value, int32_t base = 10);
182 
188  [[nodiscard]] int32_t parseInt32(const uint8_t* buffer);
189 
204  [[nodiscard]] bool parseInt32(const std::string& string, int32_t& value, int32_t base = 10);
205 
220  [[nodiscard]] bool parseUInt8(const std::string& string, uint8_t& value, int32_t base = 10);
221 
227  [[nodiscard]] uint16_t parseUInt16(const uint8_t* buffer);
228 
243  [[nodiscard]] bool parseUInt16(const std::string& string, uint16_t& value, int32_t base = 10);
244 
250  [[nodiscard]] uint32_t parseUInt32(const uint8_t* buffer);
251 
266  [[nodiscard]] bool parseUInt32(const std::string& string, uint32_t& value, int32_t base = 10);
267 
275  [[nodiscard]] double convertUTCDoubleToSeconds(double utc_double);
276 
287  [[nodiscard]] std::time_t convertUTCtoUnix(double utc_double);
288 
298  [[nodiscard]] double convertDMSToDegrees(double dms);
299 
307  [[nodiscard]] Eigen::Quaterniond convertEulerToQuaternion(double roll, double pitch,
308  double yaw);
309 
317  [[nodiscard]] QuaternionMsg convertEulerToQuaternionMsg(double roll, double pitch, double yaw);
318 
324  [[nodiscard]] QuaternionMsg quaternionToQuaternionMsg(const Eigen::Quaterniond& q);
325 
332  [[nodiscard]] Eigen::Quaterniond q_enu_ecef(double lat, double lon);
333 
340  [[nodiscard]] Eigen::Quaterniond q_ned_ecef(double lat, double lon);
341 
348  [[nodiscard]] Eigen::Matrix3d R_enu_ecef(double lat, double lon);
349 
356  [[nodiscard]] Eigen::Matrix3d R_ned_ecef(double lat, double lon);
357 
366  [[nodiscard]] std::string convertUserPeriodToRxCommand(uint32_t period_user);
367 
374  [[nodiscard]] uint16_t getCrc(const std::vector<uint8_t>& message);
375 
382  [[nodiscard]] uint16_t getId(const std::vector<uint8_t>& message);
383 
390  [[nodiscard]] uint16_t getLength(const std::vector<uint8_t>& message);
391 
398  [[nodiscard]] uint32_t getTow(const std::vector<uint8_t>& message);
399 
406  [[nodiscard]] uint16_t getWnc(const std::vector<uint8_t>& message);
407 } // namespace parsing_utilities
parsing_utilities::quaternionToQuaternionMsg
QuaternionMsg quaternionToQuaternionMsg(const Eigen::Quaterniond &q)
Convert Eigen quaternion to a QuaternionMsg.
Definition: parsing_utilities.cpp:318
parsing_utilities::getTow
uint32_t getTow(const std::vector< uint8_t > &message)
Get the time of week in ms of the SBF message.
Definition: parsing_utilities.cpp:434
parsing_utilities::rad2deg
T rad2deg(T rad)
Definition: parsing_utilities.hpp:88
parsing_utilities::wrapAngle180to180
double wrapAngle180to180(double angle)
Wraps an angle between -180 and 180 degrees.
Definition: parsing_utilities.cpp:51
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:265
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:400
parsing_utilities::getWnc
uint16_t getWnc(const std::vector< uint8_t > &message)
Get the GPS week counter of the SBF message.
Definition: parsing_utilities.cpp:439
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:130
parsing_utilities::parseFloat
float parseFloat(const uint8_t *buffer)
Converts a 4-byte-buffer into a float.
Definition: parsing_utilities.cpp:73
parsing_utilities::R_enu_ecef
Eigen::Matrix3d R_enu_ecef(double lat, double lon)
Generates the matrix to rotate from local ENU to ECEF.
Definition: parsing_utilities.cpp:356
parsing_utilities
Definition: parsing_utilities.hpp:52
parsing_utilities::convertEulerToQuaternion
Eigen::Quaterniond convertEulerToQuaternion(double roll, double pitch, double yaw)
Transforms Euler angles to a quaternion.
Definition: parsing_utilities.cpp:303
parsing_utilities::getLength
uint16_t getLength(const std::vector< uint8_t > &message)
Get the length of the SBF message.
Definition: parsing_utilities.cpp:429
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:228
parsing_utilities::pi_half
constexpr double pi_half
Definition: parsing_utilities.hpp:54
parsing_utilities::getCrc
uint16_t getCrc(const std::vector< uint8_t > &message)
Get the CRC of the SBF message.
Definition: parsing_utilities.cpp:414
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:205
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:173
parsing_utilities::convertEulerToQuaternionMsg
QuaternionMsg convertEulerToQuaternionMsg(double roll, double pitch, double yaw)
Transforms Euler angles to a QuaternionMsg.
Definition: parsing_utilities.cpp:330
parsing_utilities::R_ned_ecef
Eigen::Matrix3d R_ned_ecef(double lat, double lon)
Generates the matrix to rotate from local NED to ECEF.
Definition: parsing_utilities.cpp:378
parsing_utilities::getId
uint16_t getId(const std::vector< uint8_t > &message)
Get the ID of the SBF message.
Definition: parsing_utilities.cpp:419
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:242
parsing_utilities::square
T square(T val)
Definition: parsing_utilities.hpp:60
typedefs.hpp
parsing_utilities::rpyToRot
Eigen::Matrix3d rpyToRot(double roll, double pitch, double yaw)
Definition: parsing_utilities.hpp:96
parsing_utilities::q_ned_ecef
Eigen::Quaterniond q_ned_ecef(double lat, double lon)
Generates the quaternion to rotate from local NED to ECEF.
Definition: parsing_utilities.cpp:346
parsing_utilities::q_enu_ecef
Eigen::Quaterniond q_enu_ecef(double lat, double lon)
Generates the quaternion to rotate from local ENU to ECEF.
Definition: parsing_utilities.cpp:336
parsing_utilities::deg2radSq
T deg2radSq(T deg)
Definition: parsing_utilities.hpp:78
QuaternionMsg
geometry_msgs::Quaternion QuaternionMsg
Definition: typedefs.hpp:99
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:153
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:97
parsing_utilities::parseDouble
double parseDouble(const uint8_t *buffer)
Converts an 8-byte-buffer into a double.
Definition: parsing_utilities.cpp:56
parsing_utilities::deg2rad
T deg2rad(T deg)
Definition: parsing_utilities.hpp:69


septentrio_gnss_driver
Author(s): Tibor Dome
autogenerated on Wed Nov 22 2023 04:04:27