Public Member Functions | Protected Attributes | List of all members
teb_local_planner::Obstacle Class Referenceabstract

Abstract class that defines the interface for modelling obstacles. More...

#include <obstacles.h>

Inheritance diagram for teb_local_planner::Obstacle:
Inheritance graph
[legend]

Public Member Functions

 Obstacle ()
 Default constructor of the abstract obstacle class. More...
 
virtual ~Obstacle ()
 Virtual destructor. More...
 
Centroid coordinates (abstract, obstacle type depending)
virtual const Eigen::Vector2d & getCentroid () const =0
 Get centroid coordinates of the obstacle. More...
 
virtual std::complex< double > getCentroidCplx () const =0
 Get centroid coordinates of the obstacle as complex number. More...
 
Collision checking and distance calculations (abstract, obstacle type depending)
virtual bool checkCollision (const Eigen::Vector2d &position, double min_dist) const =0
 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 =0
 Check if a given line segment between two points intersects with the obstacle (and additionally keeps a safty distance min_dist) More...
 
virtual double getMinimumDistance (const Eigen::Vector2d &position) const =0
 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 =0
 Get the minimum euclidean distance to the obstacle (line as reference) More...
 
virtual double getMinimumDistance (const Point2dContainer &polygon) const =0
 Get the minimum euclidean distance to the obstacle (polygon as reference) More...
 
virtual Eigen::Vector2d getClosestPoint (const Eigen::Vector2d &position) const =0
 Get the closest point on the boundary of the obstacle w.r.t. a specified reference position. More...
 
Velocity related methods for non-static, moving obstacles
virtual double getMinimumSpatioTemporalDistance (const Eigen::Vector2d &position, double t) const =0
 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 =0
 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 =0
 Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity model (polygon as reference) 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...
 
Helper Functions
virtual void toPolygonMsg (geometry_msgs::Polygon &polygon)=0
 Convert the obstacle to a polygon message. More...
 
virtual void toTwistWithCovarianceMsg (geometry_msgs::TwistWithCovariance &twistWithCovariance)
 

Protected Attributes

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

Abstract class that defines the interface for modelling obstacles.

Definition at line 67 of file obstacles.h.

Constructor & Destructor Documentation

teb_local_planner::Obstacle::Obstacle ( )
inline

Default constructor of the abstract obstacle class.

Definition at line 74 of file obstacles.h.

virtual teb_local_planner::Obstacle::~Obstacle ( )
inlinevirtual

Virtual destructor.

Definition at line 81 of file obstacles.h.

Member Function Documentation

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

Check if a given point collides with the obstacle.

Parameters
position2D reference position that should be checked
min_distMinimum 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

Implemented in teb_local_planner::PolygonObstacle, teb_local_planner::LineObstacle, teb_local_planner::CircularObstacle, and teb_local_planner::PointObstacle.

virtual bool teb_local_planner::Obstacle::checkLineIntersection ( const Eigen::Vector2d &  line_start,
const Eigen::Vector2d &  line_end,
double  min_dist = 0 
) const
pure virtual

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

Parameters
line_start2D point for the end of the reference line
line_end2D point for the end of the reference line
min_distMinimum distance allowed to the obstacle to be collision/intersection free
Returns
true if given line intersects the region of the obstacle or if the minimum distance is lower than min_dist

Implemented in teb_local_planner::PolygonObstacle, teb_local_planner::LineObstacle, teb_local_planner::CircularObstacle, and teb_local_planner::PointObstacle.

virtual const Eigen::Vector2d& teb_local_planner::Obstacle::getCentroid ( ) const
pure virtual

Get centroid coordinates of the obstacle.

Returns
Eigen::Vector2d containing the centroid

Implemented in teb_local_planner::PolygonObstacle, teb_local_planner::LineObstacle, teb_local_planner::CircularObstacle, and teb_local_planner::PointObstacle.

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

Get centroid coordinates of the obstacle as complex number.

Returns
std::complex containing the centroid coordinate

Implemented in teb_local_planner::PolygonObstacle, teb_local_planner::LineObstacle, teb_local_planner::CircularObstacle, and teb_local_planner::PointObstacle.

const Eigen::Vector2d& teb_local_planner::Obstacle::getCentroidVelocity ( ) const
inline

Get the obstacle velocity (vx, vy) (w.r.t. to the centroid)

Returns
2D vector containing the velocities of the centroid in x and y directions

Definition at line 245 of file obstacles.h.

virtual Eigen::Vector2d teb_local_planner::Obstacle::getClosestPoint ( const Eigen::Vector2d &  position) const
pure virtual

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

Parameters
positionreference 2d position
Returns
closest point on the obstacle boundary

Implemented in teb_local_planner::PolygonObstacle, teb_local_planner::LineObstacle, teb_local_planner::CircularObstacle, and teb_local_planner::PointObstacle.

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

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

Parameters
position2d reference position
Returns
The nearest possible distance to the obstacle

