utils.hpp

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_HPP_
#define TF2__IMPL__UTILS_HPP_
 
#include <tf2/convert.hpp>
#include <tf2/transform_datatypes.hpp>
#include <tf2/LinearMath/Quaternion.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/quaternion_stamped.hpp>
 
 
namespace tf2
{
 
// Forward declare functions needed in this header
void fromMsg(const geometry_msgs::msg::Quaternion & in, tf2::Quaternion & out);
 
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/
  // normalization added from urdfom_headers
  double sarg = -2 * (q.x() * q.z() - q.w() * q.y()) / (sqx + sqy + sqz + sqw);
  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/
  // normalization added from urdfom_headers
  double sarg = -2 * (q.x() * q.z() - q.w() * q.y()) / (sqx + sqy + sqz + sqw);
 
  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;
}
 
}  // namespace impl
}  // namespace tf2
#endif  // TF2__IMPL__UTILS_HPP_