Public Member Functions | Private Attributes
iri_diff_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 iri_diff_local_planner::PointGrid:
Inheritance graph
[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:
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 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:
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 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:
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 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:
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.

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 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_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
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:
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 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:
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 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:
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 189 of file point_grid.h.

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

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.

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

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 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:
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 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:
ptThe point to be checked
polyThe 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:
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.

Removes points from the grid that lie within the polygon.

Parameters:
polyA specification of the polygon to clear from the grid

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 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:
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 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:
pt1The first point
pt2The 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:
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)

Member Data Documentation

Storage for the cells in the grid.

Definition at line 349 of file point_grid.h.

The height of the grid in cells.

Definition at line 348 of file point_grid.h.

The height cutoff for adding points as obstacles.

Definition at line 350 of file point_grid.h.

The origin point of the grid.

Definition at line 346 of file point_grid.h.

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

Definition at line 353 of file point_grid.h.

The resolution of the grid in meters/cell.

Definition at line 345 of file point_grid.h.

The minimum square distance required between points in the grid.

Definition at line 352 of file point_grid.h.

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

Definition at line 351 of file point_grid.h.

The width of the grid in cells.

Definition at line 347 of file point_grid.h.


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


iri_diff_local_planner
Author(s): Iri Lab
autogenerated on Fri Dec 6 2013 20:32:36