#include <costmap_prohibition_layer.h>

Inheritance diagram for costmap_prohibition_layer_namespace::CostmapProhibitionLayer:

Public Member Functions
	CostmapProhibitionLayer ()

virtual void	onInitialize ()

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

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

virtual	~CostmapProhibitionLayer ()

Public Member Functions inherited from costmap_2d::Layer
virtual void	activate ()

virtual void	deactivate ()

const std::vector< geometry_msgs::Point > &	getFootprint () const

std::string	getName () const

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

bool	isCurrent () const

	Layer ()

virtual void	matchSize ()

virtual void	onFootprintChanged ()

virtual void	reset ()

virtual	~Layer ()

Private Member Functions
void	computeMapBounds ()

bool	getPoint (XmlRpc::XmlRpcValue &val, geometry_msgs::Point &point)

bool	parseProhibitionListFromYaml (ros::NodeHandle *nhandle, const std::string &param)

void	polygonOutlineCells (const std::vector< PointInt > &polygon, std::vector< PointInt > &polygon_cells)

void	rasterizePolygon (const std::vector< PointInt > &polygon, std::vector< PointInt > &polygon_cells, bool fill)

void	raytrace (int x0, int y0, int x1, int y1, std::vector< PointInt > &cells)

void	reconfigureCB (CostmapProhibitionLayerConfig &config, uint32_t level)

void	setPolygonCost (costmap_2d::Costmap2D &master_grid, const std::vector< geometry_msgs::Point > &polygon, unsigned char cost, int min_i, int min_j, int max_i, int max_j, bool fill_polygon)

Private Attributes
double	_costmap_resolution
	resolution of the overlayed costmap to create the thinnest line out of two points More...

std::mutex	_data_mutex
	mutex for the accessing _prohibition_points and _prohibition_polygons More...

dynamic_reconfigure::Server< CostmapProhibitionLayerConfig > *	_dsrv
	dynamic_reconfigure server for the costmap More...

bool	_fill_polygons
	if true, all cells that are located in the interior of polygons are marked as obstacle as well More...

double	_max_x

double	_max_y
	cached map bounds More...

double	_min_x

double	_min_y

std::vector< geometry_msgs::Point >	_prohibition_points
	vector to save the lonely points in source coordinates More...

std::vector< std::vector< geometry_msgs::Point > >	_prohibition_polygons
	vector to save the polygons (including lines) in source coordinates More...

Additional Inherited Members
Protected Attributes inherited from costmap_2d::Layer
bool	current_

bool	enabled_

LayeredCostmap *	layered_costmap_

std::string	name_

tf::TransformListener *	tf_

Detailed Description

Definition at line 65 of file costmap_prohibition_layer.h.

Constructor & Destructor Documentation

costmap_prohibition_layer_namespace::CostmapProhibitionLayer::CostmapProhibitionLayer ( )

default constructor

Definition at line 49 of file costmap_prohibition_layer.cpp.

costmap_prohibition_layer_namespace::CostmapProhibitionLayer::~CostmapProhibitionLayer ( )

virtual

destructor

Definition at line 53 of file costmap_prohibition_layer.cpp.

Member Function Documentation

void costmap_prohibition_layer_namespace::CostmapProhibitionLayer::computeMapBounds ( )

private

Compute bounds in world coordinates for the current set of points and polygons. The result is stored in class members _min_x, _min_y, _max_x and _max_y.

Definition at line 141 of file costmap_prohibition_layer.cpp.

bool costmap_prohibition_layer_namespace::CostmapProhibitionLayer::getPoint	(	XmlRpc::XmlRpcValue &	val,
		geometry_msgs::Point &	point
	)

private

get a geometry_msgs::Point from a YAML-Array The z-coordinate get always written to zero!

Parameters

val	YAML-array with to point-coordinates (x and y)
point	variable where the determined point get saved

Returns: bool true if the determining was successful false if it wasn't

Definition at line 442 of file costmap_prohibition_layer.cpp.

void costmap_prohibition_layer_namespace::CostmapProhibitionLayer::onInitialize ( )

virtual

function which get called at initializing the costmap define the reconfige callback, get the reoslution and read the prohibitions from the ros-parameter server

Reimplemented from costmap_2d::Layer.

Definition at line 59 of file costmap_prohibition_layer.cpp.

bool costmap_prohibition_layer_namespace::CostmapProhibitionLayer::parseProhibitionListFromYaml	(	ros::NodeHandle *	nhandle,
		const std::string &	param
	)

private

read the prohibition areas in YAML-Format from the ROS parameter server in the namespace of this plugin e.g. /move_base/global_costmap/prohibition_layer/param

Parameters

nhandle	pointer to the ros-Node handle
param	name of the parameter where the prohibition areas saved in YAML format

Returns: bool true if the parsing was successful false if it wasn't

Definition at line 329 of file costmap_prohibition_layer.cpp.

void costmap_prohibition_layer_namespace::CostmapProhibitionLayer::polygonOutlineCells	(	const std::vector< PointInt > &	polygon,
		std::vector< PointInt > &	polygon_cells
	)

private

Extract the boundary of a polygon in terms of map cells

Remarks: This method is based on Costmap2D::polygonOutlineCells() but accounts for a self-implemented raytrace algorithm and allows negative map coordinates

Parameters

	polygon	polygon defined by a vector of map coordinates
[out]	polygon_cells	new cells in map coordinates are pushed back on this container

Definition at line 206 of file costmap_prohibition_layer.cpp.

