state.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2013, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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 // Use system model with angular rates.
00048 // #define USE_RATE_SYSTEM_MODEL
00049 
00050 // #define VELOCITY_IN_BODY_FRAME
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, // body frame
00071     RATE_Y, // body frame
00072     RATE_Z, // body frame
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 } // namespace hector_pose_estimation
00243 
00244 #endif // HECTOR_POSE_ESTIMATION_STATE_H


hector_pose_estimation_core
Author(s): Johannes Meyer
autogenerated on Mon Oct 6 2014 00:24:16