point_grid.h

Go to the documentation of this file.

/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef POINT_GRID_H_
#define POINT_GRID_H_
#include <vector>
#include <list>
#include <cfloat>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Point32.h>
#include <costmap_2d/observation.h>
#include <iri_diff_local_planner/world_model.h>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

namespace iri_diff_local_planner {
  class PointGrid : public WorldModel {
    public:
      PointGrid(double width, double height, double resolution, geometry_msgs::Point origin, 
          double max_z, double obstacle_range, double min_separation);

      virtual ~PointGrid(){}

      void getPointsInRange(const geometry_msgs::Point& lower_left, const geometry_msgs::Point& upper_right, std::vector< std::list<pcl::PointXYZ>* >& points);

      virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
          double inscribed_radius, double circumscribed_radius);

      void updateWorld(const std::vector<geometry_msgs::Point>& footprint, 
          const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans);

      inline bool gridCoords(geometry_msgs::Point pt, unsigned int& gx, unsigned int& gy) const {
        if(pt.x < origin_.x || pt.y < origin_.y){
          gx = 0;
          gy = 0;
          return false;
        }
        gx = (int) ((pt.x - origin_.x)/resolution_);
        gy = (int) ((pt.y - origin_.y)/resolution_);

        if(gx >= width_ || gy >= height_){
          gx = 0;
          gy = 0;
          return false;
        }

        return true;
      }

      inline void getCellBounds(unsigned int gx, unsigned int gy, geometry_msgs::Point& lower_left, geometry_msgs::Point& upper_right) const {
        lower_left.x = gx * resolution_ + origin_.x;
        lower_left.y = gy * resolution_ + origin_.y;

        upper_right.x = lower_left.x + resolution_;
        upper_right.y = lower_left.y + resolution_;
      }


      inline double sq_distance(pcl::PointXYZ& pt1, pcl::PointXYZ& pt2){
        return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y);
      }

      inline bool gridCoords(pcl::PointXYZ pt, unsigned int& gx, unsigned int& gy) const {
        if(pt.x < origin_.x || pt.y < origin_.y){
          gx = 0;
          gy = 0;
          return false;
        }
        gx = (int) ((pt.x - origin_.x)/resolution_);
        gy = (int) ((pt.y - origin_.y)/resolution_);

        if(gx >= width_ || gy >= height_){
          gx = 0;
          gy = 0;
          return false;
        }

        return true;
      }

      inline unsigned int gridIndex(unsigned int gx, unsigned int gy) const {
        /*
         * (0, 0) ---------------------- (width, 0)
         *  |                               |
         *  |                               |
         *  |                               |
         *  |                               |
         *  |                               |
         * (0, height) ----------------- (width, height)
         */
        return(gx + gy * width_);
      }

      inline double orient(const geometry_msgs::Point& a, const geometry_msgs::Point& b, const pcl::PointXYZ& c){
        double acx = a.x - c.x;
        double bcx = b.x - c.x;
        double acy = a.y - c.y;
        double bcy = b.y - c.y;
        return acx * bcy - acy * bcx;
      }

      inline double orient(const geometry_msgs::Point32& a, const geometry_msgs::Point32& b, const pcl::PointXYZ& c){
        double acx = a.x - c.x;
        double bcx = b.x - c.x;
        double acy = a.y - c.y;
        double bcy = b.y - c.y;
        return acx * bcy - acy * bcx;
      }

      inline double orient(const geometry_msgs::Point& a, const geometry_msgs::Point& b, 
          const geometry_msgs::Point& c){
        double acx = a.x - c.x;
        double bcx = b.x - c.x;
        double acy = a.y - c.y;
        double bcy = b.y - c.y;
        return acx * bcy - acy * bcx;
      }

      inline double orient(const pcl::PointXYZ& a, const pcl::PointXYZ& b, const pcl::PointXYZ& c){
        double acx = a.x - c.x;
        double bcx = b.x - c.x;
        double acy = a.y - c.y;
        double bcy = b.y - c.y;
        return acx * bcy - acy * bcx;
      }

      inline bool segIntersect(const pcl::PointXYZ& v1, const pcl::PointXYZ& v2, 
          const pcl::PointXYZ& u1, const pcl::PointXYZ& u2){
        return (orient(v1, v2, u1) * orient(v1, v2, u2) < 0) && (orient(u1, u2, v1) * orient(u1, u2, v2) < 0);
      }

      void intersectionPoint(const geometry_msgs::Point& v1, const geometry_msgs::Point& v2, 
          const geometry_msgs::Point& u1, const geometry_msgs::Point& u2, 
          geometry_msgs::Point& result);

      bool ptInPolygon(const pcl::PointXYZ& pt, const std::vector<geometry_msgs::Point>& poly);

      void insert(pcl::PointXYZ pt);

      double nearestNeighborDistance(pcl::PointXYZ& pt);

      double getNearestInCell(pcl::PointXYZ& pt, unsigned int gx, unsigned int gy); 

      void removePointsInPolygon(const std::vector<geometry_msgs::Point> poly);

      void removePointsInScanBoundry(const PlanarLaserScan& laser_scan);

      bool ptInScan(const pcl::PointXYZ& pt, const PlanarLaserScan& laser_scan);

      void getPoints(pcl::PointCloud<pcl::PointXYZ>& cloud);

    private:
      double resolution_; 
      geometry_msgs::Point origin_; 
      unsigned int width_; 
      unsigned int height_; 
      std::vector< std::list<pcl::PointXYZ> > cells_; 
      double max_z_;  
      double sq_obstacle_range_;  
      double sq_min_separation_;  
      std::vector< std::list<pcl::PointXYZ>* > points_;  
  };
};
#endif