A 2D costmap provides a mapping between points in the world and their associated "costs". More...

#include <costmap_2d.hpp>

Inheritance diagram for Costmap2D:

Collaboration diagram for Costmap2D:

[legend]

Classes
class	MarkCell

class	PolygonOutlineCells

Public Types
typedef std::recursive_mutex	mutex_t

Public Member Functions
unsigned int	cellDistance (double world_dist)
	Given distance in the world... convert it to cells.

void	convexFillCells (const std::vector< MapLocation > &polygon, std::vector< MapLocation > &polygon_cells)
	Get the map cells that fill a convex polygon.

bool	copyCostmapWindow (const Costmap2D &map, double win_origin_x, double win_origin_y, double win_size_x, double win_size_y)
	Turn this costmap into a copy of a window of a costmap passed in.

bool	copyWindow (const Costmap2D &source, unsigned int sx0, unsigned int sy0, unsigned int sxn, unsigned int syn, unsigned int dx0, unsigned int dy0)
	Copies the (x0,y0)..(xn,yn) window from source costmap into a current costmap.

	Costmap2D ()
	Default constructor.

	Costmap2D (const Costmap2D &map)
	Copy constructor for a costmap, creates a copy efficiently.

	Costmap2D (const nav_msgs::msg::OccupancyGrid &map)
	Constructor for a costmap from an OccupancyGrid map.

	Costmap2D (unsigned int cells_size_x, unsigned int cells_size_y, double resolution, double origin_x, double origin_y, unsigned char default_value=0)
	Constructor for a costmap.

unsigned char *	getCharMap () const
	Will return a pointer to the underlying unsigned char array used as the costmap.

unsigned char	getCost (unsigned int index) const
	Get the cost of a cell in the costmap.

unsigned char	getCost (unsigned int mx, unsigned int my) const
	Get the cost of a cell in the costmap.

unsigned char	getDefaultValue ()
	Get the default background value of the costmap.

unsigned int	getIndex (unsigned int mx, unsigned int my) const
	Given two map coordinates... compute the associated index.

bool	getMapRegionOccupiedByPolygon (const std::vector< geometry_msgs::msg::Point > &polygon, std::vector< MapLocation > &polygon_map_region)
	Gets the map region occupied by polygon.

mutex_t *	getMutex ()

double	getOriginX () const
	Accessor for the x origin of the costmap.

double	getOriginY () const
	Accessor for the y origin of the costmap.

double	getResolution () const
	Accessor for the resolution of the costmap.

unsigned int	getSizeInCellsX () const
	Accessor for the x size of the costmap in cells.

unsigned int	getSizeInCellsY () const
	Accessor for the y size of the costmap in cells.

double	getSizeInMetersX () const
	Accessor for the x size of the costmap in meters.

double	getSizeInMetersY () const
	Accessor for the y size of the costmap in meters.

bool	inBounds (unsigned int x, unsigned int y) const
	Checks whether a given cell coordinate is within the bounds of the costmap.

void	indexToCells (unsigned int index, unsigned int &mx, unsigned int &my) const
	Given an index... compute the associated map coordinates.

void	mapToWorld (unsigned int mx, unsigned int my, double &wx, double &wy) const
	Convert from map coordinates to world coordinates.

void	mapToWorldNoBounds (int mx, int my, double &wx, double &wy) const
	Convert from map coordinates to world coordinates with no bounds checking.

Costmap2D &	operator= (const Costmap2D &map)
	Overloaded assignment operator.

void	polygonOutlineCells (const std::vector< MapLocation > &polygon, std::vector< MapLocation > &polygon_cells)
	Get the map cells that make up the outline of a polygon.

