Stamped.h

Go to the documentation of this file.

 
#include "ReferenceFrame.h"
 
#ifndef _GAMS_POSE_STAMPED_H_
#define _GAMS_POSE_STAMPED_H_
 
#include "gams/GamsExport.h"
#include <string>
#include <cfloat>
#include <utility>
#include "gams/CPP11_compat.h"
#include "madara/knowledge/containers/DoubleVector.h"
#include "madara/knowledge/containers/NativeDoubleVector.h"
#include "ReferenceFrameFwd.h"
#include "Coordinate.h"
 
namespace gams { namespace pose {
 
using madara::utility::Clock;
using madara::utility::TimeValue;
using madara::utility::Duration;
using madara::utility::SecondsDuration;
 
MADARA_MAKE_VAL_SUPPORT_TEST(timestamp, x,
    (x.time(), x.time(TimeValue{})));
 
inline TimeValue try_get_time()
{
  return {};
}
 
template<typename T, typename... Args>
inline auto try_get_time(const T& v, Args&&...) ->
  typename std::enable_if<supports_timestamp<T>::value, TimeValue>::type
{
  return v.time();
}
 
template<typename T, typename... Args>
inline auto try_get_time(const T&, Args&&... args) ->
  typename std::enable_if<!supports_timestamp<T>::value, TimeValue>::type
{
  return try_get_time(std::forward<Args>(args)...);
}
 
template<typename Impl>
class Stamped : public Impl
{
public:
  using Impl::self;
 
  using derived_type = typename Impl::derived_type;
 
  Stamped() = default;
 
#ifdef DOXYGEN
  template<typename... Args>
  Stamped(Args&&... args);
#else
  template<typename Arg, typename... Args,
    typename std::enable_if<!std::is_same<typename std::decay<Arg>::type,
      TimeValue>::value && sizeof...(Args) >= 1, void*>::type = nullptr>
  Stamped(Arg&& arg, Args&&... args)
    : Impl((Arg &)arg, ((Args &)args)...),
      time_(try_get_time((Arg &)arg, ((Args &)args)...)) {}
 
  template<typename Arg,
    typename std::enable_if<!std::is_same<typename std::decay<Arg>::type,
      TimeValue>::value, void*>::type = nullptr>
  explicit Stamped(Arg&& arg)
    : Impl((Arg &)arg), time_(try_get_time((Arg &)arg)) {}
#endif
 
  explicit Stamped(TimeValue time)
    : Impl(), time_(time) {}
 
#ifdef DOXYGEN
  template<typename... Args>
  Stamped(TimeValue time, Args&&... args);
#else
  template<typename... Args,
    typename std::enable_if<(sizeof...(Args) >= 1), void*>::type = nullptr>
  Stamped(TimeValue time, Args&&... args)
    : Impl(std::forward<Args>(args)...), time_(time) {}
#endif
 
  TimeValue time() const { return time_; }
 
  TimeValue time(TimeValue v) { return (time_ = v); }
 
  uint64_t nanos() const {
    namespace sc = std::chrono;
    return sc::duration_cast<Duration>(
        time_.time_since_epoch()).count();
  }
 
  uint64_t nanos(uint64_t v) {
    Duration dur(v);
    time_ = TimeValue(dur);
    return v;
  }
 
  double secs() const {
    namespace sc = std::chrono;
    return sc::duration_cast<SecondsDuration>(
        time_.time_since_epoch()).count();
  }
 
  double secs(double v) {
    namespace sc = std::chrono;
    SecondsDuration dur(v);
    Duration ndur(sc::duration_cast<Duration>(dur));
    time_ = TimeValue(ndur);
    return v;
  }
 
private:
  TimeValue time_;
};
 
template<typename Base>
inline std::ostream &operator<<(std::ostream &o, const Stamped<Base> &v)
{
  return o << v << "@" << v.time();
}
 
} }
 
#endif