ReferenceFrameFwd.h

Go to the documentation of this file.

#ifndef _GAMS_POSE_REFERENCE_FRAME_FWD_H_
#define _GAMS_POSE_REFERENCE_FRAME_FWD_H_

#include "gams/GamsExport.h"
#include "gams/CPP11_compat.h"
#include "madara/knowledge/KnowledgeBase.h"
#include <stdexcept>
#include <sstream>

namespace gams { namespace pose {

class ReferenceFrameIdentity;
class ReferenceFrameVersion;
struct ReferenceFrameType;
class Pose;
class Position;
class Orientation;
class ReferenceFrame;

MADARA_MAKE_VAL_SUPPORT_TEST(transform_to, x,
    (x.transform_to(std::declval<ReferenceFrame>())));

class GAMS_EXPORT FrameEvalSettings : public madara::knowledge::EvalSettings
{
public:
  using Base = madara::knowledge::EvalSettings;

  static const FrameEvalSettings DEFAULT;

  FrameEvalSettings(Base base = Base{true}) : Base(std::move(base)) {}

  FrameEvalSettings(std::string prefix, Base base = Base{true})
    : Base(std::move(base)),
      prefix_(std::make_shared<std::string>(std::move(prefix))) {}

  FrameEvalSettings(std::nullptr_t, Base base = Base{true})
    : FrameEvalSettings(std::move(base)) {}

  FrameEvalSettings(const char *prefix, Base base = Base{true})
    : FrameEvalSettings(std::string(prefix), std::move(base)) {}

  static const std::string &default_prefix() {
    return default_prefix_;
  }

  const std::string &prefix() const {
    if (prefix_) {
      return *prefix_;
    } else {
      return default_prefix();
    }
  }

  void prefix(std::string prefix) {
    prefix_ = std::make_shared<std::string>(std::move(prefix));
  }

private:
  static const std::string default_prefix_;

  std::shared_ptr<std::string> prefix_;
};

class GAMS_EXPORT ReferenceFrame
{
private:
  std::shared_ptr<ReferenceFrameVersion> impl_;

public:
  ReferenceFrame() : impl_() {}

  ReferenceFrame(std::shared_ptr<ReferenceFrameVersion> impl) :
    impl_(std::move(impl)) {}

  template<typename P,
    typename std::enable_if<
      supports_transform_to<P>::value, void*>::type = nullptr>
  explicit ReferenceFrame(
      P &&origin,
      uint64_t timestamp = -1)
    : impl_(std::make_shared<ReferenceFrameVersion>(
          std::forward<P>(origin), timestamp)) {}

  template<typename P,
    typename std::enable_if<
      supports_transform_to<P>::value, void*>::type = nullptr>
  ReferenceFrame(
      const ReferenceFrameType *type,
      P &&origin,
      uint64_t timestamp = -1)
    : impl_(std::make_shared<ReferenceFrameVersion>(
          type, std::forward<P>(origin), timestamp)) {}

  template<typename P,
    typename std::enable_if<
      supports_transform_to<P>::value, void*>::type = nullptr>
  ReferenceFrame(
      std::string id,
      P &&origin,
      uint64_t timestamp = -1)
    : impl_(std::make_shared<ReferenceFrameVersion>(
          id, std::forward<P>(origin), timestamp)) {}

  template<typename P,
    typename std::enable_if<
      supports_transform_to<P>::value, void*>::type = nullptr>
  ReferenceFrame(
      const ReferenceFrameType *type,
      std::string id,
      P &&origin,
      uint64_t timestamp = -1)
    : impl_(std::make_shared<ReferenceFrameVersion>(
          type, id, std::forward<P>(origin), timestamp)) {}

  bool valid() const { return (bool)impl_; }

  const Pose &origin() const;

  ReferenceFrame pose(const Pose &new_origin) const;

  ReferenceFrame move(const Position &new_origin) const;

  ReferenceFrame orient(const Orientation &new_origin) const;

  ReferenceFrame pose(const Pose &new_origin, uint64_t timestamp) const;

  ReferenceFrame move(const Position &new_origin, uint64_t timestamp) const;

  ReferenceFrame orient(const Orientation &new_origin, uint64_t timestamp) const;

  ReferenceFrame origin_frame() const;

  bool operator==(const ReferenceFrame &other) const;

  bool operator!=(const ReferenceFrame &other) const;

  bool operator<(const ReferenceFrame &other) const
  {
    return impl_ < other.impl_;
  }

