Classes | Typedefs | Functions | Variables
occupancy_grid_utils Namespace Reference

Classes

struct  Cell
struct  CellOutOfBoundsException
 Exception for Cell off map. More...
struct  CellsInPolygon
struct  DataSizeException
 Exception when data field is not a vector of the right size. More...
class  DistanceField
struct  DistanceQueueItem
struct  GridUtilsException
 Base class for exceptions from this package. More...
struct  InflationQueueItem
struct  Line
struct  PointOutOfBoundsException
 Exception for point off map. More...
struct  PQItem
struct  QueueItem
class  RayTraceIterator
struct  ShortestPathResult
struct  TerminationCondition
 Termination condition for shortest path search max_dist is a distance such that as soon as we see a cell with greater than this distance, we stop. Note that, confusingly, max_dist is in cells though shortest path returns meters by default. Currently, we warn if use_cells is set to true below. In future (H-turtle), this api will actually change to only allow meters. goals is a set such that once we've expanded all the cells in this set, we stop. More...

Typedefs

typedef std::pair< Path, double > AStarResult
typedef std::set< CellCells
typedef int16_t coord_t
typedef boost::shared_ptr
< nm::OccupancyGrid const > 
GridConstPtr
typedef boost::shared_ptr
< nm::OccupancyGrid > 
GridPtr
typedef uint32_t index_t
typedef std::vector< CellPath
typedef std::pair
< RayTraceIterator,
RayTraceIterator
RayTraceIterRange
typedef boost::shared_ptr
< ShortestPathResult
ResultPtr

Functions

void addCloud (OverlayClouds *overlay, LocalizedCloud::ConstPtr cloud)
 Raytrace a cloud onto grid in overlay.
void addCloud (OverlayClouds *overlay, LocalizedCloud::ConstPtr cloud, const int inc)
void addKnownFreePoint (OverlayClouds *overlay, const geometry_msgs::Point &p, double r)
 Assert that a square centered at this point with side 2*r contains no obstacles.
void addKnownFreePoint (OverlayClouds *overlay, const gm::Point &p, const double r)
void allocateGrid (nav_msgs::OccupancyGrid &grid)
 BOOST_PYTHON_MODULE (occupancy_grid_utils)
geometry_msgs::Point cellCenter (const nav_msgs::MapMetaData &info, const Cell &c)
 Return center of a cell.
index_t cellIndex (const nav_msgs::MapMetaData &info, const Cell &c)
 Returns the index of a cell.
gm::Polygon cellPolygon (const nm::MapMetaData &info, const Cell &c)
geometry_msgs::Polygon cellPolygon (const nav_msgs::MapMetaData &info, const Cell &c)
 Return polygon corresponding to a cell.
std::set< CellcellsInConvexPolygon (const nav_msgs::MapMetaData &info, const geometry_msgs::Polygon &p)
Cells cellsInConvexPolygon (const nm::MapMetaData &info, const gm::Polygon &poly)
bool cellsIntersect (const nm::MapMetaData &info1, const Cell &c1, const nm::MapMetaData &info2, const Cell &c2)
vector< CellcellVectorInConvexPolygon (const nm::MapMetaData &info, const gm::Polygon &p)
bool cellWithinBounds (const nm::MapMetaData &info, const Cell &c)
Cell center (const nm::MapMetaData &info, const gm::Polygon &poly)
nav_msgs::OccupancyGrid::Ptr combineGrids (const std::vector< nav_msgs::OccupancyGrid::ConstPtr > &grids, double resolution)
 Combines a set of grids.
nav_msgs::OccupancyGrid::Ptr combineGrids (const std::vector< nav_msgs::OccupancyGrid::ConstPtr > &grids)
 Version of combineGrids that uses the resolution of the first grid.
GridPtr combineGrids (const vector< GridConstPtr > &grids, const double resolution)
GridPtr combineGrids (const vector< GridConstPtr > &grids)
bool containsVertex (const nm::MapMetaData &info, const Cell &c, const gm::Polygon &poly)
OverlayClouds createCloudOverlay (const nav_msgs::OccupancyGrid &grid, const std::string &frame_id, double occupancy_threshold=DEFAULT_OCCUPANCY_THRESHOLD, double max_distance=DEFAULT_MAX_DISTANCE, double min_pass_through=DEFAULT_MIN_PASS_THROUGH)
 Create a cloud overlay object to which clouds can then be added The returned (ros message) object should only be accessed using the api below.
OverlayClouds createCloudOverlay (const nm::OccupancyGrid &grid, const std::string &frame_id, double occupancy_threshold, double max_distance, double min_pass_through)
int8_t determineOccupancy (const unsigned hit_count, const unsigned pass_through_count, const double occupancy_threshold, const double min_pass_through)
boost::optional< double > distance (ResultPtr shortest_path_result, const Cell &dest)
 From result of single-source shortest paths, extract distance to some destination.
