Go to the documentation of this file.00001
00011
00012 #include "worldlib/world/Surface.h"
00013
00014 using namespace std;
00015 using namespace rail::spatial_temporal_learning::worldlib::geometry;
00016 using namespace rail::spatial_temporal_learning::worldlib::world;
00017
00018 Surface::Surface(const string &name, const string &frame_id, const geometry::Pose &pose, const double width,
00019 const double depth, const double height) : Object(name, frame_id, pose, width, depth, height)
00020 {
00021 }
00022
00023 const vector<PlacementSurface> &Surface::getPlacementSurfaces() const
00024 {
00025 return placement_surfaces_;
00026 }
00027
00028 vector<PlacementSurface> &Surface::getPlacementSurfaces()
00029 {
00030 return placement_surfaces_;
00031 }
00032
00033 size_t Surface::getNumPlacementSurfaces() const
00034 {
00035 return placement_surfaces_.size();
00036 }
00037
00038 const PlacementSurface &Surface::getPlacementSurface(const size_t index) const
00039 {
00040
00041 if (index < placement_surfaces_.size())
00042 {
00043 return placement_surfaces_[index];
00044 } else
00045 {
00046 throw out_of_range("Surface::getPlacementSurface : Placement surface index does not exist.");
00047 }
00048 }
00049
00050 PlacementSurface &Surface::getPlacementSurface(const size_t index)
00051 {
00052
00053 if (index < placement_surfaces_.size())
00054 {
00055 return placement_surfaces_[index];
00056 } else
00057 {
00058 throw out_of_range("Surface::getPlacementSurface : Placement surface index does not exist.");
00059 }
00060 }
00061
00062 void Surface::addPlacementSurface(const PlacementSurface &placement_surface)
00063 {
00064 placement_surfaces_.push_back(placement_surface);
00065 }
00066
00067 void Surface::removePlacementSurface(const size_t index)
00068 {
00069
00070 if (index < placement_surfaces_.size())
00071 {
00072 placement_surfaces_.erase(placement_surfaces_.begin() + index);
00073 } else
00074 {
00075 throw out_of_range("Surface::removePlacementSurface : Placement surface index does not exist.");
00076 }
00077 }
00078
00079 bool Surface::placementSurfaceExists(const string &name) const
00080 {
00081
00082 try
00083 {
00084 this->findPlacementSurface(name);
00085 return true;
00086 } catch (out_of_range &e)
00087 {
00088 return false;
00089 }
00090 }
00091
00092 const PlacementSurface &Surface::findPlacementSurface(const string &name) const
00093 {
00094
00095 for (size_t i = 0; i < placement_surfaces_.size(); i++)
00096 {
00097
00098 if (placement_surfaces_[i].checkName(name))
00099 {
00100 return placement_surfaces_[i];
00101 }
00102 }
00103
00104 throw out_of_range("Surface::findPlacementSurface : Placement surface name does not exist.");
00105 }
00106
00107 PlacementSurface &Surface::findPlacementSurface(const string &name)
00108 {
00109
00110 for (size_t i = 0; i < placement_surfaces_.size(); i++)
00111 {
00112
00113 if (placement_surfaces_[i].checkName(name))
00114 {
00115 return placement_surfaces_[i];
00116 }
00117 }
00118
00119 throw out_of_range("Surface::findPlacementSurface : Placement surface name does not exist.");
00120 }
00121
00122 const PlacementSurface &Surface::findClosestPlacementSurface(const Position &position) const
00123 {
00124 return placement_surfaces_[this->findClosestPlacementSurfaceIndex(position)];
00125 }
00126
00127 PlacementSurface &Surface::findClosestPlacementSurface(const Position &position)
00128 {
00129 return placement_surfaces_[this->findClosestPlacementSurfaceIndex(position)];
00130 }
00131
00132 size_t Surface::findClosestPlacementSurfaceIndex(const Position &position) const
00133 {
00134 if (placement_surfaces_.empty())
00135 {
00136 throw out_of_range("Surface::findClosestPlacementSurface : No placement surfaces exist.");
00137 } else
00138 {
00139
00140 double best = numeric_limits<double>::infinity();
00141 size_t best_index = 0;
00142 for (size_t i = 0; i < placement_surfaces_.size(); i++)
00143 {
00144
00145 double distance = position.distance(placement_surfaces_[i].getPose().getPosition());
00146 if (distance < best)
00147 {
00148 best = distance;
00149 best_index = i;
00150 }
00151 }
00152
00153 return best_index;
00154 }
00155 }
00156
00157 const vector<PointOfInterest> &Surface::getPointsOfInterest() const
00158 {
00159 return pois_;
00160 }
00161
00162 vector<PointOfInterest> &Surface::getPointsOfInterest()
00163 {
00164 return pois_;
00165 }
00166
00167 size_t Surface::getNumPointsOfInterest() const
00168 {
00169 return pois_.size();
00170 }
00171
00172 const PointOfInterest &Surface::getPointOfInterest(const size_t index) const
00173 {
00174
00175 if (index < pois_.size())
00176 {
00177 return pois_[index];
00178 } else
00179 {
00180 throw out_of_range("Surface::getPointOfInterest : Point of interest index does not exist.");
00181 }
00182 }
00183
00184 PointOfInterest &Surface::getPointOfInterest(const size_t index)
00185 {
00186
00187 if (index < pois_.size())
00188 {
00189 return pois_[index];
00190 } else
00191 {
00192 throw out_of_range("Surface::getPointOfInterest : Point of interest index does not exist.");
00193 }
00194 }
00195
00196 void Surface::addPointOfInterest(const PointOfInterest &poi)
00197 {
00198 pois_.push_back(poi);
00199 }
00200
00201 void Surface::removePointOfInterest(const size_t index)
00202 {
00203
00204 if (index < pois_.size())
00205 {
00206 pois_.erase(pois_.begin() + index);
00207 } else
00208 {
00209 throw out_of_range("Surface::removePointOfInterest : Point of interest index does not exist.");
00210 }
00211 }
00212
00213 bool Surface::pointOfInterestExists(const string &name) const
00214 {
00215
00216 try
00217 {
00218 this->findPointOfInterest(name);
00219 return true;
00220 } catch (out_of_range &e)
00221 {
00222 return false;
00223 }
00224 }
00225
00226 const PointOfInterest &Surface::findPointOfInterest(const string &name) const
00227 {
00228
00229 for (size_t i = 0; i < pois_.size(); i++)
00230 {
00231
00232 if (pois_[i].checkName(name))
00233 {
00234 return pois_[i];
00235 }
00236 }
00237
00238 throw out_of_range("Surface::findPointOfInterest : Point of interest name does not exist.");
00239 }
00240
00241
00242 PointOfInterest &Surface::findPointOfInterest(const string &name)
00243 {
00244
00245 for (size_t i = 0; i < pois_.size(); i++)
00246 {
00247
00248 if (pois_[i].checkName(name))
00249 {
00250 return pois_[i];
00251 }
00252 }
00253
00254 throw out_of_range("Surface::findPointOfInterest : Point of interest name does not exist.");
00255 }