327 lines
15 KiB
C++
327 lines
15 KiB
C++
/*********************************************************************
|
|
*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Copyright (c) 2008, Willow Garage, Inc.
|
|
* All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* * Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* * Redistributions in binary form must reproduce the above
|
|
* copyright notice, this list of conditions and the following
|
|
* disclaimer in the documentation and/or other materials provided
|
|
* with the distribution.
|
|
* * Neither the name of the Willow Garage nor the names of its
|
|
* contributors may be used to endorse or promote products derived
|
|
* from this software without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
* Author: Eitan Marder-Eppstein
|
|
*********************************************************************/
|
|
#ifndef ROBOT_POINT_GRID_H_
|
|
#define ROBOT_POINT_GRID_H_
|
|
#include <vector>
|
|
#include <list>
|
|
#include <cfloat>
|
|
#include <robot_geometry_msgs/Point.h>
|
|
#include <robot_costmap_2d/observation.h>
|
|
#include <robot_base_local_planner/world_model.h>
|
|
|
|
#include <robot_sensor_msgs/PointCloud2.h>
|
|
|
|
namespace robot_base_local_planner {
|
|
/**
|
|
* @class PointGrid
|
|
* @brief 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.
|
|
*/
|
|
class PointGrid : public WorldModel {
|
|
public:
|
|
/**
|
|
* @brief Constuctor for a grid that stores points in the plane
|
|
* @param width The width in meters of the grid
|
|
* @param height The height in meters of the gird
|
|
* @param resolution The resolution of the grid in meters/cell
|
|
* @param origin The origin of the bottom left corner of the grid
|
|
* @param max_z The maximum height for an obstacle to be added to the grid
|
|
* @param obstacle_range The maximum distance for obstacles to be added to the grid
|
|
* @param min_separation The minimum distance between points in the grid
|
|
*/
|
|
PointGrid(double width, double height, double resolution, robot_geometry_msgs::Point origin,
|
|
double max_z, double obstacle_range, double min_separation);
|
|
|
|
/**
|
|
* @brief Destructor for a point grid
|
|
*/
|
|
virtual ~PointGrid(){}
|
|
|
|
/**
|
|
* @brief Returns the points that lie within the cells contained in the specified range. Some of these points may be outside the range itself.
|
|
* @param lower_left The lower left corner of the range search
|
|
* @param upper_right The upper right corner of the range search
|
|
* @param points A vector of pointers to lists of the relevant points
|
|
*/
|
|
void getPointsInRange(const robot_geometry_msgs::Point& lower_left, const robot_geometry_msgs::Point& upper_right, std::vector< std::list<robot_geometry_msgs::Point32>* >& points);
|
|
|
|
/**
|
|
* @brief Checks if any points in the grid lie inside a convex footprint
|
|
* @param position The position of the robot in world coordinates
|
|
* @param footprint The specification of the footprint of the robot in world coordinates
|
|
* @param inscribed_radius The radius of the inscribed circle of the robot
|
|
* @param circumscribed_radius The radius of the circumscribed circle of the robot
|
|
* @return Positive if all the points lie outside the footprint, negative otherwise
|
|
*/
|
|
virtual double footprintCost(const robot_geometry_msgs::Point& position, const std::vector<robot_geometry_msgs::Point>& footprint,
|
|
double inscribed_radius, double circumscribed_radius);
|
|
|
|
using WorldModel::footprintCost;
|
|
|
|
/**
|
|
* @brief Inserts observations from sensors into the point grid
|
|
* @param footprint The footprint of the robot in its current location
|
|
* @param observations The observations from various sensors
|
|
* @param 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)
|
|
*/
|
|
void updateWorld(const std::vector<robot_geometry_msgs::Point>& footprint,
|
|
const std::vector<robot_costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans);
|
|
|
|
/**
|
|
* @brief Convert from world coordinates to grid coordinates
|
|
* @param pt A point in world space
|
|
* @param gx The x coordinate of the corresponding grid cell to be set by the function
|
|
* @param gy The y coordinate of the corresponding grid cell to be set by the function
|
|
* @return True if the conversion was successful, false otherwise
|
|
*/
|
|
inline bool gridCoords(robot_geometry_msgs::Point pt, unsigned int& gx, unsigned int& gy) const {
|
|
if(pt.x < origin_.x || pt.y < origin_.y){
|
|
gx = 0;
|
|
gy = 0;
|
|
return false;
|
|
}
|
|
gx = (int) ((pt.x - origin_.x)/resolution_);
|
|
gy = (int) ((pt.y - origin_.y)/resolution_);
|
|
|
|
if(gx >= width_ || gy >= height_){
|
|
gx = 0;
|
|
gy = 0;
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* @brief Get the bounds in world coordinates of a cell in the point grid, assumes a legal cell when called
|
|
* @param gx The x coordinate of the grid cell
|
|
* @param gy The y coordinate of the grid cell
|
|
* @param lower_left The lower left bounds of the cell in world coordinates to be filled in
|
|
* @param upper_right The upper right bounds of the cell in world coordinates to be filled in
|
|
*/
|
|
inline void getCellBounds(unsigned int gx, unsigned int gy, robot_geometry_msgs::Point& lower_left, robot_geometry_msgs::Point& upper_right) const {
|
|
lower_left.x = gx * resolution_ + origin_.x;
|
|
lower_left.y = gy * resolution_ + origin_.y;
|
|
|
|
upper_right.x = lower_left.x + resolution_;
|
|
upper_right.y = lower_left.y + resolution_;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief Compute the squared distance between two points
|
|
* @param pt1 The first point
|
|
* @param pt2 The second point
|
|
* @return The squared distance between the two points
|
|
*/
|
|
inline double sq_distance(const robot_geometry_msgs::Point32& pt1, const robot_geometry_msgs::Point32& pt2){
|
|
return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y);
|
|
}
|
|
|
|
/**
|
|
* @brief Convert from world coordinates to grid coordinates
|
|
* @param pt A point in world space
|
|
* @param gx The x coordinate of the corresponding grid cell to be set by the function
|
|
* @param gy The y coordinate of the corresponding grid cell to be set by the function
|
|
* @return True if the conversion was successful, false otherwise
|
|
*/
|
|
inline bool gridCoords(const robot_geometry_msgs::Point32& pt, unsigned int& gx, unsigned int& gy) const {
|
|
if(pt.x < origin_.x || pt.y < origin_.y){
|
|
gx = 0;
|
|
gy = 0;
|
|
return false;
|
|
}
|
|
gx = (int) ((pt.x - origin_.x)/resolution_);
|
|
gy = (int) ((pt.y - origin_.y)/resolution_);
|
|
|
|
if(gx >= width_ || gy >= height_){
|
|
gx = 0;
|
|
gy = 0;
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* @brief Converts cell coordinates to an index value that can be used to look up the correct grid cell
|
|
* @param gx The x coordinate of the cell
|
|
* @param gy The y coordinate of the cell
|
|
* @return The index of the cell in the stored cell list
|
|
*/
|
|
inline unsigned int gridIndex(unsigned int gx, unsigned int gy) const {
|
|
/*
|
|
* (0, 0) ---------------------- (width, 0)
|
|
* | |
|
|
* | |
|
|
* | |
|
|
* | |
|
|
* | |
|
|
* (0, height) ----------------- (width, height)
|
|
*/
|
|
return(gx + gy * width_);
|
|
}
|
|
|
|
/**
|
|
* @brief Check the orientation of a pt c with respect to the vector a->b
|
|
* @param a The start point of the vector
|
|
* @param b The end point of the vector
|
|
* @param c The point to compute orientation for
|
|
* @return orient(a, b, c) < 0 ----> Right, orient(a, b, c) > 0 ----> Left
|
|
*/
|
|
inline double orient(const robot_geometry_msgs::Point& a, const robot_geometry_msgs::Point& b, const robot_geometry_msgs::Point32& c){
|
|
double acx = a.x - c.x;
|
|
double bcx = b.x - c.x;
|
|
double acy = a.y - c.y;
|
|
double bcy = b.y - c.y;
|
|
return acx * bcy - acy * bcx;
|
|
}
|
|
|
|
/**
|
|
* @brief Check the orientation of a pt c with respect to the vector a->b
|
|
* @param a The start point of the vector
|
|
* @param b The end point of the vector
|
|
* @param c The point to compute orientation for
|
|
* @return orient(a, b, c) < 0 ----> Right, orient(a, b, c) > 0 ----> Left
|
|
*/
|
|
template<typename T>
|
|
inline double orient(const T& a, const T& b, const T& c){
|
|
double acx = a.x - c.x;
|
|
double bcx = b.x - c.x;
|
|
double acy = a.y - c.y;
|
|
double bcy = b.y - c.y;
|
|
return acx * bcy - acy * bcx;
|
|
}
|
|
|
|
/**
|
|
* @brief Check if two line segmenst intersect
|
|
* @param v1 The first point of the first segment
|
|
* @param v2 The second point of the first segment
|
|
* @param u1 The first point of the second segment
|
|
* @param u2 The second point of the second segment
|
|
* @return True if the segments intersect, false otherwise
|
|
*/
|
|
inline bool segIntersect(const robot_geometry_msgs::Point32& v1, const robot_geometry_msgs::Point32& v2,
|
|
const robot_geometry_msgs::Point32& u1, const robot_geometry_msgs::Point32& u2){
|
|
return (orient(v1, v2, u1) * orient(v1, v2, u2) < 0) && (orient(u1, u2, v1) * orient(u1, u2, v2) < 0);
|
|
}
|
|
|
|
/**
|
|
* @brief Find the intersection point of two lines
|
|
* @param v1 The first point of the first segment
|
|
* @param v2 The second point of the first segment
|
|
* @param u1 The first point of the second segment
|
|
* @param u2 The second point of the second segment
|
|
* @param result The point to be filled in
|
|
*/
|
|
void intersectionPoint(const robot_geometry_msgs::Point& v1, const robot_geometry_msgs::Point& v2,
|
|
const robot_geometry_msgs::Point& u1, const robot_geometry_msgs::Point& u2,
|
|
robot_geometry_msgs::Point& result);
|
|
|
|
/**
|
|
* @brief Check if a point is in a polygon
|
|
* @param pt The point to be checked
|
|
* @param poly The polygon to check against
|
|
* @return True if the point is in the polygon, false otherwise
|
|
*/
|
|
bool ptInPolygon(const robot_geometry_msgs::Point32& pt, const std::vector<robot_geometry_msgs::Point>& poly);
|
|
|
|
/**
|
|
* @brief Insert a point into the point grid
|
|
* @param pt The point to be inserted
|
|
*/
|
|
void insert(const robot_geometry_msgs::Point32& pt);
|
|
|
|
/**
|
|
* @brief Find the distance between a point and its nearest neighbor in the grid
|
|
* @param pt The point used for comparison
|
|
* @return The distance between the point passed in and its nearest neighbor in the point grid
|
|
*/
|
|
double nearestNeighborDistance(const robot_geometry_msgs::Point32& pt);
|
|
|
|
/**
|
|
* @brief Find the distance between a point and its nearest neighbor in a cell
|
|
* @param pt The point used for comparison
|
|
* @param gx The x coordinate of the cell
|
|
* @param gy The y coordinate of the cell
|
|
* @return The distance between the point passed in and its nearest neighbor in the cell
|
|
*/
|
|
double getNearestInCell(const robot_geometry_msgs::Point32& pt, unsigned int gx, unsigned int gy);
|
|
|
|
/**
|
|
* @brief Removes points from the grid that lie within the polygon
|
|
* @param poly A specification of the polygon to clear from the grid
|
|
*/
|
|
void removePointsInPolygon(const std::vector<robot_geometry_msgs::Point> poly);
|
|
|
|
/**
|
|
* @brief Removes points from the grid that lie within a laser scan
|
|
* @param laser_scan A specification of the laser scan to use for clearing
|
|
*/
|
|
void removePointsInScanBoundry(const PlanarLaserScan& laser_scan);
|
|
|
|
/**
|
|
* @brief Checks to see if a point is within a laser scan specification
|
|
* @param pt The point to check
|
|
* @param laser_scan The specification of the scan to check against
|
|
* @return True if the point is contained within the scan, false otherwise
|
|
*/
|
|
bool ptInScan(const robot_geometry_msgs::Point32& pt, const PlanarLaserScan& laser_scan);
|
|
|
|
/**
|
|
* @brief Get the points in the point grid
|
|
* @param cloud The point cloud to insert the points into
|
|
*/
|
|
void getPoints(robot_sensor_msgs::PointCloud2& cloud);
|
|
|
|
private:
|
|
double resolution_; ///< @brief The resolution of the grid in meters/cell
|
|
robot_geometry_msgs::Point origin_; ///< @brief The origin point of the grid
|
|
unsigned int width_; ///< @brief The width of the grid in cells
|
|
unsigned int height_; ///< @brief The height of the grid in cells
|
|
std::vector< std::list<robot_geometry_msgs::Point32> > cells_; ///< @brief Storage for the cells in the grid
|
|
double max_z_; ///< @brief The height cutoff for adding points as obstacles
|
|
double sq_obstacle_range_; ///< @brief The square distance at which we no longer add obstacles to the grid
|
|
double sq_min_separation_; ///< @brief The minimum square distance required between points in the grid
|
|
std::vector< std::list<robot_geometry_msgs::Point32>* > points_; ///< @brief The lists of points returned by a range search, made a member to save on memory allocation
|
|
};
|
|
}
|
|
#endif
|