caster_calibration_controller.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Author: Stuart Glaser
00032  */
00033 
00034 #include "pr2_calibration_controllers/caster_calibration_controller.h"
00035 #include "pluginlib/class_list_macros.h"
00036 
00037 PLUGINLIB_EXPORT_CLASS(controller::CasterCalibrationController, pr2_controller_interface::Controller)
00038 
00039 namespace controller {
00040 
00041 CasterCalibrationController::CasterCalibrationController()
00042 : robot_(NULL),
00043   joint_(NULL), wheel_l_joint_(NULL), wheel_r_joint_(NULL), last_publish_time_(0)
00044 {
00045 }
00046 
00047 CasterCalibrationController::~CasterCalibrationController()
00048 {
00049   for (size_t i = 0; i < fake_as.size(); ++i)
00050     delete fake_as[i];
00051   for (size_t i = 0; i < fake_js.size(); ++i)
00052     delete fake_js[i];
00053 }
00054 
00055 bool CasterCalibrationController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00056 {
00057   node_ = n;
00058   robot_ = robot;
00059 
00060   // Joints
00061 
00062   std::string joint_name;
00063   if (!node_.getParam("joints/caster", joint_name))
00064   {
00065     ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00066     return false;
00067   }
00068   if (!(joint_ = robot->getJointState(joint_name)))
00069   {
00070     ROS_ERROR("Could not find joint %s (namespace: %s)",
00071               joint_name.c_str(), node_.getNamespace().c_str());
00072     return false;
00073   }
00074   if (!joint_->joint_->calibration)
00075   {
00076     ROS_ERROR("No calibration reference position specified for joint %s (namespace: %s)",
00077               joint_name.c_str(), node_.getNamespace().c_str());
00078     return false;
00079   }
00080   if (!node_.getParam("velocity", search_velocity_))
00081   {
00082     ROS_ERROR("No velocity given (namespace: %s)", node_.getNamespace().c_str());
00083     return false;
00084   }
00085 
00086   // check if calibration fields are supported by this controller
00087   if (!joint_->joint_->calibration->falling && !joint_->joint_->calibration->rising){
00088     ROS_ERROR("No rising or falling edge is specified for calibration of joint %s. Note that the reference_position is not used any more", joint_name.c_str());
00089     return false;
00090   }
00091   if (joint_->joint_->calibration->falling && joint_->joint_->calibration->rising && joint_->joint_->type != urdf::Joint::CONTINUOUS){
00092     ROS_ERROR("Both rising and falling edge are specified for non-continuous joint %s. This is not supported.", joint_name.c_str());
00093     return false;
00094   }
00095   if (search_velocity_ < 0){
00096     search_velocity_ *= -1;
00097     ROS_WARN("Negative search velocity is not supported for joint %s. Making the search velocity positve.", joint_name.c_str());
00098   }
00099 
00100   // finds search velocity based on rising or falling edge
00101   if (joint_->joint_->calibration->falling && joint_->joint_->calibration->rising){
00102     joint_->reference_position_ = *(joint_->joint_->calibration->rising);
00103     ROS_DEBUG("Using positive search velocity for joint %s", joint_name.c_str());
00104   }
00105   else if (joint_->joint_->calibration->falling){
00106     joint_->reference_position_ = *(joint_->joint_->calibration->falling);
00107     search_velocity_ *= -1.0;
00108     ROS_DEBUG("Using negative search velocity for joint %s", joint_name.c_str());
00109   }
00110   else if (joint_->joint_->calibration->rising){
00111     joint_->reference_position_ = *(joint_->joint_->calibration->rising);
00112     ROS_DEBUG("Using positive search velocity for joint %s", joint_name.c_str());
00113   }
00114 
00115   if (!node_.getParam("joints/wheel_l", joint_name))
00116   {
00117     ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00118     return false;
00119   }
00120   if (!(wheel_l_joint_ = robot->getJointState(joint_name)))
00121   {
00122     ROS_ERROR("Could not find joint %s (namespace: %s)",
00123               joint_name.c_str(), node_.getNamespace().c_str());
00124     return false;
00125   }
00126 
00127   if (!node_.getParam("joints/wheel_r", joint_name))
00128   {
00129     ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00130     return false;
00131   }
00132   if (!(wheel_r_joint_ = robot->getJointState(joint_name)))
00133   {
00134     ROS_ERROR("Could not find joint %s (namespace: %s)",
00135               joint_name.c_str(), node_.getNamespace().c_str());
00136     return false;
00137   }
00138 
00139   // Actuator
00140 
00141   std::string actuator_name;
00142   if (!node_.getParam("actuator", actuator_name))
00143   {
00144     ROS_ERROR("No actuator given (namespace: %s)", node_.getNamespace().c_str());
00145     return false;
00146   }
00147   if (!(actuator_ = robot->model_->getActuator(actuator_name)))
00148   {
00149     ROS_ERROR("Could not find actuator %s (namespace: %s)",
00150               actuator_name.c_str(), node_.getNamespace().c_str());
00151     return false;
00152   }
00153 
00154   bool force_calibration = false;
00155   node_.getParam("force_calibration", force_calibration);
00156 
00157   state_ = INITIALIZED;
00158   joint_->calibrated_ = false;
00159   wheel_l_joint_->calibrated_ = false;
00160   wheel_r_joint_->calibrated_ = false;
00161   if (actuator_->state_.zero_offset_ != 0){
00162     if (force_calibration)
00163     {
00164       ROS_INFO("Joint %s will be recalibrated, but was already calibrated at offset %f", 
00165                joint_name.c_str(), actuator_->state_.zero_offset_);
00166     }
00167     else 
00168     {
00169       ROS_INFO("Joint %s is already calibrated at offset %f", joint_name.c_str(), actuator_->state_.zero_offset_);
00170       state_ = CALIBRATED;
00171       joint_->calibrated_ = true;
00172       wheel_l_joint_->calibrated_ = true;
00173       wheel_r_joint_->calibrated_ = true;
00174     }
00175   }
00176   else{
00177     ROS_INFO("Joint %s is not yet calibrated", joint_name.c_str());
00178   }
00179 
00180 
00181   // Transmission
00182   std::string transmission_name;
00183   if (!node_.getParam("transmission", transmission_name))
00184   {
00185     ROS_ERROR("No transmission given (namespace: %s)", node_.getNamespace().c_str());
00186     return false;
00187   }
00188   if (!(transmission_ = robot->model_->getTransmission(transmission_name)))
00189   {
00190     ROS_ERROR("Could not find transmission %s (namespace: %s)",
00191               transmission_name.c_str(), node_.getNamespace().c_str());
00192     return false;
00193   }
00194 
00195   if (!node_.getParam("velocity", search_velocity_))
00196   {
00197     ROS_ERROR("Velocity value was not specified (namespace: %s)", node_.getNamespace().c_str());
00198     return false;
00199   }
00200 
00201   fake_as.push_back(new pr2_hardware_interface::Actuator);
00202   fake_js.push_back(new pr2_mechanism_model::JointState);
00203 
00204   if (!cc_.init(robot_, node_))
00205     return false;
00206 
00207   // advertise service to check calibration
00208   is_calibrated_srv_ = node_.advertiseService("is_calibrated", &CasterCalibrationController::isCalibrated, this);
00209 
00210   // "Calibrated" topic
00211   pub_calibrated_.reset(new realtime_tools::RealtimePublisher<std_msgs::Empty>(node_, "calibrated", 1));
00212 
00213   return true;
00214 }
00215 
00216 void CasterCalibrationController::starting()
00217 {
00218   state_ = INITIALIZED;
00219   actuator_->state_.zero_offset_ = 0.0;
00220   joint_->calibrated_ = false;
00221   wheel_l_joint_->calibrated_ = false;
00222   wheel_r_joint_->calibrated_ = false;
00223 }
00224 
00225 
00226 bool CasterCalibrationController::isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request& req,
00227                                                pr2_controllers_msgs::QueryCalibrationState::Response& resp)
00228 {
00229   ROS_DEBUG("Is calibrated service %d", state_ == CALIBRATED);
00230   resp.is_calibrated = (state_ == CALIBRATED);
00231   return true;
00232 }
00233 
00234 
00235 
00236 void CasterCalibrationController::update()
00237 {
00238   assert(joint_);
00239   assert(actuator_);
00240   ros::Time time = robot_->getTime();
00241 
00242   switch(state_)
00243   {
00244   case INITIALIZED:
00245     cc_.steer_velocity_ = 0.0;
00246     cc_.drive_velocity_ = 0.0;
00247     state_ = BEGINNING;
00248     break;
00249   case BEGINNING:
00250     beginning_ = time;
00251     original_switch_state_ = actuator_->state_.calibration_reading_ & 1;
00252     original_position_ = joint_->position_;
00253     cc_.steer_velocity_ = (original_switch_state_ ? -search_velocity_ : search_velocity_);
00254     state_ = MOVING;
00255     break;
00256   case MOVING: {
00257     bool switch_state_ = actuator_->state_.calibration_reading_ & 1;
00258     if (switch_state_ != original_switch_state_)
00259     {
00260       // detect when we hit the wrong transition because someone pushed the caster during calibration
00261       if ((cc_.steer_velocity_ > 0.0 && (joint_->position_ - original_position_) < 0) ||
00262           (cc_.steer_velocity_ < 0.0 && (joint_->position_ - original_position_) > 0))
00263       {
00264         state_ = BEGINNING;
00265         ROS_ERROR("Caster hit the falling edge instead of the rising edge. Calibrating again...");
00266         ros::Duration(1.0).sleep();  // give caster some time to move away from transition
00267         break;
00268       }
00269 
00270       // Where was the joint when the optical switch triggered?
00271       if (switch_state_ == true)
00272         actuator_->state_.zero_offset_ = actuator_->state_.last_calibration_rising_edge_;
00273       else
00274         actuator_->state_.zero_offset_ = actuator_->state_.last_calibration_falling_edge_;
00275       
00276       joint_->calibrated_ = true;
00277       wheel_l_joint_->calibrated_ = true;
00278       wheel_r_joint_->calibrated_ = true;
00279       
00280       state_ = CALIBRATED;
00281       cc_.steer_velocity_ = 0.0;
00282     }
00283     else
00284       {
00285       // The caster is not strong enough to consistently move.  The
00286       // rest of this block contains the hacks to ensure that
00287       // calibration always completes.
00288       if (time > beginning_ + ros::Duration(6.0))
00289       {
00290         if ((unstick_iter_ / 1000) % 2 == 0)
00291           cc_.steer_velocity_ = 4.0 * (original_switch_state_ ? -search_velocity_ : search_velocity_);
00292         else
00293           cc_.steer_velocity_ = 0.0;
00294         ++unstick_iter_;
00295       }
00296       else
00297         unstick_iter_ = 0;
00298     }
00299     break;
00300   }
00301   case CALIBRATED:
00302     cc_.steer_velocity_ = 0.0;
00303     if (pub_calibrated_) {
00304       if (last_publish_time_ + ros::Duration(0.5) < robot_->getTime())  {
00305         if (pub_calibrated_->trylock())  {
00306           last_publish_time_ = robot_->getTime();
00307           pub_calibrated_->unlockAndPublish();
00308         }
00309       }
00310     }
00311     break;
00312   }
00313 
00314   if (state_ != CALIBRATED)
00315     cc_.update();
00316 }
00317 
00318 } // namespace


pr2_calibration_controllers
Author(s): Stuart Glaser
autogenerated on Thu Apr 24 2014 15:45:00