srh_fake_joint_calibration_controller.h

Go to the documentation of this file.

#ifndef _SRH_FAKE_JOINT_CALIBRATION_CONTROLLER_
#define _SRH_FAKE_JOINT_CALIBRATION_CONTROLLER_

#include "ros/node_handle.h"
#include "ros_ethercat_model/robot_state.hpp"
#include "velocity_controllers/joint_velocity_controller.h"
#include "realtime_tools/realtime_publisher.h"
#include "std_msgs/Empty.h"
#include "controller_interface/controller.h"
#include <boost/smart_ptr.hpp>


namespace controller
{

  class SrhFakeJointCalibrationController : public controller_interface::Controller<ros_ethercat_model::RobotState>
  {
  public:
    SrhFakeJointCalibrationController();

    virtual bool init(ros_ethercat_model::RobotState *robot, ros::NodeHandle &n);

    virtual void update(const ros::Time& time, const ros::Duration& period);

    bool calibrated() { return calibration_state_ == CALIBRATED; }
    void beginCalibration()
    {
      if (calibration_state_ == IS_INITIALIZED)
        calibration_state_ = BEGINNING;
    }

  protected:

    ros_ethercat_model::RobotState* robot_;
    ros::NodeHandle node_;
    boost::scoped_ptr<realtime_tools::RealtimePublisher<std_msgs::Empty> > pub_calibrated_;
    ros::Time last_publish_time_;

    enum { IS_INITIALIZED, BEGINNING, MOVING_TO_LOW, MOVING_TO_HIGH, CALIBRATED };
    int calibration_state_;

    ros_ethercat_model::Actuator *actuator_;
    ros_ethercat_model::JointState *joint_;

    std::string joint_name_, actuator_name_, robot_id_, joint_prefix_, ns_;

    void initialize_pids();
  };

}

/* For the emacs weenies in the crowd.
Local Variables:
   c-basic-offset: 2
End:
*/


#endif