transform_helpers.hpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Aldebaran
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  *
00016 */
00017 
00018 
00019 #ifndef TRANSFORM_HELPERS_HPP
00020 #define TRANSFORM_HELPERS_HPP
00021 
00022 #include <geometry_msgs/Transform.h>
00023 #include <geometry_msgs/Pose.h>
00024 #include <tf2/LinearMath/Matrix3x3.h>
00025 
00026 namespace naoqi
00027 {
00028 namespace helpers
00029 {
00030 namespace transform
00031 {
00032 
00033 inline double getYaw(const geometry_msgs::Pose& pose)
00034 {
00035   double yaw, _pitch, _roll;
00036   tf2::Matrix3x3(tf2::Quaternion(pose.orientation.x, pose.orientation.y,
00037                                 pose.orientation.z, pose.orientation.w)).getEulerYPR(yaw, _pitch, _roll);
00038   return yaw;
00039 }
00040 
00041 inline double getYaw( const geometry_msgs::Transform& pose)
00042 {
00043   double yaw, _pitch, _roll;
00044   tf2::Matrix3x3(tf2::Quaternion(pose.rotation.x, pose.rotation.y, pose.rotation.z, pose.rotation.w)).getEulerYPR(yaw, _pitch, _roll);
00045   return yaw;
00046 }
00047 
00048 } //transform
00049 } //helpers
00050 } // naoqi
00051 
00052 #endif


naoqi_driver
Author(s): Karsten Knese
autogenerated on Sun Sep 17 2017 02:57:14