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

#include <homotopy_class_planner.h>

Inheritance diagram for teb_local_planner::HomotopyClassPlanner:

[legend]

Public Types
using	EquivalenceClassContainer = std::vector< std::pair< EquivalenceClassPtr, bool > >

Public Member Functions
	HomotopyClassPlanner ()
	Default constructor. More...

	HomotopyClassPlanner (const TebConfig &cfg, ObstContainer obstacles=NULL, TebVisualizationPtr visualization=TebVisualizationPtr(), const ViaPointContainer via_points=NULL)
	Construct and initialize the HomotopyClassPlanner. More...

void	initialize (const TebConfig &cfg, ObstContainer obstacles=NULL, TebVisualizationPtr visualization=TebVisualizationPtr(), const ViaPointContainer via_points=NULL)
	Initialize the HomotopyClassPlanner. More...

virtual	~HomotopyClassPlanner ()
	Destruct the HomotopyClassPlanner. More...

Plan a trajectory
virtual bool	plan (const std::vector< geometry_msgs::PoseStamped > &initial_plan, const geometry_msgs::Twist *start_vel=NULL, bool free_goal_vel=false)
	Plan a trajectory based on an initial reference plan. More...

virtual bool	plan (const tf::Pose &start, const tf::Pose &goal, const geometry_msgs::Twist *start_vel=NULL, bool free_goal_vel=false)
	Plan a trajectory between a given start and goal pose (tf::Pose version). More...

virtual bool	plan (const PoseSE2 &start, const PoseSE2 &goal, const geometry_msgs::Twist *start_vel=NULL, bool free_goal_vel=false)
	Plan a trajectory between a given start and goal pose. More...

virtual bool	getVelocityCommand (double &vx, double &vy, double &omega, int look_ahead_poses) const
	Get the velocity command from a previously optimized plan to control the robot at the current sampling interval. More...

TebOptimalPlannerPtr	bestTeb () const
	Access current best trajectory candidate (that relates to the "best" homotopy class). More...

virtual bool	isTrajectoryFeasible (base_local_planner::CostmapModel *costmap_model, const std::vector< geometry_msgs::Point > &footprint_spec, double inscribed_radius=0.0, double circumscribed_radius=0.0, int look_ahead_idx=-1, double feasibility_check_lookahead_distance=-1.0)
	Check whether the planned trajectory is feasible or not. More...

TebOptimalPlannerPtr	findBestTeb ()
	In case of empty best teb, scores again the available plans to find the best one. The best_teb_ variable is updated consequently. More...

TebOptPlannerContainer::iterator	removeTeb (TebOptimalPlannerPtr &teb)
	Removes the specified teb and the corresponding homotopy class from the list of available ones. More...

Visualization
void	setVisualization (TebVisualizationPtr visualization)
	Register a TebVisualization class to enable visiualization routines (e.g. publish the local plan and pose sequence) More...

virtual void	visualize ()
	Publish the local plan, pose sequence and additional information via ros topics (e.g. subscribe with rviz). More...

Public Member Functions inherited from teb_local_planner::PlannerInterface
	PlannerInterface ()
	Default constructor. More...

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

virtual void	updateRobotModel (RobotFootprintModelPtr robot_model)

virtual void	computeCurrentCost (std::vector< double > &cost, double obst_cost_scale=1.0, bool alternative_time_cost=false)

Important steps that are called during planning
void	exploreEquivalenceClassesAndInitTebs (const PoseSE2 &start, const PoseSE2 &goal, double dist_to_obst, const geometry_msgs::Twist *start_vel, bool free_goal_vel=false)
	Explore paths in new equivalence classes (e.g. homotopy classes) and initialize TEBs from them. More...

template<typename BidirIter , typename Fun >
TebOptimalPlannerPtr	addAndInitNewTeb (BidirIter path_start, BidirIter path_end, Fun fun_position, double start_orientation, double goal_orientation, const geometry_msgs::Twist *start_velocity, bool free_goal_vel=false)
	Add a new Teb to the internal trajectory container, if this teb constitutes a new equivalence class. Initialize it using a generic 2D reference path. More...

TebOptimalPlannerPtr	addAndInitNewTeb (const PoseSE2 &start, const PoseSE2 &goal, const geometry_msgs::Twist *start_velocity, bool free_goal_vel=false)
	Add a new Teb to the internal trajectory container, if this teb constitutes a new equivalence class. Initialize it with a simple straight line between a given start and goal. More...

TebOptimalPlannerPtr	addAndInitNewTeb (const std::vector< geometry_msgs::PoseStamped > &initial_plan, const geometry_msgs::Twist *start_velocity, bool free_goal_vel=false)
	Add a new Teb to the internal trajectory container , if this teb constitutes a new equivalence class. Initialize it using a PoseStamped container. More...

void	updateAllTEBs (const PoseSE2 start, const PoseSE2 goal, const geometry_msgs::Twist *start_velocity)
	Update TEBs with new pose, goal and current velocity. More...

void	optimizeAllTEBs (int iter_innerloop, int iter_outerloop)
	Optimize all available trajectories by invoking the optimizer on each one. More...

TebOptimalPlannerPtr	getInitialPlanTEB ()
	Returns a shared pointer to the TEB related to the initial plan. More...

