#include <voxel_layer.h>

Inheritance diagram for costmap_2d::VoxelLayer:

[legend]

Public Member Functions
bool	isDiscretized ()

virtual void	matchSize ()
	Implement this to make this layer match the size of the parent costmap. More...

virtual void	onInitialize ()
	This is called at the end of initialize(). Override to implement subclass-specific initialization. More...

virtual void	reset ()

virtual void	updateBounds (double robot_x, double robot_y, double robot_yaw, double min_x, double min_y, double max_x, double max_y)
	This is called by the LayeredCostmap to poll this plugin as to how much of the costmap it needs to update. Each layer can increase the size of this bounds. More...

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

	VoxelLayer ()

virtual	~VoxelLayer ()

Public Member Functions inherited from costmap_2d::ObstacleLayer
virtual void	activate ()
	Restart publishers if they've been stopped. More...

void	addStaticObservation (costmap_2d::Observation &obs, bool marking, bool clearing)

void	clearStaticObservations (bool marking, bool clearing)

virtual void	deactivate ()
	Stop publishers. More...

void	laserScanCallback (const sensor_msgs::LaserScanConstPtr &message, const boost::shared_ptr< costmap_2d::ObservationBuffer > &buffer)
	A callback to handle buffering LaserScan messages. More...

void	laserScanValidInfCallback (const sensor_msgs::LaserScanConstPtr &message, const boost::shared_ptr< ObservationBuffer > &buffer)
	A callback to handle buffering LaserScan messages which need filtering to turn Inf values into range_max. More...

	ObstacleLayer ()

void	pointCloud2Callback (const sensor_msgs::PointCloud2ConstPtr &message, const boost::shared_ptr< costmap_2d::ObservationBuffer > &buffer)
	A callback to handle buffering PointCloud2 messages. More...

void	pointCloudCallback (const sensor_msgs::PointCloudConstPtr &message, const boost::shared_ptr< costmap_2d::ObservationBuffer > &buffer)
	A callback to handle buffering PointCloud messages. More...

