state.h

Go to the documentation of this file.

//=================================================================================================
// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
// All rights reserved.
 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the Flight Systems and Automatic Control group,
//       TU Darmstadt, nor the names of its contributors may be used to
//       endorse or promote products derived from this software without
//       specific prior written permission.
 
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================
 
#ifndef HECTOR_POSE_ESTIMATION_STATE_H
#define HECTOR_POSE_ESTIMATION_STATE_H
 
#include <hector_pose_estimation/types.h>
 
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
 
#include <ros/time.h>
 
namespace hector_pose_estimation {
 
class State {
public:
  typedef ColumnVector Vector;
  typedef SymmetricMatrix Covariance;
  typedef Matrix SystemMatrix;
 
  typedef VectorBlock<Vector> VectorSegment;
  typedef Block<Covariance::Base> CovarianceBlock;
  typedef VectorBlock<const Vector> ConstVectorSegment;
  typedef Block<const Covariance::Base> ConstCovarianceBlock;
 
  typedef SubState_<4,3> OrientationStateType;
  typedef VectorBlock<Vector,4> OrientationType;
  typedef VectorBlock<const Vector,4> ConstOrientationType;
  typedef SubState_<3,3> RateStateType;
  typedef VectorBlock<Vector,3> RateType;
  typedef VectorBlock<const Vector,3> ConstRateType;
  typedef SubState_<3,3> PositionStateType;
  typedef VectorBlock<Vector,3> PositionType;
  typedef VectorBlock<const Vector,3> ConstPositionType;
  typedef SubState_<3,3> VelocityStateType;
  typedef VectorBlock<Vector,3> VelocityType;
  typedef VectorBlock<const Vector,3> ConstVelocityType;
  typedef SubState_<3,3> AccelerationStateType;
  typedef VectorBlock<Vector,3> AccelerationType;
  typedef VectorBlock<const Vector,3> ConstAccelerationType;
 
  typedef std::vector<SubStatePtr> SubStates;
 
  typedef typename Matrix_<3,3>::type RotationMatrix;
 
public:
  virtual ~State();
 
  virtual IndexType getVectorDimension() const { return vector_.rows(); }
  virtual IndexType getCovarianceDimension() const { return covariance_.rows(); }
 
  virtual void reset();
  virtual void updated();
  virtual void normalize();
 
  virtual bool valid() const;
 
  virtual BaseState& base() { return *base_; }
  virtual const BaseState& base() const { return *base_; }
 
  virtual const Vector& getVector() const { return vector_; }
  virtual const Covariance& getCovariance() const { return covariance_; }
  template <int Size> VectorBlock<const Vector, Size> getSegment(IndexType start) const { return vector_.segment<Size>(start); }
 
  virtual Vector& x() { return vector_; }
  virtual Covariance& P() { return covariance_; }
//  virtual VectorSegment x0() { return vector_.segment<VectorDimension>(0); }
//  virtual CovarianceBlock P0() { return covariance_.block<CovarianceDimension,CovarianceDimension>(0, 0); }
 
  virtual void update(const Vector &vector_update);
  virtual void updateOrientation(const ColumnVector3 &rotation_vector);
 
  virtual SystemStatus getSystemStatus() const { return system_status_; }
  virtual SystemStatus getMeasurementStatus() const { return measurement_status_; }
 
  virtual bool inSystemStatus(SystemStatus test_status) const;
  virtual bool setSystemStatus(SystemStatus new_status);
  virtual bool setMeasurementStatus(SystemStatus new_status);
  virtual bool updateSystemStatus(SystemStatus set, SystemStatus clear);
  virtual bool updateMeasurementStatus(SystemStatus set, SystemStatus clear);
 
  typedef boost::function<bool(SystemStatus&)> SystemStatusCallback;
  virtual void addSystemStatusCallback(const SystemStatusCallback& callback);
 
  virtual const boost::shared_ptr<OrientationStateType> &orientation() const   { return orientation_; }
  virtual const boost::shared_ptr<RateStateType> &rate() const                 { return rate_; }
  virtual const boost::shared_ptr<PositionStateType> &position() const         { return position_; }
  virtual const boost::shared_ptr<VelocityStateType> &velocity() const         { return velocity_; }
  virtual const boost::shared_ptr<AccelerationStateType> &acceleration() const { return acceleration_; }
 
