angular_position_ref.h

Go to the documentation of this file.

#ifndef OPEN_WALKER_CORE_ANGULAR_POSITION_REF_H
#define OPEN_WALKER_CORE_ANGULAR_POSITION_REF_H

#include <ow_core/conversions.h>
#include <ow_core/quaternion_ref.h>

namespace ow_core
{
template <typename _Derived>
class AngularPositionRef : public QuaternionRef<_Derived>
{
public:
  typedef _Derived Derived;
  typedef QuaternionRef<Derived> Base;

  //    typedef Eigen::internal::traits<Derived>::Scalar Scalar;

public:
  explicit AngularPositionRef(Derived& ref, int startRow = 0, int startCol = 0) : 
    Base(ref, startRow, startCol)
  {
  }

  using Base::operator=;

  using Base::operator*=;

  void operator=(const tf::Quaternion& q)
  {
    quaternionTFToEigenQuaternion(q, *this);
  }

  void operator=(const geometry_msgs::Quaternion& q)
  {
    quaternionMsgToEigenQuaternion(q, *this);
  }

  void operator=(const tf::Matrix3x3& other)
  {
    matrixTFToEigenQuaternion(other, *this);
  }

  operator tf::Quaternion() const
  {
    tf::Quaternion q;
    quaternionEigenToQuaternionTF(*this, q);
    return q;
  }

  operator geometry_msgs::Quaternion() const
  {
    geometry_msgs::Quaternion q;
    quaternionEigenToQuaternionMsg(*this, q);
    return q;
  }

  operator tf::Matrix3x3() const
  {
    tf::Matrix3x3 m;
    quaternionEigenToMatrixTF(*this, m);
    return m;
  }

  tf::Quaternion toQuaternionTF()
  {
    return static_cast<tf::Quaternion>(*this);
  }

  geometry_msgs::Quaternion toQuaternionMsg()
  {
    return static_cast<geometry_msgs::Quaternion>(*this);
  }

  tf::Matrix3x3 toMatrixTF()
  {
    return static_cast<tf::Matrix3x3>(*this);
  }

private:
  AngularPositionRef();
};

}  // namespace ow_core

#endif  // OPEN_WALKER_CORE_ANGULAR_POSITION_REF_H