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();
 
  static constexpr bool positional() { return true; }
 
  static constexpr bool rotational() { return true; }
 
  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