TebOptimalPlannerPtr	selectBestTeb ()
	In case of multiple, internally stored, alternative trajectories, select the best one according to their cost values. More...

virtual void	clearPlanner ()
	Reset the planner. More...

virtual void	setPreferredTurningDir (RotType dir)
	Prefer a desired initial turning direction (by penalizing the opposing one) More...

template<typename BidirIter , typename Fun >
EquivalenceClassPtr	calculateEquivalenceClass (BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer *obstacles=NULL, boost::optional< TimeDiffSequence::iterator > timediff_start=boost::none, boost::optional< TimeDiffSequence::iterator > timediff_end=boost::none)
	Calculate the equivalence class of a path. More...

const TebOptPlannerContainer &	getTrajectoryContainer () const
	Read-only access to the internal trajectory container. More...

bool	hasDiverged () const override
	Returns true if the planner has diverged. More...

virtual void	computeCurrentCost (std::vector< double > &cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false)

void	deletePlansDetouringBackwards (const double orient_threshold, const double len_orientation_vector)
	Checks if the orientation of the computed trajectories differs from that of the best plan of more than the specified threshold and eventually deletes them. Also deletes detours with a duration much bigger than the duration of the best_teb (duration / best duration > max_ratio_detours_duration_best_duration). More...

bool	computeStartOrientation (const TebOptimalPlannerPtr plan, const double len_orientation_vector, double &orientation)
	Given a plan, computes its start orientation using a vector of length >= len_orientation_vector starting from the initial pose. More...

const TebConfig *	config () const
	Access config (read-only) More...

const ObstContainer *	obstacles () const
	Access current obstacle container (read-only) More...

bool	isInitialized () const
	Returns true if the planner is initialized. More...

void	clearGraph ()
	Clear any existing graph of the homotopy class search. More...

int	bestTebIdx () const
	find the index of the currently best TEB in the container More...

bool	addEquivalenceClassIfNew (const EquivalenceClassPtr &eq_class, bool lock=false)
	Internal helper function that adds a new equivalence class to the list of known classes only if it is unique. More...

const EquivalenceClassContainer &	getEquivalenceClassRef () const
	Return the current set of equivalence erelations (read-only) More...

bool	isInBestTebClass (const EquivalenceClassPtr &eq_class) const

int	numTebsInClass (const EquivalenceClassPtr &eq_class) const

int	numTebsInBestTebClass () const

void	randomlyDropTebs ()
	Randomly drop non-optimal TEBs to so we can explore other alternatives. More...

static bool	isHSignatureSimilar (const std::complex< long double > &h1, const std::complex< long double > &h2, double threshold)
	Check if two h-signatures are similar (w.r.t. a certain threshold) More...

Explore new paths and keep only a single one for each homotopy class
const TebConfig *	cfg_
	Config class that stores and manages all related parameters. More...

ObstContainer *	obstacles_
	Store obstacles that are relevant for planning. More...

const ViaPointContainer *	via_points_
	Store the current list of via-points. More...

TebVisualizationPtr	visualization_
	Instance of the visualization class (local/global plan, obstacles, ...) More...

TebOptimalPlannerPtr	best_teb_
	Store the current best teb. More...

EquivalenceClassPtr	best_teb_eq_class_
	Store the equivalence class of the current best teb. More...

const std::vector< geometry_msgs::PoseStamped > *	initial_plan_
	Store the initial plan if available for a better trajectory initialization. More...

EquivalenceClassPtr	initial_plan_eq_class_
	Store the equivalence class of the initial plan. More...

TebOptimalPlannerPtr	initial_plan_teb_
	Store pointer to the TEB related to the initial plan (use method getInitialPlanTEB() since it checks if initial_plan_teb_ is still included in tebs_.) More...

TebOptPlannerContainer	tebs_
	Container that stores multiple local teb planners (for alternative equivalence classes) and their corresponding costs. More...

EquivalenceClassContainer	equivalence_classes_
	Store all known quivalence classes (e.g. h-signatures) to allow checking for duplicates after finding and adding new ones. More...

boost::shared_ptr< GraphSearchInterface >	graph_search_

ros::Time	last_eq_class_switching_time_
	Store the time at which the equivalence class changed recently. More...

std::default_random_engine	random_

bool	initialized_
	Keeps track about the correct initialization of this class. More...

TebOptimalPlannerPtr	last_best_teb_
	Points to the plan used in the previous control cycle. More...

bool	hasEquivalenceClass (const EquivalenceClassPtr &eq_class) const
	Check if a h-signature exists already. More...

void	renewAndAnalyzeOldTebs (bool delete_detours)
	Renew all found h-signatures for the new planning step based on existing TEBs. Optionally detours can be discarded. More...

void	updateReferenceTrajectoryViaPoints (bool all_trajectories)
	Associate trajectories with via-points. More...

Detailed Description

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)

Equivalence classes (e.g. homotopy) are explored using the help of a search-graph.
A couple of possible candidates are sampled / generated and filtered afterwards such that only a single candidate per homotopy class remain. Filtering is applied using the H-Signature, a homotopy (resp. homology) invariant:

S. Bhattacharya et al.: Search-based Path Planning with Homotopy Class Constraints, AAAI, 2010
C. Rösmann et al.: Planning of Multiple Robot Trajectories in Distinctive Topologies, ECMR, 2015.

