Pose.h

Go to the documentation of this file.

#include "ReferenceFrame.h"

#ifndef _GAMS_POSE_POSE_H_
#define _GAMS_POSE_POSE_H_

#include <iostream>
#include <string>

#include "gams/pose/Position.h"
#include "gams/pose/Orientation.h"

namespace gams { namespace pose {

template<typename Derived>
class BasicPose : public PositionVector, public OrientationVector
{
public:
  using derived_type = Derived;

  BasicPose (double x, double y)
    : BasicPose(x, y, 0, 0, 0, 0) {}

  BasicPose (double x, double y, double z)
    : BasicPose(x, y, z, 0, 0, 0) {}


  BasicPose (double x, double y, double z,
              double rx, double ry, double rz);

  BasicPose (const PositionVector &pos);

  BasicPose (const OrientationVector &rot);

  BasicPose (const PositionVector &pos,
             const OrientationVector &rot);

  BasicPose ();

  Derived &self() { return static_cast<Derived&>(*this); }
  const Derived &self() const { return static_cast<const Derived&>(*this); }

  template<typename Derived2>
  operator BasicVector<Derived2, units::absolute<units::length>>() const
  {
    return as_position_vec();
  }

  template<typename Derived2>
  operator BasicVector<Derived2, units::absolute<units::plane_angle>>() const
  {
    return as_orientation_vec();
  }

  explicit BasicPose (
    const madara::knowledge::containers::DoubleVector &vec);

  explicit BasicPose (
    const madara::knowledge::containers::NativeDoubleVector &vec);

  explicit BasicPose (
    const madara::knowledge::containers::DoubleVector &vec_loc,
    const madara::knowledge::containers::DoubleVector &vec_rot);

  explicit BasicPose (
    const madara::knowledge::containers::NativeDoubleVector &vec_loc,
    const madara::knowledge::containers::NativeDoubleVector &vec_rot);

  bool is_set () const;

  bool is_position_set () const;
  bool is_location_set () const;

  bool is_orientation_set () const;

  bool is_position_zero () const;
  bool is_location_zero () const;

  bool is_orientation_zero () const;

  bool is_zero () const;

  bool operator== (const BasicPose &rhs) const;

  static std::string name ();

  int size () const;

  double get (int i) const;

  double set (int i, double val);

  typedef BasicPose BaseType;

  BaseType &as_vec ();

  const BaseType &as_vec () const;

  PositionVector &as_position_vec ();
  PositionVector &as_location_vec ();

  const PositionVector &as_position_vec () const;
  const PositionVector &as_location_vec () const;

  OrientationVector &as_orientation_vec ();

  const OrientationVector &as_orientation_vec () const;

  template<typename Derived2>
  bool approximately_equal(const BasicPose<Derived2> &other,
      double epsilon) const
  {
    (void)other;(void)epsilon;
    throw "unimplemented";
    //return std::fabs(self().distance_to(other.self())) < epsilon;
    return false;
  }

  template<typename Derived2>
  double distance_to(const BasicPose<Derived2> &target) const {
    (void)target;
    throw "unimplemented";
    //return (target.vec() - vec()).norm();
    return 0;
  }

  std::string to_string (
    const std::string & delimiter = ", ",
    const std::string & unset_identifier = "<unset>") const;

  void to_container (
    madara::knowledge::containers::NativeDoubleVector &container) const;

  void from_container (
    const madara::knowledge::containers::NativeDoubleVector &container);

  void from_container (
    const std::vector <double> &container);
};

class PoseVector :
  public BasicPose<PoseVector>
{
  using Base = BasicPose<PoseVector>;
  using Base::Base;

  static constexpr const char *type_name = "PoseVector";
};

class Pose :
  public Framed<BasicPose<Pose>>
{
public:
  using Base = Framed<BasicPose<Pose>>;
  using Base::Base;

  static constexpr const char *type_name = "Pose";

  Pose() = default;

  Pose(const Position &pos)
    : Base(pos.frame(), PositionVector(pos)) {}

  Pose(const StampedPosition &pos)
    : Base(pos.frame(), PositionVector(pos)) {}

  Pose(const Orientation &ori)
    : Base(ori.frame(), OrientationVector(ori)) {}

  Pose(const StampedOrientation &ori)
    : Base(ori.frame(), OrientationVector(ori)) {}

  Pose(const Position &pos, const Orientation &ori)
    : Base(pos.frame(), PositionVector(pos), OrientationVector(ori)) {}

  Pose(const StampedPosition &pos, const StampedOrientation &ori)
    : Base(pos.frame(), PositionVector(pos), OrientationVector(ori)) {}

  double angle_to (const Pose &target) const;

  double angle_to (const Orientation &target) const;

  template<typename U> double angle_to (const Pose &target, U u) const;

  template<typename U> double angle_to (const Orientation &target, U u) const;

  operator Position () const;

  operator Orientation () const;
};

class StampedPose :
  public Stamped<Framed<BasicPose<StampedPose>>> 
{
  using Base = Stamped<Framed<BasicPose<StampedPose>>>;
  using Base::Base;

  static constexpr const char *type_name = "StampedPose";

  StampedPose() = default;

  StampedPose(const Position &pos)
    : Base(pos.frame(), PositionVector(pos)) {}

  StampedPose(const StampedPosition &pos)
    : Base(pos.time(), pos.frame(), PositionVector(pos)) {}

  StampedPose(const Orientation &ori)
    : Base(ori.frame(), OrientationVector(ori)) {}

  StampedPose(const StampedOrientation &ori)
    : Base(ori.time(), ori.frame(), OrientationVector(ori)) {}

  StampedPose(const Position &pos, const Orientation &ori)
    : Base(pos.frame(), PositionVector(pos), OrientationVector(ori)) {}

  StampedPose(const StampedPosition &pos, const StampedOrientation &ori)
    : Base(pos.time(), pos.frame(),
        PositionVector(pos), OrientationVector(ori)) {}

  operator StampedPosition () const
  {
    return StampedPosition(time(), frame(), this->as_position_vec());
  }

  operator StampedOrientation () const
  {
    return StampedOrientation(time(), frame(), this->as_orientation_vec());
  }
};

template<typename Derived>
inline std::ostream &operator<<(std::ostream &o, const BasicPose<Derived> &pose)
{
  o << Derived::type_name << "(";
  o << pose.pos_vec();
  o << ";";
  o << pose.ori_vec();
  o << ")";
  return o;
}

} }

#endif

#ifndef GAMS_NO_INL
#include "Pose.inl"
#endif