reach.hpp

Go to the documentation of this file.

#ifndef QTCTRL_REACH_CTRL
#define QTCTRL_REACH_CTRL

#include <model/state.hpp>
#include <ctrl/file.hpp>

class ReachingCtrl : public Controller {

  State goal;
  
protected:
  State state;                  
  
  /* Cf Controller::newState(const State&) */
  void newState(const State& state) {
    this->state = state; 
    if ( firstOdometry() )
      // move the goal into the state's frame
      setGoal(goal + state.configuration()); 
    Controller::newState(state); 
  }

  virtual void setGoal(const State& new_goal) { goal = new_goal; }
    
public:
  ReachingCtrl(const MotionModel& model, const double& ts,
               const State& goal_aimed)
    : Controller(model, ts), goal(goal_aimed) {}

  const State& getGoal() const { return goal; }
  
  virtual void changeGoal(const State& goal)
  // Set the goal in local frame & ask for future set in global frame
  { setGoal(goal); changeFirstOdometry(); }
    
};  // end of class ReachingCtrl

#endif // QTCTRL_REACH_CTRL