Followed by the homotopy class search, each candidate is used as an initialization for the underlying trajectory optimization (in this case utilizing the TebOptimalPlanner class with the TimedElasticBand).
Depending on the config parameters, the optimization is performed in parallel.
After the optimization is completed, the best optimized candidate is selected w.r.t. to trajectory cost, since the cost already contains important features like clearance from obstacles and transition time.
Everyhting is performed by calling one of the overloaded plan() methods.
Afterwards the velocity command to control the robot is obtained from the "best" candidate via getVelocityCommand().
All steps are repeated in the subsequent sampling interval with the exception, that already planned (optimized) trajectories are preferred against new path initilizations in order to improve the hot-starting capability.

Definition at line 144 of file homotopy_class_planner.h.

Member Typedef Documentation

◆ EquivalenceClassContainer

using teb_local_planner::HomotopyClassPlanner::EquivalenceClassContainer = std::vector< std::pair<EquivalenceClassPtr, bool> >

Definition at line 148 of file homotopy_class_planner.h.

Constructor & Destructor Documentation

◆ HomotopyClassPlanner() [1/2]

teb_local_planner::HomotopyClassPlanner::HomotopyClassPlanner ( )

Default constructor.

Definition at line 82 of file homotopy_class_planner.cpp.

◆ HomotopyClassPlanner() [2/2]

teb_local_planner::HomotopyClassPlanner::HomotopyClassPlanner	(	const TebConfig &	cfg,
		ObstContainer *	obstacles = `NULL`,
		TebVisualizationPtr	visualization = `TebVisualizationPtr()`,
		const ViaPointContainer *	via_points = `NULL`
	)

Construct and initialize the HomotopyClassPlanner.

Parameters

cfg	Const reference to the TebConfig class for internal parameters
obstacles	Container storing all relevant obstacles (see Obstacle)
visualization	Shared pointer to the TebVisualization class (optional)
via_points	Container storing via-points (optional)

Definition at line 86 of file homotopy_class_planner.cpp.

◆ ~HomotopyClassPlanner()

teb_local_planner::HomotopyClassPlanner::~HomotopyClassPlanner ( )

virtual

Destruct the HomotopyClassPlanner.

Definition at line 92 of file homotopy_class_planner.cpp.

Member Function Documentation

◆ addAndInitNewTeb() [1/3]

template<typename BidirIter , typename Fun >

TebOptimalPlannerPtr teb_local_planner::HomotopyClassPlanner::addAndInitNewTeb	(	BidirIter	path_start,
		BidirIter	path_end,
		Fun	fun_position,
		double	start_orientation,
		double	goal_orientation,
		const geometry_msgs::Twist *	start_velocity,
		bool	free_goal_vel = `false`
	)

Add a new Teb to the internal trajectory container, if this teb constitutes a new equivalence class. Initialize it using a generic 2D reference path.

Refer to TimedElasticBand::initTEBtoGoal() for more details about the template parameters.

Parameters

path_start	start iterator of a generic 2d path
path_end	end iterator of a generic 2d path
fun_position	unary function that returns the Eigen::Vector2d object
start_orientation	Orientation of the first pose of the trajectory (optional, otherwise use goal heading)
goal_orientation	Orientation of the last pose of the trajectory (optional, otherwise use goal heading)
start_velocity	start velocity (optional)
free_goal_vel	if `true`, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)

Template Parameters

BidirIter	Bidirectional iterator type
Fun	unyary function that transforms the dereferenced iterator into an Eigen::Vector2d

Returns: Shared pointer to the newly created teb optimal planner

Definition at line 102 of file homotopy_class_planner.hpp.

◆ addAndInitNewTeb() [2/3]

TebOptimalPlannerPtr teb_local_planner::HomotopyClassPlanner::addAndInitNewTeb	(	const PoseSE2 &	start,
		const PoseSE2 &	goal,
		const geometry_msgs::Twist *	start_velocity,
		bool	free_goal_vel = `false`
	)

Add a new Teb to the internal trajectory container, if this teb constitutes a new equivalence class. Initialize it with a simple straight line between a given start and goal.

Parameters

start	start pose
goal	goal pose
start_velocity	start velocity (optional)
free_goal_vel	if `true`, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)

Returns: Shared pointer to the newly created teb optimal planner

Definition at line 395 of file homotopy_class_planner.cpp.

◆ addAndInitNewTeb() [3/3]

TebOptimalPlannerPtr teb_local_planner::HomotopyClassPlanner::addAndInitNewTeb	(	const std::vector< geometry_msgs::PoseStamped > &	initial_plan,
		const geometry_msgs::Twist *	start_velocity,
		bool	free_goal_vel = `false`
	)

Add a new Teb to the internal trajectory container , if this teb constitutes a new equivalence class. Initialize it using a PoseStamped container.

Parameters

initial_plan	container of poses (start and goal orientation should be valid!)
start_velocity	start velocity (optional)
free_goal_vel	if `true`, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)

Returns: Shared pointer to the newly created teb optimal planner

Definition at line 450 of file homotopy_class_planner.cpp.

◆ addEquivalenceClassIfNew()

bool teb_local_planner::HomotopyClassPlanner::addEquivalenceClassIfNew	(	const EquivalenceClassPtr &	eq_class,
		bool	lock = `false`
	)

Internal helper function that adds a new equivalence class to the list of known classes only if it is unique.

