Classes | Public Member Functions | Protected Member Functions | Protected Attributes | Private Member Functions | Private Attributes | Friends
costmap_2d::Costmap2D Class Reference

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

#include <costmap_2d.h>

Inheritance diagram for costmap_2d::Costmap2D:
Inheritance graph
[legend]

List of all members.

Classes

class  ClearCell
class  MarkCell
class  PolygonOutlineCells

Public Member Functions

virtual void clearNonLethal (double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info=false)
 Clears non lethal obstacles in a specified window.
unsigned char computeCost (double distance) const
 Given a distance... compute a cost.
void convexFillCells (const std::vector< MapLocation > &polygon, std::vector< MapLocation > &polygon_cells)
 Get the map cells that fill a convex polygon.
void 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.
 Costmap2D (unsigned int cells_size_x, unsigned int cells_size_y, double resolution, double origin_x, double origin_y, double inscribed_radius=0.0, double circumscribed_radius=0.0, double inflation_radius=0.0, double max_obstacle_range=0.0, double max_obstacle_height=0.0, double max_raytrace_range=0.0, double weight=25.0, const std::vector< unsigned char > &static_data=std::vector< unsigned char >(0), unsigned char lethal_threshold=0, bool track_unknown_space=false, unsigned char unknown_cost_value=0)
 Constructor for a costmap.
 Costmap2D (const Costmap2D &map)
 Copy constructor for a costmap, creates a copy efficiently.
 Costmap2D ()
 Default constructor.
virtual void finishConfiguration (costmap_2d::Costmap2DConfig &config)
const unsigned char * getCharMap () const
 Will return a immutable pointer to the underlying unsigned char array used as the costmap.
unsigned char getCircumscribedCost () const
double getCircumscribedRadius () const
 Accessor for the circumscribed radius of the robot.
unsigned char getCost (unsigned int mx, unsigned int my) const
 Get the cost of a cell in the costmap.
unsigned int getIndex (unsigned int mx, unsigned int my) const
 Given two map coordinates... compute the associated index.
double getInflationRadius () const
 Accessor for the inflation radius of the robot.
double getInscribedRadius () const
 Accessor for the inscribed radius of the robot.
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.
void indexToCells (unsigned int index, unsigned int &mx, unsigned int &my) const
 Given an index... compute the associated map coordinates.
bool isCircumscribedCell (unsigned int x, unsigned int y) const
 Check if a cell falls within the circumscribed radius of the robot but outside the inscribed radius of the robot.
void mapToWorld (unsigned int mx, unsigned int my, double &wx, double &wy) const
 Convert from map coordinates to world coordinates.
