sr_controller.hpp
Go to the documentation of this file.
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     virtual 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     //The max force factor is a number between 0.0 and 1.0 that will multiply the max_force_demand
00145     //it is initialized to 1.0 and can be updated via a topic.
00146     //This is intended to be used e.g. as part of a hand self protection mechanism, where max_force is reduced in certain cases
00147     double max_force_factor_;
00148     ros::Subscriber sub_max_force_factor_;
00154     void maxForceFactorCB(const std_msgs::Float64ConstPtr& msg);
00155   };
00156 } // namespace
00157 
00158 /* For the emacs weenies in the crowd.
00159 Local Variables:
00160    c-basic-offset: 2
00161 End:
00162 */
00163 
00164 
00165 #endif


sr_mechanism_controllers
Author(s): Ugo Cupcic / ugo@shadowrobot.com , contact@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:54:39