This class converts the costmap_2d into a set of convex polygons (and points) More...

#include <costmap_to_polygons.h>

Inheritance diagram for costmap_converter::CostmapToPolygonsDBSMCCH:

[legend]

Classes
struct	KeyPoint
	Defines a keypoint in metric coordinates of the map. More...

struct	Parameters
	Defines the parameters of the algorithm. More...

Public Member Functions
virtual void	compute ()
	This method performs the actual work (conversion of the costmap to polygons) More...

	CostmapToPolygonsDBSMCCH ()
	Constructor. More...

PolygonContainerConstPtr	getPolygons ()
	Get a shared instance of the current polygon container. More...

virtual void	initialize (ros::NodeHandle nh)
	Initialize the plugin. More...

virtual void	setCostmap2D (costmap_2d::Costmap2D *costmap)
	Pass a pointer to the costap to the plugin. More...

virtual void	updateCostmap2D ()
	Get updated data from the previously set Costmap2D. More...

virtual	~CostmapToPolygonsDBSMCCH ()
	Destructor. More...

Public Member Functions inherited from costmap_converter::BaseCostmapToPolygons
virtual ObstacleArrayConstPtr	getObstacles ()
	Get a shared instance of the current obstacle container If this method is not overwritten by the underlying plugin, the obstacle container just imports getPolygons(). More...

virtual void	setOdomTopic (const std::string &odom_topic)
	Set name of robot's odometry topic. More...

virtual bool	stackedCostmapConversion ()
	Determines whether an additional plugin for subsequent costmap conversion is specified. More...

void	startWorker (ros::Rate rate, costmap_2d::Costmap2D *costmap, bool spin_thread=false)
	Instantiate a worker that repeatedly coverts the most recent costmap to polygons. The worker is implemented as a timer event that is invoked at a specific `rate`. The passed `costmap` pointer must be valid at the complete time and must be lockable. By specifying the argument `spin_thread` the timer event is invoked in a separate thread and callback queue or otherwise it is called from the global callback queue (of the node in which the plugin is used). More...

void	stopWorker ()
	Stop the worker that repeatedly converts the costmap to polygons. More...

virtual	~BaseCostmapToPolygons ()
	Destructor. More...

Static Public Member Functions
template<typename Point >
static void	convertPointToPolygon (const Point &point, geometry_msgs::Polygon &polygon)
	Convert a generi point type to a geometry_msgs::Polygon. More...

Protected Member Functions
void	addPoint (double x, double y)
	helper function for adding a point to the lookup data structures More...

void	convexHull (std::vector< KeyPoint > &cluster, geometry_msgs::Polygon &polygon)
	Compute the convex hull for a single cluster (monotone chain algorithm) More...

void	convexHull2 (std::vector< KeyPoint > &cluster, geometry_msgs::Polygon &polygon)
	Compute the convex hull for a single cluster. More...

template<typename P1 , typename P2 , typename P3 >
long double	cross (const P1 &O, const P2 &A, const P3 &B)
	2D Cross product of two vectors defined by two points and a common origin More...

void	dbScan (std::vector< std::vector< KeyPoint > > &clusters)
	DBSCAN algorithm for clustering. More...

int	neighborCellsToIndex (int cx, int cy)
	offset [meters] in y for the lookup grid More...

void	pointToNeighborCells (const KeyPoint &kp, int &cx, int &cy)
	compute the cell indices of a keypoint More...

void	regionQuery (int curr_index, std::vector< int > &neighbor_indices)
	Helper function for dbScan to search for neighboring points. More...

void	simplifyPolygon (geometry_msgs::Polygon &polygon)
	Simplify a polygon by removing points. More...

void	updatePolygonContainer (PolygonContainerPtr polygons)
	Thread-safe update of the internal polygon container (that is shared using getPolygons() from outside this class) More...

Protected Member Functions inherited from costmap_converter::BaseCostmapToPolygons
	BaseCostmapToPolygons ()
	Protected constructor that should be called by subclasses. More...

void	spinThread ()
	Blocking method that checks for new timer events (active if startWorker() is called with spin_thread enabled) More...

void	workerCallback (const ros::TimerEvent &)
	The callback of the worker that performs the actual work (updating the costmap and converting it to polygons) More...

Protected Attributes
std::vector< std::vector< int > >	neighbor_lookup_

int	neighbor_size_x_
	array of cells for neighbor lookup More...

