Public Member Functions | Private Attributes | List of all members
base_local_planner::PointGrid Class Reference

A class that implements the WorldModel interface to provide free-space collision checks for the trajectory controller. This class stores points binned into a grid and performs point-in-polygon checks when necessary to determine the legality of a footprint at a given position/orientation. More...

#include <point_grid.h>

Inheritance diagram for base_local_planner::PointGrid:
Inheritance graph
[legend]

Public Member Functions

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. More...
 
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. More...
 
double getNearestInCell (pcl::PointXYZ &pt, unsigned int gx, unsigned int gy)
 Find the distance between a point and its nearest neighbor in a cell. More...
 
void getPoints (pcl::PointCloud< pcl::PointXYZ > &cloud)
 Get the points in the point grid. More...
 
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 may be outside the range itself. More...
 
bool gridCoords (geometry_msgs::Point pt, unsigned int &gx, unsigned int &gy) const
 Convert from world coordinates to grid coordinates. More...
 
bool gridCoords (pcl::PointXYZ pt, unsigned int &gx, unsigned int &gy) const
 Convert from world coordinates to grid coordinates. More...
 
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. More...
 
void insert (pcl::PointXYZ pt)
 Insert a point into the point grid. More...
 
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. More...
 
double nearestNeighborDistance (pcl::PointXYZ &pt)
 Find the distance between a point and its nearest neighbor in the grid. More...
 
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. More...
 
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. More...
 
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. More...
 
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. More...
 
 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. More...
 
bool ptInPolygon (const pcl::PointXYZ &pt, const std::vector< geometry_msgs::Point > &poly)
 Check if a point is in a polygon. More...
 
bool ptInScan (const pcl::PointXYZ &pt, const PlanarLaserScan &laser_scan)
 Checks to see if a point is within a laser scan specification. More...
 
void removePointsInPolygon (const std::vector< geometry_msgs::Point > poly)
 Removes points from the grid that lie within the polygon. More...
 
void removePointsInScanBoundry (const PlanarLaserScan &laser_scan)
 Removes points from the grid that lie within a laser scan. More...
 
bool segIntersect (const pcl::PointXYZ &v1, const pcl::PointXYZ &v2, const pcl::PointXYZ &u1, const pcl::PointXYZ &u2)
 Check if two line segmenst intersect. More...
 
double sq_distance (pcl::PointXYZ &pt1, pcl::PointXYZ &pt2)
 Compute the squared distance between two points. More...
 
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. More...
 
virtual ~PointGrid ()
 Destructor for a point grid. More...
 
- Public Member Functions inherited from base_local_planner::WorldModel
double footprintCost (double x, double y, double theta, const std::vector< geometry_msgs::Point > &footprint_spec, double inscribed_radius=0.0, double circumscribed_radius=0.0)
 
double footprintCost (const geometry_msgs::Point &position, const std::vector< geometry_msgs::Point > &footprint, double inscribed_radius, double circumscribed_radius, double extra)
 Checks if any obstacles in the costmap lie inside a convex footprint that is rasterized into the grid. More...
 
virtual ~WorldModel ()
 Subclass will implement a destructor. More...
 

Private Attributes

std::vector< std::list< pcl::PointXYZ > > cells_
 Storage for the cells in the grid. More...
 
unsigned int height_
 The height of the grid in cells. More...
 
double max_z_
 The height cutoff for adding points as obstacles. More...
 
geometry_msgs::Point origin_
 The origin point of the grid. More...
 
std::vector< std::list< pcl::PointXYZ > * > points_
 The lists of points returned by a range search, made a member to save on memory allocation. More...
 
double resolution_
 The resolution of the grid in meters/cell. More...
 
double sq_min_separation_
 The minimum square distance required between points in the grid. More...
 
double sq_obstacle_range_
 The square distance at which we no longer add obstacles to the grid. More...
 
unsigned int width_
 The width of the grid in cells. More...
 

Additional Inherited Members

- Protected Member Functions inherited from base_local_planner::WorldModel
 WorldModel ()
 

Detailed Description

A class that implements the WorldModel interface to provide free-space collision checks for the trajectory controller. This class stores points binned into a grid and performs point-in-polygon checks when necessary to determine the legality of a footprint at a given position/orientation.

Definition at line 58 of file point_grid.h.

Constructor & Destructor Documentation

base_local_planner::PointGrid::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.

Parameters
widthThe width in meters of the grid
heightThe height in meters of the gird
resolutionThe resolution of the grid in meters/cell
originThe origin of the bottom left corner of the grid
max_zThe maximum height for an obstacle to be added to the grid
obstacle_rangeThe maximum distance for obstacles to be added to the grid
min_separationThe minimum distance between points in the grid

