YARP
Yet Another Robot Platform
yarpRosHelper.h
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6#ifndef YARP_ROSMSG_IMPL_HELPER_H
7#define YARP_ROSMSG_IMPL_HELPER_H
8
9
10#include <iostream>
11#include <climits>
12#include <cmath>
13#include <yarp/os/Log.h>
14
15typedef enum
16{
23
24constexpr double PI = 3.1415926535897932384626433;
25
27inline double convertDegreesToRadians(double degrees)
28{
29 return degrees / 180.0 * PI;
30}
31
32inline void convertDegreesToRadians(std::vector<yarp::os::NetFloat64> &degrees)
33{
34 for (size_t i = 0; i < degrees.size(); i++) {
35 degrees[i] = convertDegreesToRadians(degrees[i]);
36 }
37}
38
39/* return false if errors occourr, like norm of the resulting vector is not 1*/
40inline bool convertEulerAngleYXZrads_to_quaternion(double *eulerXYZ, double *quaternion)
41{
42 bool ret = true;
43
44 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);
45 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);
46 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);
47 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);
48
49 // verifica norma vettore
50
51 double norma = 0;
52 for(int i=0; i<4; i++)
53 {
54 norma += quaternion[i] * quaternion[i];
55 }
56 norma = sqrt(norma);
57
58 if((norma -1) >= 0.05)
59 {
60 yError("convertEulerAngleYXZrads_to_quaternion: Error on quaternion conversion.");
61 ret = false;
62 }
63
64 return ret;
65}
66
67inline bool convertEulerAngleYXZdegrees_to_quaternion(double *eulerXYZ, double *quaternion)
68{
69 eulerXYZ[0] = convertDegreesToRadians(eulerXYZ[0]);
70 eulerXYZ[1] = convertDegreesToRadians(eulerXYZ[1]);
71 eulerXYZ[2] = convertDegreesToRadians(eulerXYZ[2]);
72 return convertEulerAngleYXZrads_to_quaternion(eulerXYZ, quaternion);
73}
74
75#endif // YARP_ROSMSG_IMPL_HELPER_H
bool ret
#define yError(...)
Definition: Log.h:356
bool convertEulerAngleYXZdegrees_to_quaternion(double *eulerXYZ, double *quaternion)
Definition: yarpRosHelper.h:67
bool convertEulerAngleYXZrads_to_quaternion(double *eulerXYZ, double *quaternion)
Definition: yarpRosHelper.h:40
constexpr double PI
Definition: yarpRosHelper.h:24
ROSTopicUsageType
Definition: yarpRosHelper.h:16
@ ROS_only
Definition: yarpRosHelper.h:20
@ ROS_config_error
Definition: yarpRosHelper.h:17
@ ROS_enabled
Definition: yarpRosHelper.h:19
@ ROS_disabled
Definition: yarpRosHelper.h:18
double convertDegreesToRadians(double degrees)
convert degrees to radiants for ROS messages
Definition: yarpRosHelper.h:27