15 #ifndef TF2_IMPL_UTILS_H 16 #define TF2_IMPL_UTILS_H 18 #include <tf2_geometry_msgs/tf2_geometry_msgs.h> 62 geometry_msgs::QuaternionStamped q =
toMsg(t);
73 geometry_msgs::Quaternion q =
toMsg(t);
100 double sarg = -2 * (q.x()*q.z() - q.w()*q.y()) / (sqx + sqy + sqz + sqw);
101 if (sarg <= -0.99999) {
104 yaw = -2 * atan2(q.y(), q.x());
105 }
else if (sarg >= 0.99999) {
108 yaw = 2 * atan2(q.y(), q.x());
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);
138 double sarg = -2 * (q.x()*q.z() - q.w()*q.y()) / (sqx + sqy + sqz + sqw);
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());
145 yaw = atan2(2 * (q.x()*q.y() + q.w()*q.z()), sqw + sqx - sqy - sqz);
153 #endif //TF2_IMPL_UTILS_H
void fromMsg(const A &, B &b)
double getYaw(const tf2::Quaternion &q)
void getEulerYPR(const tf2::Quaternion &q, double &yaw, double &pitch, double &roll)
tf2::Quaternion toQuaternion(const tf2::Quaternion &q)
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...