Public Member Functions | Static Public Member Functions | Protected Member Functions | Protected Attributes | Private Member Functions
costmap_2d::VoxelCostmap2D Class Reference

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

#include <voxel_costmap_2d.h>

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

List of all members.

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)

Detailed Description

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

Definition at line 57 of file voxel_costmap_2d.h.


Constructor & Destructor Documentation

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.

Parameters:
cells_size_xThe x size of the map in cells
cells_size_yThe y size of the map in cells
cells_size_zThe z size of the map in cells... up to 32 cells max
xy_resolutionThe horizontal resolution of the map in meters/cell
z_resolutionThe vertical resolution of the map in meters/cell
origin_xThe x origin of the map
origin_yThe y origin of the map
origin_zThe z 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
obstacle_rangeThe maximum range at which obstacles will be put into the costmap
raytrace_rangeThe maximum distance we'll raytrace out to
weightThe scaling factor for the cost function. Should be 0 < weight <= 1. Lower values reduce effective cost.
static_dataData used to initialize the costmap
lethal_thresholdThe cost threshold at which a point in the static data is considered a lethal obstacle
unknown_thresholdThe maximum number of unknown voxel cells that can exist in a column considered as free space
mark_thresholdThe maximum number of marked voxel cells that can exist in a column considered as free space
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 free

Definition at line 44 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 56 of file voxel_costmap_2d.cpp.

Destructor.

Definition at line 65 of file voxel_costmap_2d.cpp.


Member Function Documentation

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.

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 from costmap_2d::Costmap2D.

Definition at line 307 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 205 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 388 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.

Parameters:
cloudThe point cloud to fill

Definition at line 370 of file voxel_costmap_2d.cpp.

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

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

Reimplemented from costmap_2d::Costmap2D.

Definition at line 67 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 133 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 198 of file voxel_costmap_2d.h.

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

Clear freespace from an observation.

Parameters:
clearing_observationThe observation used to raytrace

Reimplemented from costmap_2d::Costmap2D.

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

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 from costmap_2d::Costmap2D.

Definition at line 78 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 73 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.

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

Reimplemented from costmap_2d::Costmap2D.

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

Parameters:
new_origin_xThe x coordinate of the new origin
new_origin_yThe y coordinate of the new origin

Reimplemented from costmap_2d::Costmap2D.

Definition at line 251 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 184 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 171 of file voxel_costmap_2d.h.


Member Data Documentation

Definition at line 214 of file voxel_costmap_2d.h.

Definition at line 213 of file voxel_costmap_2d.h.

unsigned int costmap_2d::VoxelCostmap2D::size_z_ [protected]

Definition at line 214 of file voxel_costmap_2d.h.

Definition at line 214 of file voxel_costmap_2d.h.

Definition at line 212 of file voxel_costmap_2d.h.

Definition at line 213 of file voxel_costmap_2d.h.

Definition at line 213 of file voxel_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