point_grid.h
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2008, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the Willow Garage nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author: Eitan Marder-Eppstein
36 *********************************************************************/
37 #ifndef POINT_GRID_H_
38 #define POINT_GRID_H_
39 #include <vector>
40 #include <list>
41 #include <cfloat>
42 #include <geometry_msgs/Point.h>
43 #include <costmap_2d/observation.h>
45 
46 #include <pcl/point_types.h>
47 #include <pcl/point_cloud.h>
48 
49 namespace base_local_planner {
58  class PointGrid : public WorldModel {
59  public:
70  PointGrid(double width, double height, double resolution, geometry_msgs::Point origin,
71  double max_z, double obstacle_range, double min_separation);
72 
76  virtual ~PointGrid(){}
77 
84  void getPointsInRange(const geometry_msgs::Point& lower_left, const geometry_msgs::Point& upper_right, std::vector< std::list<pcl::PointXYZ>* >& points);
85 
94  virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
95  double inscribed_radius, double circumscribed_radius);
96 
98 
105  void updateWorld(const std::vector<geometry_msgs::Point>& footprint,
106  const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans);
107 
115  inline bool gridCoords(geometry_msgs::Point pt, unsigned int& gx, unsigned int& gy) const {
116  if(pt.x < origin_.x || pt.y < origin_.y){
117  gx = 0;
118  gy = 0;
119  return false;
120  }
121  gx = (int) ((pt.x - origin_.x)/resolution_);
122  gy = (int) ((pt.y - origin_.y)/resolution_);
123 
124  if(gx >= width_ || gy >= height_){
125  gx = 0;
126  gy = 0;
127  return false;
128  }
129 
130  return true;
131  }
132 
140  inline void getCellBounds(unsigned int gx, unsigned int gy, geometry_msgs::Point& lower_left, geometry_msgs::Point& upper_right) const {
141  lower_left.x = gx * resolution_ + origin_.x;
142  lower_left.y = gy * resolution_ + origin_.y;
143 
144  upper_right.x = lower_left.x + resolution_;
145  upper_right.y = lower_left.y + resolution_;
146  }
147 
148 
155  inline double sq_distance(pcl::PointXYZ& pt1, pcl::PointXYZ& pt2){
156  return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y);
157  }
158 
166  inline bool gridCoords(pcl::PointXYZ pt, unsigned int& gx, unsigned int& gy) const {
167  if(pt.x < origin_.x || pt.y < origin_.y){
168  gx = 0;
169  gy = 0;
170  return false;
171  }
172  gx = (int) ((pt.x - origin_.x)/resolution_);
173  gy = (int) ((pt.y - origin_.y)/resolution_);
174 
175  if(gx >= width_ || gy >= height_){
176  gx = 0;
177  gy = 0;
178  return false;
179  }
180 
181  return true;
182  }
183 
190  inline unsigned int gridIndex(unsigned int gx, unsigned int gy) const {
191  /*
192  * (0, 0) ---------------------- (width, 0)
193  * | |
194  * | |
195  * | |
196  * | |
197  * | |
198  * (0, height) ----------------- (width, height)
199  */
200  return(gx + gy * width_);
201  }
202 
210  inline double orient(const geometry_msgs::Point& a, const geometry_msgs::Point& b, const pcl::PointXYZ& c){
211  double acx = a.x - c.x;
212  double bcx = b.x - c.x;
213  double acy = a.y - c.y;
214  double bcy = b.y - c.y;
215  return acx * bcy - acy * bcx;
216  }
217 
225  inline double orient(const geometry_msgs::Point32& a, const geometry_msgs::Point32& b, const pcl::PointXYZ& c){
226  double acx = a.x - c.x;
227  double bcx = b.x - c.x;
228  double acy = a.y - c.y;
229  double bcy = b.y - c.y;
230  return acx * bcy - acy * bcx;
231  }
232 
240  inline double orient(const geometry_msgs::Point& a, const geometry_msgs::Point& b,
241  const geometry_msgs::Point& c){
242  double acx = a.x - c.x;
243  double bcx = b.x - c.x;
244  double acy = a.y - c.y;
245  double bcy = b.y - c.y;
246  return acx * bcy - acy * bcx;
247  }
248 
256  inline double orient(const pcl::PointXYZ& a, const pcl::PointXYZ& b, const pcl::PointXYZ& c){
257  double acx = a.x - c.x;
258  double bcx = b.x - c.x;
259  double acy = a.y - c.y;
260  double bcy = b.y - c.y;
261  return acx * bcy - acy * bcx;
262  }
263 
272  inline bool segIntersect(const pcl::PointXYZ& v1, const pcl::PointXYZ& v2,
273  const pcl::PointXYZ& u1, const pcl::PointXYZ& u2){
274  return (orient(v1, v2, u1) * orient(v1, v2, u2) < 0) && (orient(u1, u2, v1) * orient(u1, u2, v2) < 0);
275  }
276 
285  void intersectionPoint(const geometry_msgs::Point& v1, const geometry_msgs::Point& v2,
286  const geometry_msgs::Point& u1, const geometry_msgs::Point& u2,
287  geometry_msgs::Point& result);
288 
295  bool ptInPolygon(const pcl::PointXYZ& pt, const std::vector<geometry_msgs::Point>& poly);
296 
301  void insert(pcl::PointXYZ pt);
302 
308  double nearestNeighborDistance(pcl::PointXYZ& pt);
309 
317  double getNearestInCell(pcl::PointXYZ& pt, unsigned int gx, unsigned int gy);
318 
323  void removePointsInPolygon(const std::vector<geometry_msgs::Point> poly);
324 
329  void removePointsInScanBoundry(const PlanarLaserScan& laser_scan);
330 
337  bool ptInScan(const pcl::PointXYZ& pt, const PlanarLaserScan& laser_scan);
338 
343  void getPoints(pcl::PointCloud<pcl::PointXYZ>& cloud);
344 
345  private:
346  double resolution_;
347  geometry_msgs::Point origin_;
348  unsigned int width_;
349  unsigned int height_;
350  std::vector< std::list<pcl::PointXYZ> > cells_;
351  double max_z_;
354  std::vector< std::list<pcl::PointXYZ>* > points_;
355  };
356 };
357 #endif
virtual double footprintCost(const geometry_msgs::Point &position, const std::vector< geometry_msgs::Point > &footprint, double inscribed_radius, double circumscribed_radius)=0
Subclass will implement this method to check a footprint at a given position and orientation for lega...
void updateWorld(const std::vector< geometry_msgs::Point > &footprint, const std::vector< costmap_2d::Observation > &observations, const std::vector< PlanarLaserScan > &laser_scans)
Inserts observations from sensors into the point grid.
Definition: point_grid.cpp:358
virtual ~PointGrid()
Destructor for a point grid.
Definition: point_grid.h:76
double resolution_
The resolution of the grid in meters/cell.
Definition: point_grid.h:346
unsigned int width_
The width of the grid in cells.
Definition: point_grid.h:348
double sq_min_separation_
The minimum square distance required between points in the grid.
Definition: point_grid.h:353
A class that implements the WorldModel interface to provide free-space collision checks for the traje...
Definition: point_grid.h:58
bool gridCoords(geometry_msgs::Point pt, unsigned int &gx, unsigned int &gy) const
Convert from world coordinates to grid coordinates.
Definition: point_grid.h:115
double sq_obstacle_range_
The square distance at which we no longer add obstacles to the grid.
Definition: point_grid.h:352
void insert(pcl::PointXYZ pt)
Insert a point into the point grid.
Definition: point_grid.cpp:234
bool segIntersect(const pcl::PointXYZ &v1, const pcl::PointXYZ &v2, const pcl::PointXYZ &u1, const pcl::PointXYZ &u2)
Check if two line segmenst intersect.
Definition: point_grid.h:272
double orient(const pcl::PointXYZ &a, const pcl::PointXYZ &b, const pcl::PointXYZ &c)
Check the orientation of a pt c with respect to the vector a->b.
Definition: point_grid.h:256
void removePointsInScanBoundry(const PlanarLaserScan &laser_scan)
Removes points from the grid that lie within a laser scan.
Definition: point_grid.cpp:392
bool ptInPolygon(const pcl::PointXYZ &pt, const std::vector< geometry_msgs::Point > &poly)
Check if a point is in a polygon.
Definition: point_grid.cpp:142
bool ptInScan(const pcl::PointXYZ &pt, const PlanarLaserScan &laser_scan)
Checks to see if a point is within a laser scan specification.
Definition: point_grid.cpp:435
geometry_msgs::Point origin_
The origin point of the grid.
Definition: point_grid.h:347
double nearestNeighborDistance(pcl::PointXYZ &pt)
Find the distance between a point and its nearest neighbor in the grid.
Definition: point_grid.cpp:265
std::vector< std::list< pcl::PointXYZ > * > points_
The lists of points returned by a range search, made a member to save on memory allocation.
Definition: point_grid.h:354
void getPoints(pcl::PointCloud< pcl::PointXYZ > &cloud)
Get the points in the point grid.
Definition: point_grid.cpp:479
bool gridCoords(pcl::PointXYZ pt, unsigned int &gx, unsigned int &gy) const
Convert from world coordinates to grid coordinates.
Definition: point_grid.h:166
void getPointsInRange(const geometry_msgs::Point &lower_left, const geometry_msgs::Point &upper_right, std::vector< std::list< pcl::PointXYZ > * > &points)
Returns the points that lie within the cells contained in the specified range. Some of these points m...
Definition: point_grid.cpp:178
unsigned int height_
The height of the grid in cells.
Definition: point_grid.h:349
double orient(const geometry_msgs::Point &a, const geometry_msgs::Point &b, const geometry_msgs::Point &c)
Check the orientation of a pt c with respect to the vector a->b.
Definition: point_grid.h:240
std::vector< std::list< pcl::PointXYZ > > cells_
Storage for the cells in the grid.
Definition: point_grid.h:350
void removePointsInPolygon(const std::vector< geometry_msgs::Point > poly)
Removes points from the grid that lie within the polygon.
Definition: point_grid.cpp:487
double sq_distance(pcl::PointXYZ &pt1, pcl::PointXYZ &pt2)
Compute the squared distance between two points.
Definition: point_grid.h:155
An interface the trajectory controller uses to interact with the world regardless of the underlying w...
Definition: world_model.h:52
unsigned int gridIndex(unsigned int gx, unsigned int gy) const
Converts cell coordinates to an index value that can be used to look up the correct grid cell...
Definition: point_grid.h:190
double orient(const geometry_msgs::Point32 &a, const geometry_msgs::Point32 &b, const pcl::PointXYZ &c)
Check the orientation of a pt c with respect to the vector a->b.
Definition: point_grid.h:225
PointGrid(double width, double height, double resolution, geometry_msgs::Point origin, double max_z, double obstacle_range, double min_separation)
Constuctor for a grid that stores points in the plane.
Definition: point_grid.cpp:77
virtual double footprintCost(const geometry_msgs::Point &position, const std::vector< geometry_msgs::Point > &footprint, double inscribed_radius, double circumscribed_radius)
Checks if any points in the grid lie inside a convex footprint.
Definition: point_grid.cpp:85
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)
Find the intersection point of two lines.
Definition: point_grid.cpp:531
double orient(const geometry_msgs::Point &a, const geometry_msgs::Point &b, const pcl::PointXYZ &c)
Check the orientation of a pt c with respect to the vector a->b.
Definition: point_grid.h:210
Stores a scan from a planar laser that can be used to clear freespace.
void getCellBounds(unsigned int gx, unsigned int gy, geometry_msgs::Point &lower_left, geometry_msgs::Point &upper_right) const
Get the bounds in world coordinates of a cell in the point grid, assumes a legal cell when called...
Definition: point_grid.h:140
double max_z_
The height cutoff for adding points as obstacles.
Definition: point_grid.h:351
double getNearestInCell(pcl::PointXYZ &pt, unsigned int gx, unsigned int gy)
Find the distance between a point and its nearest neighbor in a cell.
Definition: point_grid.cpp:254


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