impl/utils.h
Go to the documentation of this file.
1 // Copyright 2014 Open Source Robotics Foundation, Inc.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef TF2_IMPL_UTILS_H
16 #define TF2_IMPL_UTILS_H
17 
18 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
21 
22 namespace tf2 {
23 namespace impl {
24 
29 inline
31  return q;
32  }
33 
38 inline
39 tf2::Quaternion toQuaternion(const geometry_msgs::Quaternion& q) {
40  tf2::Quaternion res;
41  fromMsg(q, res);
42  return res;
43  }
44 
49 inline
50 tf2::Quaternion toQuaternion(const geometry_msgs::QuaternionStamped& q) {
51  tf2::Quaternion res;
52  fromMsg(q.quaternion, res);
53  return res;
54  }
55 
60 template<typename T>
62  geometry_msgs::QuaternionStamped q = toMsg(t);
63  return toQuaternion(q);
64  }
65 
71 template<typename T>
73  geometry_msgs::Quaternion q = toMsg(t);
74  return toQuaternion(q);
75  }
76 
86 inline
87 void getEulerYPR(const tf2::Quaternion& q, double &yaw, double &pitch, double &roll)
88 {
89  double sqw;
90  double sqx;
91  double sqy;
92  double sqz;
93 
94  sqx = q.x() * q.x();
95  sqy = q.y() * q.y();
96  sqz = q.z() * q.z();
97  sqw = q.w() * q.w();
98 
99  // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
100  double sarg = -2 * (q.x()*q.z() - q.w()*q.y()) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */
101  if (sarg <= -0.99999) {
102  pitch = -0.5*M_PI;
103  roll = 0;
104  yaw = -2 * atan2(q.y(), q.x());
105  } else if (sarg >= 0.99999) {
106  pitch = 0.5*M_PI;
107  roll = 0;
108  yaw = 2 * atan2(q.y(), q.x());
109  } else {
110  pitch = asin(sarg);
111  roll = atan2(2 * (q.y()*q.z() + q.w()*q.x()), sqw - sqx - sqy + sqz);
112  yaw = atan2(2 * (q.x()*q.y() + q.w()*q.z()), sqw + sqx - sqy - sqz);
113  }
114 };
115 
122 inline
123 double getYaw(const tf2::Quaternion& q)
124 {
125  double yaw;
126 
127  double sqw;
128  double sqx;
129  double sqy;
130  double sqz;
131 
132  sqx = q.x() * q.x();
133  sqy = q.y() * q.y();
134  sqz = q.z() * q.z();
135  sqw = q.w() * q.w();
136 
137  // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
138  double sarg = -2 * (q.x()*q.z() - q.w()*q.y()) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */
139 
140  if (sarg <= -0.99999) {
141  yaw = -2 * atan2(q.y(), q.x());
142  } else if (sarg >= 0.99999) {
143  yaw = 2 * atan2(q.y(), q.x());
144  } else {
145  yaw = atan2(2 * (q.x()*q.y() + q.w()*q.z()), sqw + sqx - sqy - sqz);
146  }
147  return yaw;
148 };
149 
150 }
151 }
152 
153 #endif //TF2_IMPL_UTILS_H
void fromMsg(const A &, B &b)
double getYaw(const tf2::Quaternion &q)
Definition: impl/utils.h:123
B toMsg(const A &a)
void getEulerYPR(const tf2::Quaternion &q, double &yaw, double &pitch, double &roll)
Definition: impl/utils.h:87
tf2::Quaternion toQuaternion(const tf2::Quaternion &q)
Definition: impl/utils.h:30
The data type which will be cross compatable with geometry_msgs This is the tf2 datatype equivilant o...
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:28


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Oct 16 2020 19:08:50