A 2D costmap provides a mapping between points in the world and their associated "costs". More...
#include <voxel_costmap_2d.h>
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. | |
void | getPoints (pcl::PointCloud< pcl::PointXYZ > &cloud) |
Returns a point cloud for visualizing the voxel grid. | |
void | getVoxelGridMessage (VoxelGrid &grid) |
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 | updateOrigin (double new_origin_x, double new_origin_y) |
Move the origin of the costmap to a new location.... keeping data when it can. | |
VoxelCostmap2D (unsigned int cells_size_x, unsigned int cells_size_y, unsigned int cells_size_z, double xy_resolution, double z_resolution, double origin_x, double origin_y, double origin_z=0.0, double inscribed_radius=0.0, double circumscribed_radius=0.0, double inflation_radius=0.0, double obstacle_range=0.0, double 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, unsigned int unknown_threshold=0, unsigned int mark_threshold=0, unsigned char unknown_cost_value=0) | |
Constructor for a voxel grid based costmap. | |
VoxelCostmap2D (costmap_2d::Costmap2D &costmap, double z_resolution, unsigned int cells_size_z, double origin_z=0.0, unsigned int mark_threshold=0, unsigned int unknown_threshold=0) | |
~VoxelCostmap2D () | |
Destructor. | |
Static Public Member Functions | |
static void | mapToWorld3D (const unsigned int mx, const unsigned int my, const unsigned int mz, const double origin_x, const double origin_y, const double origin_z, const double x_resolution, const double y_resolution, const double z_resolution, double &wx, double &wy, double &wz) |
Protected Member Functions | |
virtual void | initMaps (unsigned int size_x, unsigned int size_y) |
Initializes the costmap, static_map, voxel grid, and markers data structures. | |
virtual void | resetMaps () |
Resets the costmap, static_map, and voxel_grid to be unknown space. | |
Protected Attributes | |
unsigned int | mark_threshold_ |
double | origin_z_ |
unsigned int | size_z_ |
unsigned int | unknown_threshold_ |
voxel_grid::VoxelGrid | voxel_grid_ |
double | xy_resolution_ |
double | z_resolution_ |
Private Member Functions | |
double | dist (double x0, double y0, double z0, double x1, double y1, double z1) |
void | finishConfiguration (costmap_2d::Costmap2DConfig &config) |
void | mapToWorld3D (unsigned int mx, unsigned int my, unsigned int mz, double &wx, double &wy, double &wz) |
void | raytraceFreespace (const Observation &clearing_observation) |
Clear freespace from an observation. | |
void | updateObstacles (const std::vector< Observation > &observations, std::priority_queue< CellData > &inflation_queue) |
Insert new obstacles into the cost map. | |
bool | worldToMap3D (double wx, double wy, double wz, unsigned int &mx, unsigned int &my, unsigned int &mz) |
bool | worldToMap3DFloat (double wx, double wy, double wz, double &mx, double &my, double &mz) |
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition at line 56 of file voxel_costmap_2d.h.
costmap_2d::VoxelCostmap2D::VoxelCostmap2D | ( | unsigned int | cells_size_x, |
unsigned int | cells_size_y, | ||
unsigned int | cells_size_z, | ||
double | xy_resolution, | ||
double | z_resolution, | ||
double | origin_x, | ||
double | origin_y, | ||
double | origin_z = 0.0 , |
||
double | inscribed_radius = 0.0 , |
||
double | circumscribed_radius = 0.0 , |
||
double | inflation_radius = 0.0 , |
||
double | obstacle_range = 0.0 , |
||
double | 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 , |
||
unsigned int | unknown_threshold = 0 , |
||
unsigned int | mark_threshold = 0 , |
||
unsigned char | unknown_cost_value = 0 |
||
) |
Constructor for a voxel grid based costmap.
cells_size_x | The x size of the map in cells |
cells_size_y | The y size of the map in cells |
cells_size_z | The z size of the map in cells... up to 32 cells max |
xy_resolution | The horizontal resolution of the map in meters/cell |
z_resolution | The vertical resolution of the map in meters/cell |
origin_x | The x origin of the map |
origin_y | The y origin of the map |
origin_z | The z 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 |
obstacle_range | The maximum range at which obstacles will be put into the costmap |
raytrace_range | The maximum distance we'll raytrace out to |
weight | The scaling factor for the cost function. Should be 0 < weight <= 1. Lower values reduce effective cost. |
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 |
unknown_threshold | The maximum number of unknown voxel cells that can exist in a column considered as free space |
mark_threshold | The maximum number of marked voxel cells that can exist in a column considered as free space |
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 free |
Definition at line 46 of file voxel_costmap_2d.cpp.
costmap_2d::VoxelCostmap2D::VoxelCostmap2D | ( | costmap_2d::Costmap2D & | costmap, |
double | z_resolution, | ||
unsigned int | cells_size_z, | ||
double | origin_z = 0.0 , |
||
unsigned int | mark_threshold = 0 , |
||
unsigned int | unknown_threshold = 0 |
||
) |
Definition at line 58 of file voxel_costmap_2d.cpp.
Destructor.
Definition at line 67 of file voxel_costmap_2d.cpp.
void costmap_2d::VoxelCostmap2D::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 from costmap_2d::Costmap2D.
Definition at line 309 of file voxel_costmap_2d.cpp.
double costmap_2d::VoxelCostmap2D::dist | ( | double | x0, |
double | y0, | ||
double | z0, | ||
double | x1, | ||
double | y1, | ||
double | z1 | ||
) | [inline, private] |
Definition at line 204 of file voxel_costmap_2d.h.
void costmap_2d::VoxelCostmap2D::finishConfiguration | ( | costmap_2d::Costmap2DConfig & | config | ) | [private, virtual] |
Reimplemented from costmap_2d::Costmap2D.
Definition at line 390 of file voxel_costmap_2d.cpp.
void costmap_2d::VoxelCostmap2D::getPoints | ( | pcl::PointCloud< pcl::PointXYZ > & | cloud | ) |
Returns a point cloud for visualizing the voxel grid.
cloud | The point cloud to fill |
Definition at line 372 of file voxel_costmap_2d.cpp.
void costmap_2d::VoxelCostmap2D::getVoxelGridMessage | ( | VoxelGrid & | grid | ) |
Definition at line 355 of file voxel_costmap_2d.cpp.
void costmap_2d::VoxelCostmap2D::initMaps | ( | unsigned int | size_x, |
unsigned int | size_y | ||
) | [protected, virtual] |
Initializes the costmap, static_map, voxel grid, 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 from costmap_2d::Costmap2D.
Definition at line 69 of file voxel_costmap_2d.cpp.
static void costmap_2d::VoxelCostmap2D::mapToWorld3D | ( | const unsigned int | mx, |
const unsigned int | my, | ||
const unsigned int | mz, | ||
const double | origin_x, | ||
const double | origin_y, | ||
const double | origin_z, | ||
const double | x_resolution, | ||
const double | y_resolution, | ||
const double | z_resolution, | ||
double & | wx, | ||
double & | wy, | ||
double & | wz | ||
) | [inline, static] |
Definition at line 132 of file voxel_costmap_2d.h.
void costmap_2d::VoxelCostmap2D::mapToWorld3D | ( | unsigned int | mx, |
unsigned int | my, | ||
unsigned int | mz, | ||
double & | wx, | ||
double & | wy, | ||
double & | wz | ||
) | [inline, private] |
Definition at line 197 of file voxel_costmap_2d.h.
void costmap_2d::VoxelCostmap2D::raytraceFreespace | ( | const Observation & | clearing_observation | ) | [private, virtual] |
Clear freespace from an observation.
clearing_observation | The observation used to raytrace |
Reimplemented from costmap_2d::Costmap2D.
Definition at line 174 of file voxel_costmap_2d.cpp.
void costmap_2d::VoxelCostmap2D::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 from costmap_2d::Costmap2D.
Definition at line 80 of file voxel_costmap_2d.cpp.
void costmap_2d::VoxelCostmap2D::resetMaps | ( | ) | [protected, virtual] |
Resets the costmap, static_map, and voxel_grid to be unknown space.
Reimplemented from costmap_2d::Costmap2D.
Definition at line 75 of file voxel_costmap_2d.cpp.
void costmap_2d::VoxelCostmap2D::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 from costmap_2d::Costmap2D.
Definition at line 130 of file voxel_costmap_2d.cpp.
void costmap_2d::VoxelCostmap2D::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 from costmap_2d::Costmap2D.
Definition at line 253 of file voxel_costmap_2d.cpp.
bool costmap_2d::VoxelCostmap2D::worldToMap3D | ( | double | wx, |
double | wy, | ||
double | wz, | ||
unsigned int & | mx, | ||
unsigned int & | my, | ||
unsigned int & | mz | ||
) | [inline, private] |
Definition at line 183 of file voxel_costmap_2d.h.
bool costmap_2d::VoxelCostmap2D::worldToMap3DFloat | ( | double | wx, |
double | wy, | ||
double | wz, | ||
double & | mx, | ||
double & | my, | ||
double & | mz | ||
) | [inline, private] |
Definition at line 170 of file voxel_costmap_2d.h.
unsigned int costmap_2d::VoxelCostmap2D::mark_threshold_ [protected] |
Definition at line 213 of file voxel_costmap_2d.h.
double costmap_2d::VoxelCostmap2D::origin_z_ [protected] |
Definition at line 212 of file voxel_costmap_2d.h.
unsigned int costmap_2d::VoxelCostmap2D::size_z_ [protected] |
Definition at line 213 of file voxel_costmap_2d.h.
unsigned int costmap_2d::VoxelCostmap2D::unknown_threshold_ [protected] |
Definition at line 213 of file voxel_costmap_2d.h.
Definition at line 211 of file voxel_costmap_2d.h.
double costmap_2d::VoxelCostmap2D::xy_resolution_ [protected] |
Definition at line 212 of file voxel_costmap_2d.h.
double costmap_2d::VoxelCostmap2D::z_resolution_ [protected] |
Definition at line 212 of file voxel_costmap_2d.h.