Public Member Functions | Protected Attributes | List of all members
teb_local_planner::CircularObstacle Class Reference

Implements a 2D circular obstacle (point obstacle plus radius) More...

#include <obstacles.h>

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

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...
 
 CircularObstacle ()
 Default constructor of the circular obstacle class. More...
 
 CircularObstacle (const Eigen::Ref< const Eigen::Vector2d > &position, double radius)
 Construct CircularObstacle using a 2d center position vector and radius. More...
 
 CircularObstacle (double x, double y, double radius)
 Construct CircularObstacle using x- and y-center-coordinates and radius. 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...
 
const Eigen::Vector2d & position () const
 Return the current position of the obstacle (read-only) More...
 
Eigen::Vector2d & position ()
 Return the current position of the obstacle. More...
 
virtual void predictCentroidConstantVelocity (double t, Eigen::Ref< Eigen::Vector2d > position) const
 Predict position of the centroid assuming a constant velocity model. More...
 
double & radius ()
 Return the current radius of the obstacle. More...
 
const double & radius () const
 Return the current radius of the obstacle. More...
 
virtual void toPolygonMsg (geometry_msgs::Polygon &polygon)
 Convert the obstacle to a polygon message. More...
 
double & x ()
 Return the current x-coordinate of the obstacle. More...
 
const double & x () const
 Return the current y-coordinate of the obstacle (read-only) More...
 
double & y ()
 Return the current x-coordinate of the obstacle. More...
 
const double & y () const
 Return the current y-coordinate of the obstacle (read-only) More...
 
- Public Member Functions inherited from teb_local_planner::Obstacle
 Obstacle ()
 Default constructor of the abstract obstacle class. More...
 
virtual ~Obstacle ()
 Virtual destructor. 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 Attributes

Eigen::Vector2d pos_
 Store the center position of the CircularObstacle. More...
 
double radius_ = 0.0
 Radius of the obstacle. 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 2D circular obstacle (point obstacle plus radius)

Definition at line 447 of file obstacles.h.

Constructor & Destructor Documentation

teb_local_planner::CircularObstacle::CircularObstacle ( )
inline

Default constructor of the circular obstacle class.

Definition at line 454 of file obstacles.h.

teb_local_planner::CircularObstacle::CircularObstacle ( const Eigen::Ref< const Eigen::Vector2d > &  position,
double  radius 
)
inline

Construct CircularObstacle using a 2d center position vector and radius.

Parameters
position2d position that defines the current obstacle position
radiusradius of the obstacle

Definition at line 462 of file obstacles.h.

teb_local_planner::CircularObstacle::CircularObstacle ( double  x,
double  y,
double  radius 
)
inline

Construct CircularObstacle using x- and y-center-coordinates and radius.

Parameters
xx-coordinate
yy-coordinate
radiusradius of the obstacle

Definition at line 471 of file obstacles.h.

Member Function Documentation

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

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

Implements teb_local_planner::Obstacle.

Definition at line 476 of file obstacles.h.

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

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

Implements teb_local_planner::Obstacle.

Definition at line 483 of file obstacles.h.

virtual const Eigen::Vector2d& teb_local_planner::CircularObstacle::getCentroid ( ) const
inlinevirtual

Get centroid coordinates of the obstacle.

Returns
Eigen::Vector2d containing the centroid

Implements teb_local_planner::Obstacle.

Definition at line 550 of file obstacles.h.

virtual std::complex<double> teb_local_planner::CircularObstacle::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 556 of file obstacles.h.

virtual Eigen::Vector2d teb_local_planner::CircularObstacle::getClosestPoint ( const Eigen::Vector2d &  position) const
inlinevirtual

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

Implements teb_local_planner::Obstacle.

Definition at line 520 of file obstacles.h.

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

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

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

Implements teb_local_planner::Obstacle.

Definition at line 502 of file obstacles.h.

virtual double teb_local_planner::CircularObstacle::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_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

Implements teb_local_planner::Obstacle.

Definition at line 508 of file obstacles.h.

virtual double teb_local_planner::CircularObstacle::getMinimumDistance ( const Point2dContainer polygon) const
inlinevirtual

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

Implements teb_local_planner::Obstacle.

Definition at line 514 of file obstacles.h.

virtual double teb_local_planner::CircularObstacle::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
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

Implements teb_local_planner::Obstacle.

Definition at line 526 of file obstacles.h.

virtual double teb_local_planner::CircularObstacle::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_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

Implements teb_local_planner::Obstacle.

Definition at line 532 of file obstacles.h.

virtual double teb_local_planner::CircularObstacle::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
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

Implements teb_local_planner::Obstacle.

Definition at line 538 of file obstacles.h.

const Eigen::Vector2d& teb_local_planner::CircularObstacle::position ( ) const
inline

Return the current position of the obstacle (read-only)

Definition at line 562 of file obstacles.h.

Eigen::Vector2d& teb_local_planner::CircularObstacle::position ( )
inline

Return the current position of the obstacle.

Definition at line 563 of file obstacles.h.

virtual void teb_local_planner::CircularObstacle::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 from teb_local_planner::Obstacle.

Definition at line 544 of file obstacles.h.

double& teb_local_planner::CircularObstacle::radius ( )
inline

Return the current radius of the obstacle.

Definition at line 568 of file obstacles.h.

const double& teb_local_planner::CircularObstacle::radius ( ) const
inline

Return the current radius of the obstacle.

Definition at line 569 of file obstacles.h.

virtual void teb_local_planner::CircularObstacle::toPolygonMsg ( geometry_msgs::Polygon &  polygon)
inlinevirtual

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

Implements teb_local_planner::Obstacle.

Definition at line 572 of file obstacles.h.

double& teb_local_planner::CircularObstacle::x ( )
inline

Return the current x-coordinate of the obstacle.

Definition at line 564 of file obstacles.h.

const double& teb_local_planner::CircularObstacle::x ( ) const
inline

Return the current y-coordinate of the obstacle (read-only)

Definition at line 565 of file obstacles.h.

double& teb_local_planner::CircularObstacle::y ( )
inline

Return the current x-coordinate of the obstacle.

Definition at line 566 of file obstacles.h.

const double& teb_local_planner::CircularObstacle::y ( ) const
inline

Return the current y-coordinate of the obstacle (read-only)

Definition at line 567 of file obstacles.h.

Member Data Documentation

Eigen::Vector2d teb_local_planner::CircularObstacle::pos_
protected

Store the center position of the CircularObstacle.

Definition at line 584 of file obstacles.h.

double teb_local_planner::CircularObstacle::radius_ = 0.0
protected

Radius of the obstacle.

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