joint_qualification_controllers: checkout

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 "joint_qualification_controllers/checkout_controller.h"
00036 #include "pluginlib/class_list_macros.h"
00037 
00038 PLUGINLIB_DECLARE_CLASS(joint_qualification_controllers, CheckoutController, 
00039                         joint_qualification_controllers::CheckoutController, 
00040                         pr2_controller_interface::Controller)
00041 
00042 using namespace std;
00043 using namespace joint_qualification_controllers;
00044 
00045 CheckoutController::CheckoutController() :
00046   robot_(NULL),
00047   data_sent_(false),
00048   robot_data_pub_(NULL)
00049 {
00050   robot_data_.test_time = -1;
00051   robot_data_.num_joints = 0;
00052   robot_data_.num_actuators = 0;
00053 
00054   state_          = STARTING;
00055   joint_count_    = 0;
00056   actuator_count_ = 0;
00057 
00058   timeout_        = 30.0;
00059 }
00060 
00061 bool CheckoutController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00062 {
00063   assert(robot);
00064   robot_ = robot;
00065 
00066   joint_count_ = robot_->joint_states_.size();
00067   robot_data_.num_joints = joint_count_;
00068   robot_data_.timeout = false;
00069 
00070   robot_data_.joint_data.resize(joint_count_);
00071 
00072   for (int i = 0; i < joint_count_; i++)
00073   {
00074     pr2_mechanism_model::JointState *js = &(robot_->joint_states_[i]);
00075 
00076     robot_data_.joint_data[i].index = i;
00077     robot_data_.joint_data[i].name = js->joint_->name;
00078     robot_data_.joint_data[i].is_cal = 0;
00079     robot_data_.joint_data[i].has_safety = js->joint_->safety && js->joint_->limits;
00080 
00081     // Assign type strings based on type
00082     switch (js->joint_->type)
00083     {
00084     case (urdf::Joint::FIXED):
00085       robot_data_.joint_data[i].type = "Fixed";
00086       break;
00087     case (urdf::Joint::REVOLUTE):
00088       robot_data_.joint_data[i].type = "Rotary";
00089       break;
00090     case (urdf::Joint::CONTINUOUS):
00091       robot_data_.joint_data[i].type = "Continuous";
00092       break;
00093     case (urdf::Joint::PRISMATIC):
00094       robot_data_.joint_data[i].type = "Prismatic";
00095       break;
00096     case (urdf::Joint::PLANAR):
00097       robot_data_.joint_data[i].type = "Planar";
00098       break;
00099     default:
00100       robot_data_.joint_data[i].type = "No type given!";
00101       break;
00102     }
00103   }
00104 
00105   // Assign actuators
00106   actuator_count_ = robot_->model_->hw_->actuators_.size();
00107   robot_data_.num_actuators = actuator_count_;
00108   robot_data_.actuator_data.resize(actuator_count_);
00109 
00110   pr2_hardware_interface::ActuatorMap::const_iterator it;
00111   int i = 0;
00112   for (it = robot_->model_->hw_->actuators_.begin();
00113        it != robot_->model_->hw_->actuators_.end();
00114        ++it, ++i)
00115   {
00116     pr2_hardware_interface::Actuator *actuator = it->second;
00117 
00118     robot_data_.actuator_data[i].index = i;
00119     robot_data_.actuator_data[i].name = actuator->name_;
00120     robot_data_.actuator_data[i].id = actuator->state_.device_id_;
00121   }
00122 
00123   n.param("timeout", timeout_, 30.0);
00124 
00125   initial_time_ = robot_->getTime();
00126 
00127   robot_data_pub_.reset(new realtime_tools::RealtimePublisher<joint_qualification_controllers::RobotData>(n, "/robot_checkout", 1, true));
00128 
00129   return true;
00130 }
00131 
00132 void CheckoutController::starting()
00133 {
00134   initial_time_ = robot_->getTime();
00135 
00136   data_sent_ = false;
00137 }
00138 
00139 void CheckoutController::update()
00140 {
00141   ros::Time time = robot_->getTime();
00142   bool cal = false;
00143   //bool enabled = false;
00144 
00145   // Timeout check.
00146   if ((time - initial_time_).toSec() > timeout_ && state_ != ANALYZING && state_ != DONE) 
00147   {
00148     analysis((time - initial_time_).toSec(), true);
00149     state_ = DONE;
00150   }
00151 
00152   switch (state_)
00153   {
00154   case STARTING:
00155     initial_time_ = robot_->getTime();
00156     state_ = LISTENING;
00157     break;
00158   case LISTENING:
00159     {
00160     cal = true;
00161     for (int i = 0; i < joint_count_; ++i)
00162     {
00163       // Check for caster wheel joints and fingers
00164       // Wheel joints and fingers don't calibrate, so don't wait for them
00165       if (robot_->joint_states_[i].joint_->name.find("wheel_joint") != string::npos)
00166         continue;
00167 
00168       if (robot_->joint_states_[i].joint_->type == urdf::Joint::FIXED)
00169         continue;
00170 
00171       if (robot_->joint_states_[i].joint_->name.find("finger") != string::npos)
00172         continue;
00173       if (robot_->joint_states_[i].joint_->name.find("gripper_float_joint") != string::npos)
00174         continue;
00175       if (robot_->joint_states_[i].joint_->name.find("accelerometer_joint") != string::npos)
00176         continue;
00177       if (robot_->joint_states_[i].joint_->name.find("gripper_palm_joint") != string::npos)
00178         continue;
00179       if (robot_->joint_states_[i].joint_->name.find("gripper_tool_joint") != string::npos)
00180         continue;
00181 
00182 
00183       // Base joint is a dummy joint used to set up visualization
00184       if (robot_->joint_states_[i].joint_->name == "base_joint")
00185         continue;
00186 
00187       if (!robot_->joint_states_[i].calibrated_)
00188       {
00189         cal = false;
00190         break;
00191       }
00192     }
00193 
00194     //    enabled = true;
00195 
00196     /*
00197     pr2_hardware_interface::ActuatorMap::const_iterator it;
00198     for (it = robot_->model_->hw_->actuators_.begin();
00199          it != robot_->model_->hw_->actuators_.end();
00200          ++it)
00201     {
00202       if (it->second->name_ == "led_projector")
00203         continue;
00204 
00205       if (!it->second->state_.is_enabled_)
00206       {
00207         enabled = false;
00208         break;
00209       }
00210     }
00211     */
00212     
00213     if (cal)
00214       state_ = ANALYZING;
00215 
00216     break;
00217     }
00218   case ANALYZING:
00219     analysis((time - initial_time_).toSec());
00220     state_ = DONE;
00221     break;
00222   case DONE:
00223     if (!data_sent_)
00224       data_sent_ = sendData();
00225 
00226 
00227     break;
00228   }
00229 
00230 }
00231 
00232 bool CheckoutController::sendData()
00233 {
00234   if (robot_data_pub_->trylock())
00235   {
00236     joint_qualification_controllers::RobotData *out = &robot_data_pub_->msg_;
00237     out->test_time     = robot_data_.test_time;
00238     out->num_joints    = robot_data_.num_joints;
00239     out->num_actuators = robot_data_.num_actuators;
00240 
00241     out->joint_data.resize(robot_data_.num_joints);
00242     out->actuator_data.resize(robot_data_.num_actuators);
00243 
00244     for (int i = 0; i < joint_count_; i++)
00245     {
00246       out->joint_data[i].index      = robot_data_.joint_data[i].index;
00247       out->joint_data[i].name       = robot_data_.joint_data[i].name;
00248       out->joint_data[i].is_cal     = robot_data_.joint_data[i].is_cal;
00249       out->joint_data[i].has_safety = robot_data_.joint_data[i].has_safety;
00250       out->joint_data[i].type       = robot_data_.joint_data[i].type;
00251     }
00252 
00253     for (int i = 0; i < actuator_count_; i++)
00254     {
00255       out->actuator_data[i].index   = robot_data_.actuator_data[i].index;
00256       out->actuator_data[i].name    = robot_data_.actuator_data[i].name;
00257       out->actuator_data[i].id      = robot_data_.actuator_data[i].id;
00258     }
00259 
00260     robot_data_pub_->unlockAndPublish();
00261 
00262     return true;
00263   }
00264   return false;
00265 }
00266 
00267 void CheckoutController::analysis(double time, bool timeout)
00268 {
00269   robot_data_.test_time = time;
00270   robot_data_.timeout = timeout;
00271 
00272   for (int i = 0; i < joint_count_; i++)
00273   {
00274     robot_data_.joint_data[i].is_cal = robot_->joint_states_[i].calibrated_;
00275 
00276     if (robot_->joint_states_[i].joint_->type == urdf::Joint::FIXED)
00277       robot_data_.joint_data[i].is_cal = true;
00278   }
00279 
00280   /*
00281   pr2_hardware_interface::ActuatorMap::const_iterator it;
00282   int i = 0;
00283   for (it = robot_->model_->hw_->actuators_.begin();
00284        it != robot_->model_->hw_->actuators_.end();
00285        ++it, ++i)
00286   {
00287    
00288     if (it->second->name_ == "led_projector")
00289       robot_data_.actuator_data[i].enabled = true;
00290     else 
00291       robot_data_.actuator_data[i].enabled = it->second->state_.is_enabled_;
00292 
00293   }
00294   */
00295 }
checkout_controller.cpp
