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_UTILS_H 00016 #define TF2_UTILS_H 00017 00018 #include <tf2/LinearMath/Transform.h> 00019 #include <tf2/LinearMath/Quaternion.h> 00020 #include <tf2/impl/utils.h> 00021 00022 namespace tf2 { 00030 template <class A> 00031 void getEulerYPR(const A& a, double& yaw, double& pitch, double& roll) 00032 { 00033 tf2::Quaternion q = impl::toQuaternion(a); 00034 impl::getEulerYPR(q, yaw, pitch, roll); 00035 } 00036 00044 template <class A> 00045 double getYaw(const A& a) 00046 { 00047 tf2::Quaternion q = impl::toQuaternion(a); 00048 return impl::getYaw(q); 00049 } 00050 00054 template <class A> 00055 A getTransformIdentity() 00056 { 00057 tf2::Transform t; 00058 t.setIdentity(); 00059 A a; 00060 convert(t, a); 00061 return a; 00062 } 00063 00064 } 00065 00066 #endif //TF2_UTILS_H