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 ()
 Default constructor.
 Costmap2D (const Costmap2D &map)
 Copy constructor for a costmap, creates a copy efficiently.
 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.
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 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.
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.
virtual void raytraceFreespace (const Observation &clearing_observation)
 Clear freespace from an observation.
void raytraceFreespace (const std::vector< Observation > &clearing_observations)
 Clear freespace based on any number of observations.
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.

Friends

class CostmapTester

Detailed Description

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

Definition at line 57 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_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
inscribed_radius The inscribed radius of the robot
circumscribed_radius The circumscribed radius of the robot
inflation_radius How far out to inflate obstacles
max_obstacle_range The maximum range at which obstacles will be put into the costmap from any sensor
max_obstacle_height The maximum height of obstacles that will be considered
max_raytrace_range The maximum distance we'll raytrace out to with any sensor
weight The scaling factor for the cost function.
static_data Data used to initialize the costmap
lethal_threshold The cost threshold at which a point in the static data is considered a lethal obstacle
track_unknown_space Whether or not to keep track of what space is completely unknown, whether or not a sensor reading has seen through a cell
unknown_cost_value The 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 43 of file costmap_2d.cpp.

costmap_2d::Costmap2D::Costmap2D ( const Costmap2D map  ) 

Copy constructor for a costmap, creates a copy efficiently.

Parameters:
map The costmap to copy

Definition at line 475 of file costmap_2d.cpp.

costmap_2d::Costmap2D::Costmap2D (  ) 

Default constructor.

Definition at line 480 of file costmap_2d.cpp.

costmap_2d::Costmap2D::~Costmap2D (  )  [virtual]

Destructor.

Definition at line 483 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 583 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_dist The world distance
Returns:
The equivalent cell distance

Definition at line 488 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:
wx The x coordinate of the center point of the window in world space (meters)
wy The y coordinate of the center point of the window in world space (meters)
w_size_x The x size of the window in meters
w_size_y The y size of the window in meters
clear_no_info If 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 756 of file costmap_2d.cpp.

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

Given a distance... compute a cost.

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

Definition at line 333 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:
polygon The polygon in map coordinates to rasterize
polygon_cells Will be set to the cells that fill the polygon

Definition at line 935 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:
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

Definition at line 371 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:
map The costmap to copy kernel information from
cell_inflation_radius The radius to use when copying the kernel

Definition at line 358 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_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
data_size The size of the data-type stored in the map arrays

Definition at line 434 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:
mx The x coordinate of the current cell
my The y coordinate of the current cell
src_x The x coordinate of the source cell
src_y The y coordinate of the source cell
Returns:

Definition at line 625 of file costmap_2d.h.

void costmap_2d::Costmap2D::deleteKernels (  )  [protected]

Deletes the cached kernels.

Definition at line 321 of file costmap_2d.cpp.

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

Deletes the costmap, static_map, and markers data structures.

Definition at line 314 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:
mx The x coordinate of the current cell
my The y coordinate of the current cell
src_x The x coordinate of the source cell
src_y The y coordinate of the source cell
Returns:

Definition at line 639 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:
index The index of the cell
mx The x coordinate of the cell (can be computed from the index, but saves time to store it)
my The y coordinate of the cell (can be computed from the index, but saves time to store it)
src_x The x index of the obstacle point inflation started at
src_y The y index of the obstacle point inflation started at
inflation_queue The priority queue to insert into

Definition at line 397 of file costmap_2d.h.

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 493 of file costmap_2d.cpp.

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

Definition at line 352 of file costmap_2d.h.

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

Accessor for the circumscribed radius of the robot.

Returns:
The circumscribed radius

Definition at line 281 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:
mx The x coordinate of the cell
my The y coordinate of the cell
Returns:
The cost of the cell

Definition at line 497 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:
mx The x coordinate
my The y coordinate
Returns:
The associated index

Definition at line 208 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 287 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 275 of file costmap_2d.h.

double costmap_2d::Costmap2D::getOriginX (  )  const

Accessor for the x origin of the costmap.

Returns:
The x origin of the costmap

Definition at line 1015 of file costmap_2d.cpp.

double costmap_2d::Costmap2D::getOriginY (  )  const

Accessor for the y origin of the costmap.

Returns:
The y origin of the costmap

Definition at line 1019 of file costmap_2d.cpp.

double costmap_2d::Costmap2D::getResolution (  )  const

Accessor for the resolution of the costmap.

Returns:
The resolution of the costmap

Definition at line 1023 of file costmap_2d.cpp.

unsigned int costmap_2d::Costmap2D::getSizeInCellsX (  )  const

Accessor for the x size of the costmap in cells.

Returns:
The x size of the costmap

Definition at line 999 of file costmap_2d.cpp.

unsigned int costmap_2d::Costmap2D::getSizeInCellsY (  )  const

Accessor for the y size of the costmap in cells.

Returns:
The y size of the costmap

Definition at line 1003 of file costmap_2d.cpp.

double costmap_2d::Costmap2D::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)

Definition at line 1007 of file costmap_2d.cpp.

double costmap_2d::Costmap2D::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)

Definition at line 1011 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:
index The index
mx Will be set to the x coordinate
my Will be set to the y coordinate

Definition at line 218 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_queue A priority queue contatining the cell data for the actual obstacles
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_x The x size to use for map initialization
size_y The y size to use for map initialization

Reimplemented in costmap_2d::VoxelCostmap2D.

Definition at line 337 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:
The x coordinate of the cell
The y coordinate of the cell
Returns:
True if the cell is inside the circumscribed radius but outside the inscribed radius, false otherwise

