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 #include <tf/transform_datatypes.h>
00040 #include <geometry_msgs/PoseStamped.h>
00041 #include <geometry_msgs/PointStamped.h>
00042 #include <geometry_msgs/QuaternionStamped.h>
00043 #include <geometry_msgs/Vector3Stamped.h>
00044 #include <nav_msgs/Odometry.h>
00045 #include <sensor_msgs/NavSatFix.h>
00046
00047
00048
00049
00050
00051 #define VELOCITY_IN_WORLD_FRAME
00052
00053 namespace hector_pose_estimation {
00054
00055 class State {
00056 public:
00057 enum VectorIndex {
00058 X = 0,
00059 Y = 1,
00060 Z = 2,
00061 W = 3
00062 };
00063
00064 enum StateIndex {
00065 QUATERNION_X = 0,
00066 QUATERNION_Y,
00067 QUATERNION_Z,
00068 QUATERNION_W,
00069 #ifdef USE_RATE_SYSTEM_MODEL
00070 RATE_X,
00071 RATE_Y,
00072 RATE_Z,
00073 #endif // USE_RATE_SYSTEM_MODEL
00074 POSITION_X,
00075 POSITION_Y,
00076 POSITION_Z,
00077 VELOCITY_X,
00078 VELOCITY_Y,
00079 VELOCITY_Z,
00080 Dimension,
00081
00082 #ifndef USE_RATE_SYSTEM_MODEL
00083 RATE_X = -1,
00084 RATE_Y = -1,
00085 RATE_Z = -1,
00086 #endif // USE_RATE_SYSTEM_MODEL
00087 ACCELERATION_X = -1,
00088 ACCELERATION_Y = -1,
00089 ACCELERATION_Z = -1
00090 };
00091
00092 typedef ColumnVector Vector;
00093 typedef SymmetricMatrix Covariance;
00094 typedef VectorBlock<Vector,Dimension> VectorSegment;
00095 typedef Block<Covariance::Base,Dimension,Dimension> CovarianceBlock;
00096 typedef VectorBlock<const Vector,Dimension> ConstVectorSegment;
00097 typedef Block<const Covariance::Base,Dimension,Dimension> ConstCovarianceBlock;
00098
00099 typedef VectorBlock<Vector,4> OrientationType;
00100 typedef VectorBlock<const Vector,4> ConstOrientationType;
00101 typedef VectorBlock<Vector,3> RateType;
00102 typedef VectorBlock<const Vector,3> ConstRateType;
00103 typedef VectorBlock<Vector,3> PositionType;
00104 typedef VectorBlock<const Vector,3> ConstPositionType;
00105 typedef VectorBlock<Vector,3> VelocityType;
00106 typedef VectorBlock<const Vector,3> ConstVelocityType;
00107 typedef VectorBlock<Vector,3> AccelerationType;
00108 typedef VectorBlock<const Vector,3> ConstAccelerationType;
00109
00110 typedef std::vector<SubStatePtr> SubStates;
00111
00112 typedef Matrix_<3,3> RotationMatrix;
00113
00114 public:
00115 State();
00116 State(const Vector &vector, const Covariance& covariance);
00117 virtual ~State();
00118
00119 static IndexType getDimension0() { return Dimension; }
00120 virtual IndexType getDimension() const { return vector_.rows(); }
00121
00122 virtual void reset();
00123 virtual void updated();
00124 virtual double normalize();
00125
00126 virtual bool valid() const;
00127
00128 virtual BaseState& base() { return *base_; }
00129 virtual const BaseState& base() const { return *base_; }
00130
00131 virtual const Vector& getVector() const { return vector_; }
00132 virtual const Covariance& getCovariance() const { return covariance_; }
00133 template <int Size> VectorBlock<const Vector, Size> getSegment(IndexType start) const { return vector_.segment<Size>(start); }
00134
00135 virtual Vector& x() { return vector_; }
00136 virtual Covariance& P() { return covariance_; }
00137 virtual VectorSegment x0() { return vector_.segment<Dimension>(0); }
00138 virtual CovarianceBlock P0() { return covariance_.block<Dimension,Dimension>(0, 0); }
00139
00140 virtual SystemStatus getSystemStatus() const { return system_status_; }
00141 virtual SystemStatus getMeasurementStatus() const { return measurement_status_; }
00142
00143 virtual bool inSystemStatus(SystemStatus test_status) const;
00144 virtual bool setSystemStatus(SystemStatus new_status);
00145 virtual bool setMeasurementStatus(SystemStatus new_status);
00146 virtual bool updateSystemStatus(SystemStatus set, SystemStatus clear);
00147 virtual bool updateMeasurementStatus(SystemStatus set, SystemStatus clear);
00148
00149 typedef boost::function<bool(SystemStatus&)> SystemStatusCallback;
00150 virtual void addSystemStatusCallback(const SystemStatusCallback& callback);
00151
00152 virtual OrientationType orientation() { return (getOrientationIndex() >= 0) ? vector_.segment<4>(getOrientationIndex()) : fake_orientation_.segment<4>(0); }
00153 virtual ConstOrientationType getOrientation() const { return (getOrientationIndex() >= 0) ? vector_.segment<4>(getOrientationIndex()) : fake_orientation_.segment<4>(0); }
00154 virtual RateType rate() { return (getRateIndex() >= 0) ? vector_.segment<3>(getRateIndex()) : fake_rate_.segment<3>(0); }
00155 virtual ConstRateType getRate() const { return (getRateIndex() >= 0) ? vector_.segment<3>(getRateIndex()) : fake_rate_.segment<3>(0); }
00156 virtual PositionType position() { return (getPositionIndex() >= 0) ? vector_.segment<3>(getPositionIndex()) : fake_position_.segment<3>(0); }
00157 virtual ConstPositionType getPosition() const { return (getPositionIndex() >= 0) ? vector_.segment<3>(getPositionIndex()) : fake_position_.segment<3>(0); }
00158 virtual VelocityType velocity() { return (getVelocityIndex() >= 0) ? vector_.segment<3>(getVelocityIndex()) : fake_velocity_.segment<3>(0); }
00159 virtual ConstVelocityType getVelocity() const { return (getVelocityIndex() >= 0) ? vector_.segment<3>(getVelocityIndex()) : fake_velocity_.segment<3>(0); }
00160 virtual AccelerationType acceleration() { return (getAccelerationIndex() >= 0) ? vector_.segment<3>(getAccelerationIndex()) : fake_acceleration_.segment<3>(0); }
00161 virtual ConstAccelerationType getAcceleration() const { return (getAccelerationIndex() >= 0) ? vector_.segment<3>(getAccelerationIndex()) : fake_acceleration_.segment<3>(0); }
00162
00163 RotationMatrix getRotationMatrix() const;
00164 void getRotationMatrix(RotationMatrix &R) const;
00165
00166 double getYaw() const;
00167
00168 template <typename Derived> void setOrientation(const Eigen::MatrixBase<Derived>& orientation);
00169 template <typename Derived> void setRate(const Eigen::MatrixBase<Derived>& rate);
00170 template <typename Derived> void setPosition(const Eigen::MatrixBase<Derived>& position);
00171 template <typename Derived> void setVelocity(const Eigen::MatrixBase<Derived>& velocity);
00172 template <typename Derived> void setAcceleration(const Eigen::MatrixBase<Derived>& acceleration);
00173
00174 virtual IndexType getOrientationIndex() const { return QUATERNION_X; }
00175 virtual IndexType getRateIndex() const { return RATE_X; }
00176 virtual IndexType getPositionIndex() const { return POSITION_X; }
00177 virtual IndexType getVelocityIndex() const { return VELOCITY_X; }
00178 virtual IndexType getAccelerationIndex() const { return ACCELERATION_X; }
00179
00180 const SubStates& getSubStates() const { return substates_; }
00181 template <int SubDimension> boost::shared_ptr<SubState_<SubDimension> > getSubState(const Model *model) const;
00182 template <int SubDimension> boost::shared_ptr<SubState_<SubDimension> > getSubState(const std::string& name) const;
00183 template <int SubDimension> boost::shared_ptr<SubState_<SubDimension> > addSubState(const Model *model, const std::string& name = std::string());
00184
00185 const ros::Time& getTimestamp() const { return timestamp_; }
00186 void setTimestamp(const ros::Time& timestamp) { timestamp_ = timestamp; }
00187
00188 private:
00189 Vector vector_;
00190 Covariance covariance_;
00191
00192 SystemStatus system_status_;
00193 SystemStatus measurement_status_;
00194
00195 Vector fake_orientation_;
00196 Vector fake_rate_;
00197 Vector fake_position_;
00198 Vector fake_velocity_;
00199 Vector fake_acceleration_;
00200
00201 std::vector<SystemStatusCallback> status_callbacks_;
00202
00203 SubStates substates_;
00204 std::map<const Model *, SubStateWPtr> substates_by_model_;
00205 std::map<std::string, SubStateWPtr> substates_by_name_;
00206
00207 boost::shared_ptr<BaseState> base_;
00208
00209 ros::Time timestamp_;
00210 };
00211
00212 template <typename Derived>
00213 void State::setOrientation(const Eigen::MatrixBase<Derived>& orientation) {
00214 eigen_assert(orientation.rows() == 4 && orientation.cols() == 1);
00215 fake_orientation_ = orientation;
00216 }
00217
00218 template <typename Derived>
00219 void State::setRate(const Eigen::MatrixBase<Derived>& rate) {
00220 eigen_assert(rate.rows() == 3 && rate.cols() == 1);
00221 fake_rate_ = rate;
00222 }
00223
00224 template <typename Derived>
00225 void State::setPosition(const Eigen::MatrixBase<Derived>& position) {
00226 eigen_assert(position.rows() == 3 && position.cols() == 1);
00227 fake_position_ = position;
00228 }
00229
00230 template <typename Derived>
00231 void State::setVelocity(const Eigen::MatrixBase<Derived>& velocity) {
00232 eigen_assert(velocity.rows() == 3 && velocity.cols() == 1);
00233 fake_velocity_ = velocity;
00234 }
00235
00236 template <typename Derived>
00237 void State::setAcceleration(const Eigen::MatrixBase<Derived>& acceleration) {
00238 eigen_assert(acceleration.rows() == 3 && acceleration.cols() == 1);
00239 fake_acceleration_ = acceleration;
00240 }
00241
00242 }
00243
00244 #endif // HECTOR_POSE_ESTIMATION_STATE_H