Definition at line 77 of file point_grid.cpp.

virtual base_local_planner::PointGrid::~PointGrid ( )
inlinevirtual

Destructor for a point grid.

Definition at line 76 of file point_grid.h.

Member Function Documentation

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

Checks if any points in the grid lie inside a convex footprint.

Parameters
positionThe position of the robot in world coordinates
footprintThe specification of the footprint of the robot in world coordinates
inscribed_radiusThe radius of the inscribed circle of the robot
circumscribed_radiusThe radius of the circumscribed circle of the robot
Returns
Positive if all the points lie outside the footprint, negative otherwise

Implements base_local_planner::WorldModel.

Definition at line 85 of file point_grid.cpp.

void base_local_planner::PointGrid::getCellBounds ( unsigned int  gx,
unsigned int  gy,
geometry_msgs::Point lower_left,
geometry_msgs::Point upper_right 
) const
inline

Get the bounds in world coordinates of a cell in the point grid, assumes a legal cell when called.

Parameters
gxThe x coordinate of the grid cell
gyThe y coordinate of the grid cell
lower_leftThe lower left bounds of the cell in world coordinates to be filled in
upper_rightThe upper right bounds of the cell in world coordinates to be filled in

Definition at line 140 of file point_grid.h.

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

Find the distance between a point and its nearest neighbor in a cell.

Parameters
ptThe point used for comparison
gxThe x coordinate of the cell
gyThe y coordinate of the cell
Returns
The distance between the point passed in and its nearest neighbor in the cell

Definition at line 254 of file point_grid.cpp.

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

Get the points in the point grid.

Parameters
cloudThe point cloud to insert the points into

Definition at line 479 of file point_grid.cpp.

void base_local_planner::PointGrid::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 may be outside the range itself.

Parameters
lower_leftThe lower left corner of the range search
upper_rightThe upper right corner of the range search
pointsA vector of pointers to lists of the relevant points

Definition at line 178 of file point_grid.cpp.

bool base_local_planner::PointGrid::gridCoords ( geometry_msgs::Point  pt,
unsigned int &  gx,
unsigned int &  gy 
) const
inline

Convert from world coordinates to grid coordinates.

Parameters
ptA point in world space
gxThe x coordinate of the corresponding grid cell to be set by the function
gyThe y coordinate of the corresponding grid cell to be set by the function
Returns
True if the conversion was successful, false otherwise

Definition at line 115 of file point_grid.h.

bool base_local_planner::PointGrid::gridCoords ( pcl::PointXYZ  pt,
unsigned int &  gx,
unsigned int &  gy 
) const
inline

Convert from world coordinates to grid coordinates.

Parameters
ptA point in world space
gxThe x coordinate of the corresponding grid cell to be set by the function
gyThe y coordinate of the corresponding grid cell to be set by the function
Returns
True if the conversion was successful, false otherwise

Definition at line 166 of file point_grid.h.

unsigned int base_local_planner::PointGrid::gridIndex ( unsigned int  gx,
unsigned int  gy 
) const
inline

Converts cell coordinates to an index value that can be used to look up the correct grid cell.

Parameters
gxThe x coordinate of the cell
gyThe y coordinate of the cell
Returns
The index of the cell in the stored cell list

Definition at line 190 of file point_grid.h.

void base_local_planner::PointGrid::insert ( pcl::PointXYZ  pt)

Insert a point into the point grid.

Parameters
ptThe point to be inserted

Definition at line 234 of file point_grid.cpp.

void base_local_planner::PointGrid::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.

Parameters
v1The first point of the first segment
v2The second point of the first segment
u1The first point of the second segment
u2The second point of the second segment
resultThe point to be filled in

Definition at line 531 of file point_grid.cpp.

double base_local_planner::PointGrid::nearestNeighborDistance ( pcl::PointXYZ &  pt)

Find the distance between a point and its nearest neighbor in the grid.

Parameters
ptThe point used for comparison
Returns
The distance between the point passed in and its nearest neighbor in the point grid

Definition at line 265 of file point_grid.cpp.

double base_local_planner::PointGrid::orient ( const geometry_msgs::Point a,
const geometry_msgs::Point b,
const pcl::PointXYZ &  c 
)
inline

Check the orientation of a pt c with respect to the vector a->b.

Parameters
aThe start point of the vector
bThe end point of the vector
cThe point to compute orientation for
Returns
orient(a, b, c) < 0 -—> Right, orient(a, b, c) > 0 -—> Left

Definition at line 210 of file point_grid.h.

double base_local_planner::PointGrid::orient ( const geometry_msgs::Point32 &  a,
const geometry_msgs::Point32 &  b,
const pcl::PointXYZ &  c 
)
inline

Check the orientation of a pt c with respect to the vector a->b.

