15 #ifndef TF2_IMPL_UTILS_H
16 #define TF2_IMPL_UTILS_H
18 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
62 geometry_msgs::msg::QuaternionStamped q = toMsg<tf2::Stamped<T>, geometry_msgs::msg::QuaternionStamped>(t);
73 geometry_msgs::msg::Quaternion q = toMsg<T, geometry_msgs::msg::QuaternionStamped>(t);
89 const double pi_2 = 1.57079632679489661923;
101 double sarg = -2 * (q.x()*q.z() - q.w()*q.y()) / (sqx + sqy + sqz + sqw);
102 if (sarg <= -0.99999) {
105 yaw = -2 * atan2(q.y(), q.x());
106 }
else if (sarg >= 0.99999) {
109 yaw = 2 * atan2(q.y(), q.x());
112 roll = atan2(2 * (q.y()*q.z() + q.w()*q.x()), sqw - sqx - sqy + sqz);
113 yaw = atan2(2 * (q.x()*q.y() + q.w()*q.z()), sqw + sqx - sqy - sqz);
139 double sarg = -2 * (q.x()*q.z() - q.w()*q.y()) / (sqx + sqy + sqz + sqw);
141 if (sarg <= -0.99999) {
142 yaw = -2 * atan2(q.y(), q.x());
143 }
else if (sarg >= 0.99999) {
144 yaw = 2 * atan2(q.y(), q.x());
146 yaw = atan2(2 * (q.x()*q.y() + q.w()*q.z()), sqw + sqx - sqy - sqz);
154 #endif //TF2_IMPL_UTILS_H