int	neighbor_size_y_
	size of the neighbour lookup in x (number of cells) More...

std::vector< KeyPoint >	occupied_cells_
	List of occupied cells in the current map (updated by updateCostmap2D()) More...

double	offset_x_
	size of the neighbour lookup in y (number of cells) More...

double	offset_y_
	offset [meters] in x for the lookup grid More...

Parameters	parameter_

Parameters	parameter_buffered_

boost::mutex	parameter_mutex_
	Mutex that keeps track about the ownership of the shared polygon instance. More...

Private Member Functions
void	reconfigureCB (CostmapToPolygonsDBSMCCHConfig &config, uint32_t level)
	Callback for the dynamic_reconfigure node. More...

Private Attributes
costmap_2d::Costmap2D *	costmap_
	Pointer to the costmap2d. More...

dynamic_reconfigure::Server< CostmapToPolygonsDBSMCCHConfig > *	dynamic_recfg_
	Dynamic reconfigure server to allow config modifications at runtime. More...

boost::mutex	mutex_
	Mutex that keeps track about the ownership of the shared polygon instance. More...

PolygonContainerPtr	polygons_
	Current shared container of polygons. More...

Detailed Description

This class converts the costmap_2d into a set of convex polygons (and points)

The conversion is performed in two stages:

Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN) Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei, A density-based algorithm for discovering clusters in large spatial databases with noise. Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9.
In the subsequent stage, clusters are converted into convex polgons using the monotone chain algorithm aka Andrew's algorithm: C++ implementation example: http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull ( GNU Free Documentation License 1.2 ) Reference: A. M. Andrew, "Another Efficient Algorithm for Convex Hulls in Two Dimensions", Info. Proc. Letters 9, 216-219 (1979).

All single points that do not define a cluster (noise) are also treated as polygon (represented as a single vertex)

Definition at line 79 of file costmap_to_polygons.h.

Constructor & Destructor Documentation

costmap_converter::CostmapToPolygonsDBSMCCH::CostmapToPolygonsDBSMCCH ( )

Constructor.

Definition at line 107 of file costmap_to_polygons.cpp.

costmap_converter::CostmapToPolygonsDBSMCCH::~CostmapToPolygonsDBSMCCH ( )

virtual

Destructor.

Definition at line 115 of file costmap_to_polygons.cpp.

Member Function Documentation

void costmap_converter::CostmapToPolygonsDBSMCCH::addPoint	(	double	x,
		double	y
	)

inlineprotected

helper function for adding a point to the lookup data structures

Definition at line 203 of file costmap_to_polygons.h.

void costmap_converter::CostmapToPolygonsDBSMCCH::compute ( )

virtual

This method performs the actual work (conversion of the costmap to polygons)

Implements costmap_converter::BaseCostmapToPolygons.

Reimplemented in costmap_converter::CostmapToLinesDBSRANSAC, costmap_converter::CostmapToLinesDBSMCCH, and costmap_converter::CostmapToPolygonsDBSConcaveHull.

Definition at line 139 of file costmap_to_polygons.cpp.

template<typename Point >

static void costmap_converter::CostmapToPolygonsDBSMCCH::convertPointToPolygon	(	const Point &	point,
		geometry_msgs::Polygon &	polygon
	)

inlinestatic

Convert a generi point type to a geometry_msgs::Polygon.

Parameters

	point	generic 2D point type
[out]	polygon	already instantiated polygon that will be filled with a single point

Template Parameters

Point generic point type that should provide (writable) x and y member fiels.

Definition at line 160 of file costmap_to_polygons.h.

void costmap_converter::CostmapToPolygonsDBSMCCH::convexHull	(	std::vector< KeyPoint > &	cluster,
		geometry_msgs::Polygon &	polygon
	)

protected

Compute the convex hull for a single cluster (monotone chain algorithm)

Clusters are converted into convex polgons using the monotone chain algorithm aka Andrew's algorithm: C++ implementation example: http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull ( GNU Free Documentation License 1.2 ) Reference: A. M. Andrew, "Another Efficient Algorithm for Convex Hulls in Two Dimensions", Info. Proc. Letters 9, 216-219 (1979).

Remarks: The algorithm seems to cut edges, thus we give a try to convexHull2().

Todo:: Evaluate and figure out whether convexHull() or convexHull2() is better suited (performance, quality, ...)

Remarks: The last point is the same as the first one

