SpatialState.hpp

#pragma once
 
#include "state_representation/State.hpp"
 
namespace state_representation {
class SpatialState : public State {
private:
  std::string reference_frame_; 
 
public:
  SpatialState();
 
  explicit SpatialState(const StateType& type);
 
  explicit SpatialState(
      const StateType& type, const std::string& name, const std::string& reference_frame = "world",
      const bool& empty = true
  );
 
  SpatialState(const SpatialState& state) = default;
 
  friend void swap(SpatialState& state1, SpatialState& state2);
 
  SpatialState& operator=(const SpatialState& state);
 
  const std::string& get_reference_frame() const;
 
  virtual void set_reference_frame(const std::string& reference_frame);
 
  virtual bool is_compatible(const State& state) const override;
 
  friend std::ostream& operator<<(std::ostream& os, const SpatialState& state);
};
 
inline void swap(SpatialState& state1, SpatialState& state2) {
  swap(static_cast<State&>(state1), static_cast<State&>(state2));
  std::swap(state1.reference_frame_, state2.reference_frame_);
}
 
inline SpatialState& SpatialState::operator=(const SpatialState& state) {
  SpatialState tmp(state);
  swap(*this, tmp);
  return *this;
}
 
inline const std::string& SpatialState::get_reference_frame() const {
  return this->reference_frame_;
}
 
inline void SpatialState::set_reference_frame(const std::string& reference_frame) {
  this->reference_frame_ = reference_frame;
}
 
inline bool SpatialState::is_compatible(const State& state) const {
  bool compatible = (this->get_name() == state.get_name())
      && (this->reference_frame_ == dynamic_cast<const SpatialState&>(state).reference_frame_);
  return compatible;
}
}