Parameters

eq_class	equivalence class that should be tested
lock	if `true`, exclude the H-signature from deletion.

Returns: true if the h-signature was added and no duplicate was found, false otherwise

Definition at line 225 of file homotopy_class_planner.cpp.

◆ bestTeb()

TebOptimalPlannerPtr teb_local_planner::HomotopyClassPlanner::bestTeb ( ) const

inline

Access current best trajectory candidate (that relates to the "best" homotopy class).

If no trajectory is available, the pointer will be empty. If only a single trajectory is available, return it. Otherwise return the best one, but call selectBestTeb() before to perform the actual selection (part of the plan() methods).

Returns: Shared pointer to the best TebOptimalPlanner that contains the selected trajectory (TimedElasticBand).

Definition at line 243 of file homotopy_class_planner.h.

◆ bestTebIdx()

int teb_local_planner::HomotopyClassPlanner::bestTebIdx ( ) const

find the index of the currently best TEB in the container

Remarks: bestTeb() should be preferred whenever possible

Returns: index of the best TEB obtained with bestTEB(), if no TEB is avaiable, it returns -1.

Definition at line 705 of file homotopy_class_planner.cpp.

◆ calculateEquivalenceClass()

template<typename BidirIter , typename Fun >

EquivalenceClassPtr teb_local_planner::HomotopyClassPlanner::calculateEquivalenceClass	(	BidirIter	path_start,
		BidirIter	path_end,
		Fun	fun_cplx_point,
		const ObstContainer *	obstacles = `NULL`,
		boost::optional< TimeDiffSequence::iterator >	timediff_start = `boost::none`,
		boost::optional< TimeDiffSequence::iterator >	timediff_end = `boost::none`
	)

Calculate the equivalence class of a path.

Currently, only the H-signature (refer to HSignature) is implemented.

Parameters

path_start	Iterator to the first element in the path
path_end	Iterator to the last element in the path
obstacles	obstacle container
fun_cplx_point	function accepting the dereference iterator type and that returns the position as complex number.

Template Parameters

BidirIter	Bidirectional iterator type
Fun	function of the form std::complex< long double > (const T& point_type)

Returns: pointer to the equivalence class base type

Definition at line 83 of file homotopy_class_planner.hpp.

◆ clearGraph()

void teb_local_planner::HomotopyClassPlanner::clearGraph ( )

inline

Clear any existing graph of the homotopy class search.

Definition at line 499 of file homotopy_class_planner.h.

◆ clearPlanner()

virtual void teb_local_planner::HomotopyClassPlanner::clearPlanner ( )

inlinevirtual

Reset the planner.

Clear all previously found H-signatures, paths, tebs and the hcgraph.

Implements teb_local_planner::PlannerInterface.

Definition at line 397 of file homotopy_class_planner.h.

◆ computeCurrentCost()

void teb_local_planner::HomotopyClassPlanner::computeCurrentCost	(	std::vector< double > &	cost,
		double	obst_cost_scale = `1.0`,
		double	viapoint_cost_scale = `1.0`,
		bool	alternative_time_cost = `false`
	)

virtual

Compute and return the cost of the current optimization graph (supports multiple trajectories)

Parameters

[out]	cost	current cost value for each trajectory [for a planner with just a single trajectory: size=1, vector will not be cleared]
	obst_cost_scale	Specify extra scaling for obstacle costs
	viapoint_cost_scale	Specify extra scaling for via points.
	alternative_time_cost	Replace the cost for the time optimal objective by the actual (weighted) transition time

Definition at line 794 of file homotopy_class_planner.cpp.

◆ computeStartOrientation()

bool teb_local_planner::HomotopyClassPlanner::computeStartOrientation	(	const TebOptimalPlannerPtr	plan,
		const double	len_orientation_vector,
		double &	orientation
	)

Given a plan, computes its start orientation using a vector of length >= len_orientation_vector starting from the initial pose.

Parameters

plan	Teb to be analyzed
len_orientation_vector	min length of the vector used to compute the start orientation
orientation	computed start orientation

Returns: : Could the vector for the orientation check be computed? (False if the plan has no pose with a distance > len_orientation_vector from the start poseq)

Definition at line 855 of file homotopy_class_planner.cpp.

◆ config()

const TebConfig* teb_local_planner::HomotopyClassPlanner::config ( ) const

inline

Access config (read-only)

Returns: const pointer to the config instance

Definition at line 483 of file homotopy_class_planner.h.

◆ deletePlansDetouringBackwards()

void teb_local_planner::HomotopyClassPlanner::deletePlansDetouringBackwards	(	const double	orient_threshold,
		const double	len_orientation_vector
	)

Checks if the orientation of the computed trajectories differs from that of the best plan of more than the specified threshold and eventually deletes them. Also deletes detours with a duration much bigger than the duration of the best_teb (duration / best duration > max_ratio_detours_duration_best_duration).

Parameters

orient_threshold	Threshold paramter for allowed orientation changes in radians
len_orientation_vector	length of the vector used to compute the start orientation

Definition at line 802 of file homotopy_class_planner.cpp.

◆ exploreEquivalenceClassesAndInitTebs()

