A class that implements the WorldModel interface to provide grid based collision checks for the trajectory controller using a 3D voxel grid. More...

#include <voxel_grid_model.h>

Inheritance diagram for base_local_planner::VoxelGridModel:

[legend]

List of all members.

Public Member Functions
virtual double	footprintCost (const geometry_msgs::Point &position, const std::vector< geometry_msgs::Point > &footprint, double inscribed_radius, double circumscribed_radius)
	Checks if any obstacles in the voxel grid lie inside a convex footprint that is rasterized into the grid.
void	getPoints (pcl::PointCloud< pcl::PointXYZ > &cloud)
void	updateWorld (const std::vector< geometry_msgs::Point > &footprint, const std::vector< costmap_2d::Observation > &observations, const std::vector< PlanarLaserScan > &laser_scans)
	The costmap already keeps track of world observations, so for this world model this method does nothing.
	VoxelGridModel (double size_x, double size_y, double size_z, double xy_resolution, double z_resolution, double origin_x, double origin_y, double origin_z, double max_z, double obstacle_range)
	Constructor for the VoxelGridModel.
virtual	~VoxelGridModel ()
	Destructor for the world model.
Private Member Functions
double	dist (double x0, double y0, double z0, double x1, double y1, double z1)
void	insert (pcl::PointXYZ pt)
double	lineCost (int x0, int x1, int y0, int y1)
	Rasterizes a line in the costmap grid and checks for collisions.
void	mapToWorld2D (unsigned int mx, unsigned int my, double &wx, double &wy)
void	mapToWorld3D (unsigned int mx, unsigned int my, unsigned int mz, double &wx, double &wy, double &wz)
double	pointCost (int x, int y)
	Checks the cost of a point in the costmap.
void	removePointsInScanBoundry (const PlanarLaserScan &laser_scan, double raytrace_range)
bool	worldToMap2D (double wx, double wy, unsigned int &mx, unsigned int &my)
bool	worldToMap3D (double wx, double wy, double wz, unsigned int &mx, unsigned int &my, unsigned int &mz)
Private Attributes
double	max_z_
	The height cutoff for adding points as obstacles.
voxel_grid::VoxelGrid	obstacle_grid_
double	origin_x_
double	origin_y_
double	origin_z_
double	sq_obstacle_range_
	The square distance at which we no longer add obstacles to the grid.
double	xy_resolution_
double	z_resolution_

Detailed Description

A class that implements the WorldModel interface to provide grid based collision checks for the trajectory controller using a 3D voxel grid.

Definition at line 58 of file voxel_grid_model.h.

Constructor & Destructor Documentation

base_local_planner::VoxelGridModel::VoxelGridModel	(	double	size_x,
		double	size_y,
		double	size_z,
		double	xy_resolution,
		double	z_resolution,
		double	origin_x,
		double	origin_y,
		double	origin_z,
		double	max_z,
		double	obstacle_range
	)

Constructor for the VoxelGridModel.

Parameters:

size_x	The x size of the map
size_y	The y size of the map
size_z	The z size of the map... up to 32 cells
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 value of the origin of the map
origin_y	The y value of the origin of the map
origin_z	The z value of the origin of the map
max_z	The maximum height for an obstacle to be added to the grid
obstacle_range	The maximum distance for obstacles to be added to the grid

Definition at line 43 of file voxel_grid_model.cpp.

virtual base_local_planner::VoxelGridModel::~VoxelGridModel ( ) [inline, virtual]

Destructor for the world model.

Definition at line 79 of file voxel_grid_model.h.

Member Function Documentation

double base_local_planner::VoxelGridModel::dist	(	double	x0,
		double	y0,
		double	z0,
		double	x1,
		double	y1,
		double	z1
	)		`[inline, private]`

Definition at line 156 of file voxel_grid_model.h.

double base_local_planner::VoxelGridModel::footprintCost	(	const geometry_msgs::Point &	position,
		const std::vector< geometry_msgs::Point > &	footprint,
		double	inscribed_radius,
		double	circumscribed_radius
	)		`[virtual]`

Checks if any obstacles in the voxel grid lie inside a convex footprint that is rasterized into the grid.

Parameters:

position	The position of the robot in world coordinates
footprint	The specification of the footprint of the robot in world coordinates
inscribed_radius	The radius of the inscribed circle of the robot
circumscribed_radius	The radius of the circumscribed circle of the robot

Returns:: Positive if all the points lie outside the footprint, negative otherwise

Implements base_local_planner::WorldModel.

Definition at line 49 of file voxel_grid_model.cpp.

void base_local_planner::VoxelGridModel::getPoints ( pcl::PointCloud< pcl::PointXYZ > & cloud )

Definition at line 269 of file voxel_grid_model.cpp.

void base_local_planner::VoxelGridModel::insert ( pcl::PointXYZ pt ) [inline, private]

Definition at line 160 of file voxel_grid_model.h.

double base_local_planner::VoxelGridModel::lineCost	(	int	x0,
		int	x1,
		int	y0,
		int	y1
	)		`[private]`

Rasterizes a line in the costmap grid and checks for collisions.

Parameters:

x0	The x position of the first cell in grid coordinates
y0	The y position of the first cell in grid coordinates
x1	The x position of the second cell in grid coordinates
y1	The y position of the second cell in grid coordinates

Returns:: A positive cost for a legal line... negative otherwise

Definition at line 94 of file voxel_grid_model.cpp.

void base_local_planner::VoxelGridModel::mapToWorld2D	(	unsigned int	mx,
		unsigned int	my,
		double &	wx,
		double &	wy
	)		`[inline, private]`

Definition at line 150 of file voxel_grid_model.h.

void base_local_planner::VoxelGridModel::mapToWorld3D	(	unsigned int	mx,
		unsigned int	my,
		unsigned int	mz,
		double &	wx,
		double &	wy,
		double &	wz
	)		`[inline, private]`

Definition at line 143 of file voxel_grid_model.h.

double base_local_planner::VoxelGridModel::pointCost	(	int	x,
		int	y
	)		`[private]`

Checks the cost of a point in the costmap.

Parameters:

x	The x position of the point in cell coordinates
y	The y position of the point in cell coordinates

Returns:: A positive cost for a legal point... negative otherwise

Definition at line 173 of file voxel_grid_model.cpp.

void base_local_planner::VoxelGridModel::removePointsInScanBoundry	(	const PlanarLaserScan &	laser_scan,
		double	raytrace_range
	)		`[private]`

Definition at line 215 of file voxel_grid_model.cpp.

void base_local_planner::VoxelGridModel::updateWorld	(	const std::vector< geometry_msgs::Point > &	footprint,
		const std::vector< costmap_2d::Observation > &	observations,
		const std::vector< PlanarLaserScan > &	laser_scans
	)

The costmap already keeps track of world observations, so for this world model this method does nothing.

Parameters:

footprint	The footprint of the robot in its current location
observations	The observations from various sensors
laser_scan	The scans used to clear freespace

Definition at line 182 of file voxel_grid_model.cpp.

bool base_local_planner::VoxelGridModel::worldToMap2D	(	double	wx,
		double	wy,
		unsigned int &	mx,
		unsigned int &	my
	)		`[inline, private]`

Definition at line 135 of file voxel_grid_model.h.

bool base_local_planner::VoxelGridModel::worldToMap3D	(	double	wx,
		double	wy,
		double	wz,
		unsigned int &	mx,
		unsigned int &	my,
		unsigned int &	mz
	)		`[inline, private]`