Implemented in teb_local_planner::PolygonObstacle, teb_local_planner::LineObstacle, teb_local_planner::CircularObstacle, and teb_local_planner::PointObstacle.

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

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

Parameters
line_start2d position of the begin of the reference line
line_end2d position of the end of the reference line
Returns
The nearest possible distance to the obstacle

Implemented in teb_local_planner::PolygonObstacle, teb_local_planner::LineObstacle, teb_local_planner::CircularObstacle, and teb_local_planner::PointObstacle.

virtual double teb_local_planner::Obstacle::getMinimumDistance ( const Point2dContainer polygon) const
pure virtual

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

Parameters
polygonVertices (2D points) describing a closed polygon
Returns
The nearest possible distance to the obstacle

Implemented in teb_local_planner::PolygonObstacle, teb_local_planner::LineObstacle, teb_local_planner::CircularObstacle, and teb_local_planner::PointObstacle.

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

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

Parameters
position2d reference position
ttime, for which the minimum distance to the obstacle is estimated
Returns
The nearest possible distance to the obstacle at time t

Implemented in teb_local_planner::PolygonObstacle, teb_local_planner::LineObstacle, teb_local_planner::CircularObstacle, and teb_local_planner::PointObstacle.

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

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

Parameters
line_start2d position of the begin of the reference line
line_end2d position of the end of the reference line
ttime, for which the minimum distance to the obstacle is estimated
Returns
The nearest possible distance to the obstacle at time t

Implemented in teb_local_planner::PolygonObstacle, teb_local_planner::LineObstacle, teb_local_planner::CircularObstacle, and teb_local_planner::PointObstacle.

virtual double teb_local_planner::Obstacle::getMinimumSpatioTemporalDistance ( const Point2dContainer polygon,
double  t 
) const
pure virtual

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

Parameters
polygonVertices (2D points) describing a closed polygon
ttime, for which the minimum distance to the obstacle is estimated
Returns
The nearest possible distance to the obstacle at time t

Implemented in teb_local_planner::PolygonObstacle, teb_local_planner::LineObstacle, teb_local_planner::CircularObstacle, and teb_local_planner::PointObstacle.

bool teb_local_planner::Obstacle::isDynamic ( ) const
inline

Check if the obstacle is a moving with a (non-zero) velocity.

Returns
true if the obstacle is not marked as static, false otherwise

Definition at line 199 of file obstacles.h.

virtual void teb_local_planner::Obstacle::predictCentroidConstantVelocity ( double  t,
Eigen::Ref< Eigen::Vector2d >  position 
) const
inlinevirtual

Predict position of the centroid assuming a constant velocity model.

Parameters
[in]ttime in seconds for the prediction (t>=0)
[out]positionpredicted 2d position of the centroid

Reimplemented in teb_local_planner::CircularObstacle, and teb_local_planner::PointObstacle.

Definition at line 190 of file obstacles.h.

void teb_local_planner::Obstacle::setCentroidVelocity ( const Eigen::Ref< const Eigen::Vector2d > &  vel)
inline

Set the 2d velocity (vx, vy) of the obstacle w.r.t to the centroid.

Remarks
Setting the velocity using this function marks the obstacle as dynamic (
See also
isDynamic)
Parameters
vel2D vector containing the velocities of the centroid in x and y directions

Definition at line 206 of file obstacles.h.

void teb_local_planner::Obstacle::setCentroidVelocity ( const geometry_msgs::TwistWithCovariance &  velocity,
const geometry_msgs::Quaternion &  orientation 
)
inline

Set the 2d velocity (vx, vy) of the obstacle w.r.t to the centroid.

Remarks
Setting the velocity using this function marks the obstacle as dynamic (
See also
isDynamic)
Parameters
velocitygeometry_msgs::TwistWithCovariance containing the velocity of the obstacle
orientationgeometry_msgs::QuaternionStamped containing the orientation of the obstacle

Definition at line 214 of file obstacles.h.

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

Definition at line 235 of file obstacles.h.

virtual void teb_local_planner::Obstacle::toPolygonMsg ( geometry_msgs::Polygon &  polygon)
pure 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]polygonthe polygon message

Implemented in teb_local_planner::PolygonObstacle, teb_local_planner::LineObstacle, teb_local_planner::CircularObstacle, and teb_local_planner::PointObstacle.

virtual void teb_local_planner::Obstacle::toTwistWithCovarianceMsg ( geometry_msgs::TwistWithCovariance &  twistWithCovariance)
inlinevirtual

Definition at line 264 of file obstacles.h.

Member Data Documentation

Eigen::Vector2d teb_local_planner::Obstacle::centroid_velocity_
protected

Store the corresponding velocity (vx, vy) of the centroid (zero, if _dynamic is true)

Definition at line 285 of file obstacles.h.

bool teb_local_planner::Obstacle::dynamic_
protected

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

Definition at line 284 of file obstacles.h.


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


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Jun 3 2020 04:03:08