void teb_local_planner::HomotopyClassPlanner::exploreEquivalenceClassesAndInitTebs	(	const PoseSE2 &	start,
		const PoseSE2 &	goal,
		double	dist_to_obst,
		const geometry_msgs::Twist *	start_vel,
		bool	free_goal_vel = `false`
	)

Explore paths in new equivalence classes (e.g. homotopy classes) and initialize TEBs from them.

This "all-in-one" method creates a graph with position keypoints from which feasible paths (with clearance from obstacles) are extracted.
All obtained paths are filted to only keep a single path for each equivalence class.
Each time a new equivalence class is explored (by means of no previous trajectory/TEB remain in that equivalence class), a new trajectory/TEB will be initialized.
Everything is prepared now for the optimization step: see optimizeAllTEBs().

Parameters

start	Current start pose (e.g. pose of the robot)
goal	Goal pose (e.g. robot's goal)
dist_to_obst	Allowed distance to obstacles: if not satisfying, the path will be rejected (note, this is not the distance used for optimization).

Definition at line 373 of file homotopy_class_planner.cpp.

◆ findBestTeb()

TebOptimalPlannerPtr teb_local_planner::HomotopyClassPlanner::findBestTeb ( )

In case of empty best teb, scores again the available plans to find the best one. The best_teb_ variable is updated consequently.

Returns: Shared pointer to the best TebOptimalPlanner that contains the selected trajectory (TimedElasticBand). An empty pointer is returned if no plan is available.

Definition at line 745 of file homotopy_class_planner.cpp.

◆ getEquivalenceClassRef()

const EquivalenceClassContainer& teb_local_planner::HomotopyClassPlanner::getEquivalenceClassRef ( ) const

inline

Return the current set of equivalence erelations (read-only)

Returns: reference to the internal set of currently tracked equivalence relations

Definition at line 521 of file homotopy_class_planner.h.

◆ getInitialPlanTEB()

TebOptimalPlannerPtr teb_local_planner::HomotopyClassPlanner::getInitialPlanTEB ( )

Returns a shared pointer to the TEB related to the initial plan.

Returns: A non-empty shared ptr is returned if a match was found; Otherwise the shared ptr is empty.

Definition at line 531 of file homotopy_class_planner.cpp.

◆ getTrajectoryContainer()

const TebOptPlannerContainer& teb_local_planner::HomotopyClassPlanner::getTrajectoryContainer ( ) const

inline

Read-only access to the internal trajectory container.

Returns: read-only reference to the teb container.

Definition at line 431 of file homotopy_class_planner.h.

◆ getVelocityCommand()

bool teb_local_planner::HomotopyClassPlanner::getVelocityCommand	(	double &	vx,
		double &	vy,
		double &	omega,
		int	look_ahead_poses
	)		const

virtual

Get the velocity command from a previously optimized plan to control the robot at the current sampling interval.

Warning: Call plan() first and check if the generated plan is feasible.

Parameters

[out]	vx	translational velocity [m/s]
[out]	vy	strafing velocity which can be nonzero for holonomic robots [m/s]
[out]	omega	rotational velocity [rad/s]
[in]	look_ahead_poses	index of the final pose used to compute the velocity command.

Returns: true if command is valid, false otherwise

Implements teb_local_planner::PlannerInterface.

Definition at line 163 of file homotopy_class_planner.cpp.

◆ hasDiverged()

bool teb_local_planner::HomotopyClassPlanner::hasDiverged ( ) const

overridevirtual

Returns true if the planner has diverged.

Implements teb_local_planner::PlannerInterface.

Definition at line 785 of file homotopy_class_planner.cpp.

◆ hasEquivalenceClass()

bool teb_local_planner::HomotopyClassPlanner::hasEquivalenceClass ( const EquivalenceClassPtr & eq_class ) const

protected

Check if a h-signature exists already.

Parameters

eq_class equivalence class that should be tested

Returns: true if the h-signature is found, false otherwise

Definition at line 214 of file homotopy_class_planner.cpp.

◆ initialize()

void teb_local_planner::HomotopyClassPlanner::initialize	(	const TebConfig &	cfg,
		ObstContainer *	obstacles = `NULL`,
		TebVisualizationPtr	visualization = `TebVisualizationPtr()`,
		const ViaPointContainer *	via_points = `NULL`
	)

Initialize the HomotopyClassPlanner.

Parameters

cfg	Const reference to the TebConfig class for internal parameters
obstacles	Container storing all relevant obstacles (see Obstacle)
visualization	Shared pointer to the TebVisualization class (optional)
via_points	Container storing via-points (optional)

Definition at line 96 of file homotopy_class_planner.cpp.

◆ isHSignatureSimilar()

static bool teb_local_planner::HomotopyClassPlanner::isHSignatureSimilar	(	const std::complex< long double > &	h1,
		const std::complex< long double > &	h2,
		double	threshold
	)

inlinestatic

Check if two h-signatures are similar (w.r.t. a certain threshold)

Parameters

h1	first h-signature
h2	second h-signature

Returns: true if both h-signatures are similar, false otherwise.

Definition at line 451 of file homotopy_class_planner.h.

◆ isInBestTebClass()

bool teb_local_planner::HomotopyClassPlanner::isInBestTebClass ( const EquivalenceClassPtr & eq_class ) const

Definition at line 423 of file homotopy_class_planner.cpp.

◆ isInitialized()

bool teb_local_planner::HomotopyClassPlanner::isInitialized ( ) const

inline

Returns true if the planner is initialized.

Definition at line 494 of file homotopy_class_planner.h.

◆ isTrajectoryFeasible()

bool teb_local_planner::HomotopyClassPlanner::isTrajectoryFeasible	(	base_local_planner::CostmapModel *	costmap_model,
		const std::vector< geometry_msgs::Point > &	footprint_spec,
		double	inscribed_radius = `0.0`,
		double	circumscribed_radius = `0.0`,
		int	look_ahead_idx = `-1`,
		double	feasibility_check_lookahead_distance = `-1.0`
	)

virtual

Check whether the planned trajectory is feasible or not.

This method currently checks only that the trajectory, or a part of the trajectory is collision free. Obstacles are here represented as costmap instead of the internal ObstacleContainer.

Parameters

costmap_model	Pointer to the costmap model
footprint_spec	The specification of the footprint of the robot in world coordinates
inscribed_radius	The radius of the inscribed circle of the robot
circumscribed_radius	The radius of the circumscribed circle of the robot
look_ahead_idx	Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked.

Returns: true, if the robot footprint along the first part of the trajectory intersects with any obstacle in the costmap, false otherwise.

Implements teb_local_planner::PlannerInterface.

Definition at line 722 of file homotopy_class_planner.cpp.

◆ numTebsInBestTebClass()

int teb_local_planner::HomotopyClassPlanner::numTebsInBestTebClass ( ) const

Definition at line 442 of file homotopy_class_planner.cpp.

◆ numTebsInClass()

int teb_local_planner::HomotopyClassPlanner::numTebsInClass ( const EquivalenceClassPtr & eq_class ) const

Definition at line 431 of file homotopy_class_planner.cpp.

◆ obstacles()

const ObstContainer* teb_local_planner::HomotopyClassPlanner::obstacles ( ) const

inline

Access current obstacle container (read-only)

Returns: const pointer to the obstacle container instance

Definition at line 489 of file homotopy_class_planner.h.

◆ optimizeAllTEBs()

void teb_local_planner::HomotopyClassPlanner::optimizeAllTEBs	(	int	iter_innerloop,
		int	iter_outerloop
	)

Optimize all available trajectories by invoking the optimizer on each one.

Depending on the configuration parameters, the optimization is performed either single or multi threaded.

Parameters

iter_innerloop	Number of inner iterations (see TebOptimalPlanner::optimizeTEB())
iter_outerloop	Number of outer iterations (see TebOptimalPlanner::optimizeTEB())

Definition at line 502 of file homotopy_class_planner.cpp.

◆ plan() [1/3]

bool teb_local_planner::HomotopyClassPlanner::plan	(	const PoseSE2 &	start,
		const PoseSE2 &	goal,
		const geometry_msgs::Twist *	start_vel = `NULL`,
		bool	free_goal_vel = `false`
	)

virtual

Plan a trajectory between a given start and goal pose.

Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose.

Parameters

start	PoseSE2 containing the start pose of the trajectory
goal	PoseSE2 containing the goal pose of the trajectory
start_vel	Initial velocity at the start pose (twist message containing the translational and angular velocity).
free_goal_vel	if `true`, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)

