srh_fake_joint_calibration_controller.cpp
Go to the documentation of this file.
00001 
00027 #include "sr_mechanism_controllers/srh_fake_joint_calibration_controller.h"
00028 #include "ros/time.h"
00029 #include "pluginlib/class_list_macros.h"
00030 #include <boost/algorithm/string.hpp>
00031 #include <string>
00032 #include <std_srvs/Empty.h>
00033 
00034 PLUGINLIB_DECLARE_CLASS(sr_mechanism_controllers, SrhFakeJointCalibrationController, controller::SrhFakeJointCalibrationController, pr2_controller_interface::Controller)
00035 
00036 using namespace std;
00037 
00038 namespace controller {
00039 
00040   SrhFakeJointCalibrationController::SrhFakeJointCalibrationController()
00041     : robot_(NULL), last_publish_time_(0), state_(INITIALIZED),
00042       actuator_(NULL), joint_(NULL), transmission_(NULL)
00043   {
00044   }
00045 
00046   SrhFakeJointCalibrationController::~SrhFakeJointCalibrationController()
00047   {
00048   }
00049 
00050   bool SrhFakeJointCalibrationController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00051   {
00052     robot_ = robot;
00053     node_ = n;
00054     // Joint
00055 
00056     std::string joint_name;
00057     if (!node_.getParam("joint", joint_name))
00058     {
00059       ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00060       return false;
00061     }
00062     if (!(joint_ = robot->getJointState(joint_name)))
00063     {
00064       ROS_ERROR("Could not find joint %s (namespace: %s)",
00065                 joint_name.c_str(), node_.getNamespace().c_str());
00066       return false;
00067     }
00068     joint_name_ = joint_name;
00069 
00070     // Actuator
00071     std::string actuator_name;
00072     if (!node_.getParam("actuator", actuator_name))
00073     {
00074       ROS_ERROR("No actuator given (namespace: %s)", node_.getNamespace().c_str());
00075       return false;
00076     }
00077     if (!(actuator_ = robot->model_->getActuator(actuator_name)))
00078     {
00079       ROS_ERROR("Could not find actuator %s (namespace: %s)",
00080                 actuator_name.c_str(), node_.getNamespace().c_str());
00081       return false;
00082     }
00083     actuator_name_ = actuator_name;
00084 
00085     // Transmission
00086     std::string transmission_name;
00087     if (!node_.getParam("transmission", transmission_name))
00088     {
00089       ROS_ERROR("No transmission given (namespace: %s)", node_.getNamespace().c_str());
00090       return false;
00091     }
00092     if (!(transmission_ = robot->model_->getTransmission(transmission_name)))
00093     {
00094       ROS_ERROR("Could not find transmission %s (namespace: %s)",
00095                 transmission_name.c_str(), node_.getNamespace().c_str());
00096       return false;
00097     }
00098 
00099     // "Calibrated" topic
00100     pub_calibrated_.reset(
00101       new realtime_tools::RealtimePublisher<std_msgs::Empty>(node_, "calibrated", 1));
00102 
00103     return true;
00104   }
00105 
00106 
00107   void SrhFakeJointCalibrationController::update()
00108   {
00109     assert(joint_);
00110     assert(actuator_);
00111 
00112     switch(state_)
00113     {
00114     case INITIALIZED:
00115       state_ = BEGINNING;
00116       break;
00117     case BEGINNING:
00118       initialize_pids();
00119       joint_->calibrated_ = true;
00120       state_ = CALIBRATED;
00121       //We add the following line to delay for some time the first publish and allow the correct initialization of the subscribers in calibrate.py
00122       last_publish_time_ = robot_->getTime();
00123       break;
00124     case CALIBRATED:
00125       if (pub_calibrated_)
00126       {
00127         if (last_publish_time_ + ros::Duration(0.5) < robot_->getTime())
00128         {
00129           assert(pub_calibrated_);
00130           if (pub_calibrated_->trylock())
00131           {
00132             last_publish_time_ = robot_->getTime();
00133             pub_calibrated_->unlockAndPublish();
00134           }
00135         }
00136       }
00137       break;
00138     }
00139   }
00140 
00141   void SrhFakeJointCalibrationController::initialize_pids()
00142   {
00144     std::string service_name = "/realtime_loop/reset_motor_" + boost::to_upper_copy(actuator_name_);
00145     if( ros::service::waitForService (service_name, ros::Duration(2.0)) )
00146     {
00147       ros::ServiceClient client = node_.serviceClient<std_srvs::Empty>(service_name);
00148       std_srvs::Empty srv;
00149       if( client.call(srv) )
00150       {
00151         return;
00152       }
00153     }
00154   }
00155 
00156 } // namespace
00157 
00158 /* For the emacs weenies in the crowd.
00159 Local Variables:
00160    c-basic-offset: 2
00161 End:
00162 */


sr_mechanism_controllers
Author(s): Ugo Cupcic / ugo@shadowrobot.com , contact@shadowrobot.com
autogenerated on Fri Jan 3 2014 12:06:50