DistanceField distanceField (const nav_msgs::OccupancyGrid &m, float max_dist=-42)
boost::optional< double > distanceTo (ResultPtr shortest_path_result, const Cell &dest)
 From result of single-source shortest paths, extract distance to some destination.
double euclideanDistance (const gm::Point &p1, const gm::Point &p2)
gm::Polygon expandPolygon (const gm::Polygon &p, const double r)
void exportRosMessages ()
void exportSTL ()
boost::optional< PathextractPath (ResultPtr shortest_path_result, const Cell &dest)
 Extract a path from the result of single-source shortest paths.
template<class Visitor >
void flood_fill (const nm::MapMetaData &info, const std::set< Cell > &start, Visitor &vis)
template<class Visitor >
void flood_fill (const nm::MapMetaData &info, const Cell &start, Visitor &vis)
int getCell (const nav_msgs::OccupancyGrid &grid, const Cell &c)
nm::MapMetaData getCombinedGridInfo (const vector< GridConstPtr > &grids, const double resolution)
nav_msgs::OccupancyGrid::Ptr getGrid (const OverlayClouds &overlay)
 Get the current grid. It's fine to modify the returned object.
nav_msgs::MapMetaData gridInfo (const OverlayClouds &overlay)
gm::Polygon gridPolygon (const nm::MapMetaData &info)
geometry_msgs::Polygon gridPolygon (const nav_msgs::MapMetaData &info)
 Return polygon corresponding to grid bounds.
geometry_msgs::Pose identityPose ()
Cell indexCell (const nav_msgs::MapMetaData &info, index_t ind)
 Returns cell corresponding to index.
nav_msgs::OccupancyGrid::Ptr inflateObstacles (const nav_msgs::OccupancyGrid &g, double r, bool allow_unknown=false)
 Inflate obstacles in a grid.
GridPtr inflateObstacles (const nm::OccupancyGrid &g, const double r, const bool allow_unknown)
set< CellintersectingCells (const nm::MapMetaData &info, const nm::MapMetaData &info2, const Cell &cell2)
nav_msgs::OccupancyGrid::Ptr loadGrid (const std::string &fname, double resolution=1.0, const geometry_msgs::Pose &origin=identityPose())
 Load an occ grid from an image file.
nm::OccupancyGrid::Ptr loadGrid (const std::string &fname, const double resolution, const gm::Pose &origin_pose)
nm::OccupancyGrid::Ptr loadGrid1 (const string &fname)
nm::OccupancyGrid::Ptr loadGrid2 (const string &fname, const double resolution)
nm::OccupancyGrid::Ptr loadGrid3 (const string &fname, const double res, const gm::Pose &p)
void loadMapFromFile (nav_msgs::GetMap::Response *resp, const char *fname, double res, bool negate, double occ_th, double free_th, double *origin)
double manhattanHeuristic (const Cell &c1, const Cell &c2)
tf::Transform mapToWorld (const nav_msgs::MapMetaData &info)
double maxX (const nm::MapMetaData &info)
double maxY (const nm::MapMetaData &info)
double minX (const nm::MapMetaData &info)
double minY (const nm::MapMetaData &info)
bool myGt (const signed char x, const signed char y)
bool operator< (const QueueItem &i1, const QueueItem &i2)
std::ostream & operator<< (std::ostream &str, const Cell &c)
Cell point32Cell (const nm::MapMetaData &info, const gm::Point32 &p)
Cell pointCell (const nav_msgs::MapMetaData &info, const geometry_msgs::Point &p)
 Returns cell corresponding to a point.
index_t pointIndex (const nav_msgs::MapMetaData &info, const geometry_msgs::Point &p)
 Returns index of a point.
gm::Point rayEndPoint (const gm::Point &p0, const double theta, const double d)
RayTraceIterRange rayTrace (const Cell &c1, const Cell &c2)
RayTraceIterRange rayTrace (const nav_msgs::MapMetaData &info, const geometry_msgs::Point &p1, const geometry_msgs::Point &p2, bool project_target_onto_grid=false, bool project_source_onto_grid=false)
 Returns an iterator range over the cells on the line segment between two points (inclusive).
RayTraceIterRange rayTrace (const nm::MapMetaData &info, const gm::Point &p1, const gm::Point &p2, bool project_onto_grid, bool project_source_onto_grid)
optional< CellrayTraceOntoGrid (const nm::MapMetaData &info, const Cell &c1, const Cell &c2)
void removeCloud (OverlayClouds *overlay, LocalizedCloud::ConstPtr cloud)
 Effectively subtract a cloud (which was presumably previously added), by subtracting rather than adding counts, in overlay.
void resetCounts (OverlayClouds *overlay)
void setCell (nav_msgs::OccupancyGrid &grid, const Cell &c, const int x)
boost::optional< AStarResultshortestPath (const nav_msgs::OccupancyGrid &g, const Cell &src, const Cell &dest)
 A* search that returns distance in cells. Deprecated; use shortestPathAStar instead.using Manhattan distance cost and heuristic, with only horizontal and vertical neighbors.
