sr_controller.hpp

Go to the documentation of this file.

#ifndef _SRH_CONTROLLER_HPP_
#define _SRH_CONTROLLER_HPP_

#include <ros/node_handle.h>

#include <pr2_controller_interface/controller.h>
#include <control_toolbox/pid.h>
#include <boost/scoped_ptr.hpp>
#include <boost/thread/condition.hpp>
#include <realtime_tools/realtime_publisher.h>
#include <std_msgs/Float64.h>
#include <std_srvs/Empty.h>
#include <pr2_controllers_msgs/JointControllerState.h>

#include <utility>

#include <sr_robot_msgs/SetPidGains.h>

#include <sr_utilities/sr_deadband.hpp>

#include <sr_mechanism_controllers/sr_friction_compensation.hpp>


namespace controller
{

  class SrController : public pr2_controller_interface::Controller
  {
  public:

    SrController();
    virtual ~SrController();

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

    void setCommand(double cmd);
    virtual void setCommandCB(const std_msgs::Float64ConstPtr& msg);

    void getCommand(double & cmd);

    virtual void starting();

    virtual void update();

    virtual bool resetGains(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp);

    virtual void getGains(double &p, double &i, double &d, double &i_max, double &i_min);

    std::string getJointName();
    pr2_mechanism_model::JointState *joint_state_;        
    pr2_mechanism_model::JointState *joint_state_2;        
    bool has_j2;         
    ros::Duration dt_;
    double command_;                            
  protected:
    void after_init();

    double clamp_command( double cmd );

    void get_min_max( urdf::Model model, std::string joint_name );

    double min_, max_;

    int loop_count_;
    bool initialized_;
    pr2_mechanism_model::RobotState *robot_;              
    ros::Time last_time_;                          
    ros::NodeHandle node_, n_tilde_;

    boost::scoped_ptr<
      realtime_tools::RealtimePublisher<
        pr2_controllers_msgs::JointControllerState> > controller_state_publisher_ ;

    boost::shared_ptr<sr_friction_compensation::SrFrictionCompensator> friction_compensator;

    ros::Subscriber sub_command_;
    ros::ServiceServer serve_set_gains_;
    ros::ServiceServer serve_reset_gains_;

    double max_force_demand;
    int friction_deadband;

    sr_deadband::HysteresisDeadband<double> hysteresis_deadband;

    //The max force factor is a number between 0.0 and 1.0 that will multiply the max_force_demand
    //it is initialized to 1.0 and can be updated via a topic.
    //This is intended to be used e.g. as part of a hand self protection mechanism, where max_force is reduced in certain cases
    double max_force_factor_;
    ros::Subscriber sub_max_force_factor_;
    void maxForceFactorCB(const std_msgs::Float64ConstPtr& msg);
  };
} // namespace

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


#endif