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 "pr2_mechanism_model/robot.h"
#include "robot_mechanism_controllers/joint_velocity_controller.h"
#include "realtime_tools/realtime_publisher.h"
#include "std_msgs/Empty.h"


namespace controller
{

  class SrhFakeJointCalibrationController : public pr2_controller_interface::Controller
  {
  public:
    SrhFakeJointCalibrationController();
    virtual ~SrhFakeJointCalibrationController();

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

    virtual void update();

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

  protected:

    pr2_mechanism_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 state_;
    int countdown_;

    double search_velocity_, reference_position_;
    bool original_switch_state_;

    pr2_hardware_interface::Actuator *actuator_;
    pr2_mechanism_model::JointState *joint_;
    boost::shared_ptr<pr2_mechanism_model::Transmission> transmission_;

    std::string joint_name_, actuator_name_;

    void initialize_pids();
  };

}

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


#endif