  virtual ConstOrientationType getOrientation() const;
  virtual ConstRateType getRate() const;
  virtual ConstPositionType getPosition() const;
  virtual ConstVelocityType getVelocity() const;
  virtual ConstAccelerationType getAcceleration() const;
 
  void setOrientation(const Quaternion& orientation);
  template <typename Derived> void setOrientation(const Eigen::MatrixBase<Derived>& orientation);
  void setRollPitch(const Quaternion& orientation);
  void setRollPitch(ScalarType roll, ScalarType pitch);
  void setYaw(const Quaternion& orientation);
  void setYaw(ScalarType yaw);
  template <typename Derived> void setRate(const Eigen::MatrixBase<Derived>& rate);
  template <typename Derived> void setPosition(const Eigen::MatrixBase<Derived>& position);
  template <typename Derived> void setVelocity(const Eigen::MatrixBase<Derived>& velocity);
  template <typename Derived> void setAcceleration(const Eigen::MatrixBase<Derived>& acceleration);
 
  void getRotationMatrix(RotationMatrix &R) const;
  const State::RotationMatrix &R() const;
  double getYaw() const;
  void getEuler(double &roll, double &pitch, double &yaw) const;
  ColumnVector3 getEuler() const;
 
  const SubStates& getSubStates() const { return substates_; }
  template <int SubVectorDimension, int SubCovarianceDimension> boost::shared_ptr<SubState_<SubVectorDimension,SubCovarianceDimension> > getSubState(const Model *model) const;
  template <int SubVectorDimension, int SubCovarianceDimension> boost::shared_ptr<SubState_<SubVectorDimension,SubCovarianceDimension> > getSubState(const std::string& name) const;
  template <int SubVectorDimension, int SubCovarianceDimension> boost::shared_ptr<SubState_<SubVectorDimension,SubCovarianceDimension> > addSubState(const std::string& name = std::string());
  template <int SubVectorDimension, int SubCovarianceDimension> boost::shared_ptr<SubState_<SubVectorDimension,SubCovarianceDimension> > addSubState(const Model *model, const std::string& name = std::string());
 
  const ros::Time& getTimestamp() const { return timestamp_; }
  void setTimestamp(const ros::Time& timestamp) { timestamp_ = timestamp; }
 
protected:
  State();
  void construct();
 
  OrientationType orientationPart();
  RateType ratePart();
  PositionType positionPart();
  VelocityType velocityPart();
  AccelerationType accelerationPart();
 
  void orientationSet();
  void rollpitchSet();
  void yawSet();
  void rateSet();
  void positionSet();
  void velocitySet();
  void accelerationSet();
 
protected:
  Vector vector_;
  Covariance covariance_;
 
  SystemStatus system_status_;
  SystemStatus measurement_status_;
 
  std::vector<SystemStatusCallback> status_callbacks_;
 
  SubStates substates_;
  std::map<const Model *, SubStateWPtr> substates_by_model_;
  std::map<std::string, SubStateWPtr> substates_by_name_;
 
  boost::shared_ptr<BaseState> base_;
  boost::shared_ptr<OrientationStateType> orientation_;
  boost::shared_ptr<RateStateType> rate_;
  boost::shared_ptr<PositionStateType> position_;
  boost::shared_ptr<VelocityStateType> velocity_;
  boost::shared_ptr<AccelerationStateType> acceleration_;
 
  Vector fake_orientation_;
  Vector fake_rate_;
  Vector fake_position_;
  Vector fake_velocity_;
  Vector fake_acceleration_;
 
  ros::Time timestamp_;
 
  // cached rotation matrix
  mutable RotationMatrix R_;
  mutable bool R_valid_;
};
 
class FullState : public State
{
public:
  FullState();
  virtual ~FullState();
};
 
class OrientationPositionVelocityState : public State
{
public:
  OrientationPositionVelocityState();
  virtual ~OrientationPositionVelocityState();
};
 
class OrientationOnlyState : public State
{
public:
  OrientationOnlyState();
  virtual ~OrientationOnlyState();
};
 
class PositionVelocityState : public State
{
public:
  PositionVelocityState();
  virtual ~PositionVelocityState();
};
 
} // namespace hector_pose_estimation
 
#include <hector_pose_estimation/state.inl>
 
#endif // HECTOR_POSE_ESTIMATION_STATE_H