Classes
class	BaseRobotFootprintModel
	Abstract class that defines the interface for robot footprint/contour models. More...

class	BaseTebBinaryEdge
	Base edge connecting two vertices in the TEB optimization problem. More...

class	BaseTebMultiEdge
	Base edge connecting two vertices in the TEB optimization problem. More...

class	BaseTebUnaryEdge
	Base edge connecting a single vertex in the TEB optimization problem. More...

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

class	CircularRobotFootprint
	Class that defines the a robot of circular shape. More...

class	EdgeAcceleration
	Edge defining the cost function for limiting the translational and rotational acceleration. More...

class	EdgeAccelerationGoal
	Edge defining the cost function for limiting the translational and rotational acceleration at the end of the trajectory. More...

class	EdgeAccelerationHolonomic
	Edge defining the cost function for limiting the translational and rotational acceleration. More...

class	EdgeAccelerationHolonomicGoal
	Edge defining the cost function for limiting the translational and rotational acceleration at the end of the trajectory. More...

class	EdgeAccelerationHolonomicStart
	Edge defining the cost function for limiting the translational and rotational acceleration at the beginning of the trajectory. More...

class	EdgeAccelerationStart
	Edge defining the cost function for limiting the translational and rotational acceleration at the beginning of the trajectory. More...

class	EdgeDynamicObstacle
	Edge defining the cost function for keeping a distance from dynamic (moving) obstacles. More...

class	EdgeInflatedObstacle
	Edge defining the cost function for keeping a minimum distance from inflated obstacles. More...

class	EdgeKinematicsCarlike
	Edge defining the cost function for satisfying the non-holonomic kinematics of a carlike mobile robot. More...

class	EdgeKinematicsDiffDrive
	Edge defining the cost function for satisfying the non-holonomic kinematics of a differential drive mobile robot. More...

class	EdgeObstacle
	Edge defining the cost function for keeping a minimum distance from obstacles. More...

class	EdgePreferRotDir
	Edge defining the cost function for penalzing a specified turning direction, in particular left resp. right turns. More...

class	EdgeShortestPath
	Edge defining the cost function for minimizing the Euclidean distance between two consectuive poses. More...

class	EdgeTimeOptimal
	Edge defining the cost function for minimizing transition time of the trajectory. More...

class	EdgeVelocity
	Edge defining the cost function for limiting the translational and rotational velocity. More...

class	EdgeVelocityHolonomic
	Edge defining the cost function for limiting the translational and rotational velocity according to x,y and theta. More...

class	EdgeViaPoint
	Edge defining the cost function for pushing a configuration towards a via point. More...

class	EquivalenceClass
	Abstract class that defines an interface for computing and comparing equivalence classes. More...

class	FailureDetector
	This class implements methods in order to detect if the robot got stucked or is oscillating. More...

class	GraphSearchInterface
	Base class for graph based path planning / homotopy class sampling. More...

struct	HcGraphVertex
	Vertex in the graph that is used to find homotopy classes (only stores 2D positions) More...

class	HomotopyClassPlanner
	Local planner that explores alternative homotopy classes, create a plan for each alternative and finally return the robot controls for the current best path (repeated in each sampling interval) More...

class	HSignature
	The H-signature defines an equivalence relation based on homology in terms of complex calculus. More...

class	HSignature3d
	The H-signature in three dimensions (here: x-y-t) defines an equivalence relation based on homology using theorems from electro magnetism. More...

class	LineObstacle
	Implements a 2D line obstacle. More...

class	LineRobotFootprint
	Class that approximates the robot with line segment (zero-width) More...

class	lrKeyPointGraph

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

class	PlannerInterface
	This abstract class defines an interface for local planners. More...

class	PointObstacle
	Implements a 2D point obstacle. More...

class	PointRobotFootprint

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

class	PolygonRobotFootprint
	Class that approximates the robot with a closed polygon. More...

class	PoseSE2
	This class implements a pose in the domain SE2: $\mathbb{R}^2 \times S^1$ The pose consist of the position x and y and an orientation given as angle theta [-pi, pi]. More...

class	ProbRoadmapGraph

class	TebConfig
	Config class for the teb_local_planner and its components. More...

class	TebLocalPlannerROS
	Implements the actual abstract navigation stack routines of the teb_local_planner plugin. More...

class	TebOptimalPlanner
	This class optimizes an internal Timed Elastic Band trajectory using the g2o-framework. More...

class	TebVisualization
	Forward Declaration. More...

class	TimedElasticBand
	Class that defines a trajectory modeled as an elastic band with augmented tempoarl information. More...

class	TwoCirclesRobotFootprint
	Class that approximates the robot with two shifted circles. More...

class	VertexPose
	This class stores and wraps a SE2 pose (position and orientation) into a vertex that can be optimized via g2o. More...

class	VertexTimeDiff
	This class stores and wraps a time difference $\Delta T$ into a vertex that can be optimized via g2o. More...

Typedefs
using	EquivalenceClassPtr = boost::shared_ptr< EquivalenceClass >

typedef boost::adjacency_list< boost::listS, boost::vecS, boost::directedS, HcGraphVertex, boost::no_property >	HcGraph
	Abbrev. for the homotopy class search-graph type. More...

typedef boost::graph_traits< HcGraph >::adjacency_iterator	HcGraphAdjecencyIterator
	Abbrev. for the adjacency iterator that iterates vertices that are adjecent to the specified one. More...

typedef boost::graph_traits< HcGraph >::edge_iterator	HcGraphEdgeIterator
	Abbrev. for the edges iterator of the homotopy class search-graph. More...

typedef boost::graph_traits< HcGraph >::edge_descriptor	HcGraphEdgeType
	Abbrev. for edge type descriptors in the homotopy class search-graph. More...

typedef boost::graph_traits< HcGraph >::vertex_iterator	HcGraphVertexIterator
	Abbrev. for the vertices iterator of the homotopy class search-graph. More...

typedef boost::graph_traits< HcGraph >::vertex_descriptor	HcGraphVertexType
	Abbrev. for vertex type descriptors in the homotopy class search-graph. More...

typedef boost::shared_ptr< HomotopyClassPlanner >	HomotopyClassPlannerPtr
	Abbrev. for a shared pointer of a HomotopyClassPlanner instance. More...

typedef boost::shared_ptr< const Obstacle >	ObstacleConstPtr
	Abbrev. for shared obstacle const pointers. More...

typedef boost::shared_ptr< Obstacle >	ObstaclePtr
	Abbrev. for shared obstacle pointers. More...

typedef std::vector< ObstaclePtr >	ObstContainer
	Abbrev. for containers storing multiple obstacles. More...

typedef boost::shared_ptr< PlannerInterface >	PlannerInterfacePtr
	Abbrev. for shared instances of PlannerInterface or it's subclasses. More...

typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator< Eigen::Vector2d > >	Point2dContainer
	Abbrev. for a container storing 2d points. More...

typedef std::vector< VertexPose * >	PoseSequence
	Container of poses that represent the spatial part of the trajectory. More...

typedef boost::shared_ptr< const BaseRobotFootprintModel >	RobotFootprintModelConstPtr
	Abbrev. for shared obstacle const pointers. More...

typedef boost::shared_ptr< BaseRobotFootprintModel >	RobotFootprintModelPtr
	Abbrev. for shared obstacle pointers. More...

typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1,-1 > >	TEBBlockSolver
	Typedef for the block solver utilized for optimization. More...

typedef g2o::LinearSolverCSparse< TEBBlockSolver::PoseMatrixType >	TEBLinearSolver
	Typedef for the linear solver utilized for optimization. More...

typedef boost::shared_ptr< const TebOptimalPlanner >	TebOptimalPlannerConstPtr
	Abbrev. for shared const TebOptimalPlanner pointers. More...

typedef boost::shared_ptr< TebOptimalPlanner >	TebOptimalPlannerPtr
	Abbrev. for shared instances of the TebOptimalPlanner. More...

typedef std::vector< TebOptimalPlannerPtr >	TebOptPlannerContainer
	Abbrev. for containers storing multiple teb optimal planners. More...

typedef boost::shared_ptr< const TebVisualization >	TebVisualizationConstPtr
	Abbrev. for shared instances of the TebVisualization (read-only) More...

