srh_mixed_position_velocity_controller.hpp

Go to the documentation of this file.

#ifndef SRH_MIXED_POSITION_VELOCITY_CONTROLLER_H
#define SRH_MIXED_POSITION_VELOCITY_CONTROLLER_H

#include <sr_mechanism_controllers/sr_controller.hpp>
#include <sr_robot_msgs/SetMixedPositionVelocityPidGains.h>
#include <sr_robot_msgs/JointControllerState.h>

namespace controller
{
  class SrhMixedPositionVelocityJointController : public SrController
  {
  public:

    SrhMixedPositionVelocityJointController();
    ~SrhMixedPositionVelocityJointController();

    bool init( pr2_mechanism_model::RobotState *robot, const std::string &joint_name,
               boost::shared_ptr<control_toolbox::Pid> pid_position,
               boost::shared_ptr<control_toolbox::Pid> pid_velocity);
    bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n);

    virtual void starting();

    virtual void update();

    virtual void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
    virtual void getGains_velocity(double &p, double &i, double &d, double &i_max, double &i_min);
    virtual bool resetGains(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp);
    bool setGains(sr_robot_msgs::SetMixedPositionVelocityPidGains::Request &req, sr_robot_msgs::SetMixedPositionVelocityPidGains::Response &resp);

  private:
    boost::shared_ptr<control_toolbox::Pid> pid_controller_position_;       
    boost::shared_ptr<control_toolbox::Pid> pid_controller_velocity_;       
    //publish our joint controller state
    boost::shared_ptr<realtime_tools::RealtimePublisher<sr_robot_msgs::JointControllerState> > controller_state_publisher_;

    double max_velocity_, min_velocity_;

#ifdef DEBUG_PUBLISHER
    ros::Publisher debug_pub;
#endif

    ros::Subscriber sub_command_;
    void setCommandCB(const std_msgs::Float64ConstPtr& msg);

    ros::ServiceServer serve_set_gains_;

    double position_deadband;

    sr_deadband::HysteresisDeadband<double> hysteresis_deadband;

    void read_parameters();

    int motor_min_force_threshold;
  };
} // namespace

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


#endif