Costmap2Doperator= (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 reconfigure (costmap_2d::Costmap2DConfig &config, const costmap_2d::Costmap2DConfig &last_config)
void reinflateWindow (double wx, double wy, double w_size_x, double w_size_y, bool clear=true)
 Re-inflate obstacles within a given window.
void replaceFullMap (double win_origin_x, double win_origin_y, unsigned int data_size_x, unsigned int data_size_y, const std::vector< unsigned char > &static_data)
 Replace the costmap with a map of a different size.
virtual void resetMapOutsideWindow (double wx, double wy, double w_size_x, double w_size_y)
 Revert to the static map outside of a specified window centered at a world coordinate.
void saveMap (std::string file_name)
 Save the costmap out to a pgm file.
bool setConvexPolygonCost (const std::vector< geometry_msgs::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.
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.
void updateStaticMapWindow (double win_origin_x, double win_origin_y, unsigned int data_size_x, unsigned int data_size_y, const std::vector< unsigned char > &static_data)
 Update the costmap's static map with new data.
void updateWorld (double robot_x, double robot_y, const std::vector< Observation > &observations, const std::vector< Observation > &clearing_observations)
 Update the costmap with new observations.
bool worldToMap (double wx, double wy, unsigned int &mx, unsigned int &my) const
 Convert from world coordinates to map coordinates.
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

unsigned int cellDistance (double world_dist)
 Given distance in the world... convert it to cells.
void copyKernels (const Costmap2D &map, unsigned int cell_inflation_radius)
 Copies kernel information from a costmap.
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.
void deleteKernels ()
 Deletes the cached kernels.
virtual void deleteMaps ()
 Deletes the costmap, static_map, and markers data structures.
void enqueue (unsigned int index, unsigned int mx, unsigned int my, unsigned int src_x, unsigned int src_y, std::priority_queue< CellData > &inflation_queue)
 Given an index of a cell in the costmap, place it into a priority queue for obstacle inflation.
virtual void initMaps (unsigned int size_x, unsigned int size_y)
 Initializes the costmap, static_map, and markers data structures.
void replaceStaticMapWindow (double win_origin_x, double win_origin_y, unsigned int data_size_x, unsigned int data_size_y, const std::vector< unsigned char > &static_data)
 Replace a window of the costmap with static data.
virtual void resetMaps ()
 Resets the costmap and static_map to be unknown space.
void reshapeStaticMap (double win_origin_x, double win_origin_y, unsigned int data_size_x, unsigned int data_size_y, const std::vector< unsigned char > &static_data)
 Reshape a map to take an update that is not fully contained within the costmap.

Protected Attributes

unsigned char ** cached_costs_
double ** cached_distances_
unsigned int cell_circumscribed_radius_
unsigned int cell_inflation_radius_
unsigned int cell_inscribed_radius_
unsigned char circumscribed_cost_lb_
double circumscribed_radius_
unsigned char * costmap_
std::priority_queue< CellDatainflation_queue_
double inflation_radius_
double inscribed_radius_
unsigned char lethal_threshold_
unsigned char * markers_
double max_obstacle_height_
double max_obstacle_range_
double max_raytrace_range_
double origin_x_
double origin_y_
double resolution_
unsigned int size_x_
unsigned int size_y_
unsigned char * static_map_
bool track_unknown_space_
unsigned char unknown_cost_value_
double weight_

Private Member Functions

template<class ActionType >
void bresenham2D (ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, unsigned int offset, unsigned int max_length)
 A 2D implementation of Bresenham's raytracing algorithm... applies an action at each step.
void computeCaches ()
 Based on the inflation radius compute distance and cost caches.
char costLookup (int mx, int my, int src_x, int src_y)
 Lookup pre-computed costs.
double distanceLookup (int mx, int my, int src_x, int src_y)
 Lookup pre-computed distances.
void inflateObstacles (std::priority_queue< CellData > &inflation_queue)
 Given a priority queue with the actual obstacles, compute the inflated costs for the costmap.
void raytraceFreespace (const std::vector< Observation > &clearing_observations)
 Clear freespace based on any number of observations.
virtual void raytraceFreespace (const Observation &clearing_observation)
 Clear freespace from an observation.
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)
 Raytrace a line and apply some action at each step.
void resetInflationWindow (double wx, double wy, double w_size_x, double w_size_y, std::priority_queue< CellData > &inflation_queue, bool clear=true)
 Provides support for re-inflating obstacles within a certain window (used after raytracing)
int sign (int x)
void updateCellCost (unsigned int index, unsigned char cost)
 Takes the max of existing cost and the new cost... keeps static map obstacles from being overridden prematurely.
virtual void updateObstacles (const std::vector< Observation > &observations, std::priority_queue< CellData > &inflation_queue)
 Insert new obstacles into the cost map.

Private Attributes

boost::recursive_mutex configuration_mutex_

Friends

class CostmapTester

Detailed Description

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

Definition at line 61 of file costmap_2d.h.


Constructor & Destructor Documentation

costmap_2d::Costmap2D::Costmap2D ( unsigned int  cells_size_x,
unsigned int  cells_size_y,
double  resolution,
double  origin_x,
double  origin_y,
double  inscribed_radius = 0.0,
double  circumscribed_radius = 0.0,
double  inflation_radius = 0.0,
double  max_obstacle_range = 0.0,
double  max_obstacle_height = 0.0,
double  max_raytrace_range = 0.0,
double  weight = 25.0,
const std::vector< unsigned char > &  static_data = std::vector<unsigned char>(0),
unsigned char  lethal_threshold = 0,
bool  track_unknown_space = false,
unsigned char  unknown_cost_value = 0 
)

Constructor for a costmap.

Parameters:
cells_size_xThe x size of the map in cells
cells_size_yThe y size of the map in cells
resolutionThe resolution of the map in meters/cell
origin_xThe x origin of the map
origin_yThe y origin of the map
inscribed_radiusThe inscribed radius of the robot
circumscribed_radiusThe circumscribed radius of the robot
inflation_radiusHow far out to inflate obstacles
max_obstacle_rangeThe maximum range at which obstacles will be put into the costmap from any sensor
max_obstacle_heightThe maximum height of obstacles that will be considered
max_raytrace_rangeThe maximum distance we'll raytrace out to with any sensor
weightThe scaling factor for the cost function.
static_dataData used to initialize the costmap
lethal_thresholdThe cost threshold at which a point in the static data is considered a lethal obstacle
track_unknown_spaceWhether or not to keep track of what space is completely unknown, whether or not a sensor reading has seen through a cell
unknown_cost_valueThe cost value for which a point in the static data is considered unknown when tracking unknown space... if not tracking unknown space, costs equal to this value will be considered occupied

Definition at line 45 of file costmap_2d.cpp.

Copy constructor for a costmap, creates a copy efficiently.

Parameters:
mapThe costmap to copy

Definition at line 511 of file costmap_2d.cpp.

Default constructor.

Definition at line 516 of file costmap_2d.cpp.

Destructor.

Definition at line 519 of file costmap_2d.cpp.


Member Function Documentation

template<class ActionType >
void costmap_2d::Costmap2D::bresenham2D ( ActionType  at,
unsigned int  abs_da,
unsigned int  abs_db,
int  error_b,
int  offset_a,
int  offset_b,
unsigned int  offset,
unsigned int  max_length 
) [inline, private]

A 2D implementation of Bresenham's raytracing algorithm... applies an action at each step.

Definition at line 596 of file costmap_2d.h.

unsigned int costmap_2d::Costmap2D::cellDistance ( double  world_dist) [protected]

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

Parameters:
world_distThe world distance
Returns:
The equivalent cell distance

Definition at line 524 of file costmap_2d.cpp.

void costmap_2d::Costmap2D::clearNonLethal ( double  wx,
double  wy,
double  w_size_x,
double  w_size_y,
bool  clear_no_info = false 
) [virtual]

Clears non lethal obstacles in a specified window.

Parameters:
wxThe x coordinate of the center point of the window in world space (meters)
wyThe y coordinate of the center point of the window in world space (meters)
w_size_xThe x size of the window in meters
w_size_yThe y size of the window in meters
clear_no_infoIf set to true, NO_INFORMATION will be cleared, if set to false NO_INFORMATION will be treated as a lethal obstacle

Reimplemented in costmap_2d::VoxelCostmap2D.

Definition at line 795 of file costmap_2d.cpp.

Based on the inflation radius compute distance and cost caches.

Definition at line 886 of file costmap_2d.cpp.

unsigned char costmap_2d::Costmap2D::computeCost ( double  distance) const [inline]

Given a distance... compute a cost.

Parameters:
distanceThe distance from an obstacle in cells
Returns:
A cost value for the distance

Definition at line 341 of file costmap_2d.h.

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

Get the map cells that fill a convex polygon.

Parameters:
polygonThe polygon in map coordinates to rasterize
polygon_cellsWill be set to the cells that fill the polygon

Definition at line 987 of file costmap_2d.cpp.

void costmap_2d::Costmap2D::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:
mapThe costmap to copy
win_origin_xThe x origin (lower left corner) for the window to copy, in meters
win_origin_yThe y origin (lower left corner) for the window to copy, in meters
win_size_xThe x size of the window, in meters
win_size_yThe y size of the window, in meters

Definition at line 404 of file costmap_2d.cpp.

void costmap_2d::Costmap2D::copyKernels ( const Costmap2D map,
unsigned int  cell_inflation_radius 
) [protected]

Copies kernel information from a costmap.

Parameters:
mapThe costmap to copy kernel information from
cell_inflation_radiusThe radius to use when copying the kernel

Definition at line 391 of file costmap_2d.cpp.

template<typename data_type >
void costmap_2d::Costmap2D::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 
) [inline, protected]

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

Parameters:
source_mapThe source map
sm_lower_left_xThe lower left x point of the source map to start the copy
sm_lower_left_yThe lower left y point of the source map to start the copy
sm_size_xThe x size of the source map
dest_mapThe destination map
dm_lower_left_xThe lower left x point of the destination map to start the copy
dm_lower_left_yThe lower left y point of the destination map to start the copy
dm_size_xThe x size of the destination map
region_size_xThe x size of the region to copy
region_size_yThe y size of the region to copy
data_sizeThe size of the data-type stored in the map arrays

Definition at line 442 of file costmap_2d.h.

char costmap_2d::Costmap2D::costLookup ( int  mx,
int  my,
int  src_x,
int  src_y 
) [inline, private]

Lookup pre-computed costs.

Parameters:
mxThe x coordinate of the current cell
myThe y coordinate of the current cell
src_xThe x coordinate of the source cell
src_yThe y coordinate of the source cell
Returns:

Definition at line 638 of file costmap_2d.h.

Deletes the cached kernels.

Definition at line 354 of file costmap_2d.cpp.

void costmap_2d::Costmap2D::deleteMaps ( ) [protected, virtual]

Deletes the costmap, static_map, and markers data structures.

Definition at line 347 of file costmap_2d.cpp.

double costmap_2d::Costmap2D::distanceLookup ( int  mx,
int  my,
int  src_x,
int  src_y 
) [inline, private]