optional< AStarResultshortestPath (const nm::OccupancyGrid &g, const Cell &src, const Cell &dest)
boost::optional< AStarResultshortestPathAStar (const nav_msgs::OccupancyGrid &g, const Cell &src, const Cell &dest)
 A* search using Manhattan distance cost and heuristic, with only horizontal and/// vertical neighbors.
optional< AStarResultshortestPathAStar (const nm::OccupancyGrid &g, const Cell &src, const Cell &dest)
ResultPtr shortestPathResultFromMessage (const NavigationFunction &msg)
 Convert a shortest path result from a ros message.
NavigationFunction shortestPathResultToMessage (ResultPtr res)
 Convert a shortest path result to a ros message.
int8_t sign (const coord_t x)
sensor_msgs::LaserScan::Ptr simulateRangeScan (const nav_msgs::OccupancyGrid &grid, const geometry_msgs::Pose &sensor_pose, const sensor_msgs::LaserScan &scanner_info, bool unknown_cells_are_obstacles=false)
 Simulate a planar laser range scan.
sm::LaserScan::Ptr simulateRangeScan (const nm::OccupancyGrid &grid, const gm::Pose &sensor_pose, const sm::LaserScan &scanner_info, const bool unknown_obstacles)
sm::LaserScan::Ptr simulateRangeScan3 (const nm::OccupancyGrid &grid, const gm::Pose &sensor_pose, const sm::LaserScan &scanner_info)
sm::LaserScan::Ptr simulateRangeScan4 (const nm::OccupancyGrid &grid, const gm::Pose &sensor_pose, const sm::LaserScan &scanner_info, const bool unknown_obstacles)
ResultPtr singleSourceShortestPaths (const nav_msgs::OccupancyGrid &g, const Cell &src, bool manhattan=false)
 Single source Dijkstra's algorithm.
ResultPtr singleSourceShortestPaths (const nav_msgs::OccupancyGrid &g, const Cell &src, const TerminationCondition &term, bool manhattan=false)
 Single source Dijkstra's algorithm.
ResultPtr singleSourceShortestPaths (const nm::OccupancyGrid &g, const Cell &src, const bool manhattan)
ResultPtr singleSourceShortestPaths (const nm::OccupancyGrid &g, const Cell &src, const TerminationCondition &t, const bool manhattan)
ResultPtr sssp1 (const nav_msgs::OccupancyGrid &g, const Cell &start, const double max_dist)
double ssspDistance (ResultPtr res, const Cell &dest)
template<typename Pred >
std::set< CelltileCells (const nm::MapMetaData &info, const float d, const Pred &pred)
template<typename Pred >
std::set< CelltileCells (const nav_msgs::MapMetaData &info, float d, const Pred &p)
gm::Pose transformPose (const tf::Pose trans, const gm::Pose p)
gm::Point transformPt (const tf::Pose &trans, const gm::Point32 &p)
void verifyDataSize (const nm::OccupancyGrid &g)
void verifyDataSize (const nav_msgs::OccupancyGrid &g)
 Verify that data vector has the right size, throw DataSizeException otherwise.
bool withinBounds (const nav_msgs::MapMetaData &info, const geometry_msgs::Point &p)
 Check if a point is on the grid.
bool withinBounds (const nav_msgs::MapMetaData &info, const Cell &c)
 Check if a cell is on the grid.
bool withinBoundsCell (const nm::MapMetaData &info, const Cell &c)
bool withinBoundsPoint (const nm::MapMetaData &info, const gm::Point &p)
tf::Transform worldToMap (const nav_msgs::MapMetaData &info)

Variables

const double DEFAULT_FREE_THRESHOLD = 0.196
const double DEFAULT_MAX_DISTANCE = 10.0
 Default max_distance for OverlayClouds.
const double DEFAULT_MIN_PASS_THROUGH = 2
 Default min_pass_through for OverlayClouds.
const double DEFAULT_OCC_THRESHOLD = 0.65
const double DEFAULT_OCCUPANCY_THRESHOLD = 0.1
 Default occupancy threshold for OverlayClouds.
const int8_t OCCUPIED = 100
const int8_t UNKNOWN = 255
const int8_t UNOCCUPIED = 0

Typedef Documentation

typedef std::pair< Path, double > occupancy_grid_utils::AStarResult

Definition at line 77 of file shortest_path.h.

Definition at line 44 of file geometry.hpp.

Definition at line 60 of file coordinate_conversions.h.

typedef boost::shared_ptr< nm::OccupancyGrid const > occupancy_grid_utils::GridConstPtr

Definition at line 66 of file combine_grids.cpp.

typedef boost::shared_ptr< nm::OccupancyGrid > occupancy_grid_utils::GridPtr

Definition at line 65 of file combine_grids.cpp.

Definition at line 59 of file coordinate_conversions.h.

typedef vector< Cell > occupancy_grid_utils::Path

Definition at line 76 of file shortest_path.h.

Definition at line 20 of file ray_tracer.h.

Definition at line 73 of file shortest_path.h.


Function Documentation

