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

virtual double	footprintCost (const geometry_msgs::Point &position, const std::vector< geometry_msgs::Point > &footprint, double inscribed_radius, double circumscribed_radius)=0
	Subclass will implement this method to check a footprint at a given position and orientation for legality in the world. More...

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

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)

void	getPoints (sensor_msgs::PointCloud2 &cloud)
	Function copying the Voxel points into a point cloud. More...

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

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)

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 (const geometry_msgs::Point32 &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 90 of file voxel_grid_model.h.

Constructor & Destructor Documentation

◆ VoxelGridModel()

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 44 of file voxel_grid_model.cpp.

◆ ~VoxelGridModel()

virtual base_local_planner::VoxelGridModel::~VoxelGridModel ( )

inlinevirtual

Destructor for the world model.

Definition at line 111 of file voxel_grid_model.h.

Member Function Documentation

◆ dist()

double base_local_planner::VoxelGridModel::dist	(	double	x0,
		double	y0,
		double	z0,
		double	x1,
		double	y1,
		double	z1
	)

inlineprivate

Definition at line 192 of file voxel_grid_model.h.

◆ footprintCost() [1/4]

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 50 of file voxel_grid_model.cpp.

◆ footprintCost() [2/4]

virtual double base_local_planner::WorldModel::footprintCost

Subclass will implement this method to check a footprint at a given position and orientation for legality in the world.

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: -1 if footprint covers at least a lethal obstacle cell, or -2 if footprint covers at least a no-information cell, or -3 if footprint is partially or totally outside of the map

◆ footprintCost() [3/4]

double base_local_planner::WorldModel::footprintCost

inline

Checks if any obstacles in the costmap 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

Definition at line 134 of file world_model.h.

◆ footprintCost() [4/4]

double base_local_planner::WorldModel::footprintCost

inline

Definition at line 103 of file world_model.h.

◆ getPoints()

void base_local_planner::VoxelGridModel::getPoints ( sensor_msgs::PointCloud2 & cloud )

Function copying the Voxel points into a point cloud.

Parameters

cloud the point cloud to copy data to. It has the usual x,y,z channels

Definition at line 280 of file voxel_grid_model.cpp.

◆ insert()

void base_local_planner::VoxelGridModel::insert ( const geometry_msgs::Point32 & pt )

inlineprivate

Definition at line 196 of file voxel_grid_model.h.

◆ lineCost()

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 95 of file voxel_grid_model.cpp.

◆ mapToWorld2D()

void base_local_planner::VoxelGridModel::mapToWorld2D	(	unsigned int	mx,
		unsigned int	my,
		double &	wx,
		double &	wy
	)

inlineprivate

Definition at line 186 of file voxel_grid_model.h.

◆ mapToWorld3D()

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 179 of file voxel_grid_model.h.

◆ pointCost()

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 174 of file voxel_grid_model.cpp.

◆ removePointsInScanBoundry()

void base_local_planner::VoxelGridModel::removePointsInScanBoundry	(	const PlanarLaserScan &	laser_scan,
		double	raytrace_range
	)

private

Definition at line 226 of file voxel_grid_model.cpp.

◆ updateWorld()

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 183 of file voxel_grid_model.cpp.

◆ worldToMap2D()

bool base_local_planner::VoxelGridModel::worldToMap2D	(	double	wx,
		double	wy,
		unsigned int &	mx,
		unsigned int &	my
	)

inlineprivate

Definition at line 171 of file voxel_grid_model.h.

◆ worldToMap3D()

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 162 of file voxel_grid_model.h.

Member Data Documentation

◆ max_z_

double base_local_planner::VoxelGridModel::max_z_

private

The height cutoff for adding points as obstacles.

Definition at line 209 of file voxel_grid_model.h.

◆ obstacle_grid_

voxel_grid::VoxelGrid base_local_planner::VoxelGridModel::obstacle_grid_

private

Definition at line 203 of file voxel_grid_model.h.

◆ origin_x_

double base_local_planner::VoxelGridModel::origin_x_

private

Definition at line 206 of file voxel_grid_model.h.

◆ origin_y_

double base_local_planner::VoxelGridModel::origin_y_

private

Definition at line 207 of file voxel_grid_model.h.

◆ origin_z_

double base_local_planner::VoxelGridModel::origin_z_

private

Definition at line 208 of file voxel_grid_model.h.

◆ sq_obstacle_range_

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 210 of file voxel_grid_model.h.

◆ xy_resolution_

double base_local_planner::VoxelGridModel::xy_resolution_

private

Definition at line 204 of file voxel_grid_model.h.

◆ z_resolution_

double base_local_planner::VoxelGridModel::z_resolution_

private

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

◆ VoxelGridModel()

◆ ~VoxelGridModel()

Member Function Documentation

◆ dist()

◆ footprintCost() [1/4]

◆ footprintCost() [2/4]

◆ footprintCost() [3/4]

◆ footprintCost() [4/4]

◆ getPoints()

◆ insert()

◆ lineCost()

◆ mapToWorld2D()

◆ mapToWorld3D()

◆ pointCost()

◆ removePointsInScanBoundry()

◆ updateWorld()

◆ worldToMap2D()

◆ worldToMap3D()

Member Data Documentation

◆ max_z_

◆ obstacle_grid_

◆ origin_x_

◆ origin_y_

◆ origin_z_

◆ sq_obstacle_range_

◆ xy_resolution_

◆ z_resolution_