00001 00028 #ifndef _SRH_JOINT_VELOCITY_CONTROLLER_HPP_ 00029 #define _SRH_JOINT_VELOCITY_CONTROLLER_HPP_ 00030 00031 #include <sr_mechanism_controllers/sr_controller.hpp> 00032 #include <sr_mechanism_controllers/sr_friction_compensation.hpp> 00033 00034 00035 namespace controller 00036 { 00037 class SrhJointVelocityController : public SrController 00038 { 00039 public: 00040 00041 SrhJointVelocityController(); 00042 ~SrhJointVelocityController(); 00043 00044 bool init( pr2_mechanism_model::RobotState *robot, const std::string &joint_name, 00045 boost::shared_ptr<control_toolbox::Pid> pid_velocity); 00046 bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 00047 00048 virtual void starting(); 00049 00053 virtual void update(); 00054 00055 virtual void getGains(double &p, double &i, double &d, double &i_max, double &i_min); 00056 virtual bool resetGains(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp); 00057 bool setGains(sr_robot_msgs::SetPidGains::Request &req, sr_robot_msgs::SetPidGains::Response &resp); 00058 00059 private: 00060 boost::shared_ptr<control_toolbox::Pid> pid_controller_velocity_; 00062 00063 double velocity_deadband; 00064 00066 sr_deadband::HysteresisDeadband<double> hysteresis_deadband; 00067 00069 void read_parameters(); 00070 }; 00071 } // namespace 00072 00073 /* For the emacs weenies in the crowd. 00074 Local Variables: 00075 c-basic-offset: 2 00076 End: 00077 */ 00078 00079 00080 #endif