void occupancy_grid_utils::addCloud ( OverlayClouds *  overlay,
LocalizedCloud::ConstPtr  cloud 
)

Raytrace a cloud onto grid in overlay.

Definition at line 201 of file grid_overlay.cpp.

void occupancy_grid_utils::addCloud ( OverlayClouds *  overlay,
LocalizedCloud::ConstPtr  cloud,
const int  inc 
)

Definition at line 131 of file grid_overlay.cpp.

void occupancy_grid_utils::addKnownFreePoint ( OverlayClouds *  overlay,
const geometry_msgs::Point p,
double  r 
)

Assert that a square centered at this point with side 2*r contains no obstacles.

void occupancy_grid_utils::addKnownFreePoint ( OverlayClouds *  overlay,
const gm::Point p,
const double  r 
)

Definition at line 110 of file grid_overlay.cpp.

void occupancy_grid_utils::allocateGrid ( nav_msgs::OccupancyGrid &  grid)

Definition at line 130 of file boost_python_exports.cpp.

occupancy_grid_utils::BOOST_PYTHON_MODULE ( occupancy_grid_utils  )

Definition at line 213 of file boost_python_exports.cpp.

geometry_msgs::Point occupancy_grid_utils::cellCenter ( const nav_msgs::MapMetaData &  info,
const Cell c 
) [inline]

Return center of a cell.

Definition at line 167 of file coordinate_conversions.h.

index_t occupancy_grid_utils::cellIndex ( const nav_msgs::MapMetaData &  info,
const Cell c 
) [inline]

Returns the index of a cell.

Exceptions:
CellOutOfBoundsExceptionif cell isn't within grid bounds

Definition at line 122 of file coordinate_conversions.h.

gm::Polygon occupancy_grid_utils::cellPolygon ( const nm::MapMetaData &  info,
const Cell c 
)

Definition at line 47 of file coordinate_conversions.cpp.

geometry_msgs::Polygon occupancy_grid_utils::cellPolygon ( const nav_msgs::MapMetaData &  info,
const Cell c 
)

Return polygon corresponding to a cell.

std::set<Cell> occupancy_grid_utils::cellsInConvexPolygon ( const nav_msgs::MapMetaData &  info,
const geometry_msgs::Polygon &  p 
)
Return values:
Setof cells in the region bounded by convex polygon p

Cells that are partly in the polygon (because they intersect the boundary) are included. Cells that intersect the polygon only along an edge or corner may or may not be included.

Cells occupancy_grid_utils::cellsInConvexPolygon ( const nm::MapMetaData &  info,
const gm::Polygon &  poly 
)

Definition at line 197 of file geometry.cpp.

bool occupancy_grid_utils::cellsIntersect ( const nm::MapMetaData &  info1,
const Cell c1,
const nm::MapMetaData &  info2,
const Cell c2 
) [inline]

Definition at line 89 of file combine_grids.cpp.

vector<Cell> occupancy_grid_utils::cellVectorInConvexPolygon ( const nm::MapMetaData &  info,
const gm::Polygon &  p 
)

Definition at line 205 of file boost_python_exports.cpp.

bool occupancy_grid_utils::cellWithinBounds ( const nm::MapMetaData &  info,
const Cell c 
) [inline]

Definition at line 28 of file ray_tracer.cpp.

Cell occupancy_grid_utils::center ( const nm::MapMetaData &  info,
const gm::Polygon &  poly 
)

Definition at line 179 of file geometry.cpp.

nav_msgs::OccupancyGrid::Ptr occupancy_grid_utils::combineGrids ( const std::vector< nav_msgs::OccupancyGrid::ConstPtr > &  grids,
double  resolution 
)

Combines a set of grids.

The resulting grid's origin will be a translated version of grid 1's origin, with resolution resolution

The policy for combination is that for each cell, we look at each grid cell that intersects it and consider their values. Map these to integers, where everything above OCCUPIED (100) goes to -1. Then take the max. If there are no intersecting cells, value is -1.

Assumes all grids lie on the xy plane, and will fail in weird ways if that's not true

nav_msgs::OccupancyGrid::Ptr occupancy_grid_utils::combineGrids ( const std::vector< nav_msgs::OccupancyGrid::ConstPtr > &  grids)

Version of combineGrids that uses the resolution of the first grid.

GridPtr occupancy_grid_utils::combineGrids ( const vector< GridConstPtr > &  grids,
const double  resolution 
)

Definition at line 230 of file combine_grids.cpp.

Definition at line 261 of file combine_grids.cpp.

bool occupancy_grid_utils::containsVertex ( const nm::MapMetaData &  info,
const Cell c,
const gm::Polygon &  poly 
) [inline]

Definition at line 78 of file combine_grids.cpp.

OverlayClouds occupancy_grid_utils::createCloudOverlay ( const nav_msgs::OccupancyGrid &  grid,
const std::string &  frame_id,
double  occupancy_threshold = DEFAULT_OCCUPANCY_THRESHOLD,
double  max_distance = DEFAULT_MAX_DISTANCE,
double  min_pass_through = DEFAULT_MIN_PASS_THROUGH 
)

