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:

[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

width	The width in meters of the grid
height	The height in meters of the gird
resolution	The resolution of the grid in meters/cell
origin	The origin of the bottom left corner of the grid
max_z	The maximum height for an obstacle to be added to the grid
obstacle_range	The maximum distance for obstacles to be added to the grid
min_separation	The 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

position	The position of the robot in world coordinates
footprint	The specification of the footprint of the robot in world coordinates
inscribed_radius	The radius of the inscribed circle of the robot
circumscribed_radius	The 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

gx	The x coordinate of the grid cell
gy	The y coordinate of the grid cell
lower_left	The lower left bounds of the cell in world coordinates to be filled in
upper_right	The 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

pt	The point used for comparison
gx	The x coordinate of the cell
gy	The 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

cloud The 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_left	The lower left corner of the range search
upper_right	The upper right corner of the range search
points	A 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

pt	A point in world space
gx	The x coordinate of the corresponding grid cell to be set by the function
gy	The 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

pt	A point in world space
gx	The x coordinate of the corresponding grid cell to be set by the function
gy	The 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

gx	The x coordinate of the cell
gy	The 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

pt	The 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

v1	The first point of the first segment
v2	The second point of the first segment
u1	The first point of the second segment
u2	The second point of the second segment
result	The 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

pt	The 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

a	The start point of the vector
b	The end point of the vector
c	The 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

a	The start point of the vector
b	The end point of the vector
c	The 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

a	The start point of the vector
b	The end point of the vector
c	The 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

a	The start point of the vector
b	The end point of the vector
c	The 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

pt	The point to be checked
poly	The 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

pt	The point to check
laser_scan	The 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

poly	A 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_scan A 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

v1	The first point of the first segment
v2	The second point of the first segment
u1	The first point of the second segment
u2	The 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

pt1	The first point
pt2	The 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

footprint	The footprint of the robot in its current location
observations	The observations from various sensors
laser_scans	The 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:

Public Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation