sr_controller.cpp
Go to the documentation of this file.
00001 
00026 #include "sr_mechanism_controllers/sr_controller.hpp"
00027 #include "angles/angles.h"
00028 #include "pluginlib/class_list_macros.h"
00029 #include <sstream>
00030 #include <math.h>
00031 #include "sr_utilities/sr_math_utils.hpp"
00032 
00033 #include <std_msgs/Float64.h>
00034 
00035 using namespace std;
00036 
00037 namespace controller {
00038 
00039   SrController::SrController()
00040     : joint_state_(NULL), command_(0),
00041       min_(0.0), max_(sr_math_utils::pi),
00042       loop_count_(0),  initialized_(false), robot_(NULL), last_time_(0),
00043       n_tilde_("~"),
00044       max_force_demand(1023.), friction_deadband(5), max_force_factor_(1.0)
00045   {
00046   }
00047 
00048   SrController::~SrController()
00049   {
00050     sub_command_.shutdown();
00051   }
00052 
00053   void SrController::after_init()
00054   {
00055     sub_command_ = node_.subscribe<std_msgs::Float64>("command", 1, &SrController::setCommandCB, this);
00056     sub_max_force_factor_ = node_.subscribe<std_msgs::Float64>("max_force_factor", 1, &SrController::maxForceFactorCB, this);
00057   }
00058 
00059 
00060   std::string SrController::getJointName()
00061   {
00062     return joint_state_->joint_->name;
00063   }
00064 
00065 // Set the joint position command
00066   void SrController::setCommand(double cmd)
00067   {
00068     command_ = cmd;
00069   }
00070 
00071 // Return the current position command
00072   void SrController::getCommand(double & cmd)
00073   {
00074     cmd = command_;
00075   }
00076 
00077   void SrController::setCommandCB(const std_msgs::Float64ConstPtr& msg)
00078   {
00079     command_ = msg->data;
00080   }
00081 
00082   bool SrController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00083   {
00084     return true;
00085   }
00086 
00087   void SrController::update()
00088   {
00089   }
00090 
00091   void SrController::starting()
00092   {}
00093 
00094   bool SrController::resetGains(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp)
00095   {
00096     return true;
00097   }
00098 
00099   void SrController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
00100   {}
00101 
00102   void SrController::get_min_max( urdf::Model model, std::string joint_name )
00103   {
00104     if( joint_name.substr(3,1).compare("0") == 0)
00105     {
00106       std::string j1 = joint_name.substr(0,3) + "1";
00107       std::string j2 = joint_name.substr(0,3) + "2";
00108 
00109       boost::shared_ptr<const urdf::Joint> joint1 = model.getJoint( j1 );
00110       boost::shared_ptr<const urdf::Joint> joint2 = model.getJoint( j2 );
00111 
00112       min_ = joint1->limits->lower + joint2->limits->lower;
00113       max_ = joint1->limits->upper + joint2->limits->upper;
00114     }
00115     else
00116     {
00117       boost::shared_ptr<const urdf::Joint> joint = model.getJoint( joint_name );
00118 
00119       min_ = joint->limits->lower;
00120       max_ = joint->limits->upper;
00121     }
00122   }
00123 
00124   double SrController::clamp_command( double cmd )
00125   {
00126     if(cmd < min_)
00127       return min_;
00128 
00129     if(cmd > max_)
00130       return max_;
00131 
00132     return cmd;
00133   }
00134 
00135   void SrController::maxForceFactorCB(const std_msgs::Float64ConstPtr& msg)
00136   {
00137     if((msg->data >= 0.0) && (msg->data <= 1.0))
00138     {
00139       max_force_factor_ = msg->data;
00140     }
00141     else
00142     {
00143       ROS_ERROR("Max force factor must be between 0.0 and 1.0. Discarding received value: %f", msg->data);
00144     }
00145   }
00146 }
00147 
00148 /* For the emacs weenies in the crowd.
00149 Local Variables:
00150    c-basic-offset: 2
00151 End:
00152 */
00153 
00154 


sr_mechanism_controllers
Author(s): Ugo Cupcic
autogenerated on Fri Aug 28 2015 13:09:56