Create a cloud overlay object to which clouds can then be added The returned (ros message) object should only be accessed using the api below.

Parameters:
gridContains the geometry of the underlying grid. Additionally, iff the data field of this object is nonempty, rays will not be allowed to pass through obstacles in the grid.
frame_idThe frame name for the overlaid grid. Added clouds must have this frame_id
occupancy_thresholdwhat fraction of rays through the cell must end there for it to be considered an obstacle
max_distancehits beyond this distance from the source are ignored
min_pass_throughcells with fewer than this many rays through them are marked UNKNOWN
OverlayClouds occupancy_grid_utils::createCloudOverlay ( const nm::OccupancyGrid &  grid,
const std::string &  frame_id,
double  occupancy_threshold,
double  max_distance,
double  min_pass_through 
)

Definition at line 94 of file grid_overlay.cpp.

int8_t occupancy_grid_utils::determineOccupancy ( const unsigned  hit_count,
const unsigned  pass_through_count,
const double  occupancy_threshold,
const double  min_pass_through 
) [inline]

Definition at line 79 of file grid_overlay.cpp.

boost::optional< double > occupancy_grid_utils::distance ( ResultPtr  shortest_path_result,
const Cell dest 
)

From result of single-source shortest paths, extract distance to some destination.

deprecated

Return values:
Distanceif path exists, uninitialized otherwise

Definition at line 86 of file shortest_path.cpp.

DistanceField occupancy_grid_utils::distanceField ( const nav_msgs::OccupancyGrid &  m,
float  max_dist = -42 
)
Return values:
Distancefield d, such that d[c] is the Manhattan distance (in meters) from cell c to an obstacle cell.
Parameters:
max_distDistances will be thresholded at this value if it's positive

Definition at line 222 of file geometry.cpp.

boost::optional< double > occupancy_grid_utils::distanceTo ( ResultPtr  shortest_path_result,
const Cell dest 
)

From result of single-source shortest paths, extract distance to some destination.

Return values:
Distanceif path exists, uninitialized otherwise. In meters, not cells.

Definition at line 92 of file shortest_path.cpp.

double occupancy_grid_utils::euclideanDistance ( const gm::Point p1,
const gm::Point p2 
) [inline]

Definition at line 69 of file grid_overlay.cpp.

gm::Polygon occupancy_grid_utils::expandPolygon ( const gm::Polygon &  p,
const double  r 
) [inline]

Definition at line 96 of file combine_grids.cpp.

Definition at line 78 of file boost_python_exports.cpp.

Definition at line 70 of file boost_python_exports.cpp.

boost::optional< Path > occupancy_grid_utils::extractPath ( ResultPtr  shortest_path_result,
const Cell dest 
)

Extract a path from the result of single-source shortest paths.

Return values:
pathIf path exists, vector of cells where the source is first and dest is last; if not, uninitialized

Definition at line 102 of file shortest_path.cpp.

template<class Visitor >
void occupancy_grid_utils::flood_fill ( const nm::MapMetaData &  info,
const std::set< Cell > &  start,
Visitor &  vis 
)

Definition at line 129 of file geometry.cpp.

template<class Visitor >
void occupancy_grid_utils::flood_fill ( const nm::MapMetaData &  info,
const Cell start,
Visitor &  vis 
)

Definition at line 170 of file geometry.cpp.

int occupancy_grid_utils::getCell ( const nav_msgs::OccupancyGrid &  grid,
const Cell c 
)

Definition at line 140 of file boost_python_exports.cpp.

nm::MapMetaData occupancy_grid_utils::getCombinedGridInfo ( const vector< GridConstPtr > &  grids,
const double  resolution 
)

Definition at line 182 of file combine_grids.cpp.

GridPtr occupancy_grid_utils::getGrid ( const OverlayClouds &  overlay)

Get the current grid. It's fine to modify the returned object.

Definition at line 213 of file grid_overlay.cpp.

nm::MapMetaData occupancy_grid_utils::gridInfo ( const OverlayClouds &  overlay)

Definition at line 232 of file grid_overlay.cpp.

gm::Polygon occupancy_grid_utils::gridPolygon ( const nm::MapMetaData &  info)

Definition at line 66 of file coordinate_conversions.cpp.

geometry_msgs::Polygon occupancy_grid_utils::gridPolygon ( const nav_msgs::MapMetaData &  info)

Return polygon corresponding to grid bounds.

Definition at line 49 of file file.h.

Cell occupancy_grid_utils::indexCell ( const nav_msgs::MapMetaData &  info,
index_t  ind 
) [inline]

Returns cell corresponding to index.

Definition at line 130 of file coordinate_conversions.h.

nav_msgs::OccupancyGrid::Ptr occupancy_grid_utils::inflateObstacles ( const nav_msgs::OccupancyGrid &  g,
double  r,
bool  allow_unknown = false 
)

Inflate obstacles in a grid.

Obstacles are cell values that are not 0, unless allow_unknown is true, in which case -1 is not considered an obstacle. If there exist obstacles within radius r meters of a cell, its value is replaced by the max of their values (-1 is considered to be the same as 1 for this). Rounds up when converting from meters to cells

GridPtr occupancy_grid_utils::inflateObstacles ( const nm::OccupancyGrid &  g,
const double  r,
const bool  allow_unknown 
)

Definition at line 233 of file shortest_path.cpp.

set<Cell> occupancy_grid_utils::intersectingCells ( const nm::MapMetaData &  info,
const nm::MapMetaData &  info2,
const Cell cell2 
)

Definition at line 121 of file combine_grids.cpp.

nav_msgs::OccupancyGrid::Ptr occupancy_grid_utils::loadGrid ( const std::string &  fname,
double  resolution = 1.0,
const geometry_msgs::Pose origin = identityPose() 
)

Load an occ grid from an image file.

Parameters:
fnameFilename
resolutionin m/cell
originLower-left corner of image (only 2d-components are used)

Pixel values are considered occupied if they're above DEFAULT_OCC_THRESHOLD, free if below DEFAULT_FREE_THRESHOLD, unknown otherwise

nm::OccupancyGrid::Ptr occupancy_grid_utils::loadGrid ( const std::string &  fname,
const double  resolution,
const gm::Pose origin_pose 
)

Definition at line 155 of file file.cpp.

nm::OccupancyGrid::Ptr occupancy_grid_utils::loadGrid1 ( const string &  fname)

Definition at line 145 of file boost_python_exports.cpp.

nm::OccupancyGrid::Ptr occupancy_grid_utils::loadGrid2 ( const string &  fname,
const double  resolution 
)

Definition at line 150 of file boost_python_exports.cpp.

nm::OccupancyGrid::Ptr occupancy_grid_utils::loadGrid3 ( const string &  fname,
const double  res,
const gm::Pose p 
)

Definition at line 155 of file boost_python_exports.cpp.

void occupancy_grid_utils::loadMapFromFile ( nav_msgs::GetMap::Response *  resp,
const char *  fname,
double  res,
bool  negate,
double  occ_th,
double  free_th,
double *  origin 
)

Definition at line 71 of file file.cpp.

double occupancy_grid_utils::manhattanHeuristic ( const Cell c1,
const Cell c2 
) [inline]

Definition at line 317 of file shortest_path.cpp.

tf::Transform occupancy_grid_utils::mapToWorld ( const nav_msgs::MapMetaData &  info) [inline]

Definition at line 138 of file coordinate_conversions.h.

double occupancy_grid_utils::maxX ( const nm::MapMetaData &  info)

Definition at line 154 of file combine_grids.cpp.

double occupancy_grid_utils::maxY ( const nm::MapMetaData &  info)

Definition at line 166 of file combine_grids.cpp.

double occupancy_grid_utils::minX ( const nm::MapMetaData &  info)

Definition at line 148 of file combine_grids.cpp.

double occupancy_grid_utils::minY ( const nm::MapMetaData &  info)

Definition at line 160 of file combine_grids.cpp.

bool occupancy_grid_utils::myGt ( const signed char  x,
const signed char  y 
) [inline]

Definition at line 219 of file shortest_path.cpp.

bool occupancy_grid_utils::operator< ( const QueueItem &  i1,
const QueueItem &  i2 
)

Definition at line 137 of file shortest_path.cpp.

std::ostream& occupancy_grid_utils::operator<< ( std::ostream &  str,
const Cell c 
) [inline]

Definition at line 189 of file coordinate_conversions.h.

Cell occupancy_grid_utils::point32Cell ( const nm::MapMetaData &  info,
const gm::Point32 &  p 
) [inline]

Definition at line 68 of file combine_grids.cpp.

Cell occupancy_grid_utils::pointCell ( const nav_msgs::MapMetaData &  info,
const geometry_msgs::Point p 
) [inline]

Returns cell corresponding to a point.

The z coordinate of the point is ignored.

Definition at line 152 of file coordinate_conversions.h.

index_t occupancy_grid_utils::pointIndex ( const nav_msgs::MapMetaData &  info,
const geometry_msgs::Point p 
) [inline]

Returns index of a point.

Exceptions:
CellOutOfBoundsExceptionif point isn't within grid bounds

Ignores z coordinate of point

Definition at line 161 of file coordinate_conversions.h.

gm::Point occupancy_grid_utils::rayEndPoint ( const gm::Point p0,
const double  theta,
const double  d 
)

Definition at line 83 of file ray_tracer.cpp.

RayTraceIterRange occupancy_grid_utils::rayTrace ( const Cell c1,
const Cell c2 
)

Definition at line 22 of file ray_tracer.cpp.

RayTraceIterRange occupancy_grid_utils::rayTrace ( const nav_msgs::MapMetaData &  info,
const geometry_msgs::Point p1,
const geometry_msgs::Point p2,
bool  project_target_onto_grid = false,
bool  project_source_onto_grid = false 
)

