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:

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_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 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.

costmap_2d::VoxelCostmap2D::~VoxelCostmap2D ( )

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:

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 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:

cloud The point cloud to fill

Definition at line 370 of file voxel_costmap_2d.cpp.

void costmap_2d::VoxelCostmap2D::getVoxelGridMessage ( VoxelGrid & grid )

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_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 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_observation The 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:

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 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]`