Lookup pre-computed distances.

Parameters:
mxThe x coordinate of the current cell
myThe y coordinate of the current cell
src_xThe x coordinate of the source cell
src_yThe y coordinate of the source cell
Returns:

Definition at line 652 of file costmap_2d.h.

void costmap_2d::Costmap2D::enqueue ( unsigned int  index,
unsigned int  mx,
unsigned int  my,
unsigned int  src_x,
unsigned int  src_y,
std::priority_queue< CellData > &  inflation_queue 
) [inline, protected]

Given an index of a cell in the costmap, place it into a priority queue for obstacle inflation.

Parameters:
indexThe index of the cell
mxThe x coordinate of the cell (can be computed from the index, but saves time to store it)
myThe y coordinate of the cell (can be computed from the index, but saves time to store it)
src_xThe x index of the obstacle point inflation started at
src_yThe y index of the obstacle point inflation started at
inflation_queueThe priority queue to insert into

Definition at line 405 of file costmap_2d.h.

void costmap_2d::Costmap2D::finishConfiguration ( costmap_2d::Costmap2DConfig &  config) [virtual]

Reimplemented in costmap_2d::VoxelCostmap2D.

Definition at line 147 of file costmap_2d.cpp.

const unsigned char * costmap_2d::Costmap2D::getCharMap ( ) const

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

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

Definition at line 529 of file costmap_2d.cpp.

unsigned char costmap_2d::Costmap2D::getCircumscribedCost ( ) const [inline]

Definition at line 360 of file costmap_2d.h.

Accessor for the circumscribed radius of the robot.

Returns:
The circumscribed radius

Definition at line 289 of file costmap_2d.h.