typedef boost::shared_ptr< TebVisualization >	TebVisualizationPtr
	Abbrev. for shared instances of the TebVisualization. More...

typedef std::vector< VertexTimeDiff * >	TimeDiffSequence
	Container of time differences that define the temporal of the trajectory. More...

typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator< Eigen::Vector2d > >	ViaPointContainer
	Typedef for a container storing via-points. More...

Enumerations
enum	RotType { RotType::left, RotType::none, RotType::right }
	Symbols for left/none/right rotations. More...

Functions
double	average_angles (const std::vector< double > &angles)
	Return the average angle of an arbitrary number of given angles [rad]. More...

template<typename VectorType >
double	calc_closest_point_to_approach_distance (const VectorType &x1, const VectorType &vel1, const VectorType &x2, const VectorType &vel2, double bound_cpa_time=0)

template<typename VectorType >
double	calc_closest_point_to_approach_time (const VectorType &x1, const VectorType &vel1, const VectorType &x2, const VectorType &vel2)

double	calc_distance_line_to_line_3d (const Eigen::Ref< const Eigen::Vector3d > &x1, Eigen::Ref< const Eigen::Vector3d > &u, const Eigen::Ref< const Eigen::Vector3d > &x2, Eigen::Ref< const Eigen::Vector3d > &v)

template<typename VectorType >
double	calc_distance_point_to_line (const VectorType &point, const VectorType &line_base, const VectorType &line_dir)

template<typename VectorType >
double	calc_distance_point_to_segment (const VectorType &point, const VectorType &line_start, const VectorType &line_end)

double	calc_distance_segment_to_segment3D (const Eigen::Ref< const Eigen::Vector3d > &line1_start, Eigen::Ref< const Eigen::Vector3d > &line1_end, const Eigen::Ref< const Eigen::Vector3d > &line2_start, Eigen::Ref< const Eigen::Vector3d > &line2_end)

bool	check_line_segments_intersection_2d (const Eigen::Ref< const Eigen::Vector2d > &line1_start, const Eigen::Ref< const Eigen::Vector2d > &line1_end, const Eigen::Ref< const Eigen::Vector2d > &line2_start, const Eigen::Ref< const Eigen::Vector2d > &line2_end, Eigen::Vector2d *intersection=NULL)
	Helper function to check whether two line segments intersects. More...

Eigen::Vector2d	closest_point_on_line_segment_2d (const Eigen::Ref< const Eigen::Vector2d > &point, const Eigen::Ref< const Eigen::Vector2d > &line_start, const Eigen::Ref< const Eigen::Vector2d > &line_end)
	Helper function to obtain the closest point on a line segment w.r.t. a reference point. More...

template<typename V1 , typename V2 >
double	cross2d (const V1 &v1, const V2 &v2)
	Calculate the 2d cross product (returns length of the resulting vector along the z-axis in 3d) More...

double	distance_point_to_polygon_2d (const Eigen::Vector2d &point, const Point2dContainer &vertices)
	Helper function to calculate the smallest distance between a point and a closed polygon. More...

double	distance_point_to_segment_2d (const Eigen::Ref< const Eigen::Vector2d > &point, const Eigen::Ref< const Eigen::Vector2d > &line_start, const Eigen::Ref< const Eigen::Vector2d > &line_end)
	Helper function to calculate the distance between a line segment and a point. More...

template<typename P1 , typename P2 >
double	distance_points2d (const P1 &point1, const P2 &point2)
	Calculate Euclidean distance between two 2D point datatypes. More...

double	distance_polygon_to_polygon_2d (const Point2dContainer &vertices1, const Point2dContainer &vertices2)
	Helper function to calculate the smallest distance between two closed polygons. More...

double	distance_segment_to_polygon_2d (const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end, const Point2dContainer &vertices)
	Helper function to calculate the smallest distance between a line segment and a closed polygon. More...

double	distance_segment_to_segment_2d (const Eigen::Ref< const Eigen::Vector2d > &line1_start, const Eigen::Ref< const Eigen::Vector2d > &line1_end, const Eigen::Ref< const Eigen::Vector2d > &line2_start, const Eigen::Ref< const Eigen::Vector2d > &line2_end)
	Helper function to calculate the smallest distance between two line segments. More...

double	fast_sigmoid (double x)
	Calculate a fast approximation of a sigmoid function. More...

template<typename T >
const T &	get_const_reference (const T *ptr)
	Helper function that returns the const reference to a value defined by either its raw pointer type or const reference. More...

template<typename T >
const T &	get_const_reference (const T &val, typename boost::disable_if< boost::is_pointer< T > >::type *dummy=0)
	Helper function that returns the const reference to a value defined by either its raw pointer type or const reference. More...

std::complex< long double >	getCplxFromHcGraph (HcGraphVertexType vert_descriptor, const HcGraph &graph)
	Inline function used for initializing the TEB in combination with HCP graph vertex descriptors. More...

std::complex< long double >	getCplxFromMsgPoseStamped (const geometry_msgs::PoseStamped &pose)
	Inline function used for calculateHSignature() in combination with geometry_msgs::PoseStamped. More...

std::complex< long double >	getCplxFromVertexPosePtr (const VertexPose *pose)
	< Inline function used for calculateHSignature() in combination with VertexPose pointers More...

const Eigen::Vector2d &	getVector2dFromHcGraph (HcGraphVertexType vert_descriptor, const HcGraph &graph)

bool	is_close (double a, double b, double epsilon=1e-4)
	Check whether two variables (double) are close to each other. More...

double	penaltyBoundFromBelow (const double &var, const double &a, const double &epsilon)
	Linear penalty function for bounding `var` from below: . More...

double	penaltyBoundFromBelowDerivative (const double &var, const double &a, const double &epsilon)
	Derivative of the linear penalty function for bounding `var` from below: . More...

double	penaltyBoundToInterval (const double &var, const double &a, const double &epsilon)
	Linear penalty function for bounding `var` to the interval . More...

double	penaltyBoundToInterval (const double &var, const double &a, const double &b, const double &epsilon)
	Linear penalty function for bounding `var` to the interval . More...

double	penaltyBoundToIntervalDerivative (const double &var, const double &a, const double &epsilon)
	Derivative of the linear penalty function for bounding `var` to the interval . More...

double	penaltyBoundToIntervalDerivative (const double &var, const double &a, const double &b, const double &epsilon)
	Derivative of the linear penalty function for bounding `var` to the interval . More...

bool	smaller_than_abs (double i, double j)
	Small helper function: check if \|a\|<\|b\|. More...

Typedef Documentation

using teb_local_planner::EquivalenceClassPtr = typedef boost::shared_ptr<EquivalenceClass>

Definition at line 97 of file equivalence_relations.h.

typedef boost::adjacency_list< boost::listS, boost::vecS, boost::directedS, HcGraphVertex, boost::no_property > teb_local_planner::HcGraph

Abbrev. for the homotopy class search-graph type.

See also: HcGraphVertex

Definition at line 80 of file graph_search.h.

typedef boost::graph_traits<HcGraph>::adjacency_iterator teb_local_planner::HcGraphAdjecencyIterator

Abbrev. for the adjacency iterator that iterates vertices that are adjecent to the specified one.

Inline function used for calculateHSignature() in combination with HCP graph vertex descriptors

Definition at line 90 of file graph_search.h.

typedef boost::graph_traits<HcGraph>::edge_iterator teb_local_planner::HcGraphEdgeIterator

Abbrev. for the edges iterator of the homotopy class search-graph.

Definition at line 88 of file graph_search.h.

typedef boost::graph_traits<HcGraph>::edge_descriptor teb_local_planner::HcGraphEdgeType

Abbrev. for edge type descriptors in the homotopy class search-graph.

Definition at line 84 of file graph_search.h.

typedef boost::graph_traits<HcGraph>::vertex_iterator teb_local_planner::HcGraphVertexIterator

Abbrev. for the vertices iterator of the homotopy class search-graph.

Definition at line 86 of file graph_search.h.

typedef boost::graph_traits<HcGraph>::vertex_descriptor teb_local_planner::HcGraphVertexType

Abbrev. for vertex type descriptors in the homotopy class search-graph.

Definition at line 82 of file graph_search.h.

typedef boost::shared_ptr<HomotopyClassPlanner> teb_local_planner::HomotopyClassPlannerPtr

Abbrev. for a shared pointer of a HomotopyClassPlanner instance.