Returns: true if planning was successful, false otherwise

Implements teb_local_planner::PlannerInterface.

Definition at line 143 of file homotopy_class_planner.cpp.

◆ plan() [2/3]

bool teb_local_planner::HomotopyClassPlanner::plan	(	const std::vector< geometry_msgs::PoseStamped > &	initial_plan,
		const geometry_msgs::Twist *	start_vel = `NULL`,
		bool	free_goal_vel = `false`
	)

virtual

Plan a trajectory based on an initial reference plan.

Provide this method to create and optimize a trajectory that is initialized according to an initial reference plan (given as a container of poses).

Warning: The current implementation extracts only the start and goal pose and calls the overloaded plan()

Parameters

initial_plan	vector of geometry_msgs::PoseStamped (must be valid until clearPlanner() is called!)
start_vel	Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)
free_goal_vel	if `true`, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)

Returns: true if planning was successful, false otherwise

Implements teb_local_planner::PlannerInterface.

Definition at line 121 of file homotopy_class_planner.cpp.

◆ plan() [3/3]

bool teb_local_planner::HomotopyClassPlanner::plan	(	const tf::Pose &	start,
		const tf::Pose &	goal,
		const geometry_msgs::Twist *	start_vel = `NULL`,
		bool	free_goal_vel = `false`
	)

virtual

Plan a trajectory between a given start and goal pose (tf::Pose version).

Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose.

Parameters

start	tf::Pose containing the start pose of the trajectory
goal	tf::Pose containing the goal pose of the trajectory
start_vel	Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)
free_goal_vel	if `true`, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)

Returns: true if planning was successful, false otherwise

Implements teb_local_planner::PlannerInterface.

Definition at line 135 of file homotopy_class_planner.cpp.

◆ randomlyDropTebs()

void teb_local_planner::HomotopyClassPlanner::randomlyDropTebs ( )

Randomly drop non-optimal TEBs to so we can explore other alternatives.

The HCP has a tendency to become "fixated" once its tebs_ list becomes fully populated, repeatedly refining and evaluating paths from the same few homotopy classes until the robot moves far enough for a teb to become invalid. As a result, it can fail to discover a more optimal path. This function alleviates this problem by randomly dropping TEBs other than the current "best" one with a probability controlled by selection_dropping_probability parameter.

