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

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	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 (const std::vector< KeyPoint > &occupied_cells, std::vector< std::vector< KeyPoint > > &clusters)
	DBSCAN algorithm for clustering. More...

void	regionQuery (const std::vector< KeyPoint > &occupied_cells, int curr_index, std::vector< int > &neighbor_indices)
	Helper function for dbScan to search for neighboring 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
double	max_distance_
	Parameter for DB_Scan, maximum distance to neighbors [m]. More...

int	max_pts_
	Parameter for DB_Scan: maximum number of points that define a cluster (to avoid large L- and U-shapes) More...

double	min_keypoint_separation_
	Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all) More...

int	min_pts_
	Parameter for DB_Scan: minimum number of points that define a cluster. More...

std::vector< KeyPoint >	occupied_cells_
	List of occupied cells in the current map (updated by updateCostmap2D()) 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 49 of file costmap_to_polygons.cpp.

costmap_converter::CostmapToPolygonsDBSMCCH::~CostmapToPolygonsDBSMCCH ( )

virtual

Destructor.

Definition at line 55 of file costmap_to_polygons.cpp.

Member Function Documentation

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 84 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 145 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 232 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 285 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 227 of file costmap_to_polygons.h.

void costmap_converter::CostmapToPolygonsDBSMCCH::dbScan	(	const std::vector< KeyPoint > &	occupied_cells,
		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

	occupied_cells	a list of occupied cells of the costmap in metric coordinates
[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 156 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 408 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 61 of file costmap_to_polygons.cpp.

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 415 of file costmap_to_polygons.cpp.

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

protected

Helper function for dbScan to search for neighboring points.

Parameters

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

Definition at line 211 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 114 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 124 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 401 of file costmap_to_polygons.cpp.

Member Data Documentation

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

private

Pointer to the costmap2d.

Definition at line 267 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 265 of file costmap_to_polygons.h.

double costmap_converter::CostmapToPolygonsDBSMCCH::max_distance_

protected

Parameter for DB_Scan, maximum distance to neighbors [m].

Definition at line 243 of file costmap_to_polygons.h.

int costmap_converter::CostmapToPolygonsDBSMCCH::max_pts_

protected

Parameter for DB_Scan: maximum number of points that define a cluster (to avoid large L- and U-shapes)

Definition at line 245 of file costmap_to_polygons.h.

double costmap_converter::CostmapToPolygonsDBSMCCH::min_keypoint_separation_

protected

Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)

Definition at line 248 of file costmap_to_polygons.h.

int costmap_converter::CostmapToPolygonsDBSMCCH::min_pts_

protected

Parameter for DB_Scan: minimum number of points that define a cluster.

Definition at line 244 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 263 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 240 of file costmap_to_polygons.h.

PolygonContainerPtr costmap_converter::CostmapToPolygonsDBSMCCH::polygons_

private

Current shared container of polygons.

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