gripper_calibration_controller.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include "pr2_calibration_controllers/gripper_calibration_controller.h"
00036 #include "ros/time.h"
00037 #include "pluginlib/class_list_macros.h"
00038 
00039 using namespace std;
00040 using namespace controller;
00041 
00042 PLUGINLIB_EXPORT_CLASS(controller::GripperCalibrationController, pr2_controller_interface::Controller)
00043 
00044 namespace controller
00045 {
00046 
00047 GripperCalibrationController::GripperCalibrationController()
00048   : last_publish_time_(0), joint_(NULL)
00049 {
00050 }
00051 
00052 GripperCalibrationController::~GripperCalibrationController()
00053 {
00054 }
00055 
00056 bool GripperCalibrationController::init(pr2_mechanism_model::RobotState *robot,
00057                                         ros::NodeHandle &n)
00058 {
00059   assert(robot);
00060   robot_ = robot;
00061   node_ = n;
00062 
00063   node_.param("stopped_velocity_tolerance", stopped_velocity_tolerance_, 0.0001);
00064 
00065   XmlRpc::XmlRpcValue other_joint_names;
00066   if (node_.getParam("other_joints", other_joint_names))
00067   {
00068     if (other_joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
00069     {
00070       ROS_ERROR("\"other_joints\" was not an array (namespace: %s)", node_.getNamespace().c_str());
00071       return false;
00072     }
00073     else
00074     {
00075       for (int i = 0; i < other_joint_names.size(); ++i)
00076       {
00077         pr2_mechanism_model::JointState *j;
00078         std::string name = (std::string)other_joint_names[i];
00079         if ((j = robot->getJointState(name))){
00080           other_joints_.push_back(j);
00081         }
00082         else {
00083           ROS_ERROR("Could not find joint \"%s\" (namespace: %s)",
00084                     name.c_str(), node_.getNamespace().c_str());
00085           return false;
00086         }
00087       }
00088     }
00089   }
00090 
00091   if (!node_.getParam("velocity", search_velocity_))
00092   {
00093     ROS_ERROR("No velocity given (namespace: %s)", node_.getNamespace().c_str());
00094     return false;
00095   }
00096 
00097   std::string joint_name;
00098   if (!node_.getParam("joint", joint_name))
00099   {
00100     ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00101     return false;
00102   }
00103   if (!(joint_ = robot->getJointState(joint_name)))
00104   {
00105     ROS_ERROR("Could not find joint \"%s\" (namespace: %s)",
00106               joint_name.c_str(), node_.getNamespace().c_str());
00107     return false;
00108   }
00109 
00110   std::string actuator_name;
00111   if (!node_.getParam("actuator", actuator_name))
00112   {
00113     ROS_ERROR("No actuator given (namespace: %s)", node_.getNamespace().c_str());
00114     return false;
00115   }
00116   if (!(actuator_ = robot->model_->getActuator(actuator_name)))
00117   {
00118     ROS_ERROR("Could not find actuator \"%s\" (namespace: %s)",
00119               actuator_name.c_str(), node_.getNamespace().c_str());
00120     return false;
00121   }
00122 
00123   bool force_calibration = false;
00124   node_.getParam("force_calibration", force_calibration);
00125 
00126   state_ = INITIALIZED;
00127   joint_->calibrated_ = false;
00128   if (actuator_->state_.zero_offset_ != 0){
00129     if (force_calibration)
00130     {
00131       ROS_INFO("Joint %s will be recalibrated, but was already calibrated at offset %f", 
00132                joint_name.c_str(), actuator_->state_.zero_offset_);
00133     }
00134     else 
00135     {
00136       ROS_INFO("Joint %s is already calibrated at offset %f", joint_name.c_str(), actuator_->state_.zero_offset_);
00137       joint_->calibrated_ = true;
00138       for (size_t i = 0; i < other_joints_.size(); ++i)
00139         other_joints_[i]->calibrated_ = true;
00140       state_ = CALIBRATED;
00141     }
00142   }
00143   else{
00144     ROS_INFO("Joint %s is not yet calibrated", joint_name.c_str());
00145   }
00146 
00147   if (!vc_.init(robot, node_))
00148     return false;
00149 
00150   // advertise service to check calibration
00151   is_calibrated_srv_ = node_.advertiseService("is_calibrated", &GripperCalibrationController::isCalibrated, this);
00152 
00153   // "Calibrated" topic
00154   pub_calibrated_.reset(new realtime_tools::RealtimePublisher<std_msgs::Empty>(node_, "calibrated", 1));
00155 
00156   return true;
00157 }
00158 
00159 
00160 void GripperCalibrationController::starting()
00161 {
00162   state_ = INITIALIZED;
00163   actuator_->state_.zero_offset_ = 0.0;
00164   joint_->calibrated_ = false;
00165 }
00166 
00167 
00168 bool GripperCalibrationController::isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request& req,
00169                                                 pr2_controllers_msgs::QueryCalibrationState::Response& resp)
00170 {
00171   resp.is_calibrated = (state_ == CALIBRATED);
00172   return true;
00173 }
00174 
00175 
00176 void GripperCalibrationController::update()
00177 {
00178   assert(joint_);
00179   assert(actuator_);
00180 
00181   switch (state_)
00182   {
00183   case INITIALIZED:
00184     state_ = BEGINNING;
00185     return;
00186   case BEGINNING:
00187     count_ = 0;
00188     stop_count_ = 0;
00189     joint_->calibrated_ = false;
00190     actuator_->state_.zero_offset_ = 0.0;
00191 
00192     vc_.setCommand(search_velocity_);
00193 
00194     state_ = STARTING;
00195     break;
00196   case STARTING:
00197     // Makes sure we start moving for a bit before checking if we've stopped.
00198     if (++count_ > 100)
00199     {
00200       count_ = 0;
00201       stop_count_ = 0;
00202       state_ = CLOSING;
00203     }
00204     break;
00205   case CLOSING:
00206     // Makes sure the gripper is stopped for a while before cal
00207     if (fabs(joint_->velocity_) < this->stopped_velocity_tolerance_)
00208       stop_count_++;
00209     else
00210       stop_count_ = 0;
00211 
00212     if (stop_count_ > 100)
00213     {
00214       state_ = BACK_OFF;
00215       stop_count_ = 0;
00216       vc_.setCommand(-1 * search_velocity_);
00217     }
00218     break;
00219   case BACK_OFF: // Back off so we can reset from a known good position
00220     if (++stop_count_ > 1000)
00221     {
00222       state_ = CLOSING_SLOWLY;
00223       count_ = 0;
00224       stop_count_ = 0;
00225       vc_.setCommand(1.0 * search_velocity_);
00226     }
00227 
00228     break;
00229   case CLOSING_SLOWLY: // Close slowly to avoid windup
00230     // Makes sure the gripper is stopped for a while before cal
00231     if (fabs(joint_->velocity_) < this->stopped_velocity_tolerance_)
00232       stop_count_++;
00233     else
00234       stop_count_ = 0;
00235 
00236     if (stop_count_ > 500)
00237     {
00238       state_ = CALIBRATED;
00239       actuator_->state_.zero_offset_ = actuator_->state_.position_;
00240       joint_->calibrated_ = true;
00241       for (size_t i = 0; i < other_joints_.size(); ++i)
00242         other_joints_[i]->calibrated_ = true;
00243       vc_.setCommand(0);
00244     }
00245 
00246     break;
00247   case CALIBRATED:
00248     if (pub_calibrated_) {
00249       if (last_publish_time_ + ros::Duration(0.5) < robot_->getTime()) {
00250         if (pub_calibrated_->trylock()) {
00251           last_publish_time_ = robot_->getTime();
00252           pub_calibrated_->unlockAndPublish();
00253         }
00254       }
00255     }
00256     break;
00257   }
00258   if (state_ != CALIBRATED)
00259     vc_.update();
00260 }
00261 }
00262 


pr2_calibration_controllers
Author(s): Stuart Glaser
autogenerated on Thu Aug 27 2015 14:23:03