Definition at line 575 of file homotopy_class_planner.cpp.

◆ removeTeb()

TebOptPlannerContainer::iterator teb_local_planner::HomotopyClassPlanner::removeTeb ( TebOptimalPlannerPtr & teb )

Removes the specified teb and the corresponding homotopy class from the list of available ones.

Parameters

pointer to the teb Band to be removed

Returns: Iterator to the next valid teb if available, else to the end of the tebs container.

Definition at line 754 of file homotopy_class_planner.cpp.

◆ renewAndAnalyzeOldTebs()

void teb_local_planner::HomotopyClassPlanner::renewAndAnalyzeOldTebs ( bool delete_detours )

protected

Renew all found h-signatures for the new planning step based on existing TEBs. Optionally detours can be discarded.

Calling this method in each new planning interval is really important. First all old h-signatures are deleted, since they could be invalid for this planning step (obstacle position may changed). Afterwards the h-signatures are calculated for each existing TEB/trajectory and is inserted to the list of known h-signatures. Doing this is important to prefer already optimized trajectories in contrast to initialize newly explored coarse paths.

Parameters

delete_detours if this param is true, all existing TEBs are cleared from detour-candidates calling deletePlansGoingBackwards().

Definition at line 250 of file homotopy_class_planner.cpp.

◆ selectBestTeb()

TebOptimalPlannerPtr teb_local_planner::HomotopyClassPlanner::selectBestTeb ( )

In case of multiple, internally stored, alternative trajectories, select the best one according to their cost values.

The trajectory cost includes features such as transition time and clearance from obstacles.
The best trajectory can be accessed later by bestTeb() within the current sampling interval in order to avoid unessary recalculations.

Returns: Shared pointer to the best TebOptimalPlanner that contains the selected trajectory (TimedElasticBand).

Definition at line 600 of file homotopy_class_planner.cpp.

◆ setPreferredTurningDir()

void teb_local_planner::HomotopyClassPlanner::setPreferredTurningDir ( RotType dir )

virtual

Prefer a desired initial turning direction (by penalizing the opposing one)

A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two solutions (in the same equivalence class!) with similar cost. Check the parameters in order to adjust the weight of the penalty. Initial means that the penalty is applied only to the first few poses of the trajectory.

Parameters

dir	This parameter might be RotType::left (prefer left), RotType::right (prefer right) or RotType::none (prefer none)

Reimplemented from teb_local_planner::PlannerInterface.

Definition at line 776 of file homotopy_class_planner.cpp.

◆ setVisualization()

void teb_local_planner::HomotopyClassPlanner::setVisualization ( TebVisualizationPtr visualization )

Register a TebVisualization class to enable visiualization routines (e.g. publish the local plan and pose sequence)

Parameters

visualization shared pointer to a TebVisualization instance

See also: visualizeTeb

Definition at line 116 of file homotopy_class_planner.cpp.

◆ updateAllTEBs()

void teb_local_planner::HomotopyClassPlanner::updateAllTEBs	(	const PoseSE2 *	start,
		const PoseSE2 *	goal,
		const geometry_msgs::Twist *	start_velocity
	)

Update TEBs with new pose, goal and current velocity.

Parameters

start	New start pose (optional)
goal	New goal pose (optional)
start_velocity	start velocity (optional)

Definition at line 479 of file homotopy_class_planner.cpp.

◆ updateReferenceTrajectoryViaPoints()

void teb_local_planner::HomotopyClassPlanner::updateReferenceTrajectoryViaPoints ( bool all_trajectories )

protected

Associate trajectories with via-points.

If all_trajectories is true, all trajectory candidates are connected with the set of via_points, otherwise only the trajectory sharing the homotopy class of the initial/global plan (and all via-points from alternative trajectories are removed)

Remarks: Requires that the plan method is called with an initial plan provided and that via-points are enabled (config)

Parameters

all_trajectories see method description

Definition at line 340 of file homotopy_class_planner.cpp.

◆ visualize()

void teb_local_planner::HomotopyClassPlanner::visualize ( )

virtual

Publish the local plan, pose sequence and additional information via ros topics (e.g. subscribe with rviz).

Make sure to register a TebVisualization instance before using setVisualization() or an overlaoded constructor.

See also: setVisualization

Reimplemented from teb_local_planner::PlannerInterface.

Definition at line 180 of file homotopy_class_planner.cpp.

Member Data Documentation

◆ best_teb_

TebOptimalPlannerPtr teb_local_planner::HomotopyClassPlanner::best_teb_

protected

Store the current best teb.

Definition at line 586 of file homotopy_class_planner.h.

◆ best_teb_eq_class_

EquivalenceClassPtr teb_local_planner::HomotopyClassPlanner::best_teb_eq_class_

protected

Store the equivalence class of the current best teb.

Definition at line 587 of file homotopy_class_planner.h.

◆ cfg_

const TebConfig* teb_local_planner::HomotopyClassPlanner::cfg_

protected

Config class that stores and manages all related parameters.

Definition at line 580 of file homotopy_class_planner.h.

◆ equivalence_classes_

EquivalenceClassContainer teb_local_planner::HomotopyClassPlanner::equivalence_classes_