unsigned char costmap_2d::Costmap2D::getCost ( unsigned int  mx,
unsigned int  my 
) const

Get the cost of a cell in the costmap.

Parameters:
mxThe x coordinate of the cell
myThe y coordinate of the cell
Returns:
The cost of the cell

Definition at line 533 of file costmap_2d.cpp.

unsigned int costmap_2d::Costmap2D::getIndex ( unsigned int  mx,
unsigned int  my 
) const [inline]

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

Parameters:
mxThe x coordinate
myThe y coordinate
Returns:
The associated index

Definition at line 216 of file costmap_2d.h.

double costmap_2d::Costmap2D::getInflationRadius ( ) const [inline]

Accessor for the inflation radius of the robot.

Returns:
The inflation radius

Definition at line 295 of file costmap_2d.h.

double costmap_2d::Costmap2D::getInscribedRadius ( ) const [inline]

Accessor for the inscribed radius of the robot.

Returns:
The inscribed radius

Definition at line 283 of file costmap_2d.h.

Accessor for the x origin of the costmap.

Returns:
The x origin of the costmap

Definition at line 1067 of file costmap_2d.cpp.

Accessor for the y origin of the costmap.

Returns:
The y origin of the costmap

Definition at line 1071 of file costmap_2d.cpp.

Accessor for the resolution of the costmap.

Returns:
The resolution of the costmap

Definition at line 1075 of file costmap_2d.cpp.

Accessor for the x size of the costmap in cells.

Returns:
The x size of the costmap

Definition at line 1051 of file costmap_2d.cpp.

Accessor for the y size of the costmap in cells.

Returns:
The y size of the costmap

Definition at line 1055 of file costmap_2d.cpp.

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)

Definition at line 1059 of file costmap_2d.cpp.

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)

Definition at line 1063 of file costmap_2d.cpp.

void costmap_2d::Costmap2D::indexToCells ( unsigned int  index,
unsigned int &  mx,
unsigned int &  my 
) const [inline]

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

Parameters:
indexThe index
mxWill be set to the x coordinate
myWill be set to the y coordinate

Definition at line 226 of file costmap_2d.h.

void costmap_2d::Costmap2D::inflateObstacles ( std::priority_queue< CellData > &  inflation_queue) [private]

Given a priority queue with the actual obstacles, compute the inflated costs for the costmap.

Parameters:
inflation_queueA priority queue contatining the cell data for the actual obstacles

Definition at line 695 of file costmap_2d.cpp.

void costmap_2d::Costmap2D::initMaps ( unsigned int  size_x,
unsigned int  size_y 
) [protected, virtual]

Initializes the costmap, static_map, and markers data structures.

Parameters:
size_xThe x size to use for map initialization
size_yThe y size to use for map initialization

Reimplemented in costmap_2d::VoxelCostmap2D.

Definition at line 370 of file costmap_2d.cpp.

bool costmap_2d::Costmap2D::isCircumscribedCell ( unsigned int  x,
unsigned int  y 
) const

Check if a cell falls within the circumscribed radius of the robot but outside the inscribed radius of the robot.

Parameters:
Thex coordinate of the cell
They coordinate of the cell
Returns:
True if the cell is inside the circumscribed radius but outside the inscribed radius, false otherwise

Definition at line 1079 of file costmap_2d.cpp.

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

Convert from map coordinates to world coordinates.

Parameters:
mxThe x map coordinate
myThe y map coordinate
wxWill be set to the associated world x coordinate
wyWill be set to the associated world y coordinate

Definition at line 543 of file costmap_2d.cpp.

Costmap2D & costmap_2d::Costmap2D::operator= ( const Costmap2D map)

Overloaded assignment operator.

Parameters:
mapThe costmap to copy
Returns:
A reference to the map after the copy has finished

Definition at line 463 of file costmap_2d.cpp.

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

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

Parameters:
polygonThe polygon in map coordinates to rasterize
polygon_cellsWill be set to the cells contained in the outline of the polygon

Definition at line 975 of file costmap_2d.cpp.

void costmap_2d::Costmap2D::raytraceFreespace ( const std::vector< Observation > &  clearing_observations) [private]

Clear freespace based on any number of observations.

Parameters:
clearing_observationsThe observations used to raytrace

Definition at line 722 of file costmap_2d.cpp.

void costmap_2d::Costmap2D::raytraceFreespace ( const Observation clearing_observation) [private, virtual]

Clear freespace from an observation.

Parameters:
clearing_observationThe observation used to raytrace

Reimplemented in costmap_2d::VoxelCostmap2D.

Definition at line 728 of file costmap_2d.cpp.

template<class ActionType >
void costmap_2d::Costmap2D::raytraceLine ( ActionType  at,
unsigned int  x0,
unsigned int  y0,
unsigned int  x1,
unsigned int  y1,
unsigned int  max_length = UINT_MAX 
) [inline, private]

Raytrace a line and apply some action at each step.

Parameters:
atThe action to take... a functor
x0The starting x coordinate
y0The starting y coordinate
x1The ending x coordinate
y1The ending y coordinate
max_lengthThe maximum desired length of the segment... allows you to not go all the way to the endpoint

Definition at line 563 of file costmap_2d.h.

void costmap_2d::Costmap2D::reconfigure ( costmap_2d::Costmap2DConfig &  config,
const costmap_2d::Costmap2DConfig &  last_config 
)

Definition at line 116 of file costmap_2d.cpp.

void costmap_2d::Costmap2D::reinflateWindow ( double  wx,
double  wy,
double  w_size_x,
double  w_size_y,
bool  clear = true 
)

Re-inflate obstacles within a given window.

Parameters:
wxThe x coordinate of the center point of the window in world space (meters)
wyThe y coordinate of the center point of the window in world space (meters)
w_size_xThe x size of the window in meters
w_size_yThe y size of the window in meters
clearWhen set to true, will clear all non-lethal obstacles before inflation

Definition at line 636 of file costmap_2d.cpp.

void costmap_2d::Costmap2D::replaceFullMap ( double  win_origin_x,
double  win_origin_y,
unsigned int  data_size_x,
unsigned int  data_size_y,
const std::vector< unsigned char > &  static_data 
)

Replace the costmap with a map of a different size.

Parameters:
win_origin_xThe x origin of the map we'll be using to replace the costmap
win_origin_yThe y origin of the map we'll be using to replace the costmap
data_size_xThe x size of the map we'll be using to replace the costmap
data_size_yThe y size of the map we'll be using to replace the costmap
static_dataThe data that we'll use for our new costmap

Definition at line 150 of file costmap_2d.cpp.

void costmap_2d::Costmap2D::replaceStaticMapWindow ( double  win_origin_x,
double  win_origin_y,
unsigned int  data_size_x,
unsigned int  data_size_y,
const std::vector< unsigned char > &  static_data 
) [protected]

Replace a window of the costmap with static data.

Parameters:
win_origin_xThe x origin of the map we'll be using to replace the costmap
win_origin_yThe y origin of the map we'll be using to replace the costmap
data_size_xThe x size of the map we'll be using to replace the costmap
data_size_yThe y size of the map we'll be using to replace the costmap
static_dataThe data that we'll use for our new costmap

Definition at line 203 of file costmap_2d.cpp.

void costmap_2d::Costmap2D::resetInflationWindow ( double  wx,
double  wy,
double  w_size_x,
double  w_size_y,
std::priority_queue< CellData > &  inflation_queue,
bool  clear = true 
) [private]

Provides support for re-inflating obstacles within a certain window (used after raytracing)

Parameters:
wxThe x coordinate of the center point of the window in world space (meters)
wyThe y coordinate of the center point of the window in world space (meters)
w_size_xThe x size of the window in meters
w_size_yThe y size of the window in meters
inflation_queueThe priority queue to push items back onto for propogation
clearWhen set to true, will clear all non-lethal obstacles before inflation