virtual void	updateCosts (costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
	Actually update the underlying costmap, only within the bounds calculated during UpdateBounds(). More...

virtual	~ObstacleLayer ()

Public Member Functions inherited from costmap_2d::CostmapLayer
void	addExtraBounds (double mx0, double my0, double mx1, double my1)

virtual void	clearArea (int start_x, int start_y, int end_x, int end_y, bool invert_area=false)

	CostmapLayer ()

bool	isDiscretized ()

Public Member Functions inherited from costmap_2d::Layer
const std::vector< geometry_msgs::Point > &	getFootprint () const
	Convenience function for layered_costmap_->getFootprint(). More...

const std::string &	getName () const noexcept

void	initialize (LayeredCostmap parent, std::string name, tf2_ros::Buffer tf)

bool	isCurrent () const
	Check to make sure all the data in the layer is up to date. If the layer is not up to date, then it may be unsafe to plan using the data from this layer, and the planner may need to know. More...

bool	isEnabled () const noexcept
	getter if the current layer is enabled. More...

	Layer ()

virtual void	onFootprintChanged ()
	LayeredCostmap calls this whenever the footprint there changes (via LayeredCostmap::setFootprint()). Override to be notified of changes to the robot's footprint. More...

virtual	~Layer ()

Public Member Functions inherited from costmap_2d::Costmap2D
unsigned int	cellDistance (double world_dist)
	Given distance in the world... convert it to cells. More...

void	convexFillCells (const std::vector< MapLocation > &polygon, std::vector< MapLocation > &polygon_cells)
	Get the map cells that fill a convex polygon. More...

bool	copyCostmapWindow (const Costmap2D &map, double win_origin_x, double win_origin_y, double win_size_x, double win_size_y)
	Turn this costmap into a copy of a window of a costmap passed in. More...

	Costmap2D ()
	Default constructor. More...

	Costmap2D (const Costmap2D &map)
	Copy constructor for a costmap, creates a copy efficiently. More...

	Costmap2D (unsigned int cells_size_x, unsigned int cells_size_y, double resolution, double origin_x, double origin_y, unsigned char default_value=0)
	Constructor for a costmap. More...

unsigned char *	getCharMap () const
	Will return a pointer to the underlying unsigned char array used as the costmap. More...

unsigned char	getCost (unsigned int mx, unsigned int my) const
	Get the cost of a cell in the costmap. More...

unsigned char	getDefaultValue ()

unsigned int	getIndex (unsigned int mx, unsigned int my) const
	Given two map coordinates... compute the associated index. More...

mutex_t *	getMutex ()

double	getOriginX () const
	Accessor for the x origin of the costmap. More...

double	getOriginY () const
	Accessor for the y origin of the costmap. More...

double	getResolution () const
	Accessor for the resolution of the costmap. More...

unsigned int	getSizeInCellsX () const
	Accessor for the x size of the costmap in cells. More...

unsigned int	getSizeInCellsY () const
	Accessor for the y size of the costmap in cells. More...

double	getSizeInMetersX () const
	Accessor for the x size of the costmap in meters. More...

double	getSizeInMetersY () const
	Accessor for the y size of the costmap in meters. More...

void	indexToCells (unsigned int index, unsigned int &mx, unsigned int &my) const
	Given an index... compute the associated map coordinates. More...

void	mapToWorld (unsigned int mx, unsigned int my, double &wx, double &wy) const
	Convert from map coordinates to world coordinates. More...

Costmap2D &	operator= (const Costmap2D &map)
	Overloaded assignment operator. More...

void	polygonOutlineCells (const std::vector< MapLocation > &polygon, std::vector< MapLocation > &polygon_cells)
	Get the map cells that make up the outline of a polygon. More...

void	resetMap (unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)

void	resizeMap (unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y)

bool	saveMap (std::string file_name)
	Save the costmap out to a pgm file. More...

bool	setConvexPolygonCost (const std::vector< geometry_msgs::Point > &polygon, unsigned char cost_value)
	Sets the cost of a convex polygon to a desired value. More...

void	setCost (unsigned int mx, unsigned int my, unsigned char cost)
	Set the cost of a cell in the costmap. More...

void	setDefaultValue (unsigned char c)

bool	worldToMap (double wx, double wy, unsigned int &mx, unsigned int &my) const
	Convert from world coordinates to map coordinates. More...

void	worldToMapEnforceBounds (double wx, double wy, int &mx, int &my) const
	Convert from world coordinates to map coordinates, constraining results to legal bounds. More...

void	worldToMapNoBounds (double wx, double wy, int &mx, int &my) const
	Convert from world coordinates to map coordinates without checking for legal bounds. More...

virtual	~Costmap2D ()
	Destructor. More...

Protected Member Functions
virtual void	resetMaps ()
	Resets the costmap and static_map to be unknown space. More...

virtual void	setupDynamicReconfigure (ros::NodeHandle &nh)

Protected Member Functions inherited from costmap_2d::ObstacleLayer
bool	getClearingObservations (std::vector< costmap_2d::Observation > &clearing_observations) const
	Get the observations used to clear space. More...

bool	getMarkingObservations (std::vector< costmap_2d::Observation > &marking_observations) const
	Get the observations used to mark space. More...

void	updateFootprint (double robot_x, double robot_y, double robot_yaw, double min_x, double min_y, double max_x, double max_y)

void	updateRaytraceBounds (double ox, double oy, double wx, double wy, double range, double min_x, double min_y, double max_x, double max_y)

Protected Member Functions inherited from costmap_2d::CostmapLayer
void	touch (double x, double y, double min_x, double min_y, double max_x, double max_y)

void	updateWithAddition (costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)

void	updateWithMax (costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)

void	updateWithOverwrite (costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)

void	updateWithTrueOverwrite (costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)

void	useExtraBounds (double min_x, double min_y, double max_x, double max_y)

Protected Member Functions inherited from costmap_2d::Costmap2D
template<typename data_type >
void	copyMapRegion (data_type source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y, unsigned int sm_size_x, data_type dest_map, unsigned int dm_lower_left_x, unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x, unsigned int region_size_y)
	Copy a region of a source map into a destination map. More...

virtual void	deleteMaps ()
	Deletes the costmap, static_map, and markers data structures. More...

virtual void	initMaps (unsigned int size_x, unsigned int size_y)
	Initializes the costmap, static_map, and markers data structures. More...

template<class ActionType >
void	raytraceLine (ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, unsigned int max_length=UINT_MAX)
	Raytrace a line and apply some action at each step. More...

Private Member Functions
void	clearNonLethal (double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)

double	dist (double x0, double y0, double z0, double x1, double y1, double z1)

void	mapToWorld3D (unsigned int mx, unsigned int my, unsigned int mz, double &wx, double &wy, double &wz)

virtual void	raytraceFreespace (const costmap_2d::Observation &clearing_observation, double min_x, double min_y, double max_x, double max_y)
	Clear freespace based on one observation. More...

void	reconfigureCB (costmap_2d::VoxelPluginConfig &config, uint32_t level)

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)

