00001 //================================================================================================= 00002 // Copyright (c) 2011, 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 #include <hector_pose_estimation/measurements/height.h> 00030 #include <hector_pose_estimation/pose_estimation.h> 00031 #include <ros/console.h> 00032 00033 #include <boost/bind.hpp> 00034 00035 namespace hector_pose_estimation { 00036 00037 HeightModel::HeightModel() 00038 : MeasurementModel(MeasurementDimension) 00039 { 00040 stddev_ = 10.0; 00041 elevation_ = 0.0; 00042 parameters().add("stddev", stddev_); 00043 } 00044 00045 bool HeightModel::init() 00046 { 00047 NoiseCovariance noise = 0.0; 00048 noise(1,1) = pow(stddev_, 2); 00049 this->AdditiveNoiseSigmaSet(noise); 00050 return true; 00051 } 00052 00053 HeightModel::~HeightModel() {} 00054 00055 SystemStatus HeightModel::getStatusFlags() const { 00056 return STATE_Z_POSITION; 00057 } 00058 00059 ColumnVector HeightModel::ExpectedValueGet() const { 00060 this->y_(1) = x_(POSITION_Z) + getElevation(); 00061 return y_; 00062 } 00063 00064 Matrix HeightModel::dfGet(unsigned int i) const { 00065 if (i != 0) return Matrix(); 00066 C_(1,POSITION_Z) = 1.0; 00067 return C_; 00068 } 00069 00070 HeightBaroCommon::HeightBaroCommon(Measurement* parent) 00071 { 00072 auto_elevation_ = true; 00073 elevation_initialized_ = false; 00074 parent->parameters().add("auto_elevation", auto_elevation_); 00075 } 00076 00077 HeightBaroCommon::~HeightBaroCommon() {} 00078 00079 void HeightBaroCommon::onReset() { 00080 elevation_initialized_ = false; 00081 } 00082 00083 double HeightBaroCommon::resetElevation(PoseEstimation &estimator, boost::function<double()> altitude_func) { 00084 if (!elevation_initialized_) { 00085 estimator.globalReference()->setCurrentAltitude(estimator, altitude_func()); 00086 elevation_initialized_ = true; 00087 } 00088 00089 return estimator.globalReference()->position().altitude; 00090 } 00091 00092 void Height::onReset() { 00093 HeightBaroCommon::onReset(); 00094 } 00095 00096 template <typename T> struct functor_wrapper 00097 { 00098 functor_wrapper(const T& value) : value(value) {} 00099 T& operator()() { return value; } 00100 const T& operator()() const { return value; } 00101 private: 00102 T value; 00103 }; 00104 00105 bool Height::beforeUpdate(PoseEstimation &estimator, const Height::Update &update) { 00106 setElevation(resetElevation(estimator, functor_wrapper<double>(update.getVector()(1)))); 00107 return true; 00108 } 00109 00110 } // namespace hector_pose_estimation