  bool operator<=(const ReferenceFrame &other) const
  {
    return impl_ <= other.impl_;
  }

  bool operator>(const ReferenceFrame &other) const
  {
    return impl_ > other.impl_;
  }

  bool operator>=(const ReferenceFrame &other) const
  {
    return impl_ >= other.impl_;
  }

  std::string name() const;

  const std::string &id() const;

  const ReferenceFrameType *type() const;

  uint64_t timestamp() const;

  ReferenceFrame timestamp(uint64_t) const;

  uint64_t expiry(uint64_t age = -1) const;

  uint64_t expiry() const;

  static uint64_t default_expiry(uint64_t age);

  static uint64_t default_expiry();

  static const std::string &default_prefix();

  bool interpolated() const;

  void save(madara::knowledge::KnowledgeBase &kb,
            const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT) const;

  void save(madara::knowledge::KnowledgeBase &kb,
            uint64_t expiry,
            const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT) const;

  static ReferenceFrame load(
          madara::knowledge::KnowledgeBase &kb,
          const std::string &id,
          uint64_t timestamp = -1,
          const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT);

  template<typename InputIterator>
  static std::vector<ReferenceFrame> load_tree(
        madara::knowledge::KnowledgeBase &kb,
        InputIterator begin,
        InputIterator end,
        uint64_t timestamp = -1,
        const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT);

  template<typename Container>
  static std::vector<ReferenceFrame> load_tree(
        madara::knowledge::KnowledgeBase &kb,
        const Container &ids,
        uint64_t timestamp = -1,
        const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT);

  void save_as(
        madara::knowledge::KnowledgeBase &kb,
        const std::string &key,
        const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT) const;

  void save_as(madara::knowledge::KnowledgeBase &kb,
               const std::string &key, uint64_t expiry,
               const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT) const;

  ReferenceFrame interpolate(const ReferenceFrame &other,
      ReferenceFrame parent, uint64_t time) const;

  friend class ReferenceFrameVersion;
};

struct GAMS_EXPORT ReferenceFrameType {
  int type_id;

  const char *name;

  void (*transform_linear_to_origin)(
                  const ReferenceFrameType *origin,
                  const ReferenceFrameType *self,
                  double ox, double oy, double oz,
                  double orx, double ory, double orz,
                  double &x, double &y, double &z,
                  bool fixed);

  void (*transform_linear_from_origin)(
                  const ReferenceFrameType *origin,
                  const ReferenceFrameType *self,
                  double ox, double oy, double oz,
                  double orx, double ory, double orz,
                  double &x, double &y, double &z,
                  bool fixed);

  void (*normalize_linear)(
                  const ReferenceFrameType *self,
                  double &x, double &y, double &z);

  double (*calc_distance)(
                  const ReferenceFrameType *self,
                  double x1, double y1, double z1,
                  double x2, double y2, double z2);

  void (*transform_angular_to_origin)(
                  const ReferenceFrameType *origin,
                  const ReferenceFrameType *self,
                  double orx, double ory, double orz,
                  double &rx, double &ry, double &rz);

  void (*transform_angular_from_origin)(
                  const ReferenceFrameType *origin,
                  const ReferenceFrameType *self,
                  double orx, double ory, double orz,
                  double &rx, double &ry, double &rz);

  void (*normalize_angular)(
                  const ReferenceFrameType *self,
                  double &rx, double &ry, double &rz);

  double (*calc_angle)(
                  const ReferenceFrameType *self,
                  double rx1, double ry1, double rz1,
                  double rx2, double ry2, double rz2);

  void (*transform_pose_to_origin)(
                  const ReferenceFrameType *origin,
                  const ReferenceFrameType *self,
                  double ox, double oy, double oz,
                  double orx, double ory, double orz,
                  double &x, double &y, double &z,
                  double &rx, double &ry, double &rz,
                  bool fixed);

  void (*transform_pose_from_origin)(
                  const ReferenceFrameType *origin,
                  const ReferenceFrameType *self,
                  double ox, double oy, double oz,
                  double orx, double ory, double orz,
                  double &x, double &y, double &z,
                  double &rx, double &ry, double &rz,
                  bool fixed);

  void (*normalize_pose)(
                  const ReferenceFrameType *self,
                  double &x, double &y, double &z,
                  double &rx, double &ry, double &rz);
};

GAMS_EXPORT const ReferenceFrame &default_frame (void);

} }

#endif