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...
 
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 legality in the world. More...
 
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...
 
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)
 
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 (const geometry_msgs::Point32 &pt, unsigned int gx, unsigned int gy)
 Find the distance between a point and its nearest neighbor in a cell. More...
 
void getPoints (sensor_msgs::PointCloud2 &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< geometry_msgs::Point32 > * > &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 (const geometry_msgs::Point32 &pt, unsigned int &gx, unsigned int &gy) const
 Convert from world coordinates to grid coordinates. More...
 
bool gridCoords (geometry_msgs::Point 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 (const geometry_msgs::Point32 &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 (const geometry_msgs::Point32 &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 geometry_msgs::Point32 &c)
 Check the orientation of a pt c with respect to the vector a->b. More...
 
template<typename T >
double orient (const T &a, const T &b, const T &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 geometry_msgs::Point32 &pt, const std::vector< geometry_msgs::Point > &poly)
 Check if a point is in a polygon. More...
 
bool ptInScan (const geometry_msgs::Point32 &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 geometry_msgs::Point32 &v1, const geometry_msgs::Point32 &v2, const geometry_msgs::Point32 &u1, const geometry_msgs::Point32 &u2)
 Check if two line segmenst intersect. More...
 
double sq_distance (const geometry_msgs::Point32 &pt1, const geometry_msgs::Point32 &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 (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...
 
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)
 
virtual ~WorldModel ()
 Subclass will implement a destructor. More...
 

Private Attributes

std::vector< std::list< geometry_msgs::Point32 > > 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< geometry_msgs::Point32 > * > 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 92 of file point_grid.h.

Constructor & Destructor Documentation

◆ PointGrid()

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 53 of file point_grid.cpp.

◆ ~PointGrid()

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

Destructor for a point grid.

Definition at line 110 of file point_grid.h.

Member Function Documentation

◆ footprintCost() [1/4]

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 61 of file point_grid.cpp.

◆ footprintCost() [2/4]

virtual double base_local_planner::WorldModel::footprintCost

Subclass will implement this method to check a footprint at a given position and orientation for legality in the world.

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: -1 if footprint covers at least a lethal obstacle cell, or -2 if footprint covers at least a no-information cell, or -3 if footprint is partially or totally outside of the map

◆ footprintCost() [3/4]

double base_local_planner::WorldModel::footprintCost
inline

Checks if any obstacles in the costmap lie inside a convex footprint that is rasterized into the grid.

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

Definition at line 134 of file world_model.h.

◆ footprintCost() [4/4]

double base_local_planner::WorldModel::footprintCost
inline

Definition at line 103 of file world_model.h.

◆ getCellBounds()

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 174 of file point_grid.h.

◆ getNearestInCell()

double base_local_planner::PointGrid::getNearestInCell ( const geometry_msgs::Point32 &  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 231 of file point_grid.cpp.

◆ getPoints()

void base_local_planner::PointGrid::getPoints ( sensor_msgs::PointCloud2 &  cloud)

Get the points in the point grid.

Parameters
cloudThe point cloud to insert the points into

Definition at line 465 of file point_grid.cpp.

◆ getPointsInRange()

void base_local_planner::PointGrid::getPointsInRange ( const geometry_msgs::Point lower_left,
const geometry_msgs::Point upper_right,
std::vector< std::list< geometry_msgs::Point32 > * > &  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 154 of file point_grid.cpp.

◆ gridCoords() [1/2]

bool base_local_planner::PointGrid::gridCoords ( const geometry_msgs::Point32 &  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 200 of file point_grid.h.

◆ gridCoords() [2/2]

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 149 of file point_grid.h.

◆ gridIndex()

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 224 of file point_grid.h.

◆ insert()

void base_local_planner::PointGrid::insert ( const geometry_msgs::Point32 &  pt)

Insert a point into the point grid.

Parameters
ptThe point to be inserted

Definition at line 211 of file point_grid.cpp.

◆ intersectionPoint()

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 534 of file point_grid.cpp.

◆ nearestNeighborDistance()

double base_local_planner::PointGrid::nearestNeighborDistance ( const geometry_msgs::Point32 &  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 242 of file point_grid.cpp.

◆ orient() [1/2]

double base_local_planner::PointGrid::orient ( const geometry_msgs::Point a,
const geometry_msgs::Point b,
const geometry_msgs::Point32 &  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 244 of file point_grid.h.

◆ orient() [2/2]

template<typename T >
double base_local_planner::PointGrid::orient ( const T &  a,
const T &  b,
const T &  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 260 of file point_grid.h.

◆ ptInPolygon()

bool base_local_planner::PointGrid::ptInPolygon ( const geometry_msgs::Point32 &  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 118 of file point_grid.cpp.

◆ ptInScan()

bool base_local_planner::PointGrid::ptInScan ( const geometry_msgs::Point32 &  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 421 of file point_grid.cpp.

◆ removePointsInPolygon()

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 490 of file point_grid.cpp.

◆ removePointsInScanBoundry()

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 378 of file point_grid.cpp.

◆ segIntersect()

bool base_local_planner::PointGrid::segIntersect ( const geometry_msgs::Point32 &  v1,
const geometry_msgs::Point32 &  v2,
const geometry_msgs::Point32 &  u1,
const geometry_msgs::Point32 &  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 276 of file point_grid.h.

◆ sq_distance()

double base_local_planner::PointGrid::sq_distance ( const geometry_msgs::Point32 &  pt1,
const geometry_msgs::Point32 &  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 189 of file point_grid.h.

◆ updateWorld()

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 335 of file point_grid.cpp.

Member Data Documentation

◆ cells_

std::vector< std::list<geometry_msgs::Point32> > base_local_planner::PointGrid::cells_
private

Storage for the cells in the grid.

Definition at line 354 of file point_grid.h.

◆ height_

unsigned int base_local_planner::PointGrid::height_
private

The height of the grid in cells.

Definition at line 353 of file point_grid.h.

◆ max_z_

double base_local_planner::PointGrid::max_z_
private

The height cutoff for adding points as obstacles.

Definition at line 355 of file point_grid.h.

◆ origin_

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

The origin point of the grid.

Definition at line 351 of file point_grid.h.

◆ points_

std::vector< std::list<geometry_msgs::Point32>* > 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 358 of file point_grid.h.

◆ resolution_

double base_local_planner::PointGrid::resolution_
private

The resolution of the grid in meters/cell.

Definition at line 350 of file point_grid.h.

◆ sq_min_separation_

double base_local_planner::PointGrid::sq_min_separation_
private

The minimum square distance required between points in the grid.

Definition at line 357 of file point_grid.h.

◆ sq_obstacle_range_

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 356 of file point_grid.h.

◆ width_

unsigned int base_local_planner::PointGrid::width_
private

The width of the grid in cells.

Definition at line 352 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 Mon Mar 6 2023 03:50:24