void	resetMap (unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
	Reset the costmap in bounds.

void	resetMapToValue (unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn, unsigned char value)
	Reset the costmap in bounds to a value.

void	resizeMap (unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y)
	Resize the costmap.

void	restoreMapRegionOccupiedByPolygon (const std::vector< MapLocation > &polygon_map_region)
	Restores the corresponding map region using given map region.

bool	setConvexPolygonCost (const std::vector< geometry_msgs::msg::Point > &polygon, unsigned char cost_value)
	Sets the cost of a convex polygon to a desired value.

void	setCost (unsigned int mx, unsigned int my, unsigned char cost)
	Set the cost of a cell in the costmap.

void	setDefaultValue (unsigned char c)
	Set the default background value of the costmap.

void	setMapRegionOccupiedByPolygon (const std::vector< MapLocation > &polygon_map_region, unsigned char new_cost_value)
	Sets the given map region to desired value.

void	toOccupancyGridMsg (nav_msgs::msg::OccupancyGrid &msg) const
	Converts the current costmap into a nav_msgs::msg::OccupancyGrid message.

virtual void	updateOrigin (double new_origin_x, double new_origin_y)
	Move the origin of the costmap to a new location....

bool	worldToMap (double wx, double wy, unsigned int &mx, unsigned int &my) const
	Convert from world coordinates to map coordinates.

bool	worldToMapContinuous (double wx, double wy, float &mx, float &my) const
	Convert from world coordinates to map coordinates.

void	worldToMapEnforceBounds (double wx, double wy, int &mx, int &my) const
	Convert from world coordinates to map coordinates, constraining results to legal bounds.

void	worldToMapNoBounds (double wx, double wy, int &mx, int &my) const
	Convert from world coordinates to map coordinates without checking for legal bounds.

virtual	~Costmap2D ()
	Destructor.

Protected Member Functions
template<typename data_type>
void	copyMapRegion (data_type source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y, unsigned int sm_size_x, data_type dest_map, unsigned int dm_lower_left_x, unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x, unsigned int region_size_y)
	Copy a region of a source map into a destination map.

virtual void	deleteMaps ()
	Deletes the costmap, static_map, and markers data structures.

virtual void	initMaps (unsigned int size_x, unsigned int size_y)
	Initializes the costmap, static_map, and markers data structures.

template<class ActionType>
void	raytraceLine (ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, unsigned int max_length=UINT_MAX, unsigned int min_length=0)
	Raytrace a line and apply some action at each step.

virtual void	resetMaps ()
	Resets the costmap and static_map to be unknown space.

Protected Attributes
unsigned char *	costmap_

unsigned char	default_value_

double	origin_x_

double	origin_y_

double	resolution_

unsigned int	size_x_

unsigned int	size_y_

Friends
class	CostmapTester

Detailed Description

A 2D costmap provides a mapping between points in the world and their associated "costs".

Member Typedef Documentation

◆ mutex_t

typedef std::recursive_mutex mutex_t

Constructor & Destructor Documentation

◆ Costmap2D() [1/4]

Costmap2D	(	unsigned int	cells_size_x,
		unsigned int	cells_size_y,
		double	resolution,
		double	origin_x,
		double	origin_y,
		unsigned char	default_value = 0 )

Constructor for a costmap.

Parameters

cells_size_x	The x size of the map in cells
cells_size_y	The y size of the map in cells
resolution	The resolution of the map in meters/cell
origin_x	The x origin of the map
origin_y	The y origin of the map
default_value	Default Value

◆ Costmap2D() [2/4]

Costmap2D ( const Costmap2D & map )

Copy constructor for a costmap, creates a copy efficiently.

Parameters

map	The costmap to copy

◆ Costmap2D() [3/4]

Costmap2D ( const nav_msgs::msg::OccupancyGrid & map )

explicit

Constructor for a costmap from an OccupancyGrid map.

Parameters

map	The OccupancyGrid map to create costmap from

◆ Costmap2D() [4/4]

Costmap2D ( )

Default constructor.

◆ ~Costmap2D()

~Costmap2D ( )

virtual

Destructor.

Member Function Documentation

◆ cellDistance()

unsigned int cellDistance ( double world_dist )

Given distance in the world... convert it to cells.

Parameters

world_dist The world distance

Returns: The equivalent cell distance

◆ convexFillCells()

void convexFillCells	(	const std::vector< MapLocation > &	polygon,
		std::vector< MapLocation > &	polygon_cells )

Get the map cells that fill a convex polygon.

Parameters

polygon	The polygon in map coordinates to rasterize
polygon_cells	Will be set to the cells that fill the polygon

◆ copyCostmapWindow()

bool copyCostmapWindow	(	const Costmap2D &	map,
		double	win_origin_x,
		double	win_origin_y,
		double	win_size_x,
		double	win_size_y )

Turn this costmap into a copy of a window of a costmap passed in.

Parameters

map	The costmap to copy
win_origin_x	The x origin (lower left corner) for the window to copy, in meters
win_origin_y	The y origin (lower left corner) for the window to copy, in meters
win_size_x	The x size of the window, in meters
win_size_y	The y size of the window, in meters

◆ copyMapRegion()

template<typename data_type>

void copyMapRegion	(	data_type *	source_map,
		unsigned int	sm_lower_left_x,
		unsigned int	sm_lower_left_y,
		unsigned int	sm_size_x,
		data_type *	dest_map,
		unsigned int	dm_lower_left_x,
		unsigned int	dm_lower_left_y,
		unsigned int	dm_size_x,
		unsigned int	region_size_x,
		unsigned int	region_size_y )

protected

Copy a region of a source map into a destination map.

Parameters

source_map	The source map
sm_lower_left_x	The lower left x point of the source map to start the copy
sm_lower_left_y	The lower left y point of the source map to start the copy
sm_size_x	The x size of the source map
dest_map	The destination map
dm_lower_left_x	The lower left x point of the destination map to start the copy
dm_lower_left_y	The lower left y point of the destination map to start the copy
dm_size_x	The x size of the destination map
region_size_x	The x size of the region to copy
region_size_y	The y size of the region to copy

◆ copyWindow()

bool copyWindow	(	const Costmap2D &	source,
		unsigned int	sx0,
		unsigned int	sy0,
		unsigned int	sxn,
		unsigned int	syn,
		unsigned int	dx0,
		unsigned int	dy0 )

Copies the (x0,y0)..(xn,yn) window from source costmap into a current costmap.

Parameters

source	Source costmap where the window will be copied from
sx0	Lower x-boundary of the source window to copy, in cells
sy0	Lower y-boundary of the source window to copy, in cells
sxn	Upper x-boundary of the source window to copy, in cells
syn	Upper y-boundary of the source window to copy, in cells
dx0	Lower x-boundary of the destination window to copy, in cells
dx0	Lower y-boundary of the destination window to copy, in cells

Returns: true if copy was succeeded or false in negative case

◆ deleteMaps()

void deleteMaps ( )

protectedvirtual

Deletes the costmap, static_map, and markers data structures.

◆ getCharMap()

unsigned char * getCharMap ( ) const

Will return a pointer to the underlying unsigned char array used as the costmap.

Returns: A pointer to the underlying unsigned char array storing cost values

◆ getCost() [1/2]

unsigned char getCost ( unsigned int index ) const

Get the cost of a cell in the costmap.

Parameters

index The cell index

Returns: The cost of the cell

◆ getCost() [2/2]

unsigned char getCost	(	unsigned int	mx,
		unsigned int	my ) const

Get the cost of a cell in the costmap.

Parameters

mx	The x coordinate of the cell
my	The y coordinate of the cell

Returns: The cost of the cell

◆ getDefaultValue()

unsigned char getDefaultValue ( )

Get the default background value of the costmap.

Returns: default value

◆ getIndex()

unsigned int getIndex	(	unsigned int	mx,
		unsigned int	my ) const

Given two map coordinates... compute the associated index.

Parameters

mx	The x coordinate
my	The y coordinate

Returns: The associated index

◆ getMapRegionOccupiedByPolygon()

bool getMapRegionOccupiedByPolygon	(	const std::vector< geometry_msgs::msg::Point > &	polygon,
		std::vector< MapLocation > &	polygon_map_region )

Gets the map region occupied by polygon.

Parameters

polygon	The polygon to perform the operation on
polygon_map_region	The map region occupied by the polygon

Returns: True if the polygon_map_region was filled... false if it could not be filled

◆ getMutex()

mutex_t * getMutex ( )

◆ getOriginX()

double getOriginX ( ) const

Accessor for the x origin of the costmap.

Returns: The x origin of the costmap

◆ getOriginY()

double getOriginY ( ) const

Accessor for the y origin of the costmap.

Returns: The y origin of the costmap

◆ getResolution()

double getResolution ( ) const

Accessor for the resolution of the costmap.

Returns: The resolution of the costmap

◆ getSizeInCellsX()

unsigned int getSizeInCellsX ( ) const

Accessor for the x size of the costmap in cells.

Returns: The x size of the costmap

◆ getSizeInCellsY()

unsigned int getSizeInCellsY ( ) const

Accessor for the y size of the costmap in cells.

Returns: The y size of the costmap

◆ getSizeInMetersX()

double getSizeInMetersX ( ) const

Accessor for the x size of the costmap in meters.

Returns: The x size of the costmap (returns the centerpoint of the last legal cell in the map)

◆ getSizeInMetersY()

double getSizeInMetersY ( ) const

Accessor for the y size of the costmap in meters.

Returns: The y size of the costmap (returns the centerpoint of the last legal cell in the map)

◆ inBounds()

bool inBounds	(	unsigned int	x,
		unsigned int	y ) const

Checks whether a given cell coordinate is within the bounds of the costmap.

Parameters

x	The x coordinate of the cell
y	The y coordinate of the cell

Returns: True if the (x, y) cell lies within the bounds of the costmap, false otherwise

◆ indexToCells()

void indexToCells	(	unsigned int	index,
		unsigned int &	mx,
		unsigned int &	my ) const

Given an index... compute the associated map coordinates.

Parameters

index	The index
mx	Will be set to the x coordinate
my	Will be set to the y coordinate

◆ initMaps()

void initMaps	(	unsigned int	size_x,
		unsigned int	size_y )

protectedvirtual

Initializes the costmap, static_map, and markers data structures.

Parameters

size_x	The x size to use for map initialization
size_y	The y size to use for map initialization

◆ mapToWorld()

void mapToWorld	(	unsigned int	mx,
		unsigned int	my,
		double &	wx,
		double &	wy ) const

Convert from map coordinates to world coordinates.

Parameters

mx	The x map coordinate
my	The y map coordinate
wx	Will be set to the associated world x coordinate
wy	Will be set to the associated world y coordinate

◆ mapToWorldNoBounds()

void mapToWorldNoBounds	(	int	mx,
		int	my,
		double &	wx,
		double &	wy ) const

Convert from map coordinates to world coordinates with no bounds checking.

Parameters

wx	The x world coordinate
wy	The y world coordinate
mx	Will be set to the associated map x coordinate
my	Will be set to the associated map y coordinate

◆ operator=()

Costmap2D & operator= ( const Costmap2D & map )

Overloaded assignment operator.

Parameters

map	The costmap to copy

Returns: A reference to the map after the copy has finished

◆ polygonOutlineCells()

void polygonOutlineCells	(	const std::vector< MapLocation > &	polygon,
		std::vector< MapLocation > &	polygon_cells )

Get the map cells that make up the outline of a polygon.

Parameters

polygon	The polygon in map coordinates to rasterize
polygon_cells	Will be set to the cells contained in the outline of the polygon

◆ raytraceLine()

template<class ActionType>

void raytraceLine	(	ActionType	at,
		unsigned int	x0,
		unsigned int	y0,
		unsigned int	x1,
		unsigned int	y1,
		unsigned int	max_length = UINT_MAX,
		unsigned int	min_length = 0 )

protected

Raytrace a line and apply some action at each step.

Parameters

at	The action to take... a functor
x0	The starting x coordinate
y0	The starting y coordinate
x1	The ending x coordinate
y1	The ending y coordinate
max_length	The maximum desired length of the segment... allows you to not go all the way to the endpoint
min_length	The minimum desired length of the segment

◆ resetMap()

void resetMap	(	unsigned int	x0,
		unsigned int	y0,
		unsigned int	xn,
		unsigned int	yn )

Reset the costmap in bounds.

◆ resetMaps()

void resetMaps ( )

protectedvirtual

Resets the costmap and static_map to be unknown space.

◆ resetMapToValue()

void resetMapToValue	(	unsigned int	x0,
		unsigned int	y0,
		unsigned int	xn,
		unsigned int	yn,
		unsigned char	value )

Reset the costmap in bounds to a value.

◆ resizeMap()

void resizeMap	(	unsigned int	size_x,
		unsigned int	size_y,
		double	resolution,
		double	origin_x,
		double	origin_y )

Resize the costmap.

◆ restoreMapRegionOccupiedByPolygon()

void restoreMapRegionOccupiedByPolygon ( const std::vector< MapLocation > & polygon_map_region )

Restores the corresponding map region using given map region.

Parameters

polygon_map_region The map region to perform the operation on

◆ setConvexPolygonCost()

bool setConvexPolygonCost	(	const std::vector< geometry_msgs::msg::Point > &	polygon,
		unsigned char	cost_value )

Sets the cost of a convex polygon to a desired value.

Parameters

polygon	The polygon to perform the operation on
cost_value	The value to set costs to

Returns: True if the polygon was filled... false if it could not be filled

◆ setCost()

void setCost	(	unsigned int	mx,
		unsigned int	my,
		unsigned char	cost )

Set the cost of a cell in the costmap.

Parameters

mx	The x coordinate of the cell
my	The y coordinate of the cell
cost	The cost to set the cell to

◆ setDefaultValue()

void setDefaultValue ( unsigned char c )

Set the default background value of the costmap.

Parameters

c	default value

◆ setMapRegionOccupiedByPolygon()

void setMapRegionOccupiedByPolygon	(	const std::vector< MapLocation > &	polygon_map_region,
		unsigned char	new_cost_value )

Sets the given map region to desired value.

Parameters

polygon_map_region	The map region to perform the operation on
new_cost_value	The value to set costs to

◆ toOccupancyGridMsg()

void toOccupancyGridMsg ( nav_msgs::msg::OccupancyGrid & msg ) const

Converts the current costmap into a nav_msgs::msg::OccupancyGrid message.

Parameters

msg	A reference to the OccupancyGrid message that will be filled

The resulting OccupancyGrid message contains metadata such as resolution, size, and origin, and its data field is populated with cost values from the costmap. Cells with a value of NO_INFORMATION are mapped to -1; other values are cast to int8_t directly.

◆ updateOrigin()

void updateOrigin	(	double	new_origin_x,
		double	new_origin_y )

virtual

Move the origin of the costmap to a new location....

keeping data when it can

Parameters

new_origin_x	The x coordinate of the new origin
new_origin_y	The y coordinate of the new origin

◆ worldToMap()

bool worldToMap	(	double	wx,
		double	wy,
		unsigned int &	mx,
		unsigned int &	my ) const

Convert from world coordinates to map coordinates.

Parameters

wx	The x world coordinate
wy	The y world coordinate
mx	Will be set to the associated map x coordinate
my	Will be set to the associated map y coordinate

Returns: True if the conversion was successful (legal bounds) false otherwise

◆ worldToMapContinuous()

bool worldToMapContinuous	(	double	wx,
		double	wy,
		float &	mx,
		float &	my ) const

Convert from world coordinates to map coordinates.

Parameters

wx	The x world coordinate
wy	The y world coordinate
mx	Will be set to the associated map x coordinate
my	Will be set to the associated map y coordinate

Returns: True if the conversion was successful (legal bounds) false otherwise

◆ worldToMapEnforceBounds()

void worldToMapEnforceBounds	(	double	wx,
		double	wy,
		int &	mx,
		int &	my ) const

Convert from world coordinates to map coordinates, constraining results to legal bounds.

Parameters

wx	The x world coordinate
wy	The y world coordinate
mx	Will be set to the associated map x coordinate
my	Will be set to the associated map y coordinate

Note: The returned map coordinates are guaranteed to lie within the map.

◆ worldToMapNoBounds()

void worldToMapNoBounds	(	double	wx,
		double	wy,
		int &	mx,
		int &	my ) const

Convert from world coordinates to map coordinates without checking for legal bounds.

Parameters

wx	The x world coordinate
wy	The y world coordinate
mx	Will be set to the associated map x coordinate
my	Will be set to the associated map y coordinate

Note: The returned map coordinates are not guaranteed to lie within the map.

Friends And Related Symbol Documentation

◆ CostmapTester

friend class CostmapTester

friend

Member Data Documentation

◆ costmap_

unsigned char* costmap_

protected

◆ default_value_

unsigned char default_value_

protected

◆ origin_x_

double origin_x_

protected

◆ origin_y_

double origin_y_

protected

◆ resolution_

double resolution_

protected

◆ size_x_

unsigned int size_x_

protected

◆ size_y_

unsigned int size_y_

protected

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

easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp
easynav_costmap_common/src/easynav_costmap_common/costmap_2d.cpp

Classes

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Friends

Detailed Description

Member Typedef Documentation

◆ mutex_t

Constructor & Destructor Documentation

◆ Costmap2D() [1/4]

◆ Costmap2D() [2/4]

◆ Costmap2D() [3/4]

◆ Costmap2D() [4/4]

◆ ~Costmap2D()

Member Function Documentation

◆ cellDistance()

◆ convexFillCells()

◆ copyCostmapWindow()

◆ copyMapRegion()

◆ copyWindow()

◆ deleteMaps()

◆ getCharMap()

◆ getCost() [1/2]

◆ getCost() [2/2]

◆ getDefaultValue()

◆ getIndex()

◆ getMapRegionOccupiedByPolygon()

◆ getMutex()

◆ getOriginX()

◆ getOriginY()

◆ getResolution()

◆ getSizeInCellsX()

◆ getSizeInCellsY()

◆ getSizeInMetersX()

◆ getSizeInMetersY()

◆ inBounds()

◆ indexToCells()

◆ initMaps()

◆ mapToWorld()

◆ mapToWorldNoBounds()

◆ operator=()

◆ polygonOutlineCells()

◆ raytraceLine()

◆ resetMap()

◆ resetMaps()

◆ resetMapToValue()

◆ resizeMap()

◆ restoreMapRegionOccupiedByPolygon()

◆ setConvexPolygonCost()

◆ setCost()

◆ setDefaultValue()

◆ setMapRegionOccupiedByPolygon()

◆ toOccupancyGridMsg()

◆ updateOrigin()

◆ worldToMap()

◆ worldToMapContinuous()

◆ worldToMapEnforceBounds()

◆ worldToMapNoBounds()

Friends And Related Symbol Documentation

◆ CostmapTester

Member Data Documentation

◆ costmap_

◆ default_value_

◆ origin_x_

◆ origin_y_

◆ resolution_

◆ size_x_

◆ size_y_