State.hpp

#pragma once
 
#include <cassert>
#include <chrono>
#include <iostream>
#include <memory>
#include <typeinfo>
 
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
 
#include "state_representation/StateType.hpp"
#include "state_representation/MathTools.hpp"
 
namespace state_representation {
 
class State : public std::enable_shared_from_this<State> {
public:
  State();
 
  explicit State(const std::string& name);
 
  State(const State& state);
 
  virtual ~State() = default;
 
  friend void swap(State& state1, State& state2);
 
  State& operator=(const State& state);
 
  const StateType& get_type() const;
 
  const std::string& get_name() const;
 
  bool is_empty() const;
 
  const std::chrono::time_point<std::chrono::steady_clock>& get_timestamp() const;
 
  virtual void set_name(const std::string& name);
 
  void reset_timestamp();
 
  virtual void set_data(const Eigen::VectorXd& data);
 
  virtual void set_data(const std::vector<double>& data);
 
  virtual void set_data(const Eigen::MatrixXd& data);
 
  double get_age() const;
 
  virtual bool is_incompatible(const State& state) const;
 
  bool is_deprecated(double time_delay) const;
 
  template<typename DurationT>
  bool is_deprecated(const std::chrono::duration<int64_t, DurationT>& time_delay) const;
 
  virtual void reset();
 
  explicit operator bool() const noexcept;
 
  friend std::ostream& operator<<(std::ostream& os, const State& state);
 
protected:
  void set_type(const StateType& type);
 
  void set_empty(bool empty = true);
 
  void assert_not_empty() const;
 
  virtual std::string to_string() const;
 
private:
  StateType type_;                                              
  std::string name_;                                            
  bool empty_;                                                  
  std::chrono::time_point<std::chrono::steady_clock> timestamp_;
};
 
inline void swap(State& state1, State& state2) {
  std::swap(state1.name_, state2.name_);
  std::swap(state1.empty_, state2.empty_);
  std::swap(state1.timestamp_, state2.timestamp_);
}
 
template<typename DurationT>
inline bool State::is_deprecated(const std::chrono::duration<int64_t, DurationT>& time_delay) const {
  return ((std::chrono::steady_clock::now() - this->timestamp_) > time_delay);
}
 
template<typename T>
std::shared_ptr<State> make_shared_state(const T& state) {
  return std::make_shared<T>(state);
}
}// namespace state_representation