protected

Store all known quivalence classes (e.g. h-signatures) to allow checking for duplicates after finding and adding new ones.

Definition at line 595 of file homotopy_class_planner.h.

◆ graph_search_

boost::shared_ptr<GraphSearchInterface> teb_local_planner::HomotopyClassPlanner::graph_search_

protected

Definition at line 598 of file homotopy_class_planner.h.

◆ initial_plan_

const std::vector<geometry_msgs::PoseStamped>* teb_local_planner::HomotopyClassPlanner::initial_plan_

protected

Store the initial plan if available for a better trajectory initialization.

Definition at line 589 of file homotopy_class_planner.h.

◆ initial_plan_eq_class_

EquivalenceClassPtr teb_local_planner::HomotopyClassPlanner::initial_plan_eq_class_

protected

Store the equivalence class of the initial plan.

Definition at line 590 of file homotopy_class_planner.h.

◆ initial_plan_teb_

TebOptimalPlannerPtr teb_local_planner::HomotopyClassPlanner::initial_plan_teb_

protected

Store pointer to the TEB related to the initial plan (use method getInitialPlanTEB() since it checks if initial_plan_teb_ is still included in tebs_.)

Definition at line 591 of file homotopy_class_planner.h.

◆ initialized_

bool teb_local_planner::HomotopyClassPlanner::initialized_

protected

Keeps track about the correct initialization of this class.

Definition at line 603 of file homotopy_class_planner.h.

◆ last_best_teb_

TebOptimalPlannerPtr teb_local_planner::HomotopyClassPlanner::last_best_teb_

protected

Points to the plan used in the previous control cycle.

Definition at line 605 of file homotopy_class_planner.h.

◆ last_eq_class_switching_time_

ros::Time teb_local_planner::HomotopyClassPlanner::last_eq_class_switching_time_

protected

Store the time at which the equivalence class changed recently.

Definition at line 600 of file homotopy_class_planner.h.

◆ obstacles_

ObstContainer* teb_local_planner::HomotopyClassPlanner::obstacles_

protected

Store obstacles that are relevant for planning.

Definition at line 581 of file homotopy_class_planner.h.

◆ random_

std::default_random_engine teb_local_planner::HomotopyClassPlanner::random_

protected

Definition at line 602 of file homotopy_class_planner.h.

◆ tebs_

TebOptPlannerContainer teb_local_planner::HomotopyClassPlanner::tebs_

protected

Container that stores multiple local teb planners (for alternative equivalence classes) and their corresponding costs.

Definition at line 593 of file homotopy_class_planner.h.

◆ via_points_

const ViaPointContainer* teb_local_planner::HomotopyClassPlanner::via_points_

protected

Store the current list of via-points.

Definition at line 582 of file homotopy_class_planner.h.

◆ visualization_

TebVisualizationPtr teb_local_planner::HomotopyClassPlanner::visualization_

protected

Instance of the visualization class (local/global plan, obstacles, ...)

Definition at line 585 of file homotopy_class_planner.h.

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

Public Types

Public Member Functions

Important steps that are called during planning

Explore new paths and keep only a single one for each homotopy class

Detailed Description

Member Typedef Documentation

◆ EquivalenceClassContainer

Constructor & Destructor Documentation

◆ HomotopyClassPlanner() [1/2]

◆ HomotopyClassPlanner() [2/2]

◆ ~HomotopyClassPlanner()

Member Function Documentation

◆ addAndInitNewTeb() [1/3]

◆ addAndInitNewTeb() [2/3]

◆ addAndInitNewTeb() [3/3]

◆ addEquivalenceClassIfNew()

◆ bestTeb()

◆ bestTebIdx()

◆ calculateEquivalenceClass()

◆ clearGraph()

◆ clearPlanner()

◆ computeCurrentCost()

◆ computeStartOrientation()

◆ config()

◆ deletePlansDetouringBackwards()

◆ exploreEquivalenceClassesAndInitTebs()

◆ findBestTeb()

◆ getEquivalenceClassRef()

◆ getInitialPlanTEB()

◆ getTrajectoryContainer()

◆ getVelocityCommand()

◆ hasDiverged()

◆ hasEquivalenceClass()

◆ initialize()

◆ isHSignatureSimilar()

◆ isInBestTebClass()

◆ isInitialized()

◆ isTrajectoryFeasible()

◆ numTebsInBestTebClass()

◆ numTebsInClass()

◆ obstacles()

◆ optimizeAllTEBs()

◆ plan() [1/3]

◆ plan() [2/3]

◆ plan() [3/3]

◆ randomlyDropTebs()

◆ removeTeb()

◆ renewAndAnalyzeOldTebs()

◆ selectBestTeb()

◆ setPreferredTurningDir()

◆ setVisualization()

◆ updateAllTEBs()

◆ updateReferenceTrajectoryViaPoints()

◆ visualize()

Member Data Documentation

◆ best_teb_

◆ best_teb_eq_class_

◆ cfg_

◆ equivalence_classes_

◆ graph_search_

◆ initial_plan_

◆ initial_plan_eq_class_

◆ initial_plan_teb_

◆ initialized_

◆ last_best_teb_

◆ last_eq_class_switching_time_

◆ obstacles_

◆ random_

◆ tebs_

◆ via_points_

◆ visualization_