pr2_calibration_controllers: caster_calibration

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_DECLARE_CLASS(pr2_calibration_controllers, CasterCalibrationController,
00038                         controller::CasterCalibrationController, pr2_controller_interface::Controller)
00039 
00040 namespace controller {
00041 
00042 CasterCalibrationController::CasterCalibrationController()
00043 : robot_(NULL),
00044   joint_(NULL), wheel_l_joint_(NULL), wheel_r_joint_(NULL), last_publish_time_(0)
00045 {
00046 }
00047 
00048 CasterCalibrationController::~CasterCalibrationController()
00049 {
00050   for (size_t i = 0; i < fake_as.size(); ++i)
00051     delete fake_as[i];
00052   for (size_t i = 0; i < fake_js.size(); ++i)
00053     delete fake_js[i];
00054 }
00055 
00056 bool CasterCalibrationController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00057 {
00058   node_ = n;
00059   robot_ = robot;
00060 
00061   // Joints
00062 
00063   std::string joint_name;
00064   if (!node_.getParam("joints/caster", joint_name))
00065   {
00066     ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00067     return false;
00068   }
00069   if (!(joint_ = robot->getJointState(joint_name)))
00070   {
00071     ROS_ERROR("Could not find joint %s (namespace: %s)",
00072               joint_name.c_str(), node_.getNamespace().c_str());
00073     return false;
00074   }
00075   if (!joint_->joint_->calibration)
00076   {
00077     ROS_ERROR("No calibration reference position specified for joint %s (namespace: %s)",
00078               joint_name.c_str(), node_.getNamespace().c_str());
00079     return false;
00080   }
00081   if (!node_.getParam("velocity", search_velocity_))
00082   {
00083     ROS_ERROR("No velocity given (namespace: %s)", node_.getNamespace().c_str());
00084     return false;
00085   }
00086 
00087   // check if calibration fields are supported by this controller
00088   if (!joint_->joint_->calibration->falling && !joint_->joint_->calibration->rising){
00089     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());
00090     return false;
00091   }
00092   if (joint_->joint_->calibration->falling && joint_->joint_->calibration->rising && joint_->joint_->type != urdf::Joint::CONTINUOUS){
00093     ROS_ERROR("Both rising and falling edge are specified for non-continuous joint %s. This is not supported.", joint_name.c_str());
00094     return false;
00095   }
00096   if (search_velocity_ < 0){
00097     search_velocity_ *= -1;
00098     ROS_WARN("Negative search velocity is not supported for joint %s. Making the search velocity positve.", joint_name.c_str());
00099   }
00100 
00101   // finds search velocity based on rising or falling edge
00102   if (joint_->joint_->calibration->falling && joint_->joint_->calibration->rising){
00103     joint_->reference_position_ = *(joint_->joint_->calibration->rising);
00104     ROS_DEBUG("Using positive search velocity for joint %s", joint_name.c_str());
00105   }
00106   else if (joint_->joint_->calibration->falling){
00107     joint_->reference_position_ = *(joint_->joint_->calibration->falling);
00108     search_velocity_ *= -1.0;
00109     ROS_DEBUG("Using negative search velocity for joint %s", joint_name.c_str());
00110   }
00111   else if (joint_->joint_->calibration->rising){
00112     joint_->reference_position_ = *(joint_->joint_->calibration->rising);
00113     ROS_DEBUG("Using positive search velocity for joint %s", joint_name.c_str());
00114   }
00115 
00116   if (!node_.getParam("joints/wheel_l", joint_name))
00117   {
00118     ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00119     return false;
00120   }
00121   if (!(wheel_l_joint_ = robot->getJointState(joint_name)))
00122   {
00123     ROS_ERROR("Could not find joint %s (namespace: %s)",
00124               joint_name.c_str(), node_.getNamespace().c_str());
00125     return false;
00126   }
00127 
00128   if (!node_.getParam("joints/wheel_r", joint_name))
00129   {
00130     ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00131     return false;
00132   }
00133   if (!(wheel_r_joint_ = robot->getJointState(joint_name)))
00134   {
00135     ROS_ERROR("Could not find joint %s (namespace: %s)",
00136               joint_name.c_str(), node_.getNamespace().c_str());
00137     return false;
00138   }
00139 
00140   // Actuator
00141 
00142   std::string actuator_name;
00143   if (!node_.getParam("actuator", actuator_name))
00144   {
00145     ROS_ERROR("No actuator given (namespace: %s)", node_.getNamespace().c_str());
00146     return false;
00147   }
00148   if (!(actuator_ = robot->model_->getActuator(actuator_name)))
00149   {
00150     ROS_ERROR("Could not find actuator %s (namespace: %s)",
00151               actuator_name.c_str(), node_.getNamespace().c_str());
00152     return false;
00153   }
00154   if (actuator_->state_.zero_offset_ != 0){
00155     ROS_INFO("Joint %s is already calibrated at offset %f", joint_name.c_str(), actuator_->state_.zero_offset_);
00156     state_ = CALIBRATED;
00157     joint_->calibrated_ = true;
00158     wheel_l_joint_->calibrated_ = true;
00159     wheel_r_joint_->calibrated_ = true;
00160   }
00161   else{
00162     ROS_INFO("Joint %s is not yet calibrated", joint_name.c_str());
00163     state_ = INITIALIZED;
00164     joint_->calibrated_ = false;
00165     wheel_l_joint_->calibrated_ = false;
00166     wheel_r_joint_->calibrated_ = false;
00167   }
00168 
00169 
00170 
00171   // Transmission
00172   std::string transmission_name;
00173   if (!node_.getParam("transmission", transmission_name))
00174   {
00175     ROS_ERROR("No transmission given (namespace: %s)", node_.getNamespace().c_str());
00176     return false;
00177   }
00178   if (!(transmission_ = robot->model_->getTransmission(transmission_name)))
00179   {
00180     ROS_ERROR("Could not find transmission %s (namespace: %s)",
00181               transmission_name.c_str(), node_.getNamespace().c_str());
00182     return false;
00183   }
00184 
00185   if (!node_.getParam("velocity", search_velocity_))
00186   {
00187     ROS_ERROR("Velocity value was not specified (namespace: %s)", node_.getNamespace().c_str());
00188     return false;
00189   }
00190 
00191   fake_as.push_back(new pr2_hardware_interface::Actuator);
00192   fake_js.push_back(new pr2_mechanism_model::JointState);
00193 
00194   if (!cc_.init(robot_, node_))
00195     return false;
00196 
00197   // advertise service to check calibration
00198   is_calibrated_srv_ = node_.advertiseService("is_calibrated", &CasterCalibrationController::isCalibrated, this);
00199 
00200   // "Calibrated" topic
00201   pub_calibrated_.reset(new realtime_tools::RealtimePublisher<std_msgs::Empty>(node_, "calibrated", 1));
00202 
00203   return true;
00204 }
00205 
00206 void CasterCalibrationController::starting()
00207 {
00208   state_ = INITIALIZED;
00209   actuator_->state_.zero_offset_ = 0.0;
00210   joint_->calibrated_ = false;
00211   wheel_l_joint_->calibrated_ = false;
00212   wheel_r_joint_->calibrated_ = false;
00213 }
00214 
00215 
00216 bool CasterCalibrationController::isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request& req,
00217                                                pr2_controllers_msgs::QueryCalibrationState::Response& resp)
00218 {
00219   ROS_DEBUG("Is calibrated service %d", state_ == CALIBRATED);
00220   resp.is_calibrated = (state_ == CALIBRATED);
00221   return true;
00222 }
00223 
00224 
00225 
00226 void CasterCalibrationController::update()
00227 {
00228   assert(joint_);
00229   assert(actuator_);
00230   ros::Time time = robot_->getTime();
00231 
00232   switch(state_)
00233   {
00234   case INITIALIZED:
00235     cc_.steer_velocity_ = 0.0;
00236     cc_.drive_velocity_ = 0.0;
00237     state_ = BEGINNING;
00238     break;
00239   case BEGINNING:
00240     beginning_ = time;
00241     original_switch_state_ = actuator_->state_.calibration_reading_ & 1;
00242     original_position_ = joint_->position_;
00243     cc_.steer_velocity_ = (original_switch_state_ ? -search_velocity_ : search_velocity_);
00244     state_ = MOVING;
00245     break;
00246   case MOVING: {
00247     bool switch_state_ = actuator_->state_.calibration_reading_ & 1;
00248     if (switch_state_ != original_switch_state_)
00249     {
00250       // detect when we hit the wrong transition because someone pushed the caster during calibration
00251       if ((cc_.steer_velocity_ > 0.0 && (joint_->position_ - original_position_) < 0) ||
00252           (cc_.steer_velocity_ < 0.0 && (joint_->position_ - original_position_) > 0))
00253       {
00254         state_ = BEGINNING;
00255         ROS_ERROR("Caster hit the falling edge instead of the rising edge. Calibrating again...");
00256         ros::Duration(1.0).sleep();  // give caster some time to move away from transition
00257         break;
00258       }
00259 
00260       // Where was the joint when the optical switch triggered?
00261       if (switch_state_ == true)
00262         actuator_->state_.zero_offset_ = actuator_->state_.last_calibration_rising_edge_;
00263       else
00264         actuator_->state_.zero_offset_ = actuator_->state_.last_calibration_falling_edge_;
00265       
00266       joint_->calibrated_ = true;
00267       wheel_l_joint_->calibrated_ = true;
00268       wheel_r_joint_->calibrated_ = true;
00269       
00270       state_ = CALIBRATED;
00271       cc_.steer_velocity_ = 0.0;
00272     }
00273     else
00274       {
00275       // The caster is not strong enough to consistently move.  The
00276       // rest of this block contains the hacks to ensure that
00277       // calibration always completes.
00278       if (time > beginning_ + ros::Duration(6.0))
00279       {
00280         if ((unstick_iter_ / 1000) % 2 == 0)
00281           cc_.steer_velocity_ = 4.0 * (original_switch_state_ ? -search_velocity_ : search_velocity_);
00282         else
00283           cc_.steer_velocity_ = 0.0;
00284         ++unstick_iter_;
00285       }
00286       else
00287         unstick_iter_ = 0;
00288     }
00289     break;
00290   }
00291   case CALIBRATED:
00292     cc_.steer_velocity_ = 0.0;
00293     if (pub_calibrated_) {
00294       if (last_publish_time_ + ros::Duration(0.5) < robot_->getTime())  {
00295         if (pub_calibrated_->trylock())  {
00296           last_publish_time_ = robot_->getTime();
00297           pub_calibrated_->unlockAndPublish();
00298         }
00299       }
00300     }
00301     break;
00302   }
00303 
00304   if (state_ != CALIBRATED)
00305     cc_.update();
00306 }
00307 
00308 } // namespace
caster_calibration_controller.cpp
