Go to the documentation of this file.00001
00014
00015 #include "graspdb/GraspModel.h"
00016
00017
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
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
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
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
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
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
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
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
00148 if (grasps_.size() > 0)
00149 {
00150 size_t index = 0;
00151 double best = 0;
00152
00153
00154 for (size_t i = 0; i < grasps_.size(); i++)
00155 {
00156
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
00174 if (grasps_.size() > 0)
00175 {
00176
00177 double best = 0;
00178 for (size_t i = 0; i < grasps_.size(); i++)
00179 {
00180
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
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
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
00220 if (grasps_.size() > 0)
00221 {
00222 size_t index = 0;
00223 double worst = 1.0;
00224
00225
00226 for (size_t i = 0; i < grasps_.size(); i++)
00227 {
00228
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
00246 if (grasps_.size() > 0)
00247 {
00248
00249 double worst = 1.0;
00250 for (size_t i = 0; i < grasps_.size(); i++)
00251 {
00252
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
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 }