parsing_utilities.hpp

Go to the documentation of this file.

// *****************************************************************************
//
// © 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
#ifdef ROS2
#include <septentrio_gnss_driver/abstraction/typedefs.hpp>
#endif
#ifdef ROS1
#include <septentrio_gnss_driver/abstraction/typedefs_ros1.hpp>
#endif
 
namespace parsing_utilities {
 
    const double pihalf = boost::math::constants::half_pi<double>();
 
    /***********************************************************************
     * 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]] inline double wrapAngle180to180(double angle)
    {
        return std::remainder(angle, 360.0);
    }
 
    [[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]] inline Eigen::Quaterniond
    convertEulerToQuaternion(double roll, double pitch, double yaw)
    {
        double cy = std::cos(yaw * 0.5);
        double sy = std::sin(yaw * 0.5);
        double cp = std::cos(pitch * 0.5);
        double sp = std::sin(pitch * 0.5);
        double cr = std::cos(roll * 0.5);
        double sr = std::sin(roll * 0.5);
 
        return Eigen::Quaterniond(
            cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy,
            cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy);
    }
 
    [[nodiscard]] inline QuaternionMsg
    quaternionToQuaternionMsg(const Eigen::Quaterniond& q)
    {
        QuaternionMsg qm;
 
        qm.w = q.w();
        qm.x = q.x();
        qm.y = q.y();
        qm.z = q.z();
 
        return qm;
    }
 
    [[nodiscard]] inline QuaternionMsg
    convertEulerToQuaternionMsg(double roll, double pitch, double yaw)
    {
        return quaternionToQuaternionMsg(convertEulerToQuaternion(roll, pitch, yaw));
    }
 
    inline void setQuaternionNaN(QuaternionMsg& quaternion)
    {
        quaternion.w = std::numeric_limits<double>::quiet_NaN();
        quaternion.x = std::numeric_limits<double>::quiet_NaN();
        quaternion.y = std::numeric_limits<double>::quiet_NaN();
        quaternion.z = std::numeric_limits<double>::quiet_NaN();
    }
 
    inline void setVector3NaN(Vector3Msg& v)
    {
        v.x = std::numeric_limits<double>::quiet_NaN();
        v.y = std::numeric_limits<double>::quiet_NaN();
        v.z = std::numeric_limits<double>::quiet_NaN();
    }
 
    [[nodiscard]] inline Eigen::Quaterniond q_enu_ecef(double lat, double lon)
    {
        double sr = sin((pihalf - lat) / 2.0);
        double cr = cos((pihalf - lat) / 2.0);
        double sy = sin((lon + pihalf) / 2.0);
        double cy = cos((lon + pihalf) / 2.0);
 
        return Eigen::Quaterniond(cr * cy, sr * cy, sr * sy, cr * sy);
    }
 
    [[nodiscard]] inline Eigen::Quaterniond q_ned_ecef(double lat, double lon)
    {
        double sp = sin((-lat - pihalf) / 2.0);
        double cp = cos((-lat - pihalf) / 2.0);
        double sy = sin(lon / 2.0);
        double cy = cos(lon / 2.0);
 
        return Eigen::Quaterniond(cp * cy, -sp * sy, sp * cy, cp * sy);
    }
 
    [[nodiscard]] inline Eigen::Matrix3d R_enu_ecef(double lat, double lon)
    {
        Eigen::Matrix3d R;
 
        double sin_lat = sin(lat);
        double cos_lat = cos(lat);
        double sin_lon = sin(lon);
        double cos_lon = cos(lon);
 
        R(0, 0) = -sin_lon;
        R(0, 1) = -cos_lon * sin_lat;
        R(0, 2) = cos_lon * cos_lat;
        R(1, 0) = cos_lon;
        R(1, 1) = -sin_lon * sin_lat;
        R(1, 2) = sin_lon * cos_lat;
        R(2, 0) = 0.0;
        R(2, 1) = cos_lat;
        R(2, 2) = sin_lat;
 
        return R;
    }
 
    [[nodiscard]] inline Eigen::Matrix3d R_ned_ecef(double lat, double lon)
    {
        Eigen::Matrix3d R;
 
        double sin_lat = sin(lat);
        double cos_lat = cos(lat);
        double sin_lon = sin(lon);
        double cos_lon = cos(lon);
 
        R(0, 0) = -cos_lon * sin_lat;
        R(0, 1) = -sin_lon;
        R(0, 2) = -cos_lon * cos_lat;
        R(1, 0) = -sin_lon * sin_lat;
        R(1, 1) = cos_lon;
        R(1, 2) = -sin_lon * cos_lat;
        R(2, 0) = cos_lat;
        R(2, 1) = 0.0;
        R(2, 2) = -sin_lat;
 
        return R;
    }
 
    [[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);
 
    inline double convertAutoCovariance(double val)
    {
        return std::isnan(val) ? -1.0 : deg2radSq(val);
    }
 
    inline double convertCovariance(double val)
    {
        return std::isnan(val) ? 0.0 : deg2radSq(val);
    }
} // namespace parsing_utilities