A 2D costmap provides a mapping between points in the world and their associated "costs". More...
#include <costmap_2d.h>
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. | |
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 | 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. | |
void | saveRawMap (std::string file_name) |
Save the RAW costmap out to a pgm file using the original byte values. | |
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_ |
double | cell_inscribed_radius_ |
unsigned char | circumscribed_cost_lb_ |
double | circumscribed_radius_ |
unsigned char * | costmap_ |
std::priority_queue< CellData > | inflation_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 |
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition at line 61 of file costmap_2d.h.
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.
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 47 of file costmap_2d.cpp.
costmap_2d::Costmap2D::Costmap2D | ( | const Costmap2D & | map | ) |
Copy constructor for a costmap, creates a copy efficiently.
map | The costmap to copy |
Definition at line 513 of file costmap_2d.cpp.
Default constructor.
Definition at line 518 of file costmap_2d.cpp.
costmap_2d::Costmap2D::~Costmap2D | ( | ) | [virtual] |
Destructor.
Definition at line 521 of file costmap_2d.cpp.
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 602 of file costmap_2d.h.
unsigned int costmap_2d::Costmap2D::cellDistance | ( | double | world_dist | ) | [protected] |
Given distance in the world... convert it to cells.
world_dist | The world distance |
Definition at line 526 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.
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 797 of file costmap_2d.cpp.
void costmap_2d::Costmap2D::computeCaches | ( | ) | [private] |
Based on the inflation radius compute distance and cost caches.
Definition at line 888 of file costmap_2d.cpp.
unsigned char costmap_2d::Costmap2D::computeCost | ( | double | distance | ) | const [inline] |
Given a distance... compute a cost.
distance | The distance from an obstacle in cells |
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.
polygon | The polygon in map coordinates to rasterize |
polygon_cells | Will be set to the cells that fill the polygon |
Definition at line 989 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.
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 406 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.
map | The costmap to copy kernel information from |
cell_inflation_radius | The radius to use when copying the kernel |
Definition at line 393 of file costmap_2d.cpp.
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.
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 448 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.
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 |
Definition at line 644 of file costmap_2d.h.
void costmap_2d::Costmap2D::deleteKernels | ( | ) | [protected] |
Deletes the cached kernels.
Definition at line 356 of file costmap_2d.cpp.
void costmap_2d::Costmap2D::deleteMaps | ( | ) | [protected, virtual] |
Deletes the costmap, static_map, and markers data structures.
Definition at line 349 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.
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 |
Definition at line 658 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.
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 411 of file costmap_2d.h.
void costmap_2d::Costmap2D::finishConfiguration | ( | costmap_2d::Costmap2DConfig & | config | ) | [virtual] |
Reimplemented in costmap_2d::VoxelCostmap2D.
Definition at line 149 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.
Definition at line 531 of file costmap_2d.cpp.
unsigned char costmap_2d::Costmap2D::getCircumscribedCost | ( | ) | const [inline] |
Definition at line 360 of file costmap_2d.h.
double costmap_2d::Costmap2D::getCircumscribedRadius | ( | ) | const [inline] |
Accessor for the circumscribed radius of the robot.
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.
mx | The x coordinate of the cell |
my | The y coordinate of the cell |
Definition at line 535 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.
mx | The x coordinate |
my | The y coordinate |
Definition at line 216 of file costmap_2d.h.
double costmap_2d::Costmap2D::getInflationRadius | ( | ) | const [inline] |
Accessor for the inflation radius of the robot.
Definition at line 295 of file costmap_2d.h.
double costmap_2d::Costmap2D::getInscribedRadius | ( | ) | const [inline] |
Accessor for the inscribed radius of the robot.
Definition at line 283 of file costmap_2d.h.
double costmap_2d::Costmap2D::getOriginX | ( | ) | const |
Accessor for the x origin of the costmap.
Definition at line 1069 of file costmap_2d.cpp.
double costmap_2d::Costmap2D::getOriginY | ( | ) | const |
Accessor for the y origin of the costmap.
Definition at line 1073 of file costmap_2d.cpp.
double costmap_2d::Costmap2D::getResolution | ( | ) | const |
Accessor for the resolution of the costmap.
Definition at line 1077 of file costmap_2d.cpp.
unsigned int costmap_2d::Costmap2D::getSizeInCellsX | ( | ) | const |
Accessor for the x size of the costmap in cells.
Definition at line 1053 of file costmap_2d.cpp.
unsigned int costmap_2d::Costmap2D::getSizeInCellsY | ( | ) | const |
Accessor for the y size of the costmap in cells.
Definition at line 1057 of file costmap_2d.cpp.
double costmap_2d::Costmap2D::getSizeInMetersX | ( | ) | const |
Accessor for the x size of the costmap in meters.
Definition at line 1061 of file costmap_2d.cpp.
double costmap_2d::Costmap2D::getSizeInMetersY | ( | ) | const |
Accessor for the y size of the costmap in meters.
Definition at line 1065 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.
index | The index |
mx | Will be set to the x coordinate |
my | Will 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.
inflation_queue | A priority queue contatining the cell data for the actual obstacles |
Definition at line 697 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.
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 372 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.
The | x coordinate of the cell |
The | y coordinate of the cell |
Definition at line 1081 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.
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 545 of file costmap_2d.cpp.
Overloaded assignment operator.
map | The costmap to copy |
Definition at line 465 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.
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 977 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.
clearing_observations | The observations used to raytrace |
Definition at line 724 of file costmap_2d.cpp.
void costmap_2d::Costmap2D::raytraceFreespace | ( | const Observation & | clearing_observation | ) | [private, virtual] |
Clear freespace from an observation.
clearing_observation | The observation used to raytrace |
Reimplemented in costmap_2d::VoxelCostmap2D.
Definition at line 730 of file costmap_2d.cpp.
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.
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 569 of file costmap_2d.h.
void costmap_2d::Costmap2D::reconfigure | ( | costmap_2d::Costmap2DConfig & | config, |
const costmap_2d::Costmap2DConfig & | last_config | ||
) |
Definition at line 118 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.
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 638 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.
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 152 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.
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 205 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)
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 |
Definition at line 841 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.
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 568 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 381 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.
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 275 of file costmap_2d.cpp.
void costmap_2d::Costmap2D::saveMap | ( | std::string | file_name | ) |
Save the costmap out to a pgm file.
file_name | The name of the file to save |
Definition at line 1088 of file costmap_2d.cpp.
void costmap_2d::Costmap2D::saveRawMap | ( | std::string | file_name | ) |
Save the RAW costmap out to a pgm file using the original byte values.
file_name | The name of the file to save, including extension. |
Definition at line 1121 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.
polygon | The polygon to perform the operation on |
cost_value | The value to set costs to |
Definition at line 952 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.
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 540 of file costmap_2d.cpp.
int costmap_2d::Costmap2D::sign | ( | int | x | ) | [inline, private] |
Definition at line 664 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.
index | The index od the cell to assign a cost to |
cost | The cost |
Definition at line 628 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.
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.
Definition at line 654 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.
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 902 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.
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 322 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.
obstacles | The point clouds of obstacles to insert into the map |
clearing_observations | The set of observations to use for raytracing |
Definition at line 610 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.
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 |
Definition at line 550 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.
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 |
Definition at line 563 of file costmap_2d.cpp.
friend class CostmapTester [friend] |
Definition at line 62 of file costmap_2d.h.
unsigned char** costmap_2d::Costmap2D::cached_costs_ [protected] |
Definition at line 681 of file costmap_2d.h.
double** costmap_2d::Costmap2D::cached_distances_ [protected] |
Definition at line 682 of file costmap_2d.h.
unsigned int costmap_2d::Costmap2D::cell_circumscribed_radius_ [protected] |
Definition at line 685 of file costmap_2d.h.
unsigned int costmap_2d::Costmap2D::cell_inflation_radius_ [protected] |
Definition at line 685 of file costmap_2d.h.
double costmap_2d::Costmap2D::cell_inscribed_radius_ [protected] |
Definition at line 684 of file costmap_2d.h.
unsigned char costmap_2d::Costmap2D::circumscribed_cost_lb_ [protected] |
Definition at line 687 of file costmap_2d.h.
double costmap_2d::Costmap2D::circumscribed_radius_ [protected] |
Definition at line 683 of file costmap_2d.h.
boost::recursive_mutex costmap_2d::Costmap2D::configuration_mutex_ [private] |
Definition at line 668 of file costmap_2d.h.
unsigned char* costmap_2d::Costmap2D::costmap_ [protected] |
Definition at line 676 of file costmap_2d.h.
std::priority_queue<CellData> costmap_2d::Costmap2D::inflation_queue_ [protected] |
Definition at line 690 of file costmap_2d.h.
double costmap_2d::Costmap2D::inflation_radius_ [protected] |
Definition at line 683 of file costmap_2d.h.
double costmap_2d::Costmap2D::inscribed_radius_ [protected] |
Definition at line 683 of file costmap_2d.h.
unsigned char costmap_2d::Costmap2D::lethal_threshold_ [protected] |
Definition at line 687 of file costmap_2d.h.
unsigned char* costmap_2d::Costmap2D::markers_ [protected] |
Definition at line 677 of file costmap_2d.h.
double costmap_2d::Costmap2D::max_obstacle_height_ [protected] |
Definition at line 679 of file costmap_2d.h.
double costmap_2d::Costmap2D::max_obstacle_range_ [protected] |
Definition at line 678 of file costmap_2d.h.
double costmap_2d::Costmap2D::max_raytrace_range_ [protected] |
Definition at line 680 of file costmap_2d.h.
double costmap_2d::Costmap2D::origin_x_ [protected] |
Definition at line 673 of file costmap_2d.h.
double costmap_2d::Costmap2D::origin_y_ [protected] |
Definition at line 674 of file costmap_2d.h.
double costmap_2d::Costmap2D::resolution_ [protected] |
Definition at line 672 of file costmap_2d.h.
unsigned int costmap_2d::Costmap2D::size_x_ [protected] |
Definition at line 670 of file costmap_2d.h.
unsigned int costmap_2d::Costmap2D::size_y_ [protected] |
Definition at line 671 of file costmap_2d.h.
unsigned char* costmap_2d::Costmap2D::static_map_ [protected] |
Definition at line 675 of file costmap_2d.h.
bool costmap_2d::Costmap2D::track_unknown_space_ [protected] |
Definition at line 688 of file costmap_2d.h.
unsigned char costmap_2d::Costmap2D::unknown_cost_value_ [protected] |
Definition at line 689 of file costmap_2d.h.
double costmap_2d::Costmap2D::weight_ [protected] |
Definition at line 686 of file costmap_2d.h.