Definition at line 1027 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:
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

Definition at line 507 of file costmap_2d.cpp.

Costmap2D & costmap_2d::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

Definition at line 428 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:
polygon The polygon in map coordinates to rasterize
polygon_cells Will be set to the cells contained in the outline of the polygon

Definition at line 923 of file costmap_2d.cpp.

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

Clear freespace from an observation.

Parameters:
clearing_observation The observation used to raytrace

Reimplemented in costmap_2d::VoxelCostmap2D.

Definition at line 689 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_observations The observations used to raytrace

Definition at line 683 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:
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

Definition at line 550 of file costmap_2d.h.

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:
wx The x coordinate of the center point of the window in world space (meters)
wy The y coordinate of the center point of the window in world space (meters)
w_size_x The x size of the window in meters
w_size_y The y size of the window in meters
clear When set to true, will clear all non-lethal obstacles before inflation

Definition at line 598 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_x The x origin of the map we'll be using to replace the costmap
win_origin_y The y origin of the map we'll be using to replace the costmap
data_size_x The x size of the map we'll be using to replace the costmap
data_size_y The y size of the map we'll be using to replace the costmap
static_data The data that we'll use for our new costmap

Definition at line 124 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_x The x origin of the map we'll be using to replace the costmap
win_origin_y The y origin of the map we'll be using to replace the costmap
data_size_x The x size of the map we'll be using to replace the costmap
data_size_y The y size of the map we'll be using to replace the costmap
static_data The data that we'll use for our new costmap

Definition at line 175 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:
wx The x coordinate of the center point of the window in world space (meters)
wy The y coordinate of the center point of the window in world space (meters)
w_size_x The x size of the window in meters
w_size_y The y size of the window in meters
inflation_queue The priority queue to push items back onto for propogation
clear When set to true, will clear all non-lethal obstacles before inflation
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:
wx The x coordinate of the center point of the window in world space (meters)
wy The y coordinate of the center point of the window in world space (meters)
w_size_x The x size of the window in meters
w_size_y The y size of the window in meters

Reimplemented in costmap_2d::VoxelCostmap2D.

Definition at line 530 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 346 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_x The x origin of the map we'll be using to replace the costmap
win_origin_y The y origin of the map we'll be using to replace the costmap
data_size_x The x size of the map we'll be using to replace the costmap
data_size_y The y size of the map we'll be using to replace the costmap
static_data The data that we'll use for our new costmap

Definition at line 240 of file costmap_2d.cpp.

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

Save the costmap out to a pgm file.

Parameters:
file_name The name of the file to save

Definition at line 1034 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:
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

Definition at line 898 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:
mx The x coordinate of the cell
my The y coordinate of the cell
cost The cost to set the cell to

Definition at line 502 of file costmap_2d.cpp.

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

Definition at line 645 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:
index The index od the cell to assign a cost to
cost The cost

Definition at line 609 of file costmap_2d.h.

virtual 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:
obstacles The point clouds of obstacles to insert into the map
inflation_queue The queue to place the obstacles into for inflation

Reimplemented in costmap_2d::VoxelCostmap2D.

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_x The x coordinate of the new origin
new_origin_y The y coordinate of the new origin

Reimplemented in costmap_2d::VoxelCostmap2D.

Definition at line 847 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_x The x origin of the map we'll be using to replace the static map in meters
win_origin_y The y origin of the map we'll be using to replace the static map in meters
data_size_x The x size of the map we'll be using to replace the static map in cells
data_size_y The y size of the map we'll be using to replace the static map in cells
static_data The data that we'll use for our new costmap

Definition at line 287 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:
obstacles The point clouds of obstacles to insert into the map
clearing_observations The set of observations to use for raytracing
bool costmap_2d::Costmap2D::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

Definition at line 512 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:
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.

Definition at line 525 of file costmap_2d.cpp.


Friends And Related Function Documentation

friend class CostmapTester [friend]

Definition at line 58 of file costmap_2d.h.


Member Data Documentation

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

Definition at line 661 of file costmap_2d.h.

Definition at line 662 of file costmap_2d.h.

Definition at line 664 of file costmap_2d.h.

Definition at line 664 of file costmap_2d.h.

Definition at line 664 of file costmap_2d.h.

Definition at line 666 of file costmap_2d.h.

Definition at line 663 of file costmap_2d.h.

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

Definition at line 656 of file costmap_2d.h.

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

Definition at line 669 of file costmap_2d.h.

Definition at line 663 of file costmap_2d.h.

Definition at line 663 of file costmap_2d.h.

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

Definition at line 666 of file costmap_2d.h.

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

Definition at line 657 of file costmap_2d.h.

Definition at line 659 of file costmap_2d.h.

Definition at line 658 of file costmap_2d.h.

Definition at line 660 of file costmap_2d.h.

Definition at line 653 of file costmap_2d.h.

Definition at line 654 of file costmap_2d.h.

Definition at line 652 of file costmap_2d.h.

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

Definition at line 650 of file costmap_2d.h.

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

Definition at line 651 of file costmap_2d.h.

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

Definition at line 655 of file costmap_2d.h.

Definition at line 667 of file costmap_2d.h.

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

Definition at line 668 of file costmap_2d.h.

double costmap_2d::Costmap2D::weight_ [protected]

Definition at line 665 of file costmap_2d.h.


The documentation for this class was generated from the following files:
 All Classes Namespaces Files Functions Variables Typedefs Friends Defines


costmap_2d
Author(s): Eitan Marder-Eppstein
autogenerated on Fri Jan 11 09:34:26 2013