Gazebo Math

API Reference

9.0.0~pre1
OccupancyGrid Class Reference

Class representing an occupancy grid. More...

#include <gz/math/OccupancyGrid.hh>

Public Member Functions

 OccupancyGrid (double _resolutionMeters, int _widthCells, int _heightCells, double _originX=0.0, double _originY=0.0)
 Constructor.
 
int CalculateIGain (int _x0, int _y0, int _x1, int _y1)
 Calculate information gain along a line. This is useful when implementing safe frontier exploration algorithms.
 
OccupancyCellState CellState (int _gridX, int _gridY) const
 Get the state of a cell.
 
void CellState (int _gridX, int _gridY, OccupancyCellState _state)
 Set the state of a cell.
 
void ExportToRGBImage (std::vector< uint8_t > &_pixels) const
 Export the occupancy grid to a RGB image buffer.
 
void GridToWorld (int _gridX, int _gridY, double &_worldX, double &_worldY) const
 Convert grid coordinates (cells) to world coordinates (meters - center of cell).
 
int Height () const
 Get the number of cells in height.
 
bool IsValidGridCoordinate (int _gridX, int _gridY) const
 Check if grid coordinates are within bounds.
 
bool MarkFree (double _worldX0, double _worldY0, double _worldX1, double _worldY1)
 Mark a path as free (e.g., a clear line of sight).
 
void MarkLine (int _x0, int _y0, int _x1, int _y1, OccupancyCellState _state)
 Use Bresenham's Line Algorithm to mark cells along a line. This function will not modify cells that are already marked as Occupied. It marks cells from (_x0, _y0) to (_x1, _y1) with the specified state.
 
bool MarkOccupied (double _worldX, double _worldY)
 Mark a single point as occupied (e.g., an obstacle detection).
 
double OriginX () const
 Get the origin X position.
 
double OriginY () const
 Get the origin Y position.
 
void RawOccupancy (std::vector< int8_t > &_data) const
 Export the occupancy grid to a raw buffer.
 
double Resolution () const
 Get the resolution of the occupancy grid.
 
int Width () const
 Get the number of cells in width.
 
bool WorldToGrid (double _worldX, double _worldY, int &_gridX, int &_gridY) const
 Convert world coordinates (meters) to grid coordinates (cells).
 

Detailed Description

Class representing an occupancy grid.

Constructor & Destructor Documentation

◆ OccupancyGrid()

OccupancyGrid ( double  _resolutionMeters,
int  _widthCells,
int  _heightCells,
double  _originX = 0.0,
double  _originY = 0.0 
)

Constructor.

Parameters
[in]_resolutionMetersResolution of the grid in meters per cell.
[in]_widthCellsWidth of the grid in cells.
[in]_heightCellsHeight of the grid in cells.
[in]_originXX-coordinate of the grid's origin in world coordinates.
[in]_originYY-coordinate of the grid's origin in world coordinates.

Member Function Documentation

◆ CalculateIGain()

int CalculateIGain ( int  _x0,
int  _y0,
int  _x1,
int  _y1 
)

Calculate information gain along a line. This is useful when implementing safe frontier exploration algorithms.

Parameters
[in]_x0X coordinate of the start point in cells.
[in]_y0Y coordinate of the start point in cells.
[in]_x1X coordinate of the end point in cells.
[in]_y1Y coordinate of the end point in cells.
Returns
The information gain.

◆ CellState() [1/2]

OccupancyCellState CellState ( int  _gridX,
int  _gridY 
) const

Get the state of a cell.

Parameters
[in]_gridXGrid X coordinate in cells.
[in]_gridYGrid Y coordinate in cells.
Returns
The state of the cell.

◆ CellState() [2/2]

void CellState ( int  _gridX,
int  _gridY,
OccupancyCellState  _state 
)

Set the state of a cell.

Parameters
[in]_gridXGrid X coordinate in cells.
[in]_gridYGrid Y coordinate in cells.
[in]_stateThe new state of the cell.

