base_local_planner: base_local_planner: point

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 <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          * (0, 0) ---------------------- (width, 0)
00192          *  |                               |
00193          *  |                               |
00194          *  |                               |
00195          *  |                               |
00196          *  |                               |
00197          * (0, height) ----------------- (width, height)
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
point_grid.h
