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 
15 typedef enum
16 {
20  ROS_only
21 }
23 
24 constexpr double PI = 3.1415926535897932384626433;
25 
27 inline double convertDegreesToRadians(double degrees)
28 {
29  return degrees / 180.0 * PI;
30 }
31 
32 inline 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*/
40 inline 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 
67 inline 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:279
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