Parameters

	cluster	list of keypoints that should be converted into a polygon
[out]	polygon	the resulting convex polygon

Definition at line 319 of file costmap_to_polygons.cpp.

void costmap_converter::CostmapToPolygonsDBSMCCH::convexHull2	(	std::vector< KeyPoint > &	cluster,
		geometry_msgs::Polygon &	polygon
	)

protected

Compute the convex hull for a single cluster.

Clusters are converted into convex polgons using an algorithm provided here: https://bitbucket.org/vostreltsov/concave-hull/overview The convex hull algorithm is part of the concave hull algorithm. The license is WTFPL 2.0 and permits any usage.

Remarks: The last point is the same as the first one

Parameters

	cluster	list of keypoints that should be converted into a polygon
[out]	polygon	the resulting convex polygon

Todo:: Evaluate and figure out whether convexHull() or convexHull2() is better suited (performance, quality, ...)

Definition at line 363 of file costmap_to_polygons.cpp.

template<typename P1 , typename P2 , typename P3 >

long double costmap_converter::CostmapToPolygonsDBSMCCH::cross	(	const P1 &	O,
		const P2 &	A,
		const P3 &	B
	)

inlineprotected

2D Cross product of two vectors defined by two points and a common origin

Parameters

O	Origin
A	First point
B	Second point

Template Parameters

P1	2D Point type with x and y member fields
P2	2D Point type with x and y member fields
P3	2D Point type with x and y member fields

Definition at line 262 of file costmap_to_polygons.h.

void costmap_converter::CostmapToPolygonsDBSMCCH::dbScan ( std::vector< std::vector< KeyPoint > > & clusters )

protected

DBSCAN algorithm for clustering.

Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN) Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei, A density-based algorithm for discovering clusters in large spatial databases with noise. Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9.

Parameters