Definition at line 557 of file homotopy_class_planner.h.

typedef boost::shared_ptr<const Obstacle> teb_local_planner::ObstacleConstPtr

Abbrev. for shared obstacle const pointers.

Definition at line 295 of file obstacles.h.

typedef boost::shared_ptr<Obstacle> teb_local_planner::ObstaclePtr

Abbrev. for shared obstacle pointers.

Definition at line 293 of file obstacles.h.

typedef std::vector<ObstaclePtr> teb_local_planner::ObstContainer

Abbrev. for containers storing multiple obstacles.

Definition at line 297 of file obstacles.h.

typedef boost::shared_ptr<PlannerInterface> teb_local_planner::PlannerInterfacePtr

Abbrev. for shared instances of PlannerInterface or it's subclasses.

Definition at line 193 of file planner_interface.h.

typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > teb_local_planner::Point2dContainer

Abbrev. for a container storing 2d points.

Definition at line 50 of file distance_calculations.h.

typedef std::vector<VertexPose*> teb_local_planner::PoseSequence

Container of poses that represent the spatial part of the trajectory.

Definition at line 63 of file timed_elastic_band.h.

typedef boost::shared_ptr<const BaseRobotFootprintModel> teb_local_planner::RobotFootprintModelConstPtr

Abbrev. for shared obstacle const pointers.

Definition at line 122 of file robot_footprint_model.h.

typedef boost::shared_ptr<BaseRobotFootprintModel> teb_local_planner::RobotFootprintModelPtr

Abbrev. for shared obstacle pointers.

Definition at line 120 of file robot_footprint_model.h.

typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > teb_local_planner::TEBBlockSolver

Typedef for the block solver utilized for optimization.

Definition at line 86 of file optimal_planner.h.

typedef g2o::LinearSolverCSparse<TEBBlockSolver::PoseMatrixType> teb_local_planner::TEBLinearSolver

Typedef for the linear solver utilized for optimization.

Definition at line 89 of file optimal_planner.h.

typedef boost::shared_ptr<const TebOptimalPlanner> teb_local_planner::TebOptimalPlannerConstPtr

Abbrev. for shared const TebOptimalPlanner pointers.

Definition at line 704 of file optimal_planner.h.

typedef boost::shared_ptr<TebOptimalPlanner> teb_local_planner::TebOptimalPlannerPtr

Abbrev. for shared instances of the TebOptimalPlanner.

Definition at line 702 of file optimal_planner.h.

typedef std::vector< TebOptimalPlannerPtr > teb_local_planner::TebOptPlannerContainer

Abbrev. for containers storing multiple teb optimal planners.

Definition at line 706 of file optimal_planner.h.

typedef boost::shared_ptr<const TebVisualization> teb_local_planner::TebVisualizationConstPtr

Abbrev. for shared instances of the TebVisualization (read-only)

Definition at line 272 of file visualization.h.

typedef boost::shared_ptr<TebVisualization> teb_local_planner::TebVisualizationPtr

Abbrev. for shared instances of the TebVisualization.

Definition at line 269 of file visualization.h.

typedef std::vector<VertexTimeDiff*> teb_local_planner::TimeDiffSequence

Container of time differences that define the temporal of the trajectory.

Definition at line 65 of file timed_elastic_band.h.

typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > teb_local_planner::ViaPointContainer

Typedef for a container storing via-points.

Definition at line 93 of file optimal_planner.h.

Enumeration Type Documentation

enum teb_local_planner::RotType

strong

Symbols for left/none/right rotations.

Enumerator
left
none
right

Definition at line 53 of file misc.h.

Function Documentation

double teb_local_planner::average_angles ( const std::vector< double > & angles )

inline

Return the average angle of an arbitrary number of given angles [rad].

Parameters

angles vector containing all angles

Returns: average / mean angle, that is normalized to [-pi, pi]

Definition at line 72 of file misc.h.

template<typename VectorType >

double teb_local_planner::calc_closest_point_to_approach_distance	(	const VectorType &	x1,
		const VectorType &	vel1,
		const VectorType &	x2,
		const VectorType &	vel2,
		double	bound_cpa_time = `0`
	)

Definition at line 408 of file distance_calculations.h.

template<typename VectorType >

double teb_local_planner::calc_closest_point_to_approach_time	(	const VectorType &	x1,
		const VectorType &	vel1,
		const VectorType &	x2,
		const VectorType &	vel2
	)

Definition at line 393 of file distance_calculations.h.

double teb_local_planner::calc_distance_line_to_line_3d	(	const Eigen::Ref< const Eigen::Vector3d > &	x1,
		Eigen::Ref< const Eigen::Vector3d > &	u,
		const Eigen::Ref< const Eigen::Vector3d > &	x2,
		Eigen::Ref< const Eigen::Vector3d > &	v
	)

inline

Definition at line 278 of file distance_calculations.h.

template<typename VectorType >

double teb_local_planner::calc_distance_point_to_line	(	const VectorType &	point,
		const VectorType &	line_base,
		const VectorType &	line_dir
	)

Definition at line 424 of file distance_calculations.h.

template<typename VectorType >

double teb_local_planner::calc_distance_point_to_segment	(	const VectorType &	point,
		const VectorType &	line_start,
		const VectorType &	line_end
	)

Definition at line 442 of file distance_calculations.h.

double teb_local_planner::calc_distance_segment_to_segment3D	(	const Eigen::Ref< const Eigen::Vector3d > &	line1_start,
		Eigen::Ref< const Eigen::Vector3d > &	line1_end,
		const Eigen::Ref< const Eigen::Vector3d > &	line2_start,
		Eigen::Ref< const Eigen::Vector3d > &	line2_end
	)

inline

Definition at line 310 of file distance_calculations.h.

bool teb_local_planner::check_line_segments_intersection_2d	(	const Eigen::Ref< const Eigen::Vector2d > &	line1_start,
		const Eigen::Ref< const Eigen::Vector2d > &	line1_end,
		const Eigen::Ref< const Eigen::Vector2d > &	line2_start,
		const Eigen::Ref< const Eigen::Vector2d > &	line2_end,
		Eigen::Vector2d *	intersection = `NULL`
	)

inline

Helper function to check whether two line segments intersects.

Parameters

	line1_start	2D point representing the start of the first line segment
	line1_end	2D point representing the end of the first line segment
	line2_start	2D point representing the start of the second line segment
	line2_end	2D point representing the end of the second line segment
[out]	intersection	[optional] Write intersection point to destination (the value is only written, if both lines intersect, e.g. if the function returns `true`)

Returns: true if both line segments intersect

Definition at line 97 of file distance_calculations.h.

Eigen::Vector2d teb_local_planner::closest_point_on_line_segment_2d	(	const Eigen::Ref< const Eigen::Vector2d > &	point,
		const Eigen::Ref< const Eigen::Vector2d > &	line_start,
		const Eigen::Ref< const Eigen::Vector2d > &	line_end
	)

inline

Helper function to obtain the closest point on a line segment w.r.t. a reference point.

Parameters

point	2D point
line_start	2D point representing the start of the line segment
line_end	2D point representing the end of the line segment

Returns: Closest point on the line segment

Definition at line 60 of file distance_calculations.h.

template<typename V1 , typename V2 >

double teb_local_planner::cross2d	(	const V1 &	v1,
		const V2 &	v2
	)

inline

Calculate the 2d cross product (returns length of the resulting vector along the z-axis in 3d)

Parameters

v1	object containing public methods x() and y()
v2	object containing fields x() and y()

Returns: magnitude that would result in the 3D case (along the z-axis)

Definition at line 120 of file misc.h.

double teb_local_planner::distance_point_to_polygon_2d	(	const Eigen::Vector2d &	point,
		const Point2dContainer &	vertices
	)

inline

Helper function to calculate the smallest distance between a point and a closed polygon.

Parameters

point	2D point
vertices	Vertices describing the closed polygon (the first vertex is not repeated at the end)

Returns: smallest distance between point and polygon

Definition at line 165 of file distance_calculations.h.

double teb_local_planner::distance_point_to_segment_2d	(	const Eigen::Ref< const Eigen::Vector2d > &	point,
		const Eigen::Ref< const Eigen::Vector2d > &	line_start,
		const Eigen::Ref< const Eigen::Vector2d > &	line_end
	)

inline

Helper function to calculate the distance between a line segment and a point.

Parameters

point	2D point
line_start	2D point representing the start of the line segment
line_end	2D point representing the end of the line segment

Returns: minimum distance to a given line segment

Definition at line 83 of file distance_calculations.h.

template<typename P1 , typename P2 >

double teb_local_planner::distance_points2d	(	const P1 &	point1,
		const P2 &	point2
	)

inline

Calculate Euclidean distance between two 2D point datatypes.

Parameters

point1	object containing fields x and y
point2	object containing fields x and y

Returns: Euclidean distance: ||point2-point1||

Definition at line 107 of file misc.h.

double teb_local_planner::distance_polygon_to_polygon_2d	(	const Point2dContainer &	vertices1,
		const Point2dContainer &	vertices2
	)

inline

Helper function to calculate the smallest distance between two closed polygons.

Parameters

vertices1	Vertices describing the first closed polygon (the first vertex is not repeated at the end)
vertices2	Vertices describing the second closed polygon (the first vertex is not repeated at the end)

Returns: smallest distance between point and polygon

Definition at line 236 of file distance_calculations.h.

double teb_local_planner::distance_segment_to_polygon_2d	(	const Eigen::Vector2d &	line_start,
		const Eigen::Vector2d &	line_end,
		const Point2dContainer &	vertices
	)

inline

Helper function to calculate the smallest distance between a line segment and a closed polygon.

Parameters

line_start	2D point representing the start of the line segment
line_end	2D point representing the end of the line segment
vertices	Vertices describing the closed polygon (the first vertex is not repeated at the end)

Returns: smallest distance between point and polygon

Definition at line 201 of file distance_calculations.h.

double teb_local_planner::distance_segment_to_segment_2d	(	const Eigen::Ref< const Eigen::Vector2d > &	line1_start,
		const Eigen::Ref< const Eigen::Vector2d > &	line1_end,
		const Eigen::Ref< const Eigen::Vector2d > &	line2_start,
		const Eigen::Ref< const Eigen::Vector2d > &	line2_end
	)

inline

Helper function to calculate the smallest distance between two line segments.

Parameters

line1_start	2D point representing the start of the first line segment
line1_end	2D point representing the end of the first line segment
line2_start	2D point representing the start of the second line segment
line2_end	2D point representing the end of the second line segment

Returns: smallest distance between both segments

Definition at line 138 of file distance_calculations.h.

double teb_local_planner::fast_sigmoid ( double x )

inline

Calculate a fast approximation of a sigmoid function.

The following function is implemented: $ x / (1 + |x|) $

Parameters

x	the argument of the function

Definition at line 95 of file misc.h.

template<typename T >

const T& teb_local_planner::get_const_reference ( const T * ptr )

inline

Helper function that returns the const reference to a value defined by either its raw pointer type or const reference.

Return a constant reference for boths input variants (pointer or reference).

Remarks: Makes only sense in combination with the overload getConstReference(const T& val).

Parameters

ptr	pointer of type T

Template Parameters

T	arbitrary type

Returns: If T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion

Definition at line 135 of file misc.h.

template<typename T >

const T& teb_local_planner::get_const_reference	(	const T &	val,
		typename boost::disable_if< boost::is_pointer< T > >::type *	dummy = `0`
	)

inline

Helper function that returns the const reference to a value defined by either its raw pointer type or const reference.

Return a constant reference for boths input variants (pointer or reference).

Remarks: Makes only sense in combination with the overload getConstReference(const T* val).

Parameters

val
dummy	SFINAE helper variable

Template Parameters

T	arbitrary type

Returns: If T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion

Definition at line 148 of file misc.h.

std::complex<long double> teb_local_planner::getCplxFromHcGraph	(	HcGraphVertexType	vert_descriptor,
		const HcGraph &	graph
	)

inline

Inline function used for initializing the TEB in combination with HCP graph vertex descriptors.

Definition at line 93 of file graph_search.h.

std::complex<long double> teb_local_planner::getCplxFromMsgPoseStamped ( const geometry_msgs::PoseStamped & pose )

inline

Inline function used for calculateHSignature() in combination with geometry_msgs::PoseStamped.

