00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef POINT_GRID_H_
00038 #define POINT_GRID_H_
00039 #include <vector>
00040 #include <list>
00041 #include <cfloat>
00042 #include <geometry_msgs/Point.h>
00043 #include <costmap_2d/observation.h>
00044 #include <base_local_planner/world_model.h>
00045
00046 #include <pcl/point_types.h>
00047 #include <pcl/point_cloud.h>
00048
00049 namespace base_local_planner {
00058 class PointGrid : public WorldModel {
00059 public:
00070 PointGrid(double width, double height, double resolution, geometry_msgs::Point origin,
00071 double max_z, double obstacle_range, double min_separation);
00072
00076 virtual ~PointGrid(){}
00077
00084 void getPointsInRange(const geometry_msgs::Point& lower_left, const geometry_msgs::Point& upper_right, std::vector< std::list<pcl::PointXYZ>* >& points);
00085
00094 virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
00095 double inscribed_radius, double circumscribed_radius);
00096
00097 using WorldModel::footprintCost;
00098
00105 void updateWorld(const std::vector<geometry_msgs::Point>& footprint,
00106 const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans);
00107
00115 inline bool gridCoords(geometry_msgs::Point pt, unsigned int& gx, unsigned int& gy) const {
00116 if(pt.x < origin_.x || pt.y < origin_.y){
00117 gx = 0;
00118 gy = 0;
00119 return false;
00120 }
00121 gx = (int) ((pt.x - origin_.x)/resolution_);
00122 gy = (int) ((pt.y - origin_.y)/resolution_);
00123
00124 if(gx >= width_ || gy >= height_){
00125 gx = 0;
00126 gy = 0;
00127 return false;
00128 }
00129
00130 return true;
00131 }
00132
00140 inline void getCellBounds(unsigned int gx, unsigned int gy, geometry_msgs::Point& lower_left, geometry_msgs::Point& upper_right) const {
00141 lower_left.x = gx * resolution_ + origin_.x;
00142 lower_left.y = gy * resolution_ + origin_.y;
00143
00144 upper_right.x = lower_left.x + resolution_;
00145 upper_right.y = lower_left.y + resolution_;
00146 }
00147
00148
00155 inline double sq_distance(pcl::PointXYZ& pt1, pcl::PointXYZ& pt2){
00156 return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y);
00157 }
00158
00166 inline bool gridCoords(pcl::PointXYZ pt, unsigned int& gx, unsigned int& gy) const {
00167 if(pt.x < origin_.x || pt.y < origin_.y){
00168 gx = 0;
00169 gy = 0;
00170 return false;
00171 }
00172 gx = (int) ((pt.x - origin_.x)/resolution_);
00173 gy = (int) ((pt.y - origin_.y)/resolution_);
00174
00175 if(gx >= width_ || gy >= height_){
00176 gx = 0;
00177 gy = 0;
00178 return false;
00179 }
00180
00181 return true;
00182 }
00183
00190 inline unsigned int gridIndex(unsigned int gx, unsigned int gy) const {
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200 return(gx + gy * width_);
00201 }
00202
00210 inline double orient(const geometry_msgs::Point& a, const geometry_msgs::Point& b, const pcl::PointXYZ& c){
00211 double acx = a.x - c.x;
00212 double bcx = b.x - c.x;
00213 double acy = a.y - c.y;
00214 double bcy = b.y - c.y;
00215 return acx * bcy - acy * bcx;
00216 }
00217
00225 inline double orient(const geometry_msgs::Point32& a, const geometry_msgs::Point32& b, const pcl::PointXYZ& c){
00226 double acx = a.x - c.x;
00227 double bcx = b.x - c.x;
00228 double acy = a.y - c.y;
00229 double bcy = b.y - c.y;
00230 return acx * bcy - acy * bcx;
00231 }
00232
00240 inline double orient(const geometry_msgs::Point& a, const geometry_msgs::Point& b,
00241 const geometry_msgs::Point& c){
00242 double acx = a.x - c.x;
00243 double bcx = b.x - c.x;
00244 double acy = a.y - c.y;
00245 double bcy = b.y - c.y;
00246 return acx * bcy - acy * bcx;
00247 }
00248
00256 inline double orient(const pcl::PointXYZ& a, const pcl::PointXYZ& b, const pcl::PointXYZ& c){
00257 double acx = a.x - c.x;
00258 double bcx = b.x - c.x;
00259 double acy = a.y - c.y;
00260 double bcy = b.y - c.y;
00261 return acx * bcy - acy * bcx;
00262 }
00263
00272 inline bool segIntersect(const pcl::PointXYZ& v1, const pcl::PointXYZ& v2,
00273 const pcl::PointXYZ& u1, const pcl::PointXYZ& u2){
00274 return (orient(v1, v2, u1) * orient(v1, v2, u2) < 0) && (orient(u1, u2, v1) * orient(u1, u2, v2) < 0);
00275 }
00276
00285 void intersectionPoint(const geometry_msgs::Point& v1, const geometry_msgs::Point& v2,
00286 const geometry_msgs::Point& u1, const geometry_msgs::Point& u2,
00287 geometry_msgs::Point& result);
00288
00295 bool ptInPolygon(const pcl::PointXYZ& pt, const std::vector<geometry_msgs::Point>& poly);
00296
00301 void insert(pcl::PointXYZ pt);
00302
00308 double nearestNeighborDistance(pcl::PointXYZ& pt);
00309
00317 double getNearestInCell(pcl::PointXYZ& pt, unsigned int gx, unsigned int gy);
00318
00323 void removePointsInPolygon(const std::vector<geometry_msgs::Point> poly);
00324
00329 void removePointsInScanBoundry(const PlanarLaserScan& laser_scan);
00330
00337 bool ptInScan(const pcl::PointXYZ& pt, const PlanarLaserScan& laser_scan);
00338
00343 void getPoints(pcl::PointCloud<pcl::PointXYZ>& cloud);
00344
00345 private:
00346 double resolution_;
00347 geometry_msgs::Point origin_;
00348 unsigned int width_;
00349 unsigned int height_;
00350 std::vector< std::list<pcl::PointXYZ> > cells_;
00351 double max_z_;
00352 double sq_obstacle_range_;
00353 double sq_min_separation_;
00354 std::vector< std::list<pcl::PointXYZ>* > points_;
00355 };
00356 };
00357 #endif