point_grid.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2008, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of the Willow Garage nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Eitan Marder-Eppstein
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          * (0, 0) ---------------------- (width, 0)
00193          *  |                               |
00194          *  |                               |
00195          *  |                               |
00196          *  |                               |
00197          *  |                               |
00198          * (0, height) ----------------- (width, height)
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


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:30