Definition at line 78 of file homotopy_class_planner.h.

std::complex<long double> teb_local_planner::getCplxFromVertexPosePtr ( const VertexPose * pose )

inline

< Inline function used for calculateHSignature() in combination with VertexPose pointers

Definition at line 71 of file homotopy_class_planner.h.

const Eigen::Vector2d& teb_local_planner::getVector2dFromHcGraph	(	HcGraphVertexType	vert_descriptor,
		const HcGraph &	graph
	)

inline

Definition at line 99 of file graph_search.h.

bool teb_local_planner::is_close	(	double	a,
		double	b,
		double	epsilon = `1e-4`
	)

inline

Check whether two variables (double) are close to each other.

Parameters

a	the first value to compare
b	the second value to compare
epsilon	precision threshold

Returns: true if |a-b| < epsilon, false otherwise

Definition at line 62 of file misc.h.

double teb_local_planner::penaltyBoundFromBelow	(	const double &	var,
		const double &	a,
		const double &	epsilon
	)

inline

Linear penalty function for bounding var from below: $ a < var $ .

Parameters

var	The scalar that should be bounded
a	lower bound
epsilon	safty margin (move bound to the interior of the interval)

See also: penaltyBoundFromBelowDerivative

Returns: Penalty / cost value that is nonzero if the constraint is not satisfied

Definition at line 107 of file penalties.h.

double teb_local_planner::penaltyBoundFromBelowDerivative	(	const double &	var,
		const double &	a,
		const double &	epsilon
	)

inline

Derivative of the linear penalty function for bounding var from below: $ a < var $ .

Parameters

var	The scalar that should be bounded
a	lower bound
epsilon	safty margin (move bound to the interior of the interval)

See also: penaltyBoundFromBelow

Returns: Derivative of the penalty function w.r.t. var

Definition at line 177 of file penalties.h.

double teb_local_planner::penaltyBoundToInterval	(	const double &	var,
		const double &	a,
		const double &	epsilon
	)

inline

Linear penalty function for bounding var to the interval $ -a < var < a $ .

Parameters

var	The scalar that should be bounded
a	lower and upper absolute bound
epsilon	safty margin (move bound to the interior of the interval)

See also: penaltyBoundToIntervalDerivative

Returns: Penalty / cost value that is nonzero if the constraint is not satisfied

Definition at line 57 of file penalties.h.

double teb_local_planner::penaltyBoundToInterval	(	const double &	var,
		const double &	a,
		const double &	b,
		const double &	epsilon
	)

inline

Linear penalty function for bounding var to the interval $ a < var < b $ .

Parameters

var	The scalar that should be bounded
a	lower bound
b	upper bound
epsilon	safty margin (move bound to the interior of the interval)

See also: penaltyBoundToIntervalDerivative

Returns: Penalty / cost value that is nonzero if the constraint is not satisfied

Definition at line 82 of file penalties.h.

double teb_local_planner::penaltyBoundToIntervalDerivative	(	const double &	var,
		const double &	a,
		const double &	epsilon
	)

inline

Derivative of the linear penalty function for bounding var to the interval $ -a < var < a $ .

Parameters

var	The scalar that should be bounded
a	lower and upper absolute bound
epsilon	safty margin (move bound to the interior of the interval)

See also: penaltyBoundToInterval

Returns: Derivative of the penalty function w.r.t. var

Definition at line 127 of file penalties.h.

double teb_local_planner::penaltyBoundToIntervalDerivative	(	const double &	var,
		const double &	a,
		const double &	b,
		const double &	epsilon
	)

inline

Derivative of the linear penalty function for bounding var to the interval $ a < var < b $ .

Parameters

var	The scalar that should be bounded
a	lower bound
b	upper bound
epsilon	safty margin (move bound to the interior of the interval)

See also: penaltyBoundToInterval

Returns: Derivative of the penalty function w.r.t. var

Definition at line 152 of file penalties.h.

bool teb_local_planner::smaller_than_abs	(	double	i,
		double	j
	)

inline

Small helper function: check if |a|<|b|.

Definition at line 87 of file misc.h.

Classes

Typedefs

Enumerations

Functions

Typedef Documentation

Enumeration Type Documentation

Function Documentation