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>
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 () | |
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.
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.
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 53 of file point_grid.cpp.
|
inlinevirtual |
Destructor for a point grid.
Definition at line 110 of file point_grid.h.
|
virtual |
Checks if any points in the grid lie inside a convex footprint.
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 |
Implements base_local_planner::WorldModel.
Definition at line 61 of file point_grid.cpp.
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.
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 |
|
inline |
Checks if any obstacles in the costmap lie inside a convex footprint that is rasterized into the grid.
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 |
Definition at line 134 of file world_model.h.
|
inline |
Definition at line 103 of file world_model.h.
|
inline |
Get the bounds in world coordinates of a cell in the point grid, assumes a legal cell when called.
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 174 of file point_grid.h.
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.
pt | The point used for comparison |
gx | The x coordinate of the cell |
gy | The y coordinate of the cell |
Definition at line 231 of file point_grid.cpp.
void base_local_planner::PointGrid::getPoints | ( | sensor_msgs::PointCloud2 & | cloud | ) |
Get the points in the point grid.
cloud | The point cloud to insert the points into |
Definition at line 465 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< 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.
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 154 of file point_grid.cpp.
|
inline |
Convert from world coordinates to grid coordinates.
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 |
Definition at line 200 of file point_grid.h.
|
inline |
Convert from world coordinates to grid coordinates.
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 |
Definition at line 149 of file point_grid.h.
|
inline |
Converts cell coordinates to an index value that can be used to look up the correct grid cell.
gx | The x coordinate of the cell |
gy | The y coordinate of the cell |
Definition at line 224 of file point_grid.h.
void base_local_planner::PointGrid::insert | ( | const geometry_msgs::Point32 & | pt | ) |
Insert a point into the point grid.
pt | The point to be inserted |
Definition at line 211 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.
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 534 of file point_grid.cpp.
double base_local_planner::PointGrid::nearestNeighborDistance | ( | const geometry_msgs::Point32 & | pt | ) |
Find the distance between a point and its nearest neighbor in the grid.
pt | The point used for comparison |
Definition at line 242 of file point_grid.cpp.
|
inline |
Check the orientation of a pt c with respect to the vector a->b.
a | The start point of the vector |
b | The end point of the vector |
c | The point to compute orientation for |
Definition at line 244 of file point_grid.h.
|
inline |
Check the orientation of a pt c with respect to the vector a->b.
a | The start point of the vector |
b | The end point of the vector |
c | The point to compute orientation for |
Definition at line 260 of file point_grid.h.
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.
pt | The point to be checked |
poly | The polygon to check against |
Definition at line 118 of file point_grid.cpp.
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.
pt | The point to check |
laser_scan | The specification of the scan to check against |
Definition at line 421 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.
poly | A specification of the polygon to clear from the grid |
Definition at line 490 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.
laser_scan | A specification of the laser scan to use for clearing |
Definition at line 378 of file point_grid.cpp.
|
inline |
Check if two line segmenst intersect.
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 |
Definition at line 276 of file point_grid.h.
|
inline |
Compute the squared distance between two points.
pt1 | The first point |
pt2 | The second point |
Definition at line 189 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.
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 335 of file point_grid.cpp.
|
private |
Storage for the cells in the grid.
Definition at line 354 of file point_grid.h.
|
private |
The height of the grid in cells.
Definition at line 353 of file point_grid.h.
|
private |
The height cutoff for adding points as obstacles.
Definition at line 355 of file point_grid.h.
|
private |
The origin point of the grid.
Definition at line 351 of file point_grid.h.
|
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.
|
private |
The resolution of the grid in meters/cell.
Definition at line 350 of file point_grid.h.
|
private |
The minimum square distance required between points in the grid.
Definition at line 357 of file point_grid.h.
|
private |
The square distance at which we no longer add obstacles to the grid.
Definition at line 356 of file point_grid.h.
|
private |
The width of the grid in cells.
Definition at line 352 of file point_grid.h.