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 #ifndef HECTOR_POSE_ESTIMATION_SYSTEM_H 00030 #define HECTOR_POSE_ESTIMATION_SYSTEM_H 00031 00032 #include <hector_pose_estimation/system_model.h> 00033 #include <hector_pose_estimation/types.h> 00034 #include <hector_pose_estimation/state.h> 00035 #include <hector_pose_estimation/input.h> 00036 #include <hector_pose_estimation/filter.h> 00037 00038 #include <ros/console.h> 00039 00040 namespace hector_pose_estimation { 00041 00042 template <class ConcreteModel> class System_; 00043 00044 class System 00045 { 00046 public: 00047 System(const std::string& name); 00048 virtual ~System(); 00049 00050 template <class ConcreteModel> static boost::shared_ptr<System_<ConcreteModel> > create(ConcreteModel *model, const std::string& name = "system"); 00051 00052 virtual const std::string& getName() const { return name_; } 00053 virtual void setName(const std::string& name) { name_ = name; } 00054 00055 virtual SystemModel *getModel() const { return 0; } 00056 00057 virtual Filter *filter() const = 0; 00058 virtual Filter::Predictor *predictor() const = 0; 00059 virtual void setFilter(Filter *filter) = 0; 00060 00061 virtual bool init(PoseEstimation& estimator, State& state); 00062 virtual void cleanup(); 00063 virtual void reset(State& state); 00064 00065 virtual bool active(const State& state); 00066 virtual SystemStatus getStatusFlags() const { return status_flags_; } 00067 00068 virtual ParameterList& parameters() { return parameters_; } 00069 virtual const ParameterList& parameters() const { return parameters_; } 00070 00071 virtual void getPrior(State &state) const; 00072 00073 virtual bool update(double dt); 00074 00075 virtual void updated(); 00076 virtual bool limitState(State& state); 00077 00078 protected: 00079 virtual bool updateImpl(double dt) = 0; 00080 virtual bool prepareUpdate(State &state, double dt) { return getModel()->prepareUpdate(state, dt); } 00081 virtual void afterUpdate(State &state) { getModel()->afterUpdate(state); } 00082 00083 protected: 00084 std::string name_; 00085 ParameterList parameters_; 00086 SystemStatus status_flags_; 00087 }; 00088 00089 template <class ConcreteModel> 00090 class System_ : public System 00091 { 00092 public: 00093 typedef ConcreteModel Model; 00094 typedef typename traits::Input<ConcreteModel>::Type InputType; 00095 typedef typename traits::Input<ConcreteModel>::Vector InputVector; 00096 00097 System_(const std::string& name = "system") 00098 : System(name) 00099 , model_(new Model) 00100 { 00101 parameters_.add(model_->parameters()); 00102 } 00103 00104 System_(Model *model, const std::string& name) 00105 : System(name) 00106 , model_(model) 00107 { 00108 parameters_.add(model_->parameters()); 00109 } 00110 00111 virtual ~System_() {} 00112 00113 virtual void reset(State& state) { 00114 System::reset(state); 00115 if (predictor()) predictor()->reset(); 00116 } 00117 00118 virtual Model *getModel() const { return model_.get(); } 00119 00120 virtual Filter *filter() const { return predictor_ ? predictor_->base() : 0; } 00121 virtual Filter::Predictor_<Model> *predictor() const { return predictor_.get(); } 00122 virtual void setFilter(Filter *filter = 0); // implemented in filter/set_filter.h 00123 00124 protected: 00125 virtual bool updateImpl(double dt); 00126 00127 private: 00128 boost::shared_ptr<Model> model_; 00129 boost::shared_ptr< Filter::Predictor_<Model> > predictor_; 00130 }; 00131 00132 template <class ConcreteModel> 00133 boost::shared_ptr<System_<ConcreteModel> > System::create(ConcreteModel *model, const std::string& name) 00134 { 00135 return boost::make_shared<System_<ConcreteModel> >(model, name); 00136 } 00137 00138 } // namespace hector_pose_estimation 00139 00140 #include "system.inl" 00141 00142 #endif // HECTOR_POSE_ESTIMATION_SYSTEM_H