Implements a polygon obstacle with an arbitrary number of vertices. More...

#include <obstacles.h>

Inheritance diagram for teb_local_planner::PolygonObstacle:

Public Member Functions
virtual bool	checkCollision (const Eigen::Vector2d &point, double min_dist) const
	Check if a given point collides with the obstacle. More...

virtual bool	checkLineIntersection (const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end, double min_dist=0) const
	Check if a given line segment between two points intersects with the obstacle (and additionally keeps a safty distance `min_dist`) More...

virtual const Eigen::Vector2d &	getCentroid () const
	Get centroid coordinates of the obstacle. More...

virtual std::complex< double >	getCentroidCplx () const
	Get centroid coordinates of the obstacle as complex number. More...

virtual Eigen::Vector2d	getClosestPoint (const Eigen::Vector2d &position) const
	Get the closest point on the boundary of the obstacle w.r.t. a specified reference position. More...

virtual double	getMinimumDistance (const Eigen::Vector2d &position) const
	Get the minimum euclidean distance to the obstacle (point as reference) More...

virtual double	getMinimumDistance (const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end) const
	Get the minimum euclidean distance to the obstacle (line as reference) More...

virtual double	getMinimumDistance (const Point2dContainer &polygon) const
	Get the minimum euclidean distance to the obstacle (polygon as reference) More...

virtual double	getMinimumSpatioTemporalDistance (const Eigen::Vector2d &position, double t) const
	Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity model (point as reference) More...

virtual double	getMinimumSpatioTemporalDistance (const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end, double t) const
	Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity model (line as reference) More...

virtual double	getMinimumSpatioTemporalDistance (const Point2dContainer &polygon, double t) const
	Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity model (polygon as reference) More...

	PolygonObstacle ()
	Default constructor of the polygon obstacle class. More...

	PolygonObstacle (const Point2dContainer &vertices)
	Construct polygon obstacle with a list of vertices. More...

virtual void	predictVertices (double t, Point2dContainer &pred_vertices) const

virtual void	toPolygonMsg (geometry_msgs::Polygon &polygon)
	Convert the obstacle to a polygon message. More...

Define the polygon
const Point2dContainer &	vertices () const
	Access vertices container (read-only) More...

Point2dContainer &	vertices ()
	Access vertices container. More...

void	pushBackVertex (const Eigen::Ref< const Eigen::Vector2d > &vertex)
	Add a vertex to the polygon (edge-point) More...

void	pushBackVertex (double x, double y)
	Add a vertex to the polygon (edge-point) More...

void	finalizePolygon ()
	Call finalizePolygon after the polygon is created with the help of pushBackVertex() methods. More...

void	clearVertices ()
	Clear all vertices (Afterwards the polygon is not valid anymore) More...

int	noVertices () const
	Get the number of vertices defining the polygon (the first vertex is counted once) More...

Public Member Functions inherited from teb_local_planner::Obstacle
	Obstacle ()
	Default constructor of the abstract obstacle class. More...

virtual	~Obstacle ()
	Virtual destructor. More...

virtual void	predictCentroidConstantVelocity (double t, Eigen::Ref< Eigen::Vector2d > position) const
	Predict position of the centroid assuming a constant velocity model. More...

bool	isDynamic () const
	Check if the obstacle is a moving with a (non-zero) velocity. More...

void	setCentroidVelocity (const Eigen::Ref< const Eigen::Vector2d > &vel)
	Set the 2d velocity (vx, vy) of the obstacle w.r.t to the centroid. More...

void	setCentroidVelocity (const geometry_msgs::TwistWithCovariance &velocity, const geometry_msgs::Quaternion &orientation)
	Set the 2d velocity (vx, vy) of the obstacle w.r.t to the centroid. More...

void	setCentroidVelocity (const geometry_msgs::TwistWithCovariance &velocity, const geometry_msgs::QuaternionStamped &orientation)

const Eigen::Vector2d &	getCentroidVelocity () const
	Get the obstacle velocity (vx, vy) (w.r.t. to the centroid) More...

virtual void	toTwistWithCovarianceMsg (geometry_msgs::TwistWithCovariance &twistWithCovariance)

Protected Member Functions
void	calcCentroid ()
	Compute the centroid of the polygon (called inside finalizePolygon()) More...

void	fixPolygonClosure ()
	Check if the current polygon contains the first vertex twice (as start and end) and in that case erase the last redundant one. More...

Protected Attributes
Eigen::Vector2d	centroid_
	Store the centroid coordinates of the polygon (. More...

bool	finalized_
	Flat that keeps track if the polygon was finalized after adding all vertices. More...

Point2dContainer	vertices_
	Store vertices defining the polygon (. More...

Protected Attributes inherited from teb_local_planner::Obstacle
Eigen::Vector2d	centroid_velocity_
	Store the corresponding velocity (vx, vy) of the centroid (zero, if _dynamic is `true`) More...

bool	dynamic_
	Store flag if obstacle is dynamic (resp. a moving obstacle) More...

Detailed Description

Implements a polygon obstacle with an arbitrary number of vertices.

If the polygon has only 2 vertices, than it is considered as a line, otherwise the polygon will always be closed (a connection between the first and the last vertex is included automatically).

Definition at line 748 of file obstacles.h.

Constructor & Destructor Documentation

teb_local_planner::PolygonObstacle::PolygonObstacle ( )

inline

Default constructor of the polygon obstacle class.

Definition at line 755 of file obstacles.h.

teb_local_planner::PolygonObstacle::PolygonObstacle ( const Point2dContainer & vertices )

inline

Construct polygon obstacle with a list of vertices.

Definition at line 763 of file obstacles.h.

Member Function Documentation

void teb_local_planner::PolygonObstacle::calcCentroid ( )

protected

Compute the centroid of the polygon (called inside finalizePolygon())

Definition at line 57 of file obstacles.cpp.

virtual bool teb_local_planner::PolygonObstacle::checkCollision	(	const Eigen::Vector2d &	position,
		double	min_dist
	)		const

inlinevirtual

Check if a given point collides with the obstacle.

Parameters

position	2D reference position that should be checked
min_dist	Minimum distance allowed to the obstacle to be collision free

Returns: true if position is inside the region of the obstacle or if the minimum distance is lower than min_dist

Implements teb_local_planner::Obstacle.

Definition at line 781 of file obstacles.h.

bool teb_local_planner::PolygonObstacle::checkLineIntersection	(	const Eigen::Vector2d &	line_start,
		const Eigen::Vector2d &	line_end,
		double	min_dist = `0`
	)		const

virtual

Check if a given line segment between two points intersects with the obstacle (and additionally keeps a safty distance min_dist)

Parameters

line_start	2D point for the end of the reference line
line_end	2D point for the end of the reference line
min_dist	Minimum distance allowed to the obstacle to be collision/intersection free

Remarks: we ignore min_dist here

Returns: true if given line intersects the region of the obstacle or if the minimum distance is lower than min_dist

Implements teb_local_planner::Obstacle.

Definition at line 178 of file obstacles.cpp.

void teb_local_planner::PolygonObstacle::clearVertices ( )

inline

Clear all vertices (Afterwards the polygon is not valid anymore)

Definition at line 936 of file obstacles.h.

void teb_local_planner::PolygonObstacle::finalizePolygon ( )

inline

Call finalizePolygon after the polygon is created with the help of pushBackVertex() methods.

Definition at line 926 of file obstacles.h.

void teb_local_planner::PolygonObstacle::fixPolygonClosure ( )

protected

Check if the current polygon contains the first vertex twice (as start and end) and in that case erase the last redundant one.

Definition at line 48 of file obstacles.cpp.

virtual const Eigen::Vector2d& teb_local_planner::PolygonObstacle::getCentroid ( ) const

inlinevirtual

Get centroid coordinates of the obstacle.

Returns: Eigen::Vector2d containing the centroid

Implements teb_local_planner::Obstacle.

Definition at line 874 of file obstacles.h.

virtual std::complex<double> teb_local_planner::PolygonObstacle::getCentroidCplx ( ) const

inlinevirtual

Get centroid coordinates of the obstacle as complex number.

Returns: std::complex containing the centroid coordinate

Implements teb_local_planner::Obstacle.

Definition at line 881 of file obstacles.h.

Eigen::Vector2d teb_local_planner::PolygonObstacle::getClosestPoint ( const Eigen::Vector2d & position ) const

virtual

Get the closest point on the boundary of the obstacle w.r.t. a specified reference position.

Parameters

position reference 2d position

Returns: closest point on the obstacle boundary

Implements teb_local_planner::Obstacle.

Definition at line 130 of file obstacles.cpp.

virtual double teb_local_planner::PolygonObstacle::getMinimumDistance ( const Eigen::Vector2d & position ) const

inlinevirtual

Get the minimum euclidean distance to the obstacle (point as reference)

Parameters

position 2d reference position

Returns: The nearest possible distance to the obstacle

Implements teb_local_planner::Obstacle.

Definition at line 818 of file obstacles.h.

virtual double teb_local_planner::PolygonObstacle::getMinimumDistance	(	const Eigen::Vector2d &	line_start,
		const Eigen::Vector2d &	line_end
	)		const

inlinevirtual

Get the minimum euclidean distance to the obstacle (line as reference)

Parameters

line_start	2d position of the begin of the reference line
line_end	2d position of the end of the reference line

Returns: The nearest possible distance to the obstacle

Implements teb_local_planner::Obstacle.

Definition at line 824 of file obstacles.h.

virtual double teb_local_planner::PolygonObstacle::getMinimumDistance ( const Point2dContainer & polygon ) const

inlinevirtual

Get the minimum euclidean distance to the obstacle (polygon as reference)

Parameters

polygon Vertices (2D points) describing a closed polygon

Returns: The nearest possible distance to the obstacle

Implements teb_local_planner::Obstacle.

Definition at line 830 of file obstacles.h.

virtual double teb_local_planner::PolygonObstacle::getMinimumSpatioTemporalDistance	(	const Eigen::Vector2d &	position,
		double	t
	)		const

inlinevirtual

Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity model (point as reference)

Parameters

position	2d reference position
t	time, for which the minimum distance to the obstacle is estimated

Returns: The nearest possible distance to the obstacle at time t

Implements teb_local_planner::Obstacle.

Definition at line 839 of file obstacles.h.

virtual double teb_local_planner::PolygonObstacle::getMinimumSpatioTemporalDistance	(	const Eigen::Vector2d &	line_start,
		const Eigen::Vector2d &	line_end,
		double	t
	)		const

inlinevirtual

Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity model (line as reference)

Parameters

line_start	2d position of the begin of the reference line
line_end	2d position of the end of the reference line
t	time, for which the minimum distance to the obstacle is estimated

Returns: The nearest possible distance to the obstacle at time t

Implements teb_local_planner::Obstacle.

Definition at line 847 of file obstacles.h.

virtual double teb_local_planner::PolygonObstacle::getMinimumSpatioTemporalDistance	(	const Point2dContainer &	polygon,
		double	t
	)		const

inlinevirtual

Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity model (polygon as reference)

Parameters

polygon	Vertices (2D points) describing a closed polygon
t	time, for which the minimum distance to the obstacle is estimated

Returns: The nearest possible distance to the obstacle at time t

Implements teb_local_planner::Obstacle.

Definition at line 855 of file obstacles.h.

int teb_local_planner::PolygonObstacle::noVertices ( ) const

inline

Get the number of vertices defining the polygon (the first vertex is counted once)

Definition at line 941 of file obstacles.h.

virtual void teb_local_planner::PolygonObstacle::predictVertices	(	double	t,
		Point2dContainer &	pred_vertices
	)		const

inlinevirtual

Definition at line 862 of file obstacles.h.

void teb_local_planner::PolygonObstacle::pushBackVertex ( const Eigen::Ref< const Eigen::Vector2d > & vertex )

inline

Add a vertex to the polygon (edge-point)

Remarks: You do not need to close the polygon (do not repeat the first vertex)

Warning: Do not forget to call finalizePolygon() after adding all vertices

Parameters

vertex 2D point defining a new polygon edge

Definition at line 904 of file obstacles.h.

void teb_local_planner::PolygonObstacle::pushBackVertex	(	double	x,
		double	y
	)

inline

Add a vertex to the polygon (edge-point)

Remarks: You do not need to close the polygon (do not repeat the first vertex)

Warning: Do not forget to call finalizePolygon() after adding all vertices

Parameters

x	x-coordinate of the new vertex
y	y-coordinate of the new vertex

Definition at line 917 of file obstacles.h.

void teb_local_planner::PolygonObstacle::toPolygonMsg ( geometry_msgs::Polygon & polygon )

virtual

Convert the obstacle to a polygon message.

Convert the obstacle to a corresponding polygon msg. Point obstacles have one vertex, lines have two vertices and polygons might are implictly closed such that the start vertex must not be repeated.

Parameters

[out] polygon the polygon message

Implements teb_local_planner::Obstacle.

Definition at line 196 of file obstacles.cpp.

const Point2dContainer& teb_local_planner::PolygonObstacle::vertices ( ) const

inline

Access vertices container (read-only)

Definition at line 895 of file obstacles.h.

Point2dContainer& teb_local_planner::PolygonObstacle::vertices ( )

inline

Access vertices container.

Definition at line 896 of file obstacles.h.

Member Data Documentation

Eigen::Vector2d teb_local_planner::PolygonObstacle::centroid_

protected

Store the centroid coordinates of the polygon (.

See also: calcCentroid)

Definition at line 954 of file obstacles.h.

bool teb_local_planner::PolygonObstacle::finalized_

protected

Flat that keeps track if the polygon was finalized after adding all vertices.

Definition at line 956 of file obstacles.h.

Point2dContainer teb_local_planner::PolygonObstacle::vertices_

protected

Store vertices defining the polygon (.

See also: pushBackVertex)

Definition at line 953 of file obstacles.h.

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

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation