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 iri_diff_local_planner::PointGrid:

[legend]

List of all members.

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.
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.
double	getNearestInCell (pcl::PointXYZ &pt, unsigned int gx, unsigned int gy)
	Find the distance between a point and its nearest neighbor in a cell.
void	getPoints (pcl::PointCloud< pcl::PointXYZ > &cloud)
	Get the points in the point grid.
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.
bool	gridCoords (geometry_msgs::Point pt, unsigned int &gx, unsigned int &gy) const
	Convert from world coordinates to grid coordinates.
bool	gridCoords (pcl::PointXYZ pt, unsigned int &gx, unsigned int &gy) const
	Convert from world coordinates to grid coordinates.
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.
void	insert (pcl::PointXYZ pt)
	Insert a point into the point grid.
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.
double	nearestNeighborDistance (pcl::PointXYZ &pt)
	Find the distance between a point and its nearest neighbor in the grid.
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.
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.
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.
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.
	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.
bool	ptInPolygon (const pcl::PointXYZ &pt, const std::vector< geometry_msgs::Point > &poly)
	Check if a point is in a polygon.
bool	ptInScan (const pcl::PointXYZ &pt, const PlanarLaserScan &laser_scan)
	Checks to see if a point is within a laser scan specification.
void	removePointsInPolygon (const std::vector< geometry_msgs::Point > poly)
	Removes points from the grid that lie within the polygon.
void	removePointsInScanBoundry (const PlanarLaserScan &laser_scan)
	Removes points from the grid that lie within a laser scan.
bool	segIntersect (const pcl::PointXYZ &v1, const pcl::PointXYZ &v2, const pcl::PointXYZ &u1, const pcl::PointXYZ &u2)
	Check if two line segmenst intersect.
double	sq_distance (pcl::PointXYZ &pt1, pcl::PointXYZ &pt2)
	Compute the squared distance between two points.
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.
virtual	~PointGrid ()
	Destructor for a point grid.
Private Attributes
std::vector< std::list < pcl::PointXYZ > >	cells_
	Storage for the cells in the grid.
unsigned int	height_
	The height of the grid in cells.
double	max_z_
	The height cutoff for adding points as obstacles.
geometry_msgs::Point	origin_
	The origin point of the grid.
std::vector< std::list < pcl::PointXYZ > * >	points_
	The lists of points returned by a range search, made a member to save on memory allocation.
double	resolution_
	The resolution of the grid in meters/cell.
double	sq_min_separation_
	The minimum square distance required between points in the grid.
double	sq_obstacle_range_
	The square distance at which we no longer add obstacles to the grid.
unsigned int	width_
	The width of the grid in cells.

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

Constructor & Destructor Documentation

iri_diff_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 iri_diff_local_planner::PointGrid::~PointGrid ( ) [inline, virtual]

Destructor for a point grid.

Definition at line 77 of file point_grid.h.

Member Function Documentation

virtual double iri_diff_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 iri_diff_local_planner::WorldModel.

void iri_diff_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 139 of file point_grid.h.

double iri_diff_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 iri_diff_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 iri_diff_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

bool iri_diff_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 114 of file point_grid.h.

bool iri_diff_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 165 of file point_grid.h.

unsigned int iri_diff_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 189 of file point_grid.h.

void iri_diff_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 iri_diff_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 iri_diff_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 iri_diff_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 209 of file point_grid.h.

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

double iri_diff_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 239 of file point_grid.h.

double iri_diff_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 255 of file point_grid.h.

bool iri_diff_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

bool iri_diff_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 iri_diff_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

void iri_diff_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 iri_diff_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 271 of file point_grid.h.

double iri_diff_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 154 of file point_grid.h.

void iri_diff_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)