GraspModel.cpp
Go to the documentation of this file.
00001 
00014 // graspdb
00015 #include "graspdb/GraspModel.h"
00016 
00017 // ROS
00018 #include <ros/ros.h>
00019 
00020 using namespace std;
00021 using namespace rail::pick_and_place::graspdb;
00022 
00023 GraspModel::GraspModel(const uint32_t id, const string &object_name, const vector<Grasp> &grasps,
00024     const sensor_msgs::PointCloud2 &point_cloud, const time_t created)
00025     : Entity(id, created),
00026       object_name_(object_name), grasps_(grasps), point_cloud_(point_cloud)
00027 {
00028 }
00029 
00030 GraspModel::GraspModel(const string &object_name, const vector<Grasp> &grasps,
00031     const sensor_msgs::PointCloud2 &point_cloud)
00032     : object_name_(object_name), grasps_(grasps), point_cloud_(point_cloud)
00033 {
00034 }
00035 
00036 GraspModel::GraspModel(const rail_pick_and_place_msgs::GraspModel &gm)
00037     : Entity(gm.id, gm.created.sec),
00038       object_name_(gm.object_name), point_cloud_(gm.point_cloud)
00039 {
00040   // copy over the grasp values
00041   for (size_t i = 0; i < gm.grasps.size(); i++)
00042   {
00043     Grasp grasp(gm.grasps[i], gm.id);
00044     grasps_.push_back(grasp);
00045   }
00046 }
00047 
00048 const string &GraspModel::getObjectName() const
00049 {
00050   return object_name_;
00051 }
00052 
00053 void GraspModel::setObjectName(const string &object_name)
00054 {
00055   object_name_ = object_name;
00056 }
00057 
00058 const vector<Grasp> &GraspModel::getGrasps() const
00059 {
00060   return grasps_;
00061 }
00062 
00063 vector<Grasp> &GraspModel::getGrasps()
00064 {
00065   return grasps_;
00066 }
00067 
00068 size_t GraspModel::getNumGrasps() const
00069 {
00070   return grasps_.size();
00071 }
00072 
00073 const Grasp &GraspModel::getGrasp(const size_t index) const
00074 {
00075   // check the index value first
00076   if (index < grasps_.size())
00077   {
00078     return grasps_[index];
00079   } else
00080   {
00081     throw std::out_of_range("GraspModel::getGrasp : Grasp index does not exist.");
00082   }
00083 }
00084 
00085 Grasp &GraspModel::getGrasp(const size_t index)
00086 {
00087   // check the index value first
00088   if (index < grasps_.size())
00089   {
00090     return grasps_[index];
00091   } else
00092   {
00093     throw std::out_of_range("GraspModel::getGrasp : Grasp index does not exist.");
00094   }
00095 }
00096 
00097 void GraspModel::addGrasp(const Grasp &grasp)
00098 {
00099   // verify the ID
00100   if (grasp.getGraspModelID() == this->getID())
00101   {
00102     grasps_.push_back(grasp);
00103   } else
00104   {
00105     ROS_WARN("GraspModel::addGrasp : Grasp Model ID mismatch. Grasp not added to model.");
00106   }
00107 }
00108 
00109 void GraspModel::removeGrasp(const size_t index)
00110 {
00111   // check the index value first
00112   if (index < grasps_.size())
00113   {
00114     grasps_.erase(grasps_.begin() + index);
00115   } else
00116   {
00117     throw std::out_of_range("GraspModel::removeGraspPose : Grasp index does not exist.");
00118   }
00119 }
00120 
00121 const Grasp &GraspModel::getBestGrasp() const
00122 {
00123   // check the index value first
00124   if (grasps_.size() > 0)
00125   {
00126     return grasps_[this->getBestGraspIndex()];
00127   } else
00128   {
00129     throw std::out_of_range("GraspModel::getBestGrasp : Grasp list is empty.");
00130   }
00131 }
00132 
00133 Grasp &GraspModel::getBestGrasp()
00134 {
00135   // check the index value first
00136   if (grasps_.size() > 0)
00137   {
00138     return grasps_[this->getBestGraspIndex()];
00139   } else
00140   {
00141     throw std::out_of_range("GraspModel::getBestGrasp : Grasp list is empty.");
00142   }
00143 }
00144 
00145 size_t GraspModel::getBestGraspIndex() const
00146 {
00147   // check the index value first
00148   if (grasps_.size() > 0)
00149   {
00150     size_t index = 0;
00151     double best = 0;
00152 
00153     // check each success rate
00154     for (size_t i = 0; i < grasps_.size(); i++)
00155     {
00156       // check for a better value
00157       if (grasps_[i].getSuccessRate() > best)
00158       {
00159         best = grasps_[i].getSuccessRate();
00160         index = i;
00161       }
00162     }
00163 
00164     return index;
00165   } else
00166   {
00167     throw std::out_of_range("GraspModel::getBestGraspIndex : Grasp list is empty.");
00168   }
00169 }
00170 
00171 double GraspModel::getBestSuccessRate() const
00172 {
00173   // check the index value first
00174   if (grasps_.size() > 0)
00175   {
00176     // check each success rate
00177     double best = 0;
00178     for (size_t i = 0; i < grasps_.size(); i++)
00179     {
00180       // check for a better value
00181       if (grasps_[i].getSuccessRate() > best)
00182       {
00183         best = grasps_[i].getSuccessRate();
00184       }
00185     }
00186     return best;
00187   } else
00188   {
00189     throw std::out_of_range("GraspModel::getBestSuccessRate : Grasp list is empty.");
00190   }
00191 }
00192 
00193 const Grasp &GraspModel::getWorstGrasp() const
00194 {
00195   // check the index value first
00196   if (grasps_.size() > 0)
00197   {
00198     return grasps_[this->getWorstGraspIndex()];
00199   } else
00200   {
00201     throw std::out_of_range("GraspModel::getWorstGrasp : Grasp list is empty.");
00202   }
00203 }
00204 
00205 Grasp &GraspModel::getWorstGrasp()
00206 {
00207   // check the index value first
00208   if (grasps_.size() > 0)
00209   {
00210     return grasps_[this->getWorstGraspIndex()];
00211   } else
00212   {
00213     throw std::out_of_range("GraspModel::getWorstGrasp : Grasp list is empty.");
00214   }
00215 }
00216 
00217 size_t GraspModel::getWorstGraspIndex() const
00218 {
00219   // check the index value first
00220   if (grasps_.size() > 0)
00221   {
00222     size_t index = 0;
00223     double worst = 1.0;
00224 
00225     // check each success rate
00226     for (size_t i = 0; i < grasps_.size(); i++)
00227     {
00228       // check for a better value
00229       if (grasps_[i].getSuccessRate() < worst)
00230       {
00231         worst = grasps_[i].getSuccessRate();
00232         index = i;
00233       }
00234     }
00235 
00236     return index;
00237   } else
00238   {
00239     throw std::out_of_range("GraspModel::getWorstGraspIndex : Grasp list is empty.");
00240   }
00241 }
00242 
00243 double GraspModel::getWorstSuccessRate() const
00244 {
00245   // check the index value first
00246   if (grasps_.size() > 0)
00247   {
00248     // check each success rate
00249     double worst = 1.0;
00250     for (size_t i = 0; i < grasps_.size(); i++)
00251     {
00252       // check for a better value
00253       if (grasps_[i].getSuccessRate() < worst)
00254       {
00255         worst = grasps_[i].getSuccessRate();
00256       }
00257     }
00258     return worst;
00259   } else
00260   {
00261     throw std::out_of_range("GraspModel::getWorstSuccessRate : Grasp list is empty.");
00262   }
00263 }
00264 
00265 const sensor_msgs::PointCloud2 &GraspModel::getPointCloud() const
00266 {
00267   return point_cloud_;
00268 }
00269 
00270 sensor_msgs::PointCloud2 &GraspModel::getPointCloud()
00271 {
00272   return point_cloud_;
00273 }
00274 
00275 void GraspModel::setPointCloud(const sensor_msgs::PointCloud2 &point_cloud)
00276 {
00277   point_cloud_ = point_cloud;
00278 }
00279 
00280 rail_pick_and_place_msgs::GraspModel GraspModel::toROSGraspModelMessage() const
00281 {
00282   rail_pick_and_place_msgs::GraspModel gm;
00283   gm.id = this->getID();
00284   gm.object_name = object_name_;
00285   // copy over vector data
00286   for (size_t i = 0; i < grasps_.size(); i++)
00287   {
00288     gm.grasps.push_back(grasps_[i].toROSGraspWithSuccessRateMessage());
00289   }
00290   gm.point_cloud = point_cloud_;
00291   gm.created.nsec = 0;
00292   gm.created.sec = this->getCreated();
00293   return gm;
00294 }


graspdb
Author(s): Russell Toris
autogenerated on Sun Mar 6 2016 11:38:59