Private Attributes
sensor_msgs::PointCloud	clearing_endpoints_

ros::Publisher	clearing_endpoints_pub_

unsigned int	mark_threshold_

double	origin_z_

bool	publish_voxel_

unsigned int	size_z_

unsigned int	unknown_threshold_

dynamic_reconfigure::Server< costmap_2d::VoxelPluginConfig > *	voxel_dsrv_

voxel_grid::VoxelGrid	voxel_grid_

ros::Publisher	voxel_pub_

double	z_resolution_

Additional Inherited Members
Public Types inherited from costmap_2d::Costmap2D
typedef boost::recursive_mutex	mutex_t

Protected Attributes inherited from costmap_2d::ObstacleLayer
std::vector< boost::shared_ptr< costmap_2d::ObservationBuffer > >	clearing_buffers_
	Used to store observation buffers used for clearing obstacles. More...

int	combination_method_

dynamic_reconfigure::Server< costmap_2d::ObstaclePluginConfig > *	dsrv_

bool	footprint_clearing_enabled_

std::string	global_frame_
	The global frame for the costmap. More...

std::vector< boost::shared_ptr< costmap_2d::ObservationBuffer > >	marking_buffers_
	Used to store observation buffers used for marking obstacles. More...

double	max_obstacle_height_
	Max Obstacle Height. More...

std::vector< boost::shared_ptr< costmap_2d::ObservationBuffer > >	observation_buffers_
	Used to store observations from various sensors. More...

std::vector< boost::shared_ptr< tf2_ros::MessageFilterBase > >	observation_notifiers_
	Used to make sure that transforms are available for each sensor. More...

std::vector< boost::shared_ptr< message_filters::SubscriberBase > >	observation_subscribers_
	Used for the observation message filters. More...

laser_geometry::LaserProjection	projector_
	Used to project laser scans into point clouds. More...

bool	rolling_window_

std::vector< costmap_2d::Observation >	static_clearing_observations_

std::vector< costmap_2d::Observation >	static_marking_observations_

std::vector< geometry_msgs::Point >	transformed_footprint_

Protected Attributes inherited from costmap_2d::CostmapLayer
bool	has_extra_bounds_

Protected Attributes inherited from costmap_2d::Layer
bool	current_

bool	enabled_

LayeredCostmap *	layered_costmap_

std::string	name_

tf2_ros::Buffer *	tf_

Protected Attributes inherited from costmap_2d::Costmap2D
unsigned char *	costmap_

unsigned char	default_value_

double	origin_x_

double	origin_y_

double	resolution_

unsigned int	size_x_

unsigned int	size_y_

Detailed Description

Definition at line 97 of file voxel_layer.h.

Constructor & Destructor Documentation

◆ VoxelLayer()

costmap_2d::VoxelLayer::VoxelLayer ( )

inline

Definition at line 136 of file voxel_layer.h.

◆ ~VoxelLayer()

costmap_2d::VoxelLayer::~VoxelLayer ( )

virtual

Definition at line 75 of file voxel_layer.cpp.

Member Function Documentation

◆ clearNonLethal()

void costmap_2d::VoxelLayer::clearNonLethal	(	double	wx,
		double	wy,
		double	w_size_x,
		double	w_size_y,
		bool	clear_no_info
	)

private

Definition at line 217 of file voxel_layer.cpp.

◆ dist()

double costmap_2d::VoxelLayer::dist	(	double	x0,
		double	y0,
		double	z0,
		double	x1,
		double	y1,
		double	z1
	)

inlineprivate

Definition at line 214 of file voxel_layer.h.

◆ isDiscretized()

bool costmap_2d::VoxelLayer::isDiscretized ( )

inline

Definition at line 149 of file voxel_layer.h.

◆ mapToWorld3D()

void costmap_2d::VoxelLayer::mapToWorld3D	(	unsigned int	mx,
		unsigned int	my,
		unsigned int	mz,
		double &	wx,
		double &	wy,
		double &	wz
	)

inlineprivate

Definition at line 206 of file voxel_layer.h.

◆ matchSize()

void costmap_2d::VoxelLayer::matchSize ( )

virtual

Implement this to make this layer match the size of the parent costmap.

Reimplemented from costmap_2d::CostmapLayer.

Definition at line 95 of file voxel_layer.cpp.

◆ onInitialize()

void costmap_2d::VoxelLayer::onInitialize ( )

virtual

This is called at the end of initialize(). Override to implement subclass-specific initialization.

tf_, name_, and layered_costmap_ will all be set already when this is called.

Reimplemented from costmap_2d::ObstacleLayer.

Definition at line 55 of file voxel_layer.cpp.

◆ raytraceFreespace()

void costmap_2d::VoxelLayer::raytraceFreespace	(	const costmap_2d::Observation &	clearing_observation,
		double *	min_x,
		double *	min_y,
		double *	max_x,
		double *	max_y
	)

privatevirtual

Clear freespace based on one observation.

Parameters

clearing_observation	The observation used to raytrace
min_x
min_y
max_x
max_y

Reimplemented from costmap_2d::ObstacleLayer.

Definition at line 268 of file voxel_layer.cpp.

◆ reconfigureCB()

void costmap_2d::VoxelLayer::reconfigureCB	(	costmap_2d::VoxelPluginConfig &	config,
		uint32_t	level
	)

private

Definition at line 81 of file voxel_layer.cpp.

◆ reset()

void costmap_2d::VoxelLayer::reset ( )

virtual

Reimplemented from costmap_2d::ObstacleLayer.

Definition at line 102 of file voxel_layer.cpp.

◆ resetMaps()

void costmap_2d::VoxelLayer::resetMaps ( )

protectedvirtual

Resets the costmap and static_map to be unknown space.

Reimplemented from costmap_2d::Costmap2D.

Definition at line 110 of file voxel_layer.cpp.

◆ setupDynamicReconfigure()

void costmap_2d::VoxelLayer::setupDynamicReconfigure ( ros::NodeHandle & nh )

protectedvirtual

Reimplemented from costmap_2d::ObstacleLayer.

Definition at line 67 of file voxel_layer.cpp.

◆ updateBounds()

void costmap_2d::VoxelLayer::updateBounds	(	double	robot_x,
		double	robot_y,
		double	robot_yaw,
		double *	min_x,
		double *	min_y,
		double *	max_x,
		double *	max_y
	)

