system.h

Go to the documentation of this file.

//=================================================================================================
// Copyright (c) 2011, Johannes Meyer, TU Darmstadt
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the Flight Systems and Automatic Control group,
//       TU Darmstadt, nor the names of its contributors may be used to
//       endorse or promote products derived from this software without
//       specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================

#ifndef HECTOR_POSE_ESTIMATION_SYSTEM_H
#define HECTOR_POSE_ESTIMATION_SYSTEM_H

#include <hector_pose_estimation/system_model.h>
#include <hector_pose_estimation/types.h>
#include <hector_pose_estimation/state.h>
#include <hector_pose_estimation/input.h>
#include <hector_pose_estimation/filter.h>

#include <ros/console.h>

namespace hector_pose_estimation {

template <class ConcreteModel> class System_;

class System
{
public:
  System(const std::string& name);
  virtual ~System();

  template <class ConcreteModel> static boost::shared_ptr<System_<ConcreteModel> > create(ConcreteModel *model, const std::string& name = "system");

  virtual const std::string& getName() const { return name_; }
  virtual void setName(const std::string& name) { name_ = name; }

  virtual SystemModel *getModel() const { return 0; }

  virtual Filter *filter() const = 0;
  virtual Filter::Predictor *predictor() const = 0;
  virtual void setFilter(Filter *filter) = 0;

  virtual bool init(PoseEstimation& estimator, State& state);
  virtual void cleanup();
  virtual void reset(State& state);

  virtual bool active(const State& state);
  virtual SystemStatus getStatusFlags() const { return status_flags_; }

  virtual ParameterList& parameters() { return parameters_; }
  virtual const ParameterList& parameters() const { return parameters_; }

  virtual void getPrior(State &state) const;

  virtual bool update(double dt);

  virtual void updated();
  virtual bool limitState(State& state);

protected:
  virtual bool updateImpl(double dt) = 0;
  virtual bool prepareUpdate(State &state, double dt) { return getModel()->prepareUpdate(state, dt); }
  virtual void afterUpdate(State &state) { getModel()->afterUpdate(state); }

protected:
  std::string name_;
  ParameterList parameters_;
  SystemStatus status_flags_;
};

template <class ConcreteModel>
class System_ : public System
{
public:
  typedef ConcreteModel Model;
  typedef typename traits::Input<ConcreteModel>::Type InputType;
  typedef typename traits::Input<ConcreteModel>::Vector InputVector;

  System_(const std::string& name = "system")
    : System(name)
    , model_(new Model)
  {
    parameters_.add(model_->parameters());
  }

  System_(Model *model, const std::string& name)
    : System(name)
    , model_(model)
  {
    parameters_.add(model_->parameters());
  }

  virtual ~System_() {}

  virtual void reset(State& state) {
    System::reset(state);
    if (predictor()) predictor()->reset();
  }

  virtual Model *getModel() const { return model_.get(); }

  virtual Filter *filter() const { return predictor_ ? predictor_->base() : 0; }
  virtual Filter::Predictor_<Model> *predictor() const { return predictor_.get(); }
  virtual void setFilter(Filter *filter = 0); // implemented in filter/set_filter.h

protected:
  virtual bool updateImpl(double dt);

private:
  boost::shared_ptr<Model> model_;
  boost::shared_ptr< Filter::Predictor_<Model> > predictor_;
};

template <class ConcreteModel>
boost::shared_ptr<System_<ConcreteModel> > System::create(ConcreteModel *model, const std::string& name)
{
  return boost::make_shared<System_<ConcreteModel> >(model, name);
}

} // namespace hector_pose_estimation

#include "system.inl"

#endif // HECTOR_POSE_ESTIMATION_SYSTEM_H