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]

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

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

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

virtual	~VoxelGridModel ()
	Destructor for the world model. More...

Public Member Functions inherited from base_local_planner::WorldModel
double	footprintCost (double x, double y, double theta, const std::vector< geometry_msgs::Point > &footprint_spec, double inscribed_radius=0.0, double circumscribed_radius=0.0)

double	footprintCost (const geometry_msgs::Point &position, const std::vector< geometry_msgs::Point > &footprint, double inscribed_radius, double circumscribed_radius, double extra)
	Checks if any obstacles in the costmap lie inside a convex footprint that is rasterized into the grid. More...

virtual	~WorldModel ()
	Subclass will implement a destructor. More...

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

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

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

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

double	xy_resolution_

double	z_resolution_

Additional Inherited Members
Protected Member Functions inherited from base_local_planner::WorldModel
	WorldModel ()

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 ( )

inlinevirtual

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
	)

inlineprivate

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 )

inlineprivate

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
	)

inlineprivate

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
	)

inlineprivate

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
	)

inlineprivate

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
	)

inlineprivate

Definition at line 126 of file voxel_grid_model.h.

Member Data Documentation

double base_local_planner::VoxelGridModel::max_z_

private

The height cutoff for adding points as obstacles.

Definition at line 173 of file voxel_grid_model.h.

voxel_grid::VoxelGrid base_local_planner::VoxelGridModel::obstacle_grid_

private

Definition at line 167 of file voxel_grid_model.h.

double base_local_planner::VoxelGridModel::origin_x_

private

Definition at line 170 of file voxel_grid_model.h.

double base_local_planner::VoxelGridModel::origin_y_

private

Definition at line 171 of file voxel_grid_model.h.

double base_local_planner::VoxelGridModel::origin_z_

private

Definition at line 172 of file voxel_grid_model.h.

double base_local_planner::VoxelGridModel::sq_obstacle_range_

private

The square distance at which we no longer add obstacles to the grid.

Definition at line 174 of file voxel_grid_model.h.

double base_local_planner::VoxelGridModel::xy_resolution_

private

Definition at line 168 of file voxel_grid_model.h.

double base_local_planner::VoxelGridModel::z_resolution_

private

Definition at line 169 of file voxel_grid_model.h.

The documentation for this class was generated from the following files:

Public Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation