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_SYSTEM_H
00030 #define HECTOR_POSE_ESTIMATION_SYSTEM_H
00031
00032 #include "system_model.h"
00033 #include "system_input.h"
00034
00035 namespace BFL { class KalmanFilter; }
00036
00037 namespace hector_pose_estimation {
00038
00039 class PoseEstimation;
00040
00041 class System
00042 {
00043 public:
00044 System(const std::string& name);
00045 virtual ~System();
00046
00047 template <typename ConcreteModel> static boost::shared_ptr<System> create(ConcreteModel *model, const std::string& name = "system");
00048
00049 virtual const std::string& getName() const { return name_; }
00050 virtual void setName(const std::string& name) { name_ = name; }
00051
00052 virtual SystemModel *getModel() const = 0;
00053
00054 virtual bool init();
00055 virtual void cleanup();
00056 virtual void reset();
00057
00058 virtual SystemStatus getStatusFlags() const { return status_flags_; }
00059
00060 virtual ParameterList& parameters() { return parameters_; }
00061 virtual const ParameterList& parameters() const { return parameters_; }
00062
00063 BFL::Gaussian *getPrior();
00064 virtual const SystemInput& getInput() const = 0;
00065 virtual void setInput(const SystemInput& input) = 0;
00066
00067 virtual bool update(PoseEstimation &estimator, double dt) = 0;
00068 virtual void updated();
00069 virtual StateVector limitState(StateVector state) const;
00070
00071 protected:
00072 void updateInternal(PoseEstimation &estimator, double dt, ColumnVector const& u);
00073
00074 protected:
00075 std::string name_;
00076 ParameterList parameters_;
00077 SystemStatus status_flags_;
00078 BFL::Gaussian prior_;
00079 };
00080
00081 typedef boost::shared_ptr<System> SystemPtr;
00082
00083 template <typename ConcreteModel, typename ConcreteInput = typename Input_<ConcreteModel>::Type >
00084 class System_ : public System
00085 {
00086 public:
00087 typedef ConcreteModel Model;
00088 typedef ConcreteInput Input;
00089 static const unsigned int InputDimension = Model::InputDimension;
00090 typedef typename Model::InputVector InputVector;
00091
00092 System_(const std::string& name = "system")
00093 : System(name)
00094 , model_(new Model)
00095 {
00096 parameters_.add(model_->parameters());
00097 }
00098
00099 System_(Model *model, const std::string& name)
00100 : System(name)
00101 , model_(model)
00102 {
00103 parameters_.add(model_->parameters());
00104 }
00105 virtual SystemModel *getModel() const { return model_.get(); }
00106
00107 virtual const Input& getInput() const { return input_; }
00108 virtual void setInput(const Input& input) { input_ = input; }
00109 virtual void setInput(const SystemInput& input) {
00110 try {
00111 input_ = dynamic_cast<const Input&>(input);
00112 } catch(std::bad_cast& e) {
00113 std::cerr << "In system " << getName() << ": " << e.what() << std::endl;
00114 std::cerr << "Expected type " << std::string(typeid(Input).name()) << ", you gave me " << std::string(typeid(input).name()) << std::endl;
00115 }
00116 }
00117
00118 virtual bool update(PoseEstimation &estimator, double dt);
00119
00120 private:
00121 boost::shared_ptr<Model> model_;
00122 Input input_;
00123 };
00124
00125 template <class ConcreteModel, class ConcreteInput>
00126 bool System_<ConcreteModel, ConcreteInput>::update(PoseEstimation &estimator, double dt)
00127 {
00128 updateInternal(estimator, dt, input_.getVector());
00129 return true;
00130 }
00131
00132 template <typename ConcreteModel>
00133 SystemPtr System::create(ConcreteModel *model, const std::string& name)
00134 {
00135 return SystemPtr(new System_<ConcreteModel>(model, name));
00136 }
00137
00138 }
00139
00140 #endif // HECTOR_POSE_ESTIMATION_SYSTEM_H