Definition at line 839 of file costmap_2d.cpp.

void costmap_2d::Costmap2D::resetMapOutsideWindow ( double  wx,
double  wy,
double  w_size_x,
double  w_size_y 
) [virtual]

Revert to the static map outside of a specified window centered at a world coordinate.

Parameters:
wxThe x coordinate of the center point of the window in world space (meters)
wyThe y coordinate of the center point of the window in world space (meters)
w_size_xThe x size of the window in meters
w_size_yThe y size of the window in meters

Reimplemented in costmap_2d::VoxelCostmap2D.

Definition at line 566 of file costmap_2d.cpp.

void costmap_2d::Costmap2D::resetMaps ( ) [protected, virtual]

Resets the costmap and static_map to be unknown space.

Reimplemented in costmap_2d::VoxelCostmap2D.

Definition at line 379 of file costmap_2d.cpp.

void costmap_2d::Costmap2D::reshapeStaticMap ( double  win_origin_x,
double  win_origin_y,
unsigned int  data_size_x,
unsigned int  data_size_y,
const std::vector< unsigned char > &  static_data 
) [protected]

Reshape a map to take an update that is not fully contained within the costmap.

Parameters:
win_origin_xThe x origin of the map we'll be using to replace the costmap
win_origin_yThe y origin of the map we'll be using to replace the costmap
data_size_xThe x size of the map we'll be using to replace the costmap
data_size_yThe y size of the map we'll be using to replace the costmap
static_dataThe data that we'll use for our new costmap

Definition at line 273 of file costmap_2d.cpp.

void costmap_2d::Costmap2D::saveMap ( std::string  file_name)

Save the costmap out to a pgm file.

Parameters:
file_nameThe name of the file to save

Definition at line 1086 of file costmap_2d.cpp.

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

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

Parameters:
polygonThe polygon to perform the operation on
cost_valueThe value to set costs to
Returns:
True if the polygon was filled... false if it could not be filled

Definition at line 950 of file costmap_2d.cpp.

void costmap_2d::Costmap2D::setCost ( unsigned int  mx,
unsigned int  my,
unsigned char  cost 
)

Set the cost of a cell in the costmap.

Parameters:
mxThe x coordinate of the cell
myThe y coordinate of the cell
costThe cost to set the cell to

Definition at line 538 of file costmap_2d.cpp.

int costmap_2d::Costmap2D::sign ( int  x) [inline, private]

Definition at line 658 of file costmap_2d.h.

void costmap_2d::Costmap2D::updateCellCost ( unsigned int  index,
unsigned char  cost 
) [inline, private]

Takes the max of existing cost and the new cost... keeps static map obstacles from being overridden prematurely.

Parameters:
indexThe index od the cell to assign a cost to
costThe cost

Definition at line 622 of file costmap_2d.h.

void costmap_2d::Costmap2D::updateObstacles ( const std::vector< Observation > &  observations,
std::priority_queue< CellData > &  inflation_queue 
) [private, virtual]

Insert new obstacles into the cost map.

Parameters:
obstaclesThe point clouds of obstacles to insert into the map
inflation_queueThe queue to place the obstacles into for inflation

Reimplemented in costmap_2d::VoxelCostmap2D.

Definition at line 652 of file costmap_2d.cpp.

void costmap_2d::Costmap2D::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_xThe x coordinate of the new origin
new_origin_yThe y coordinate of the new origin

Reimplemented in costmap_2d::VoxelCostmap2D.

Definition at line 900 of file costmap_2d.cpp.

void costmap_2d::Costmap2D::updateStaticMapWindow ( double  win_origin_x,
double  win_origin_y,
unsigned int  data_size_x,
unsigned int  data_size_y,
const std::vector< unsigned char > &  static_data 
)

