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 ReferenceFrameArena;
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() {
    std::lock_guard<std::mutex> guard(defaults_lock_);
    return default_prefix_;
  }
 
  static void set_default_prefix(std::string new_prefix) {
    std::lock_guard<std::mutex> guard(defaults_lock_);
    default_prefix_ = new_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 std::mutex defaults_lock_;
  static std::string default_prefix_;
 
  std::shared_ptr<std::string> prefix_;
};
 
class GAMS_EXPORT ReferenceFrame
{
private:
  std::shared_ptr<ReferenceFrameVersion> impl_;
 
public:
  static const uint64_t ETERNAL = 0UL - 1;
  static const uint64_t TEMP = 0UL - 2;
 
  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 = ETERNAL)
    : 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 = ETERNAL)
    : 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 = ETERNAL)
    : 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 = ETERNAL)
    : 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 = ETERNAL) 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;
 
  bool temp() 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 = ETERNAL,
          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 = ETERNAL,
        const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT,
        ReferenceFrameArena *arena = nullptr);
 
  template<typename Container>
  static std::vector<ReferenceFrame> load_tree(
        madara::knowledge::KnowledgeBase &kb,
        const Container &ids,
        uint64_t timestamp = ETERNAL,
        const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT,
        ReferenceFrameArena *arena = nullptr);
 
  static std::vector<ReferenceFrame> load_tree(
        madara::knowledge::KnowledgeBase &kb,
        const std::initializer_list<const char *> &ids,
        uint64_t timestamp = ETERNAL,
        const FrameEvalSettings &settings = FrameEvalSettings::DEFAULT,
        ReferenceFrameArena *arena = nullptr);
 
  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;
  friend GAMS_EXPORT const ReferenceFrame *find_common_frame(
    const ReferenceFrame *from, const ReferenceFrame *to,
    std::vector<const ReferenceFrame *> *to_stack);
};
 
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