Class Costmap2D

Nested Relationships

Nested Types

Class Documentation

class Costmap2D

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

Public Types

typedef std::recursive_mutex mutex_t

Public Functions

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(const Costmap2D &map)

Copy constructor for a costmap, creates a copy efficiently.

Parameters:

map – The costmap to copy

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

Constructor for a costmap from an OccupancyGrid map.

Parameters:

map – The OccupancyGrid map to create costmap from

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

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

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

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

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

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.

Parameters:

msg – A reference to the OccupancyGrid message that will be filled

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

Costmap2D()

Default constructor.

virtual ~Costmap2D()

Destructor.

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

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

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

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

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

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

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

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

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

Note

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

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

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

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

Note

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

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

inline 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

inline 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

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

unsigned int getSizeInCellsX() const

Accessor for the x size of the costmap in cells.

Returns:

The x size of the costmap

unsigned int getSizeInCellsY() const

Accessor for the y size of the costmap in cells.

Returns:

The y size of the costmap

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)

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)

double getOriginX() const

Accessor for the x origin of the costmap.

Returns:

The x origin of the costmap

double getOriginY() const

Accessor for the y origin of the costmap.

Returns:

The y origin of the costmap

double getResolution() const

Accessor for the resolution of the costmap.

Returns:

The resolution of the costmap

inline void setDefaultValue(unsigned char c)

Set the default background value of the costmap.

Parameters:

c – default value

inline unsigned char getDefaultValue()

Get the default background value of the costmap.

Returns:

default value

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

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

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

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

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

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

virtual void updateOrigin(double new_origin_x, double new_origin_y)

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

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

Resize the costmap.

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.

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

inline mutex_t *getMutex()

Protected Functions

template<typename data_type>
inline 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.

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

virtual void deleteMaps()

Deletes the costmap, static_map, and markers data structures.

virtual void resetMaps()

Resets the costmap and static_map to be unknown space.

virtual void initMaps(unsigned int size_x, unsigned int size_y)

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

template<class ActionType>
inline 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.

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

Protected Attributes

unsigned int size_x_
unsigned int size_y_
double resolution_
double origin_x_
double origin_y_
unsigned char *costmap_
unsigned char default_value_
class MarkCell

Public Functions

inline MarkCell(unsigned char *costmap, unsigned char value)
inline void operator()(unsigned int offset)
class PolygonOutlineCells

Public Functions

inline PolygonOutlineCells(const Costmap2D &costmap, const unsigned char*, std::vector<MapLocation> &cells)
inline void operator()(unsigned int offset)