Update the costmap's static map with new data.

Parameters:
win_origin_xThe x origin of the map we'll be using to replace the static map in meters
win_origin_yThe y origin of the map we'll be using to replace the static map in meters
data_size_xThe x size of the map we'll be using to replace the static map in cells
data_size_yThe y size of the map we'll be using to replace the static map in cells
static_dataThe data that we'll use for our new costmap

Definition at line 320 of file costmap_2d.cpp.

void costmap_2d::Costmap2D::updateWorld ( double  robot_x,
double  robot_y,
const std::vector< Observation > &  observations,
const std::vector< Observation > &  clearing_observations 
)

Update the costmap with new observations.

Parameters:
obstaclesThe point clouds of obstacles to insert into the map
clearing_observationsThe set of observations to use for raytracing

Definition at line 608 of file costmap_2d.cpp.

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

Convert from world coordinates to map coordinates.

Parameters:
wxThe x world coordinate
wyThe y world coordinate
mxWill be set to the associated map x coordinate
myWill be set to the associated map y coordinate
Returns:
True if the conversion was successful (legal bounds) false otherwise

Definition at line 548 of file costmap_2d.cpp.

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

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

Parameters:
wxThe x world coordinate
wyThe y world coordinate
mxWill be set to the associated map x coordinate
myWill be set to the associated map y coordinate
Note:
The returned map coordinates are not guaranteed to lie within the map.

Definition at line 561 of file costmap_2d.cpp.


Friends And Related Function Documentation

friend class CostmapTester [friend]

Definition at line 62 of file costmap_2d.h.


Member Data Documentation

unsigned char** costmap_2d::Costmap2D::cached_costs_ [protected]

Definition at line 675 of file costmap_2d.h.

Definition at line 676 of file costmap_2d.h.

Definition at line 678 of file costmap_2d.h.

Definition at line 678 of file costmap_2d.h.

Definition at line 678 of file costmap_2d.h.

Definition at line 680 of file costmap_2d.h.

Definition at line 677 of file costmap_2d.h.

boost::recursive_mutex costmap_2d::Costmap2D::configuration_mutex_ [private]

Definition at line 662 of file costmap_2d.h.

unsigned char* costmap_2d::Costmap2D::costmap_ [protected]

Definition at line 670 of file costmap_2d.h.

std::priority_queue<CellData> costmap_2d::Costmap2D::inflation_queue_ [protected]

Definition at line 683 of file costmap_2d.h.

Definition at line 677 of file costmap_2d.h.

Definition at line 677 of file costmap_2d.h.

unsigned char costmap_2d::Costmap2D::lethal_threshold_ [protected]

Definition at line 680 of file costmap_2d.h.

unsigned char* costmap_2d::Costmap2D::markers_ [protected]

Definition at line 671 of file costmap_2d.h.

Definition at line 673 of file costmap_2d.h.

Definition at line 672 of file costmap_2d.h.

Definition at line 674 of file costmap_2d.h.

Definition at line 667 of file costmap_2d.h.

Definition at line 668 of file costmap_2d.h.

Definition at line 666 of file costmap_2d.h.

unsigned int costmap_2d::Costmap2D::size_x_ [protected]

Definition at line 664 of file costmap_2d.h.

unsigned int costmap_2d::Costmap2D::size_y_ [protected]

Definition at line 665 of file costmap_2d.h.

unsigned char* costmap_2d::Costmap2D::static_map_ [protected]

Definition at line 669 of file costmap_2d.h.

Definition at line 681 of file costmap_2d.h.

unsigned char costmap_2d::Costmap2D::unknown_cost_value_ [protected]

Definition at line 682 of file costmap_2d.h.

double costmap_2d::Costmap2D::weight_ [protected]

Definition at line 679 of file costmap_2d.h.


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


costmap_2d
Author(s): Eitan Marder-Eppstein
autogenerated on Sat Dec 28 2013 17:13:40