sr_controller.hpp

Go to the documentation of this file.

#ifndef _SRH_CONTROLLER_HPP_
#define _SRH_CONTROLLER_HPP_

#include <ros/node_handle.h>

#include <controller_interface/controller.h>
#include <ros_ethercat_model/robot_state_interface.hpp>
#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 <control_msgs/JointControllerState.h>

#include <utility>
#include <string>

#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 controller_interface::Controller<ros_ethercat_model::RobotStateInterface>
{
public:
  SrController();

  virtual ~SrController();

  virtual bool init(ros_ethercat_model::RobotStateInterface *robot, ros::NodeHandle &n) = 0;

  virtual void starting(const ros::Time &time)
  {
  };

  virtual void update(const ros::Time &time, const ros::Duration &period) = 0;

  virtual bool resetGains(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
  {
    return true;
  };

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

  std::string getJointName();

  ros_ethercat_model::JointState *joint_state_;
  ros_ethercat_model::JointState *joint_state_2;
  bool has_j2;
  double command_;                                
protected:
  // true if this is joint 0
  bool is_joint_0();

  // set joint_state_ and joint_state_2
  void get_joints_states_1_2();

  void after_init();

  double clamp_command(double cmd, double min_cmd, double max_cmd);

  virtual double clamp_command(double cmd);

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

  double min_, max_;
  double vel_min_, vel_max_;
  double eff_min_, eff_max_;

  int loop_count_;
  bool initialized_;
  ros_ethercat_model::RobotState *robot_;
  ros::NodeHandle node_, n_tilde_;
  std::string joint_name_;

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

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

  virtual void setCommandCB(const std_msgs::Float64ConstPtr &msg)
  {
  }

  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 controller

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


#endif