[out] clusters clusters will added to this output-argument (a sequence of keypoints for each cluster) the first cluster (clusters[0]) will contain all noise points (that does not fulfil the min_pts condition

Definition at line 227 of file costmap_to_polygons.cpp.

PolygonContainerConstPtr costmap_converter::CostmapToPolygonsDBSMCCH::getPolygons ( )

virtual

Get a shared instance of the current polygon container.

Remarks: If compute() or startWorker() has not been called before, this method returns an empty instance!

Returns: Shared instance of the current polygon container

Reimplemented from costmap_converter::BaseCostmapToPolygons.

Definition at line 485 of file costmap_to_polygons.cpp.

void costmap_converter::CostmapToPolygonsDBSMCCH::initialize ( ros::NodeHandle nh )

virtual

Initialize the plugin.

Parameters

nh	Nodehandle that defines the namespace for parameters

Implements costmap_converter::BaseCostmapToPolygons.

Reimplemented in costmap_converter::CostmapToLinesDBSRANSAC, costmap_converter::CostmapToLinesDBSMCCH, and costmap_converter::CostmapToPolygonsDBSConcaveHull.

Definition at line 121 of file costmap_to_polygons.cpp.

int costmap_converter::CostmapToPolygonsDBSMCCH::neighborCellsToIndex	(	int	cx,
		int	cy
	)

inlineprotected

offset [meters] in y for the lookup grid

convert a 2d cell coordinate into the 1D index of the array

Parameters

cx	the x index of the cell
cy	the y index of the cell

Definition at line 288 of file costmap_to_polygons.h.

void costmap_converter::CostmapToPolygonsDBSMCCH::pointToNeighborCells	(	const KeyPoint &	kp,
		int &	cx,
		int &	cy
	)

inlineprotected

compute the cell indices of a keypoint

Parameters

kp	key point given in world coordinates [m, m]
cx	output cell index in x direction
cy	output cell index in y direction

Definition at line 301 of file costmap_to_polygons.h.

void costmap_converter::CostmapToPolygonsDBSMCCH::reconfigureCB	(	CostmapToPolygonsDBSMCCHConfig &	config,
		uint32_t	level
	)

private

Callback for the dynamic_reconfigure node.

This callback allows to modify parameters dynamically at runtime without restarting the node

Parameters

config	Reference to the dynamic reconfigure config
level	Dynamic reconfigure level

Definition at line 492 of file costmap_to_polygons.cpp.

void costmap_converter::CostmapToPolygonsDBSMCCH::regionQuery	(	int	curr_index,
		std::vector< int > &	neighbor_indices
	)

protected

Helper function for dbScan to search for neighboring points.

Parameters

	curr_index	index to the current item in `occupied_cells`
[out]	neighbor_indices	list of neighbors (indices of `occupied` cells)

Definition at line 282 of file costmap_to_polygons.cpp.

void costmap_converter::CostmapToPolygonsDBSMCCH::setCostmap2D ( costmap_2d::Costmap2D * costmap )

virtual

Pass a pointer to the costap to the plugin.

See also: updateCostmap2D

Parameters

costmap Pointer to the costmap2d source

Implements costmap_converter::BaseCostmapToPolygons.

Definition at line 169 of file costmap_to_polygons.cpp.

void costmap_converter::CostmapToPolygonsDBSMCCH::simplifyPolygon ( geometry_msgs::Polygon & polygon )

protected

Simplify a polygon by removing points.

We apply the Douglas-Peucker Algorithm to simplify the edges of the polygon. Internally, the parameter min_keypoint_separation is used for deciding whether a point is considered close to an edge and to be merged into the line.

Definition at line 461 of file costmap_to_polygons.cpp.

void costmap_converter::CostmapToPolygonsDBSMCCH::updateCostmap2D ( )

virtual

Get updated data from the previously set Costmap2D.

See also: setCostmap2D

Implements costmap_converter::BaseCostmapToPolygons.

Definition at line 179 of file costmap_to_polygons.cpp.

void costmap_converter::CostmapToPolygonsDBSMCCH::updatePolygonContainer ( PolygonContainerPtr polygons )

protected

Thread-safe update of the internal polygon container (that is shared using getPolygons() from outside this class)

Parameters

polygons Updated polygon container

Definition at line 478 of file costmap_to_polygons.cpp.

Member Data Documentation

costmap_2d::Costmap2D* costmap_converter::CostmapToPolygonsDBSMCCH::costmap_

private

Pointer to the costmap2d.

Definition at line 329 of file costmap_to_polygons.h.

dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>* costmap_converter::CostmapToPolygonsDBSMCCH::dynamic_recfg_

private

Dynamic reconfigure server to allow config modifications at runtime.

Definition at line 327 of file costmap_to_polygons.h.

boost::mutex costmap_converter::CostmapToPolygonsDBSMCCH::mutex_

private

Mutex that keeps track about the ownership of the shared polygon instance.

Definition at line 325 of file costmap_to_polygons.h.

std::vector<std::vector<int> > costmap_converter::CostmapToPolygonsDBSMCCH::neighbor_lookup_

protected

Definition at line 277 of file costmap_to_polygons.h.

int costmap_converter::CostmapToPolygonsDBSMCCH::neighbor_size_x_

protected

array of cells for neighbor lookup

Definition at line 278 of file costmap_to_polygons.h.

int costmap_converter::CostmapToPolygonsDBSMCCH::neighbor_size_y_

protected

size of the neighbour lookup in x (number of cells)

Definition at line 279 of file costmap_to_polygons.h.

std::vector<KeyPoint> costmap_converter::CostmapToPolygonsDBSMCCH::occupied_cells_

protected

List of occupied cells in the current map (updated by updateCostmap2D())

Definition at line 275 of file costmap_to_polygons.h.

double costmap_converter::CostmapToPolygonsDBSMCCH::offset_x_

protected

size of the neighbour lookup in y (number of cells)

Definition at line 280 of file costmap_to_polygons.h.

double costmap_converter::CostmapToPolygonsDBSMCCH::offset_y_

protected

offset [meters] in x for the lookup grid

Definition at line 281 of file costmap_to_polygons.h.

Parameters costmap_converter::CostmapToPolygonsDBSMCCH::parameter_

protected

Definition at line 308 of file costmap_to_polygons.h.

Parameters costmap_converter::CostmapToPolygonsDBSMCCH::parameter_buffered_

protected

Definition at line 309 of file costmap_to_polygons.h.

boost::mutex costmap_converter::CostmapToPolygonsDBSMCCH::parameter_mutex_

protected

Mutex that keeps track about the ownership of the shared polygon instance.

Definition at line 310 of file costmap_to_polygons.h.

PolygonContainerPtr costmap_converter::CostmapToPolygonsDBSMCCH::polygons_

private

Current shared container of polygons.

Definition at line 324 of file costmap_to_polygons.h.

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

Classes

Public Member Functions

Static Public Member Functions

Protected Member Functions

Protected Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation