utils.h
Go to the documentation of this file.
00001 // Copyright 2014 Open Source Robotics Foundation, Inc.
00002 //
00003 // Licensed under the Apache License, Version 2.0 (the "License");
00004 // you may not use this file except in compliance with the License.
00005 // You may obtain a copy of the License at
00006 //
00007 //     http://www.apache.org/licenses/LICENSE-2.0
00008 //
00009 // Unless required by applicable law or agreed to in writing, software
00010 // distributed under the License is distributed on an "AS IS" BASIS,
00011 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00012 // See the License for the specific language governing permissions and
00013 // limitations under the License.
00014 
00015 #ifndef TF2_IMPL_UTILS_H
00016 #define TF2_IMPL_UTILS_H
00017 
00018 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00019 #include <tf2/transform_datatypes.h>
00020 #include <tf2/LinearMath/Quaternion.h>
00021 
00022 namespace tf2 {
00023 namespace impl {
00024 
00029 inline
00030 tf2::Quaternion toQuaternion(const tf2::Quaternion& q) {
00031     return q;
00032   }
00033 
00038 inline
00039 tf2::Quaternion toQuaternion(const geometry_msgs::Quaternion& q) {
00040     tf2::Quaternion res;
00041     fromMsg(q, res);
00042     return res;
00043   }
00044 
00049 inline
00050 tf2::Quaternion toQuaternion(const geometry_msgs::QuaternionStamped& q) {
00051     tf2::Quaternion res;
00052     fromMsg(q.quaternion, res);
00053     return res;
00054   }
00055 
00060 template<typename T>
00061   tf2::Quaternion toQuaternion(const tf2::Stamped<T>& t) {
00062     geometry_msgs::QuaternionStamped q = toMsg(t);
00063     return toQuaternion(q);
00064   }
00065 
00071 template<typename T>
00072   tf2::Quaternion toQuaternion(const T& t) {
00073     geometry_msgs::Quaternion q = toMsg(t);
00074     return toQuaternion(q);
00075   }
00076 
00086 inline
00087 void getEulerYPR(const tf2::Quaternion& q, double &yaw, double &pitch, double &roll)
00088 {
00089   double sqw;
00090   double sqx;
00091   double sqy;
00092   double sqz;
00093 
00094   sqx = q.x() * q.x();
00095   sqy = q.y() * q.y();
00096   sqz = q.z() * q.z();
00097   sqw = q.w() * q.w();
00098 
00099   // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
00100   double sarg = -2 * (q.x()*q.z() - q.w()*q.y()) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */
00101   if (sarg <= -0.99999) {
00102     pitch = -0.5*M_PI;
00103     roll  = 0;
00104     yaw   = -2 * atan2(q.y(), q.x());
00105   } else if (sarg >= 0.99999) {
00106     pitch = 0.5*M_PI;
00107     roll  = 0;
00108     yaw   = 2 * atan2(q.y(), q.x());
00109   } else {
00110     pitch = asin(sarg);
00111     roll  = atan2(2 * (q.y()*q.z() + q.w()*q.x()), sqw - sqx - sqy + sqz);
00112     yaw   = atan2(2 * (q.x()*q.y() + q.w()*q.z()), sqw + sqx - sqy - sqz);
00113   }
00114 };
00115 
00122 inline
00123 double getYaw(const tf2::Quaternion& q)
00124 {
00125   double yaw;
00126 
00127   double sqw;
00128   double sqx;
00129   double sqy;
00130   double sqz;
00131 
00132   sqx = q.x() * q.x();
00133   sqy = q.y() * q.y();
00134   sqz = q.z() * q.z();
00135   sqw = q.w() * q.w();
00136 
00137   // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
00138   double sarg = -2 * (q.x()*q.z() - q.w()*q.y()) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */
00139 
00140   if (sarg <= -0.99999) {
00141     yaw   = -2 * atan2(q.y(), q.x());
00142   } else if (sarg >= 0.99999) {
00143     yaw   = 2 * atan2(q.y(), q.x());
00144   } else {
00145     yaw   = atan2(2 * (q.x()*q.y() + q.w()*q.z()), sqw + sqx - sqy - sqz);
00146   }
00147   return yaw;
00148 };
00149 
00150 }
00151 }
00152 
00153 #endif //TF2_IMPL_UTILS_H


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Wed Nov 7 2018 07:51:22