◆ ExportToRGBImage()

void ExportToRGBImage ( std::vector< uint8_t > &  _pixels) const

Export the occupancy grid to a RGB image buffer.

Parameters
[out]_pixelsThe output buffer to store the RGB image data.

◆ GridToWorld()

void GridToWorld ( int  _gridX,
int  _gridY,
double _worldX,
double _worldY 
) const

Convert grid coordinates (cells) to world coordinates (meters - center of cell).

Parameters
[in]_gridXGrid X coordinate in cells.
[in]_gridYGrid Y coordinate in cells.
[out]_worldXWorld X coordinate in meters.
[out]_worldYWorld Y coordinate in meters.

◆ Height()

int Height ( ) const

Get the number of cells in height.

Returns
The height of the grid in cells.

◆ IsValidGridCoordinate()

bool IsValidGridCoordinate ( int  _gridX,
int  _gridY 
) const

Check if grid coordinates are within bounds.

Parameters
[in]_gridXGrid X coordinate in cells.
[in]_gridYGrid Y coordinate in cells.
Returns
True if the coordinates are valid, false otherwise.

◆ MarkFree()

bool MarkFree ( double  _worldX0,
double  _worldY0,
double  _worldX1,
double  _worldY1 
)

Mark a path as free (e.g., a clear line of sight).

Parameters
[in]_worldX0World X coordinate of the start point in meters.
[in]_worldY0World Y coordinate of the start point in meters.
[in]_worldX1World X coordinate of the end point in meters.
[in]_worldY1World Y coordinate of the end point in meters.
Returns
true if the grid was inside the coordinates, false otherwise

◆ MarkLine()

void MarkLine ( int  _x0,
int  _y0,
int  _x1,
int  _y1,
OccupancyCellState  _state 
)

Use Bresenham's Line Algorithm to mark cells along a line. This function will not modify cells that are already marked as Occupied. It marks cells from (_x0, _y0) to (_x1, _y1) with the specified state.

Parameters
[in]_x0X coordinate of the start point in cells.
[in]_y0Y coordinate of the start point in cells.
[in]_x1X coordinate of the end point in cells.
[in]_y1Y coordinate of the end point in cells.
[in]_stateThe state to mark the cells with.

◆ MarkOccupied()

bool MarkOccupied ( double  _worldX,
double  _worldY 
)

Mark a single point as occupied (e.g., an obstacle detection).

Parameters
[in]_worldXWorld X coordinate in meters.
[in]_worldYWorld Y coordinate in meters.
Returns
true if the grid was inside the coordinates, false otherwise

◆ OriginX()

double OriginX ( ) const

Get the origin X position.

Returns
The X-coordinate of the grid's origin in world coordinates.

◆ OriginY()

double OriginY ( ) const

Get the origin Y position.

Returns
The Y-coordinate of the grid's origin in world coordinates.

◆ RawOccupancy()

void RawOccupancy ( std::vector< int8_t > &  _data) const

Export the occupancy grid to a raw buffer.

Parameters
[out]_dataThe output buffer to store the raw occupancy data.

◆ Resolution()

double Resolution ( ) const

Get the resolution of the occupancy grid.

Returns
The resolution in meters per cell.

◆ Width()

int Width ( ) const

Get the number of cells in width.

Returns
The width of the grid in cells.

◆ WorldToGrid()

bool WorldToGrid ( double  _worldX,
double  _worldY,
int _gridX,
int _gridY 
) const

Convert world coordinates (meters) to grid coordinates (cells).

Parameters
[in]_worldXWorld X coordinate in meters.
[in]_worldYWorld Y coordinate in meters.
[out]_gridXGrid X coordinate in cells.
[out]_gridYGrid Y coordinate in cells.
Returns
True if coordinates are within bounds, false otherwise. May also

The documentation for this class was generated from the following file: