Program Listing for File parsing_utilities.hpp
↰ Return to documentation for file (/tmp/ws/src/septentrio_gnss_driver/include/septentrio_gnss_driver/parsers/parsing_utilities.hpp
)
// *****************************************************************************
//
// © Copyright 2020, Septentrio NV/SA.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// 3. Neither the name of the copyright holder nor the names of its
// contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// *****************************************************************************
#pragma once
// C++ library includes
#include <cmath> // C++ header, corresponds to <math.h> in C
#include <cstdint> // C++ header, corresponds to <stdint.h> in C
#include <ctime> // C++ header, corresponds to <time.h> in C
#include <string> // C++ header, corresponds to <string.h> in C
// Eigen Includes
#include <Eigen/Core>
#include <Eigen/LU>
// Boost includes
#include <boost/math/constants/constants.hpp>
// ROS includes
#include <septentrio_gnss_driver/abstraction/typedefs.hpp>
namespace parsing_utilities {
constexpr double pi_half = boost::math::constants::pi<double>() / 2.0;
/***********************************************************************
* Square value
**********************************************************************/
template <class T>
[[nodiscard]] inline T square(T val)
{
return val * val;
}
/***********************************************************************
* Convert degrees to radians
**********************************************************************/
template <class T>
[[nodiscard]] inline T deg2rad(T deg)
{
return deg * boost::math::constants::degree<T>();
}
/***********************************************************************
* Convert degrees^2 to radians^2
**********************************************************************/
template <class T>
[[nodiscard]] inline T deg2radSq(T deg)
{
return deg * boost::math::constants::degree<T>() *
boost::math::constants::degree<T>();
}
/***********************************************************************
* Convert radians to degree
**********************************************************************/
template <class T>
[[nodiscard]] inline T rad2deg(T rad)
{
return rad * boost::math::constants::radian<T>();
}
/***********************************************************************
* Convert Euler angles to rotation matrix
**********************************************************************/
[[nodiscard]] inline Eigen::Matrix3d rpyToRot(double roll, double pitch, double yaw)
{
Eigen::Matrix3d M;
double sa, ca, sb, cb, sc, cc;
sa = std::sin(roll);
ca = std::cos(roll);
sb = std::sin(pitch);
cb = std::cos(pitch);
sc = std::sin(yaw);
cc = std::cos(yaw);
M << cb * cc, -ca * sc + sa * sb * cc, sc * sa + ca * sb * cc, cb * sc,
ca * cc + sa * sb * sc, -sa * cc + ca * sb * sc, -sb, sa * cb, ca * cb;
return M;
}
[[nodiscard]] double wrapAngle180to180(double angle);
[[nodiscard]] double parseDouble(const uint8_t* buffer);
[[nodiscard]] bool parseDouble(const std::string& string, double& value);
[[nodiscard]] float parseFloat(const uint8_t* buffer);
[[nodiscard]] bool parseFloat(const std::string& string, float& value);
[[nodiscard]] int16_t parseInt16(const uint8_t* buffer);
[[nodiscard]] bool parseInt16(const std::string& string, int16_t& value, int32_t base = 10);
[[nodiscard]] int32_t parseInt32(const uint8_t* buffer);
[[nodiscard]] bool parseInt32(const std::string& string, int32_t& value, int32_t base = 10);
[[nodiscard]] bool parseUInt8(const std::string& string, uint8_t& value, int32_t base = 10);
[[nodiscard]] uint16_t parseUInt16(const uint8_t* buffer);
[[nodiscard]] bool parseUInt16(const std::string& string, uint16_t& value, int32_t base = 10);
[[nodiscard]] uint32_t parseUInt32(const uint8_t* buffer);
[[nodiscard]] bool parseUInt32(const std::string& string, uint32_t& value, int32_t base = 10);
[[nodiscard]] double convertUTCDoubleToSeconds(double utc_double);
[[nodiscard]] std::time_t convertUTCtoUnix(double utc_double);
[[nodiscard]] double convertDMSToDegrees(double dms);
[[nodiscard]] Eigen::Quaterniond convertEulerToQuaternion(double roll, double pitch,
double yaw);
[[nodiscard]] QuaternionMsg convertEulerToQuaternionMsg(double roll, double pitch, double yaw);
[[nodiscard]] QuaternionMsg quaternionToQuaternionMsg(const Eigen::Quaterniond& q);
[[nodiscard]] Eigen::Quaterniond q_enu_ecef(double lat, double lon);
[[nodiscard]] Eigen::Quaterniond q_ned_ecef(double lat, double lon);
[[nodiscard]] Eigen::Matrix3d R_enu_ecef(double lat, double lon);
[[nodiscard]] Eigen::Matrix3d R_ned_ecef(double lat, double lon);
[[nodiscard]] std::string convertUserPeriodToRxCommand(uint32_t period_user);
[[nodiscard]] uint16_t getCrc(const std::vector<uint8_t>& message);
[[nodiscard]] uint16_t getId(const std::vector<uint8_t>& message);
[[nodiscard]] uint16_t getLength(const std::vector<uint8_t>& message);
[[nodiscard]] uint32_t getTow(const std::vector<uint8_t>& message);
[[nodiscard]] uint16_t getWnc(const std::vector<uint8_t>& message);
} // namespace parsing_utilities