diff_drive_pose_controller.hpp

Go to the documentation of this file.

#ifndef YOCS_DIFF_DRIVE_POSE_CONTROLLER_HPP_
#define YOCS_DIFF_DRIVE_POSE_CONTROLLER_HPP_

#include <string>
#include <yocs_controllers/default_controller.hpp>

namespace yocs
{

class DiffDrivePoseController : public Controller
{
public:
  DiffDrivePoseController(std::string name, double v_max, double w_max, double dist_thres = 0.01, double orient_thres =
                              0.02,
                          double dist_eps = 0.01 * 0.2, double orient_eps = 0.02 * 0.2, double orientation_gain = 0.3,
                          double k_1 = 1.0, double k_2 = 3.0, double beta = 0.4, double lambda = 2.0, double v_min =
                              0.01,
                          double v_min_movement = 0.01, double w_min_movement = 0.01);
  virtual ~DiffDrivePoseController()
  {
  }

  void setVerbosity(const bool& verbose) { verbose_ = verbose; }

  virtual bool init()
  {
    return true;
  }

  void setCurrentLimits(double v_min, double w_min, double v_max, double w_max);

  virtual void setInput(double distance_to_goal, double delta, double theta);

  virtual bool step();

  virtual void getControlOutput(double& v, double& w);

protected:
  virtual void calculateControls();

  virtual void controlPose();

  virtual double enforceMinMax(double& value, double min, double max);

  virtual double enforceMinVelocity(double value, double min);

  virtual void controlOrientation(double angle_difference);

  virtual void onGoalReached()
  {

  }

protected:
  std::string name_;
  double r_;
  double delta_;
  double theta_;
  double v_;
  double v_min_movement_;
  double v_min_;
  double v_max_;
  double w_;
  double w_min_movement_;
  double w_min_;
  double w_max_;

  double cur_;
  double k_1_;
  double k_2_;
  double beta_;
  double lambda_;

  double orientation_gain_;

  double dist_thres_;
  double orient_thres_;
  bool pose_reached_;
  double dist_eps_;
  double orient_eps_;
  bool verbose_;
};

} /* end namespace */
#endif