42 #include <geometry_msgs/Point.h> 46 #include <pcl/point_types.h> 47 #include <pcl/point_cloud.h> 70 PointGrid(
double width,
double height,
double resolution, geometry_msgs::Point origin,
71 double max_z,
double obstacle_range,
double min_separation);
84 void getPointsInRange(
const geometry_msgs::Point& lower_left,
const geometry_msgs::Point& upper_right, std::vector< std::list<pcl::PointXYZ>* >& points);
94 virtual double footprintCost(
const geometry_msgs::Point& position,
const std::vector<geometry_msgs::Point>& footprint,
95 double inscribed_radius,
double circumscribed_radius);
105 void updateWorld(
const std::vector<geometry_msgs::Point>& footprint,
106 const std::vector<costmap_2d::Observation>& observations,
const std::vector<PlanarLaserScan>& laser_scans);
115 inline bool gridCoords(geometry_msgs::Point pt,
unsigned int& gx,
unsigned int& gy)
const {
140 inline void getCellBounds(
unsigned int gx,
unsigned int gy, geometry_msgs::Point& lower_left, geometry_msgs::Point& upper_right)
const {
155 inline double sq_distance(pcl::PointXYZ& pt1, pcl::PointXYZ& pt2){
156 return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y);
166 inline bool gridCoords(pcl::PointXYZ pt,
unsigned int& gx,
unsigned int& gy)
const {
190 inline unsigned int gridIndex(
unsigned int gx,
unsigned int gy)
const {
210 inline double orient(
const geometry_msgs::Point& a,
const geometry_msgs::Point& b,
const pcl::PointXYZ& c){
211 double acx = a.x - c.x;
212 double bcx = b.x - c.x;
213 double acy = a.y - c.y;
214 double bcy = b.y - c.y;
215 return acx * bcy - acy * bcx;
225 inline double orient(
const geometry_msgs::Point32& a,
const geometry_msgs::Point32& b,
const pcl::PointXYZ& 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;
240 inline double orient(
const geometry_msgs::Point& a,
const geometry_msgs::Point& b,
241 const geometry_msgs::Point& c){
242 double acx = a.x - c.x;
243 double bcx = b.x - c.x;
244 double acy = a.y - c.y;
245 double bcy = b.y - c.y;
246 return acx * bcy - acy * bcx;
256 inline double orient(
const pcl::PointXYZ& a,
const pcl::PointXYZ& b,
const pcl::PointXYZ& c){
257 double acx = a.x - c.x;
258 double bcx = b.x - c.x;
259 double acy = a.y - c.y;
260 double bcy = b.y - c.y;
261 return acx * bcy - acy * bcx;
272 inline bool segIntersect(
const pcl::PointXYZ& v1,
const pcl::PointXYZ& v2,
273 const pcl::PointXYZ& u1,
const pcl::PointXYZ& u2){
285 void intersectionPoint(
const geometry_msgs::Point& v1,
const geometry_msgs::Point& v2,
286 const geometry_msgs::Point& u1,
const geometry_msgs::Point& u2,
287 geometry_msgs::Point& result);
295 bool ptInPolygon(
const pcl::PointXYZ& pt,
const std::vector<geometry_msgs::Point>& poly);
301 void insert(pcl::PointXYZ pt);
317 double getNearestInCell(pcl::PointXYZ& pt,
unsigned int gx,
unsigned int gy);
343 void getPoints(pcl::PointCloud<pcl::PointXYZ>& cloud);
350 std::vector< std::list<pcl::PointXYZ> >
cells_;
354 std::vector< std::list<pcl::PointXYZ>* >
points_;
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.
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...
bool gridCoords(geometry_msgs::Point pt, unsigned int &gx, unsigned int &gy) const
Convert from world coordinates to grid coordinates.
double sq_obstacle_range_
The square distance at which we no longer add obstacles to the grid.
void insert(pcl::PointXYZ pt)
Insert a point into the point grid.
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 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.
void removePointsInScanBoundry(const PlanarLaserScan &laser_scan)
Removes points from the grid that lie within a laser scan.
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.
geometry_msgs::Point origin_
The origin point of the grid.
double nearestNeighborDistance(pcl::PointXYZ &pt)
Find the distance between a point and its nearest neighbor in 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.
void getPoints(pcl::PointCloud< pcl::PointXYZ > &cloud)
Get the points in the point grid.
bool gridCoords(pcl::PointXYZ pt, unsigned int &gx, unsigned int &gy) const
Convert from world coordinates to grid coordinates.
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 m...
unsigned int height_
The height of the grid in cells.
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.
std::vector< std::list< pcl::PointXYZ > > cells_
Storage for the cells in the grid.
void removePointsInPolygon(const std::vector< geometry_msgs::Point > poly)
Removes points from the grid that lie within the polygon.
double sq_distance(pcl::PointXYZ &pt1, pcl::PointXYZ &pt2)
Compute the squared distance between two points.
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...
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.
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.
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 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.
Stores a scan from a planar laser that can be used to clear freespace.
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 max_z_
The height cutoff for adding points as obstacles.
double getNearestInCell(pcl::PointXYZ &pt, unsigned int gx, unsigned int gy)
Find the distance between a point and its nearest neighbor in a cell.