00001 00027 #ifndef _SRH_CONTROLLER_HPP_ 00028 #define _SRH_CONTROLLER_HPP_ 00029 00030 #include <ros/node_handle.h> 00031 00032 #include <pr2_controller_interface/controller.h> 00033 #include <control_toolbox/pid.h> 00034 #include <boost/scoped_ptr.hpp> 00035 #include <boost/thread/condition.hpp> 00036 #include <realtime_tools/realtime_publisher.h> 00037 #include <std_msgs/Float64.h> 00038 #include <std_srvs/Empty.h> 00039 #include <pr2_controllers_msgs/JointControllerState.h> 00040 00041 #include <utility> 00042 00043 #include <sr_robot_msgs/SetPidGains.h> 00044 00045 #include <sr_utilities/sr_deadband.hpp> 00046 00047 #include <sr_mechanism_controllers/sr_friction_compensation.hpp> 00048 00049 00050 namespace controller 00051 { 00052 00053 class SrController : public pr2_controller_interface::Controller 00054 { 00055 public: 00056 00057 SrController(); 00058 virtual ~SrController(); 00059 00060 virtual bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 00061 00067 void setCommand(double cmd); 00068 void setCommandCB(const std_msgs::Float64ConstPtr& msg); 00069 00073 void getCommand(double & cmd); 00074 00075 virtual void starting(); 00076 00080 virtual void update(); 00081 00082 virtual bool resetGains(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp); 00083 00084 virtual void getGains(double &p, double &i, double &d, double &i_max, double &i_min); 00085 00086 std::string getJointName(); 00087 pr2_mechanism_model::JointState *joint_state_; 00088 pr2_mechanism_model::JointState *joint_state_2; 00089 bool has_j2; 00090 ros::Duration dt_; 00091 double command_; 00093 protected: 00095 void after_init(); 00096 00105 double clamp_command( double cmd ); 00106 00114 void get_min_max( urdf::Model model, std::string joint_name ); 00115 00117 double min_, max_; 00118 00119 int loop_count_; 00120 bool initialized_; 00121 pr2_mechanism_model::RobotState *robot_; 00122 ros::Time last_time_; 00124 ros::NodeHandle node_, n_tilde_; 00125 00126 boost::scoped_ptr< 00127 realtime_tools::RealtimePublisher< 00128 pr2_controllers_msgs::JointControllerState> > controller_state_publisher_ ; 00129 00130 boost::shared_ptr<sr_friction_compensation::SrFrictionCompensator> friction_compensator; 00131 00132 ros::Subscriber sub_command_; 00133 ros::ServiceServer serve_set_gains_; 00134 ros::ServiceServer serve_reset_gains_; 00135 00137 double max_force_demand; 00139 int friction_deadband; 00140 00142 sr_deadband::HysteresisDeadband<double> hysteresis_deadband; 00143 }; 00144 } // namespace 00145 00146 /* For the emacs weenies in the crowd. 00147 Local Variables: 00148 c-basic-offset: 2 00149 End: 00150 */ 00151 00152 00153 #endif