yarpRosHelper.h

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
 * SPDX-License-Identifier: BSD-3-Clause
 */
 
#ifndef YARP_ROSMSG_IMPL_HELPER_H
#define YARP_ROSMSG_IMPL_HELPER_H
 
 
#include <iostream>
#include <climits>
#include <cmath>
#include <yarp/os/Log.h>
 
typedef enum
{
    ROS_config_error,
    ROS_disabled,
    ROS_enabled,
    ROS_only
}
ROSTopicUsageType;
 
constexpr double PI = 3.1415926535897932384626433;
 
inline double convertDegreesToRadians(double degrees)
{
    return degrees / 180.0 * PI;
}
 
inline void convertDegreesToRadians(std::vector<yarp::os::NetFloat64>  &degrees)
{
    for (size_t i = 0; i < degrees.size(); i++) {
        degrees[i] = convertDegreesToRadians(degrees[i]);
    }
}
 
/* return false if errors occourr, like norm of the resulting vector is not 1*/
inline bool convertEulerAngleYXZrads_to_quaternion(double *eulerXYZ, double *quaternion)
{
    bool ret = true;
 
    quaternion[0] = -sin(eulerXYZ[0]/2) * sin(eulerXYZ[1]/2) * sin(eulerXYZ[2]/2) + cos(eulerXYZ[0]/2) * cos(eulerXYZ[1]/2) * cos(eulerXYZ[2]/2);
    quaternion[1] =  sin(eulerXYZ[0]/2) * cos(eulerXYZ[1]/2) * cos(eulerXYZ[2]/2) + cos(eulerXYZ[0]/2) * sin(eulerXYZ[1]/2) * sin(eulerXYZ[2]/2);
    quaternion[2] = -sin(eulerXYZ[0]/2) * cos(eulerXYZ[1]/2) * sin(eulerXYZ[2]/2) + cos(eulerXYZ[0]/2) * sin(eulerXYZ[1]/2) * cos(eulerXYZ[2]/2);
    quaternion[3] =  sin(eulerXYZ[0]/2) * sin(eulerXYZ[1]/2) * cos(eulerXYZ[2]/2) + cos(eulerXYZ[0]/2) * cos(eulerXYZ[1]/2) * sin(eulerXYZ[2]/2);
 
    // verifica norma vettore
 
    double norma = 0;
    for(int i=0; i<4; i++)
    {
        norma += quaternion[i] * quaternion[i];
    }
    norma = sqrt(norma);
 
    if((norma -1)  >= 0.05)
    {
        yError("convertEulerAngleYXZrads_to_quaternion: Error on quaternion conversion.");
        ret = false;
    }
 
    return ret;
}
 
inline bool convertEulerAngleYXZdegrees_to_quaternion(double *eulerXYZ, double *quaternion)
{
    eulerXYZ[0] = convertDegreesToRadians(eulerXYZ[0]);
    eulerXYZ[1] = convertDegreesToRadians(eulerXYZ[1]);
    eulerXYZ[2] = convertDegreesToRadians(eulerXYZ[2]);
    return convertEulerAngleYXZrads_to_quaternion(eulerXYZ, quaternion);
}
 
#endif  // YARP_ROSMSG_IMPL_HELPER_H