Returns an iterator range over the cells on the line segment between two points (inclusive).

Parameters:
project_target_onto_gridIf true, p2 may be off the grid, in which case the ray stops right before falling off the grid
project_source_onto_gridIf true, p1 may be off the grid, in which case the ray starts at the point where it enters the grid
Exceptions:
PointOutOfBoundsExceptionif p1 is off grid and project_source_onto_grid is false
PointOutOfBoundsExceptionif p2 is off grid and project_target_onto_grid is false
Return values:
rangemodels Forward range with reference type const Cell&, i.e., it's a pair of iterators that you can use in for loops, std::for_each, etc.
RayTraceIterRange occupancy_grid_utils::rayTrace ( const nm::MapMetaData &  info,
const gm::Point p1,
const gm::Point p2,
bool  project_onto_grid,
bool  project_source_onto_grid 
)

Definition at line 43 of file ray_tracer.cpp.

optional<Cell> occupancy_grid_utils::rayTraceOntoGrid ( const nm::MapMetaData &  info,
const Cell c1,
const Cell c2 
)

Definition at line 33 of file ray_tracer.cpp.

void occupancy_grid_utils::removeCloud ( OverlayClouds *  overlay,
LocalizedCloud::ConstPtr  cloud 
)

Effectively subtract a cloud (which was presumably previously added), by subtracting rather than adding counts, in overlay.

Definition at line 206 of file grid_overlay.cpp.

void occupancy_grid_utils::resetCounts ( OverlayClouds *  overlay)

Definition at line 226 of file grid_overlay.cpp.

void occupancy_grid_utils::setCell ( nav_msgs::OccupancyGrid &  grid,
const Cell c,
const int  x 
)

Definition at line 135 of file boost_python_exports.cpp.

boost::optional<AStarResult> occupancy_grid_utils::shortestPath ( const nav_msgs::OccupancyGrid &  g,
const Cell src,
const Cell dest 
)

A* search that returns distance in cells. Deprecated; use shortestPathAStar instead.using Manhattan distance cost and heuristic, with only horizontal and vertical neighbors.

Returns:
If a path is not found, an uninitialized optional. Else, the path and the cost. Currently, though, the path is not returned, just the cost.
optional<AStarResult> occupancy_grid_utils::shortestPath ( const nm::OccupancyGrid &  g,
const Cell src,
const Cell dest 
)

Definition at line 403 of file shortest_path.cpp.

boost::optional<AStarResult> occupancy_grid_utils::shortestPathAStar ( const nav_msgs::OccupancyGrid &  g,
const Cell src,
const Cell dest 
)

A* search using Manhattan distance cost and heuristic, with only horizontal and/// vertical neighbors.

Returns:
If a path is not found, an uninitialized optional. Else, the path and the cost (meters). Currently, though, the path is not returned, just the cost.
optional<AStarResult> occupancy_grid_utils::shortestPathAStar ( const nm::OccupancyGrid &  g,
const Cell src,
const Cell dest 
)

Definition at line 323 of file shortest_path.cpp.

Convert a shortest path result from a ros message.

Definition at line 468 of file shortest_path.cpp.

NavigationFunction occupancy_grid_utils::shortestPathResultToMessage ( ResultPtr  res)

Convert a shortest path result to a ros message.

Definition at line 503 of file shortest_path.cpp.

int8_t occupancy_grid_utils::sign ( const coord_t  x) [inline]

Definition at line 12 of file ray_trace_iterator.h.

sensor_msgs::LaserScan::Ptr occupancy_grid_utils::simulateRangeScan ( const nav_msgs::OccupancyGrid &  grid,
const geometry_msgs::Pose sensor_pose,
const sensor_msgs::LaserScan &  scanner_info,
bool  unknown_cells_are_obstacles = false 
)

Simulate a planar laser range scan.

Parameters:
gridOccupancy grid
sensor_poseAssumed to lie on the grid and point along the grid
scanner_infoOnly the angle_{min,max,increment} and range_max fields of this are used.
unknown_cells_are_obstaclesIf true, rays that hit unknown space are considered to have hit an obstacle rather than being max-range
Return values:
Simulatedlaser range scan from the given pose on the grid
sm::LaserScan::Ptr occupancy_grid_utils::simulateRangeScan ( const nm::OccupancyGrid &  grid,
const gm::Pose sensor_pose,
const sm::LaserScan &  scanner_info,
const bool  unknown_obstacles 
)

Definition at line 92 of file ray_tracer.cpp.

sm::LaserScan::Ptr occupancy_grid_utils::simulateRangeScan3 ( const nm::OccupancyGrid &  grid,
const gm::Pose sensor_pose,
const sm::LaserScan &  scanner_info 
)

Definition at line 162 of file boost_python_exports.cpp.

sm::LaserScan::Ptr occupancy_grid_utils::simulateRangeScan4 ( const nm::OccupancyGrid &  grid,
const gm::Pose sensor_pose,
const sm::LaserScan &  scanner_info,
const bool  unknown_obstacles 
)

