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 <geometry_msgs/Point32.h>
00044 #include <costmap_2d/observation.h>
00045 #include <base_local_planner/world_model.h>
00046
00047 #include <pcl/point_types.h>
00048 #include <pcl/point_cloud.h>
00049
00050 namespace base_local_planner {
00059 class PointGrid : public WorldModel {
00060 public:
00071 PointGrid(double width, double height, double resolution, geometry_msgs::Point origin,
00072 double max_z, double obstacle_range, double min_separation);
00073
00077 virtual ~PointGrid(){}
00078
00085 void getPointsInRange(const geometry_msgs::Point& lower_left, const geometry_msgs::Point& upper_right, std::vector< std::list<pcl::PointXYZ>* >& points);
00086
00095 virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
00096 double inscribed_radius, double circumscribed_radius);
00097
00104 void updateWorld(const std::vector<geometry_msgs::Point>& footprint,
00105 const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans);
00106
00114 inline bool gridCoords(geometry_msgs::Point pt, unsigned int& gx, unsigned int& gy) const {
00115 if(pt.x < origin_.x || pt.y < origin_.y){
00116 gx = 0;
00117 gy = 0;
00118 return false;
00119 }
00120 gx = (int) ((pt.x - origin_.x)/resolution_);
00121 gy = (int) ((pt.y - origin_.y)/resolution_);
00122
00123 if(gx >= width_ || gy >= height_){
00124 gx = 0;
00125 gy = 0;
00126 return false;
00127 }
00128
00129 return true;
00130 }
00131
00139 inline void getCellBounds(unsigned int gx, unsigned int gy, geometry_msgs::Point& lower_left, geometry_msgs::Point& upper_right) const {
00140 lower_left.x = gx * resolution_ + origin_.x;
00141 lower_left.y = gy * resolution_ + origin_.y;
00142
00143 upper_right.x = lower_left.x + resolution_;
00144 upper_right.y = lower_left.y + resolution_;
00145 }
00146
00147
00154 inline double sq_distance(pcl::PointXYZ& pt1, pcl::PointXYZ& pt2){
00155 return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y);
00156 }
00157
00165 inline bool gridCoords(pcl::PointXYZ pt, unsigned int& gx, unsigned int& gy) const {
00166 if(pt.x < origin_.x || pt.y < origin_.y){
00167 gx = 0;
00168 gy = 0;
00169 return false;
00170 }
00171 gx = (int) ((pt.x - origin_.x)/resolution_);
00172 gy = (int) ((pt.y - origin_.y)/resolution_);
00173
00174 if(gx >= width_ || gy >= height_){
00175 gx = 0;
00176 gy = 0;
00177 return false;
00178 }
00179
00180 return true;
00181 }
00182
00189 inline unsigned int gridIndex(unsigned int gx, unsigned int gy) const {
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199 return(gx + gy * width_);
00200 }
00201
00209 inline double orient(const geometry_msgs::Point& a, const geometry_msgs::Point& b, const pcl::PointXYZ& c){
00210 double acx = a.x - c.x;
00211 double bcx = b.x - c.x;
00212 double acy = a.y - c.y;
00213 double bcy = b.y - c.y;
00214 return acx * bcy - acy * bcx;
00215 }
00216
00224 inline double orient(const geometry_msgs::Point32& a, const geometry_msgs::Point32& b, const pcl::PointXYZ& c){
00225 double acx = a.x - c.x;
00226 double bcx = b.x - c.x;
00227 double acy = a.y - c.y;
00228 double bcy = b.y - c.y;
00229 return acx * bcy - acy * bcx;
00230 }
00231
00239 inline double orient(const geometry_msgs::Point& a, const geometry_msgs::Point& b,
00240 const geometry_msgs::Point& c){
00241 double acx = a.x - c.x;
00242 double bcx = b.x - c.x;
00243 double acy = a.y - c.y;
00244 double bcy = b.y - c.y;
00245 return acx * bcy - acy * bcx;
00246 }
00247
00255 inline double orient(const pcl::PointXYZ& a, const pcl::PointXYZ& b, const pcl::PointXYZ& c){
00256 double acx = a.x - c.x;
00257 double bcx = b.x - c.x;
00258 double acy = a.y - c.y;
00259 double bcy = b.y - c.y;
00260 return acx * bcy - acy * bcx;
00261 }
00262
00271 inline bool segIntersect(const pcl::PointXYZ& v1, const pcl::PointXYZ& v2,
00272 const pcl::PointXYZ& u1, const pcl::PointXYZ& u2){
00273 return (orient(v1, v2, u1) * orient(v1, v2, u2) < 0) && (orient(u1, u2, v1) * orient(u1, u2, v2) < 0);
00274 }
00275
00284 void intersectionPoint(const geometry_msgs::Point& v1, const geometry_msgs::Point& v2,
00285 const geometry_msgs::Point& u1, const geometry_msgs::Point& u2,
00286 geometry_msgs::Point& result);
00287
00294 bool ptInPolygon(const pcl::PointXYZ& pt, const std::vector<geometry_msgs::Point>& poly);
00295
00300 void insert(pcl::PointXYZ pt);
00301
00307 double nearestNeighborDistance(pcl::PointXYZ& pt);
00308
00316 double getNearestInCell(pcl::PointXYZ& pt, unsigned int gx, unsigned int gy);
00317
00322 void removePointsInPolygon(const std::vector<geometry_msgs::Point> poly);
00323
00328 void removePointsInScanBoundry(const PlanarLaserScan& laser_scan);
00329
00336 bool ptInScan(const pcl::PointXYZ& pt, const PlanarLaserScan& laser_scan);
00337
00342 void getPoints(pcl::PointCloud<pcl::PointXYZ>& cloud);
00343
00344 private:
00345 double resolution_;
00346 geometry_msgs::Point origin_;
00347 unsigned int width_;
00348 unsigned int height_;
00349 std::vector< std::list<pcl::PointXYZ> > cells_;
00350 double max_z_;
00351 double sq_obstacle_range_;
00352 double sq_min_separation_;
00353 std::vector< std::list<pcl::PointXYZ>* > points_;
00354 };
00355 };
00356 #endif