sr_controller.cpp

Go to the documentation of this file.

#include "sr_mechanism_controllers/sr_controller.hpp"
#include "angles/angles.h"
#include "pluginlib/class_list_macros.h"
#include <sstream>
#include <math.h>
#include "sr_utilities/sr_math_utils.hpp"

#include <std_msgs/Float64.h>

using namespace std;

namespace controller {

  SrController::SrController()
    : joint_state_(NULL), command_(0),
      min_(0.0), max_(sr_math_utils::pi),
      loop_count_(0),  initialized_(false), robot_(NULL), last_time_(0),
      n_tilde_("~"),
      max_force_demand(1023.), friction_deadband(5), max_force_factor_(1.0)
  {
  }

  SrController::~SrController()
  {
    sub_command_.shutdown();
  }

  void SrController::after_init()
  {
    sub_command_ = node_.subscribe<std_msgs::Float64>("command", 1, &SrController::setCommandCB, this);
    sub_max_force_factor_ = node_.subscribe<std_msgs::Float64>("max_force_factor", 1, &SrController::maxForceFactorCB, this);
  }


  std::string SrController::getJointName()
  {
    return joint_state_->joint_->name;
  }

// Set the joint position command
  void SrController::setCommand(double cmd)
  {
    command_ = cmd;
  }

// Return the current position command
  void SrController::getCommand(double & cmd)
  {
    cmd = command_;
  }

  void SrController::setCommandCB(const std_msgs::Float64ConstPtr& msg)
  {
    command_ = msg->data;
  }

  bool SrController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
  {
    return true;
  }

  void SrController::update()
  {
  }

  void SrController::starting()
  {}

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

  void SrController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
  {}

  void SrController::get_min_max( urdf::Model model, std::string joint_name )
  {
    if( joint_name.substr(3,1).compare("0") == 0)
    {
      std::string j1 = joint_name.substr(0,3) + "1";
      std::string j2 = joint_name.substr(0,3) + "2";

      boost::shared_ptr<const urdf::Joint> joint1 = model.getJoint( j1 );
      boost::shared_ptr<const urdf::Joint> joint2 = model.getJoint( j2 );

      min_ = joint1->limits->lower + joint2->limits->lower;
      max_ = joint1->limits->upper + joint2->limits->upper;
    }
    else
    {
      boost::shared_ptr<const urdf::Joint> joint = model.getJoint( joint_name );

      min_ = joint->limits->lower;
      max_ = joint->limits->upper;
    }
  }

  double SrController::clamp_command( double cmd )
  {
    if(cmd < min_)
      return min_;

    if(cmd > max_)
      return max_;

    return cmd;
  }

  void SrController::maxForceFactorCB(const std_msgs::Float64ConstPtr& msg)
  {
    if((msg->data >= 0.0) && (msg->data <= 1.0))
    {
      max_force_factor_ = msg->data;
    }
    else
    {
      ROS_ERROR("Max force factor must be between 0.0 and 1.0. Discarding received value: %f", msg->data);
    }
  }
}

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