Definition at line 169 of file boost_python_exports.cpp.

ResultPtr occupancy_grid_utils::singleSourceShortestPaths ( const nav_msgs::OccupancyGrid &  g,
const Cell src,
bool  manhattan = false 
)

Single source Dijkstra's algorithm.

Return values:
Structurefrom which paths can be extracted or distances computed
Parameters:
manhattanIf false, Euclidean

Each cell is connected to its horizontal and vertical neighbors, with cost 1, and diagonal neighbors with cost sqrt(2). Only cells with value UNOCCUPIED are considered.

ResultPtr occupancy_grid_utils::singleSourceShortestPaths ( const nav_msgs::OccupancyGrid &  g,
const Cell src,
const TerminationCondition &  term,
bool  manhattan = false 
)

Single source Dijkstra's algorithm.

Return values:
Structurefrom which paths can be extracted or distances computed
Parameters:
termallows stopping early when a distance bound is reached or when a a set of goal cells have been found
manhattanIf false, Euclidean

Each cell is connected to its horizontal and vertical neighbors, with cost 1, and diagonal neighbors with cost sqrt(2). Only cells with value UNOCCUPIED are considered.

ResultPtr occupancy_grid_utils::singleSourceShortestPaths ( const nm::OccupancyGrid &  g,
const Cell src,
const bool  manhattan 
)

Definition at line 143 of file shortest_path.cpp.

ResultPtr occupancy_grid_utils::singleSourceShortestPaths ( const nm::OccupancyGrid &  g,
const Cell src,
const TerminationCondition &  t,
const bool  manhattan 
)

Definition at line 149 of file shortest_path.cpp.

ResultPtr occupancy_grid_utils::sssp1 ( const nav_msgs::OccupancyGrid &  g,
const Cell start,
const double  max_dist 
)

Definition at line 187 of file boost_python_exports.cpp.

double occupancy_grid_utils::ssspDistance ( ResultPtr  res,
const Cell dest 
)

Definition at line 194 of file boost_python_exports.cpp.

template<typename Pred >
std::set<Cell> occupancy_grid_utils::tileCells ( const nm::MapMetaData &  info,
const float  d,
const Pred &  pred 
)

Definition at line 47 of file geometry.hpp.

template<typename Pred >
std::set<Cell> occupancy_grid_utils::tileCells ( const nav_msgs::MapMetaData &  info,
float  d,
const Pred &  p 
)
Return values:
Locallymaximal set of cells which are at least d apart in Euclidean distance, and all of which satisfy pred
Template Parameters:
predDefines operator(), a boolean predicate on Cell objects

Definition at line 172 of file combine_grids.cpp.

gm::Point occupancy_grid_utils::transformPt ( const tf::Pose trans,
const gm::Point32 &  p 
) [inline]

Definition at line 61 of file grid_overlay.cpp.

void occupancy_grid_utils::verifyDataSize ( const nm::OccupancyGrid &  g)

Definition at line 85 of file coordinate_conversions.cpp.

void occupancy_grid_utils::verifyDataSize ( const nav_msgs::OccupancyGrid &  g)

Verify that data vector has the right size, throw DataSizeException otherwise.

bool occupancy_grid_utils::withinBounds ( const nav_msgs::MapMetaData &  info,
const geometry_msgs::Point p 
) [inline]

Check if a point is on the grid.

Definition at line 196 of file coordinate_conversions.h.

bool occupancy_grid_utils::withinBounds ( const nav_msgs::MapMetaData &  info,
const Cell c 
) [inline]

Check if a cell is on the grid.

Definition at line 202 of file coordinate_conversions.h.

bool occupancy_grid_utils::withinBoundsCell ( const nm::MapMetaData &  info,
const Cell c 
)

Definition at line 176 of file boost_python_exports.cpp.

bool occupancy_grid_utils::withinBoundsPoint ( const nm::MapMetaData &  info,
const gm::Point p 
)

Definition at line 182 of file boost_python_exports.cpp.

tf::Transform occupancy_grid_utils::worldToMap ( const nav_msgs::MapMetaData &  info) [inline]

Definition at line 146 of file coordinate_conversions.h.


Variable Documentation

Definition at line 58 of file file.h.

Default max_distance for OverlayClouds.

Definition at line 66 of file ray_tracer.h.

Default min_pass_through for OverlayClouds.

Definition at line 69 of file ray_tracer.h.

Definition at line 57 of file file.h.

Default occupancy threshold for OverlayClouds.

Definition at line 63 of file ray_tracer.h.

const int8_t occupancy_grid_utils::OCCUPIED = 100

Definition at line 75 of file coordinate_conversions.h.

const int8_t occupancy_grid_utils::UNKNOWN = 255

Definition at line 76 of file coordinate_conversions.h.

Definition at line 74 of file coordinate_conversions.h.



occupancy_grid_utils
Author(s): Bhaskara Marthi
autogenerated on Thu Dec 12 2013 13:17:54