Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #ifndef HECTOR_POSE_ESTIMATION_STATE_H
00030 #define HECTOR_POSE_ESTIMATION_STATE_H
00031
00032 #include <hector_pose_estimation/types.h>
00033
00034 #include <boost/function.hpp>
00035 #include <boost/shared_ptr.hpp>
00036 #include <boost/weak_ptr.hpp>
00037
00038 #include <ros/time.h>
00039
00040 namespace hector_pose_estimation {
00041
00042 class State {
00043 public:
00044 typedef ColumnVector Vector;
00045 typedef SymmetricMatrix Covariance;
00046 typedef Matrix SystemMatrix;
00047
00048 typedef VectorBlock<Vector> VectorSegment;
00049 typedef Block<Covariance::Base> CovarianceBlock;
00050 typedef VectorBlock<const Vector> ConstVectorSegment;
00051 typedef Block<const Covariance::Base> ConstCovarianceBlock;
00052
00053 typedef SubState_<4,3> OrientationStateType;
00054 typedef VectorBlock<Vector,4> OrientationType;
00055 typedef VectorBlock<const Vector,4> ConstOrientationType;
00056 typedef SubState_<3,3> RateStateType;
00057 typedef VectorBlock<Vector,3> RateType;
00058 typedef VectorBlock<const Vector,3> ConstRateType;
00059 typedef SubState_<3,3> PositionStateType;
00060 typedef VectorBlock<Vector,3> PositionType;
00061 typedef VectorBlock<const Vector,3> ConstPositionType;
00062 typedef SubState_<3,3> VelocityStateType;
00063 typedef VectorBlock<Vector,3> VelocityType;
00064 typedef VectorBlock<const Vector,3> ConstVelocityType;
00065 typedef SubState_<3,3> AccelerationStateType;
00066 typedef VectorBlock<Vector,3> AccelerationType;
00067 typedef VectorBlock<const Vector,3> ConstAccelerationType;
00068
00069 typedef std::vector<SubStatePtr> SubStates;
00070
00071 typedef Matrix_<3,3> RotationMatrix;
00072
00073 public:
00074 virtual ~State();
00075
00076 virtual IndexType getVectorDimension() const { return vector_.rows(); }
00077 virtual IndexType getCovarianceDimension() const { return covariance_.rows(); }
00078
00079 virtual void reset();
00080 virtual void updated();
00081 virtual void normalize();
00082
00083 virtual bool valid() const;
00084
00085 virtual BaseState& base() { return *base_; }
00086 virtual const BaseState& base() const { return *base_; }
00087
00088 virtual const Vector& getVector() const { return vector_; }
00089 virtual const Covariance& getCovariance() const { return covariance_; }
00090 template <int Size> VectorBlock<const Vector, Size> getSegment(IndexType start) const { return vector_.segment<Size>(start); }
00091
00092 virtual Vector& x() { return vector_; }
00093 virtual Covariance& P() { return covariance_; }
00094
00095
00096
00097 virtual void update(const Vector &vector_update);
00098 virtual void updateOrientation(const ColumnVector3 &rotation_vector);
00099
00100 virtual SystemStatus getSystemStatus() const { return system_status_; }
00101 virtual SystemStatus getMeasurementStatus() const { return measurement_status_; }
00102
00103 virtual bool inSystemStatus(SystemStatus test_status) const;
00104 virtual bool setSystemStatus(SystemStatus new_status);
00105 virtual bool setMeasurementStatus(SystemStatus new_status);
00106 virtual bool updateSystemStatus(SystemStatus set, SystemStatus clear);
00107 virtual bool updateMeasurementStatus(SystemStatus set, SystemStatus clear);
00108
00109 typedef boost::function<bool(SystemStatus&)> SystemStatusCallback;
00110 virtual void addSystemStatusCallback(const SystemStatusCallback& callback);
00111
00112 virtual const boost::shared_ptr<OrientationStateType> &orientation() const { return orientation_; }
00113 virtual const boost::shared_ptr<RateStateType> &rate() const { return rate_; }
00114 virtual const boost::shared_ptr<PositionStateType> &position() const { return position_; }
00115 virtual const boost::shared_ptr<VelocityStateType> &velocity() const { return velocity_; }
00116 virtual const boost::shared_ptr<AccelerationStateType> &acceleration() const { return acceleration_; }
00117
00118 virtual ConstOrientationType getOrientation() const;
00119 virtual ConstRateType getRate() const;
00120 virtual ConstPositionType getPosition() const;
00121 virtual ConstVelocityType getVelocity() const;
00122 virtual ConstAccelerationType getAcceleration() const;
00123
00124 void setOrientation(const Quaternion& orientation);
00125 template <typename Derived> void setOrientation(const Eigen::MatrixBase<Derived>& orientation);
00126 void setRollPitch(const Quaternion& orientation);
00127 void setRollPitch(ScalarType roll, ScalarType pitch);
00128 void setYaw(const Quaternion& orientation);
00129 void setYaw(ScalarType yaw);
00130 template <typename Derived> void setRate(const Eigen::MatrixBase<Derived>& rate);
00131 template <typename Derived> void setPosition(const Eigen::MatrixBase<Derived>& position);
00132 template <typename Derived> void setVelocity(const Eigen::MatrixBase<Derived>& velocity);
00133 template <typename Derived> void setAcceleration(const Eigen::MatrixBase<Derived>& acceleration);
00134
00135 void getRotationMatrix(RotationMatrix &R) const;
00136 const State::RotationMatrix &R() const;
00137 double getYaw() const;
00138 void getEuler(double &roll, double &pitch, double &yaw) const;
00139 ColumnVector3 getEuler() const;
00140
00141 const SubStates& getSubStates() const { return substates_; }
00142 template <int SubVectorDimension, int SubCovarianceDimension> boost::shared_ptr<SubState_<SubVectorDimension,SubCovarianceDimension> > getSubState(const Model *model) const;
00143 template <int SubVectorDimension, int SubCovarianceDimension> boost::shared_ptr<SubState_<SubVectorDimension,SubCovarianceDimension> > getSubState(const std::string& name) const;
00144 template <int SubVectorDimension, int SubCovarianceDimension> boost::shared_ptr<SubState_<SubVectorDimension,SubCovarianceDimension> > addSubState(const std::string& name = std::string());
00145 template <int SubVectorDimension, int SubCovarianceDimension> boost::shared_ptr<SubState_<SubVectorDimension,SubCovarianceDimension> > addSubState(const Model *model, const std::string& name = std::string());
00146
00147 const ros::Time& getTimestamp() const { return timestamp_; }
00148 void setTimestamp(const ros::Time& timestamp) { timestamp_ = timestamp; }
00149
00150 protected:
00151 State();
00152 void construct();
00153
00154 OrientationType orientationPart();
00155 RateType ratePart();
00156 PositionType positionPart();
00157 VelocityType velocityPart();
00158 AccelerationType accelerationPart();
00159
00160 void orientationSet();
00161 void rollpitchSet();
00162 void yawSet();
00163 void rateSet();
00164 void positionSet();
00165 void velocitySet();
00166 void accelerationSet();
00167
00168 protected:
00169 Vector vector_;
00170 Covariance covariance_;
00171
00172 SystemStatus system_status_;
00173 SystemStatus measurement_status_;
00174
00175 std::vector<SystemStatusCallback> status_callbacks_;
00176
00177 SubStates substates_;
00178 std::map<const Model *, SubStateWPtr> substates_by_model_;
00179 std::map<std::string, SubStateWPtr> substates_by_name_;
00180
00181 boost::shared_ptr<BaseState> base_;
00182 boost::shared_ptr<OrientationStateType> orientation_;
00183 boost::shared_ptr<RateStateType> rate_;
00184 boost::shared_ptr<PositionStateType> position_;
00185 boost::shared_ptr<VelocityStateType> velocity_;
00186 boost::shared_ptr<AccelerationStateType> acceleration_;
00187
00188 Vector fake_orientation_;
00189 Vector fake_rate_;
00190 Vector fake_position_;
00191 Vector fake_velocity_;
00192 Vector fake_acceleration_;
00193
00194 ros::Time timestamp_;
00195
00196
00197 mutable RotationMatrix R_;
00198 mutable bool R_valid_;
00199 };
00200
00201 class FullState : public State
00202 {
00203 public:
00204 FullState();
00205 virtual ~FullState();
00206 };
00207
00208 class OrientationPositionVelocityState : public State
00209 {
00210 public:
00211 OrientationPositionVelocityState();
00212 virtual ~OrientationPositionVelocityState();
00213 };
00214
00215 class OrientationOnlyState : public State
00216 {
00217 public:
00218 OrientationOnlyState();
00219 virtual ~OrientationOnlyState();
00220 };
00221
00222 class PositionVelocityState : public State
00223 {
00224 public:
00225 PositionVelocityState();
00226 virtual ~PositionVelocityState();
00227 };
00228
00229 }
00230
00231 #include <hector_pose_estimation/state.inl>
00232
00233 #endif // HECTOR_POSE_ESTIMATION_STATE_H