srh_fake_joint_calibration_controller.h

Go to the documentation of this file.

#ifndef SR_MECHANISM_CONTROLLERS_SRH_FAKE_JOINT_CALIBRATION_CONTROLLER_H
#define SR_MECHANISM_CONTROLLERS_SRH_FAKE_JOINT_CALIBRATION_CONTROLLER_H

#include "ros/node_handle.h"
#include <string>
#include "ros_ethercat_model/robot_state_interface.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::RobotStateInterface>
{
public:
  SrhFakeJointCalibrationController();

  virtual bool init(ros_ethercat_model::RobotStateInterface *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();
};
}  // namespace controller

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


#endif  // SR_MECHANISM_CONTROLLERS_SRH_FAKE_JOINT_CALIBRATION_CONTROLLER_H