virtual

This is called by the LayeredCostmap to poll this plugin as to how much of the costmap it needs to update. Each layer can increase the size of this bounds.

For more details, see "Layered Costmaps for Context-Sensitive Navigation", by Lu et. Al, IROS 2014.

Reimplemented from costmap_2d::ObstacleLayer.

Definition at line 116 of file voxel_layer.cpp.

◆ updateOrigin()

void costmap_2d::VoxelLayer::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_x	The x coordinate of the new origin
new_origin_y	The y coordinate of the new origin

Reimplemented from costmap_2d::Costmap2D.

Definition at line 392 of file voxel_layer.cpp.

◆ worldToMap3D()

bool costmap_2d::VoxelLayer::worldToMap3D	(	double	wx,
		double	wy,
		double	wz,
		unsigned int &	mx,
		unsigned int &	my,
		unsigned int &	mz
	)

inlineprivate

Definition at line 191 of file voxel_layer.h.

◆ worldToMap3DFloat()

bool costmap_2d::VoxelLayer::worldToMap3DFloat	(	double	wx,
		double	wy,
		double	wz,
		double &	mx,
		double &	my,
		double &	mz
	)

inlineprivate

Definition at line 178 of file voxel_layer.h.

Member Data Documentation

◆ clearing_endpoints_

sensor_msgs::PointCloud costmap_2d::VoxelLayer::clearing_endpoints_

private

Definition at line 176 of file voxel_layer.h.

◆ clearing_endpoints_pub_

ros::Publisher costmap_2d::VoxelLayer::clearing_endpoints_pub_

private

Definition at line 175 of file voxel_layer.h.

◆ mark_threshold_

unsigned int costmap_2d::VoxelLayer::mark_threshold_

private

Definition at line 174 of file voxel_layer.h.

◆ origin_z_

double costmap_2d::VoxelLayer::origin_z_

private

Definition at line 173 of file voxel_layer.h.

◆ publish_voxel_

bool costmap_2d::VoxelLayer::publish_voxel_

private

Definition at line 170 of file voxel_layer.h.

◆ size_z_

unsigned int costmap_2d::VoxelLayer::size_z_

private

Definition at line 174 of file voxel_layer.h.

◆ unknown_threshold_

unsigned int costmap_2d::VoxelLayer::unknown_threshold_

private

Definition at line 174 of file voxel_layer.h.

◆ voxel_dsrv_

dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>* costmap_2d::VoxelLayer::voxel_dsrv_

private

Definition at line 168 of file voxel_layer.h.

◆ voxel_grid_

voxel_grid::VoxelGrid costmap_2d::VoxelLayer::voxel_grid_

private

Definition at line 172 of file voxel_layer.h.

◆ voxel_pub_

ros::Publisher costmap_2d::VoxelLayer::voxel_pub_

private

Definition at line 171 of file voxel_layer.h.

◆ z_resolution_

double costmap_2d::VoxelLayer::z_resolution_

private

Definition at line 173 of file voxel_layer.h.

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

Public Member Functions

Protected Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ VoxelLayer()

◆ ~VoxelLayer()

Member Function Documentation

◆ clearNonLethal()

◆ dist()

◆ isDiscretized()

◆ mapToWorld3D()

◆ matchSize()

◆ onInitialize()

◆ raytraceFreespace()

◆ reconfigureCB()

◆ reset()

◆ resetMaps()

◆ setupDynamicReconfigure()

◆ updateBounds()

◆ updateOrigin()

◆ worldToMap3D()

◆ worldToMap3DFloat()

Member Data Documentation

◆ clearing_endpoints_

◆ clearing_endpoints_pub_

◆ mark_threshold_

◆ origin_z_

◆ publish_voxel_

◆ size_z_

◆ unknown_threshold_

◆ voxel_dsrv_

◆ voxel_grid_

◆ voxel_pub_

◆ z_resolution_