utils.h

Go to the documentation of this file.

// Copyright 2014 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#ifndef TF2_IMPL_UTILS_H
#define TF2_IMPL_UTILS_H
 
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/transform_datatypes.h>
#include <tf2/LinearMath/Quaternion.h>
 
namespace tf2 {
namespace impl {
 
inline
tf2::Quaternion toQuaternion(const tf2::Quaternion& q) {
    return q;
  }
 
inline
tf2::Quaternion toQuaternion(const geometry_msgs::msg::Quaternion& q) {
    tf2::Quaternion res;
    fromMsg(q, res);
    return res;
  }
 
inline
tf2::Quaternion toQuaternion(const geometry_msgs::msg::QuaternionStamped& q) {
    tf2::Quaternion res;
    fromMsg(q.quaternion, res);
    return res;
  }
 
template<typename T>
  tf2::Quaternion toQuaternion(const tf2::Stamped<T>& t) {
    geometry_msgs::msg::QuaternionStamped q = toMsg<tf2::Stamped<T>, geometry_msgs::msg::QuaternionStamped>(t);
    return toQuaternion(q);
  }
 
template<typename T>
  tf2::Quaternion toQuaternion(const T& t) {
    geometry_msgs::msg::Quaternion q = toMsg<T, geometry_msgs::msg::QuaternionStamped>(t);
    return toQuaternion(q);
  }
 
inline
void getEulerYPR(const tf2::Quaternion& q, double &yaw, double &pitch, double &roll)
{
  const double pi_2 = 1.57079632679489661923;
  double sqw;
  double sqx;
  double sqy;
  double sqz;
 
  sqx = q.x() * q.x();
  sqy = q.y() * q.y();
  sqz = q.z() * q.z();
  sqw = q.w() * q.w();
 
  // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
  double sarg = -2 * (q.x()*q.z() - q.w()*q.y()) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */
  if (sarg <= -0.99999) {
    pitch = -0.5*pi_2;
    roll  = 0;
    yaw   = -2 * atan2(q.y(), q.x());
  } else if (sarg >= 0.99999) {
    pitch = 0.5*pi_2;
    roll  = 0;
    yaw   = 2 * atan2(q.y(), q.x());
  } else {
    pitch = asin(sarg);
    roll  = atan2(2 * (q.y()*q.z() + q.w()*q.x()), sqw - sqx - sqy + sqz);
    yaw   = atan2(2 * (q.x()*q.y() + q.w()*q.z()), sqw + sqx - sqy - sqz);
  }
}
 
inline
double getYaw(const tf2::Quaternion& q)
{
  double yaw;
 
  double sqw;
  double sqx;
  double sqy;
  double sqz;
 
  sqx = q.x() * q.x();
  sqy = q.y() * q.y();
  sqz = q.z() * q.z();
  sqw = q.w() * q.w();
 
  // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
  double sarg = -2 * (q.x()*q.z() - q.w()*q.y()) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */
 
  if (sarg <= -0.99999) {
    yaw   = -2 * atan2(q.y(), q.x());
  } else if (sarg >= 0.99999) {
    yaw   = 2 * atan2(q.y(), q.x());
  } else {
    yaw   = atan2(2 * (q.x()*q.y() + q.w()*q.z()), sqw + sqx - sqy - sqz);
  }
  return yaw;
}
 
}
}
 
#endif //TF2_IMPL_UTILS_H