Parameters
aThe start point of the vector
bThe end point of the vector
cThe point to compute orientation for
Returns
orient(a, b, c) < 0 -—> Right, orient(a, b, c) > 0 -—> Left

Definition at line 225 of file point_grid.h.

double base_local_planner::PointGrid::orient ( const geometry_msgs::Point a,
const geometry_msgs::Point b,
const geometry_msgs::Point c 
)
inline

Check the orientation of a pt c with respect to the vector a->b.

Parameters
aThe start point of the vector
bThe end point of the vector
cThe point to compute orientation for
Returns
orient(a, b, c) < 0 -—> Right, orient(a, b, c) > 0 -—> Left

Definition at line 240 of file point_grid.h.

double base_local_planner::PointGrid::orient ( const pcl::PointXYZ &  a,
const pcl::PointXYZ &  b,
const pcl::PointXYZ &  c 
)
inline

Check the orientation of a pt c with respect to the vector a->b.

Parameters
aThe start point of the vector
bThe end point of the vector
cThe point to compute orientation for
Returns
orient(a, b, c) < 0 -—> Right, orient(a, b, c) > 0 -—> Left

Definition at line 256 of file point_grid.h.

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

Check if a point is in a polygon.

Parameters
ptThe point to be checked
polyThe polygon to check against
Returns
True if the point is in the polygon, false otherwise

Definition at line 142 of file point_grid.cpp.

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

Checks to see if a point is within a laser scan specification.

Parameters
ptThe point to check
laser_scanThe specification of the scan to check against
Returns
True if the point is contained within the scan, false otherwise

Definition at line 435 of file point_grid.cpp.

void base_local_planner::PointGrid::removePointsInPolygon ( const std::vector< geometry_msgs::Point poly)

Removes points from the grid that lie within the polygon.

Parameters
polyA specification of the polygon to clear from the grid

Definition at line 487 of file point_grid.cpp.

void base_local_planner::PointGrid::removePointsInScanBoundry ( const PlanarLaserScan laser_scan)

Removes points from the grid that lie within a laser scan.

Parameters
laser_scanA specification of the laser scan to use for clearing

Definition at line 392 of file point_grid.cpp.

bool base_local_planner::PointGrid::segIntersect ( const pcl::PointXYZ &  v1,
const pcl::PointXYZ &  v2,
const pcl::PointXYZ &  u1,
const pcl::PointXYZ &  u2 
)
inline

Check if two line segmenst intersect.

Parameters
v1The first point of the first segment
v2The second point of the first segment
u1The first point of the second segment
u2The second point of the second segment
Returns
True if the segments intersect, false otherwise

Definition at line 272 of file point_grid.h.

double base_local_planner::PointGrid::sq_distance ( pcl::PointXYZ &  pt1,
pcl::PointXYZ &  pt2 
)
inline

Compute the squared distance between two points.

Parameters
pt1The first point
pt2The second point
Returns
The squared distance between the two points

Definition at line 155 of file point_grid.h.

void base_local_planner::PointGrid::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.

Parameters
footprintThe footprint of the robot in its current location
observationsThe observations from various sensors
laser_scansThe laser scans used to clear freespace (the point grid only uses the first scan which is assumed to be the base laser)

Definition at line 358 of file point_grid.cpp.

Member Data Documentation

std::vector< std::list<pcl::PointXYZ> > base_local_planner::PointGrid::cells_
private

Storage for the cells in the grid.

Definition at line 350 of file point_grid.h.

unsigned int base_local_planner::PointGrid::height_
private

The height of the grid in cells.

Definition at line 349 of file point_grid.h.

double base_local_planner::PointGrid::max_z_
private

The height cutoff for adding points as obstacles.

Definition at line 351 of file point_grid.h.

geometry_msgs::Point base_local_planner::PointGrid::origin_
private

The origin point of the grid.

Definition at line 347 of file point_grid.h.

std::vector< std::list<pcl::PointXYZ>* > base_local_planner::PointGrid::points_
private

The lists of points returned by a range search, made a member to save on memory allocation.

Definition at line 354 of file point_grid.h.

double base_local_planner::PointGrid::resolution_
private

The resolution of the grid in meters/cell.

Definition at line 346 of file point_grid.h.

double base_local_planner::PointGrid::sq_min_separation_
private

The minimum square distance required between points in the grid.

Definition at line 353 of file point_grid.h.

double base_local_planner::PointGrid::sq_obstacle_range_
private

The square distance at which we no longer add obstacles to the grid.

Definition at line 352 of file point_grid.h.

unsigned int base_local_planner::PointGrid::width_
private

The width of the grid in cells.

Definition at line 348 of file point_grid.h.


The documentation for this class was generated from the following files:


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:49