void costmap_prohibition_layer_namespace::CostmapProhibitionLayer::rasterizePolygon	(	const std::vector< PointInt > &	polygon,
		std::vector< PointInt > &	polygon_cells,
		bool	fill
	)

private

Convert polygon (in map coordinates) to a set of cells in the map

Remarks: This method is mainly based on Costmap2D::convexFillCells() but accounts for a self-implemented polygonOutlineCells() method and allows negative map coordinates

Parameters

	polygon	polygon defined by a vector of map coordinates
[out]	polygon_cells	new cells in map coordinates are pushed back on this container
	fill	if true, the interior of the polygon will be considered as well

Definition at line 252 of file costmap_prohibition_layer.cpp.

void costmap_prohibition_layer_namespace::CostmapProhibitionLayer::raytrace	(	int	x0,
		int	y0,
		int	x1,
		int	y1,
		std::vector< PointInt > &	cells
	)

private

Rasterize line between two map coordinates into a set of cells

Remarks: Since Costmap2D::raytraceLine() is based on the size_x and since we want to rasterize polygons that might also be located outside map bounds we provide a modified raytrace implementation (also Bresenham) based on the integer version presented here: http://playtechs.blogspot.de/2007/03/raytracing-on-grid.html

Parameters

	x0	line start x-coordinate (map frame)
	y0	line start y-coordinate (map frame)
	x1	line end x-coordinate (map frame)
	y1	line end y-coordinate (map frame)
[out]	cells	new cells in map coordinates are pushed back on this container

Definition at line 220 of file costmap_prohibition_layer.cpp.

void costmap_prohibition_layer_namespace::CostmapProhibitionLayer::reconfigureCB	(	CostmapProhibitionLayerConfig &	config,
		uint32_t	level
	)

private

overlayed reconfigure callback function

Definition at line 91 of file costmap_prohibition_layer.cpp.

void costmap_prohibition_layer_namespace::CostmapProhibitionLayer::setPolygonCost	(	costmap_2d::Costmap2D &	master_grid,
		const std::vector< geometry_msgs::Point > &	polygon,
		unsigned char	cost,
		int	min_i,
		int	min_j,
		int	max_i,
		int	max_j,
		bool	fill_polygon
	)

private

Set cost in a Costmap2D for a polygon (polygon may be located outside bounds)

Parameters

master_grid	reference to the Costmap2D object
polygon	polygon defined by a vector of points (in world coordinates)
cost	the cost value to be set (0,255)
min_i	minimum bound on the horizontal map index/coordinate
min_j	minimum bound on the vertical map index/coordinate
max_i	maximum bound on the horizontal map index/coordinate
max_j	maximum bound on the vertical map index/coordinate
fill_polygon	if true, tue cost for the interior of the polygon will be set as well

Definition at line 175 of file costmap_prohibition_layer.cpp.

void costmap_prohibition_layer_namespace::CostmapProhibitionLayer::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.

Reimplemented from costmap_2d::Layer.

Definition at line 98 of file costmap_prohibition_layer.cpp.

void costmap_prohibition_layer_namespace::CostmapProhibitionLayer::updateCosts	(	costmap_2d::Costmap2D &	master_grid,
		int	min_i,
		int	min_j,
		int	max_i,
		int	max_j
	)

virtual

function which get called at every cost updating procdure of the overlayed costmap. The before readed costs will get filled

Reimplemented from costmap_2d::Layer.

Definition at line 116 of file costmap_prohibition_layer.cpp.

Member Data Documentation

double costmap_prohibition_layer_namespace::CostmapProhibitionLayer::_costmap_resolution

private

resolution of the overlayed costmap to create the thinnest line out of two points

Definition at line 198 of file costmap_prohibition_layer.h.

std::mutex costmap_prohibition_layer_namespace::CostmapProhibitionLayer::_data_mutex

private

mutex for the accessing _prohibition_points and _prohibition_polygons

Definition at line 197 of file costmap_prohibition_layer.h.

dynamic_reconfigure::Server<CostmapProhibitionLayerConfig>* costmap_prohibition_layer_namespace::CostmapProhibitionLayer::_dsrv

private

dynamic_reconfigure server for the costmap

Definition at line 196 of file costmap_prohibition_layer.h.

bool costmap_prohibition_layer_namespace::CostmapProhibitionLayer::_fill_polygons

private

if true, all cells that are located in the interior of polygons are marked as obstacle as well

Definition at line 199 of file costmap_prohibition_layer.h.

double costmap_prohibition_layer_namespace::CostmapProhibitionLayer::_max_x

private

Definition at line 202 of file costmap_prohibition_layer.h.

double costmap_prohibition_layer_namespace::CostmapProhibitionLayer::_max_y

private

cached map bounds

Definition at line 202 of file costmap_prohibition_layer.h.

double costmap_prohibition_layer_namespace::CostmapProhibitionLayer::_min_x

private

Definition at line 202 of file costmap_prohibition_layer.h.

double costmap_prohibition_layer_namespace::CostmapProhibitionLayer::_min_y

private

Definition at line 202 of file costmap_prohibition_layer.h.

std::vector<geometry_msgs::Point> costmap_prohibition_layer_namespace::CostmapProhibitionLayer::_prohibition_points

private

vector to save the lonely points in source coordinates

Definition at line 200 of file costmap_prohibition_layer.h.

std::vector<std::vector<geometry_msgs::Point> > costmap_prohibition_layer_namespace::CostmapProhibitionLayer::_prohibition_polygons

private

vector to save the polygons (including lines) in source coordinates

Definition at line 201 of file costmap_prohibition_layer.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