42 #include <geometry_msgs/Point.h> 46 #include <sensor_msgs/PointCloud2.h> 69 PointGrid(
double width,
double height,
double resolution, geometry_msgs::Point origin,
70 double max_z,
double obstacle_range,
double min_separation);
83 void getPointsInRange(
const geometry_msgs::Point& lower_left,
const geometry_msgs::Point& upper_right, std::vector< std::list<geometry_msgs::Point32>* >& points);
93 virtual double footprintCost(
const geometry_msgs::Point& position,
const std::vector<geometry_msgs::Point>& footprint,
94 double inscribed_radius,
double circumscribed_radius);
104 void updateWorld(
const std::vector<geometry_msgs::Point>& footprint,
105 const std::vector<costmap_2d::Observation>& observations,
const std::vector<PlanarLaserScan>& laser_scans);
114 inline bool gridCoords(geometry_msgs::Point pt,
unsigned int& gx,
unsigned int& gy)
const {
139 inline void getCellBounds(
unsigned int gx,
unsigned int gy, geometry_msgs::Point& lower_left, geometry_msgs::Point& upper_right)
const {
154 inline double sq_distance(
const geometry_msgs::Point32& pt1,
const geometry_msgs::Point32& pt2){
155 return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y);
165 inline bool gridCoords(
const geometry_msgs::Point32& pt,
unsigned int& gx,
unsigned int& gy)
const {
189 inline unsigned int gridIndex(
unsigned int gx,
unsigned int gy)
const {
209 inline double orient(
const geometry_msgs::Point& a,
const geometry_msgs::Point& b,
const geometry_msgs::Point32& c){
210 double acx = a.x - c.x;
211 double bcx = b.x - c.x;
212 double acy = a.y - c.y;
213 double bcy = b.y - c.y;
214 return acx * bcy - acy * bcx;
225 inline double orient(
const T& a,
const T& b,
const T& c){
226 double acx = a.x - c.x;
227 double bcx = b.x - c.x;
228 double acy = a.y - c.y;
229 double bcy = b.y - c.y;
230 return acx * bcy - acy * bcx;
241 inline bool segIntersect(
const geometry_msgs::Point32& v1,
const geometry_msgs::Point32& v2,
242 const geometry_msgs::Point32& u1,
const geometry_msgs::Point32& u2){
254 void intersectionPoint(
const geometry_msgs::Point& v1,
const geometry_msgs::Point& v2,
255 const geometry_msgs::Point& u1,
const geometry_msgs::Point& u2,
256 geometry_msgs::Point& result);
264 bool ptInPolygon(
const geometry_msgs::Point32& pt,
const std::vector<geometry_msgs::Point>& poly);
270 void insert(
const geometry_msgs::Point32& pt);
286 double getNearestInCell(
const geometry_msgs::Point32& pt,
unsigned int gx,
unsigned int gy);
312 void getPoints(sensor_msgs::PointCloud2& cloud);
319 std::vector< std::list<geometry_msgs::Point32> >
cells_;
323 std::vector< std::list<geometry_msgs::Point32>* >
points_;
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...
void getPoints(sensor_msgs::PointCloud2 &cloud)
Get the points in the point grid.
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.
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 lega...
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.
double resolution_
The resolution of the grid in meters/cell.
bool gridCoords(geometry_msgs::Point pt, unsigned int &gx, unsigned int &gy) const
Convert from world coordinates to grid coordinates.
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.
unsigned int width_
The width of the grid in cells.
double sq_min_separation_
The minimum square distance required between points in the grid.
A class that implements the WorldModel interface to provide free-space collision checks for the traje...
std::vector< std::list< geometry_msgs::Point32 > > cells_
Storage for the cells in the grid.
bool ptInPolygon(const geometry_msgs::Point32 &pt, const std::vector< geometry_msgs::Point > &poly)
Check if a point is in a polygon.
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.
double sq_obstacle_range_
The square distance at which we no longer add obstacles to the grid.
void insert(const geometry_msgs::Point32 &pt)
Insert a point into the point grid.
void removePointsInScanBoundry(const PlanarLaserScan &laser_scan)
Removes points from the grid that lie within a laser scan.
double sq_distance(const geometry_msgs::Point32 &pt1, const geometry_msgs::Point32 &pt2)
Compute the squared distance between two points.
geometry_msgs::Point origin_
The origin point of the grid.
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 m...
unsigned int height_
The height of the grid in cells.
void removePointsInPolygon(const std::vector< geometry_msgs::Point > poly)
Removes points from the grid that lie within the polygon.
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.
bool gridCoords(const geometry_msgs::Point32 &pt, unsigned int &gx, unsigned int &gy) const
Convert from world coordinates to grid coordinates.
An interface the trajectory controller uses to interact with the world regardless of the underlying w...
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...
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.
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.
bool ptInScan(const geometry_msgs::Point32 &pt, const PlanarLaserScan &laser_scan)
Checks to see if a point is within a laser scan specification.
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.
Stores a scan from a planar laser that can be used to clear freespace.
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.
double max_z_
The height cutoff for adding points as obstacles.
double nearestNeighborDistance(const geometry_msgs::Point32 &pt)
Find the distance between a point and its nearest neighbor in the grid.