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]

List of all members.

Public Member Functions
int	bestTebIdx () const
	find the index of the currently best TEB in the container
template<typename BidirIter , typename Fun >
EquivalenceClassPtr	calculateEquivalenceClass (BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer *obstacles=NULL)
	Calculate the equivalence class of a path.
void	clearGraph ()
	Clear any existing graph of the homotopy class search.
void	clearPlanner ()
	Reset the planner.
virtual void	computeCurrentCost (std::vector< double > &cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false)
const TebConfig *	config () const
	Access config (read-only)
const TebOptPlannerContainer &	getTrajectoryContainer () const
	Read-only access to the internal trajectory container.
	HomotopyClassPlanner ()
	Default constructor.
	HomotopyClassPlanner (const TebConfig &cfg, ObstContainer obstacles=NULL, RobotFootprintModelPtr robot_model=boost::make_shared< PointRobotFootprint >(), TebVisualizationPtr visualization=TebVisualizationPtr(), const ViaPointContainer via_points=NULL)
	Construct and initialize the HomotopyClassPlanner.
void	initialize (const TebConfig &cfg, ObstContainer obstacles=NULL, RobotFootprintModelPtr robot_model=boost::make_shared< PointRobotFootprint >(), TebVisualizationPtr visualization=TebVisualizationPtr(), const ViaPointContainer via_points=NULL)
	Initialize the HomotopyClassPlanner.
virtual bool	isHorizonReductionAppropriate (const std::vector< geometry_msgs::PoseStamped > &initial_plan) const
	Check if the planner suggests a shorter horizon (e.g. to resolve problems)
bool	isInitialized () const
	Returns true if the planner is initialized.
const ObstContainer *	obstacles () const
	Access current obstacle container (read-only)
virtual	~HomotopyClassPlanner ()
	Destruct the HomotopyClassPlanner.
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.
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).
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.
virtual bool	getVelocityCommand (double &vx, double &vy, double &omega) const
	Get the velocity command from a previously optimized plan to control the robot at the current sampling interval.
TebOptimalPlannerPtr	bestTeb () const
	Access current best trajectory candidate (that relates to the "best" homotopy class).
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)
	Check whether the planned trajectory is feasible or not.
Visualization
void	setVisualization (TebVisualizationPtr visualization)
	Register a TebVisualization class to enable visiualization routines (e.g. publish the local plan and pose sequence)
virtual void	visualize ()
	Publish the local plan, pose sequence and additional information via ros topics (e.g. subscribe with rviz).
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)
	Explore paths in new equivalence classes (e.g. homotopy classes) and initialize TEBs from them.
void	deleteTebDetours (double threshold=0.0)
	Check all available trajectories (TEBs) for detours and delete found ones.
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)
	Add a new Teb to the internal trajectory container and initialize it using a generic 2D reference path.
TebOptimalPlannerPtr	addAndInitNewTeb (const PoseSE2 &start, const PoseSE2 &goal, const geometry_msgs::Twist *start_velocity)
	Add a new Teb to the internal trajectory container and initialize it with a simple straight line between a given start and goal.
TebOptimalPlannerPtr	addAndInitNewTeb (const std::vector< geometry_msgs::PoseStamped > &initial_plan, const geometry_msgs::Twist *start_velocity)
	Add a new Teb to the internal trajectory container and initialize it using a PoseStamped container.
void	updateAllTEBs (const PoseSE2 start, const PoseSE2 goal, const geometry_msgs::Twist *start_velocity)
	Update TEBs with new pose, goal and current velocity.
void	optimizeAllTEBs (int iter_innerloop, int iter_outerloop)
	Optimize all available trajectories by invoking the optimizer on each one.
TebOptimalPlannerPtr	getInitialPlanTEB ()
	Returns a shared pointer to the TEB related to the initial plan.
TebOptimalPlannerPtr	selectBestTeb ()
	In case of multiple, internally stored, alternative trajectories, select the best one according to their cost values.
Static Public Member Functions
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)
Protected Member Functions
Explore new paths and keep only a single one for each homotopy class
void	createGraph (const PoseSE2 &start, const PoseSE2 &goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::Twist *start_velocity)
	Create a graph containing points in the global frame that can be used to explore new possible paths between start and goal.
void	createProbRoadmapGraph (const PoseSE2 &start, const PoseSE2 &goal, double dist_to_obst, int no_samples, double obstacle_heading_threshold, const geometry_msgs::Twist *start_velocity)
	Create a graph and sample points in the global frame that can be used to explore new possible paths between start and goal.
bool	hasEquivalenceClass (const EquivalenceClassPtr &eq_class) const
	Check if a h-signature exists already.
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.
void	renewAndAnalyzeOldTebs (bool delete_detours)
	Renew all found h-signatures for the new planning step based on existing TEBs. Optionally detours can be discarded.
void	updateReferenceTrajectoryViaPoints (bool all_trajectories)
	Associate trajectories with via-points.
void	DepthFirst (HcGraph &g, std::vector< HcGraphVertexType > &visited, const HcGraphVertexType &goal, double start_orientation, double goal_orientation, const geometry_msgs::Twist *start_velocity)
	Depth First Search implementation to find all paths between the start and the specified goal vertex.
Protected Attributes
TebOptimalPlannerPtr	best_teb_
	Store the current best teb.
const TebConfig *	cfg_
	Config class that stores and manages all related parameters.
EquivalenceClassContainer	equivalence_classes_
	Store all known quivalence classes (e.g. h-signatures) to allow checking for duplicates after finding and adding new ones.
HcGraph	graph_
	Store the graph that is utilized to find alternative homotopy classes.
const std::vector < geometry_msgs::PoseStamped > *	initial_plan_
	Store the initial plan if available for a better trajectory initialization.
EquivalenceClassPtr	initial_plan_eq_class_
	Store the equivalence class of the initial plan.
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_.)
bool	initialized_
	Keeps track about the correct initialization of this class.
ObstContainer *	obstacles_
	Store obstacles that are relevant for planning.
boost::random::mt19937	rnd_generator_
	Random number generator used by createProbRoadmapGraph to sample graph keypoints.
RobotFootprintModelPtr	robot_model_
	Robot model shared instance.
TebOptPlannerContainer	tebs_
	Container that stores multiple local teb planners (for alternative equivalence classes) and their corresponding costs.
const ViaPointContainer *	via_points_
	Store the current list of via-points.
TebVisualizationPtr	visualization_
	Instance of the visualization class (local/global plan, obstacles, ...)

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 131 of file homotopy_class_planner.h.

Constructor & Destructor Documentation

teb_local_planner::HomotopyClassPlanner::HomotopyClassPlanner ( )

Default constructor.

Definition at line 69 of file homotopy_class_planner.cpp.

teb_local_planner::HomotopyClassPlanner::HomotopyClassPlanner	(	const TebConfig &	cfg,
		ObstContainer *	obstacles = `NULL`,
		RobotFootprintModelPtr	robot_model = `boost::make_shared<PointRobotFootprint>()`,
		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)
robot_model	Shared pointer to the robot shape model used for optimization (optional)
visualization	Shared pointer to the TebVisualization class (optional)
via_points	Container storing via-points (optional)

Definition at line 73 of file homotopy_class_planner.cpp.

teb_local_planner::HomotopyClassPlanner::~HomotopyClassPlanner ( ) [virtual]

Destruct the HomotopyClassPlanner.

Definition at line 79 of file homotopy_class_planner.cpp.

Member Function Documentation

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
	)

Add a new Teb to the internal trajectory container and 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)

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 56 of file homotopy_class_planner.hpp.

TebOptimalPlannerPtr teb_local_planner::HomotopyClassPlanner::addAndInitNewTeb	(	const PoseSE2 &	start,
		const PoseSE2 &	goal,
		const geometry_msgs::Twist *	start_velocity
	)

Add a new Teb to the internal trajectory container and 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)

Returns:: Shared pointer to the newly created teb optimal planner

Definition at line 667 of file homotopy_class_planner.cpp.

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

Add a new Teb to the internal trajectory container and initialize it using a PoseStamped container.

Parameters:

initial_plan	container of poses (start and goal orientation should be valid!)
start_velocity	start velocity (optional)

Returns:: Shared pointer to the newly created teb optimal planner

Definition at line 678 of file homotopy_class_planner.cpp.

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

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, e.g. in deleteTebDetours().

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

Definition at line 512 of file homotopy_class_planner.cpp.

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 230 of file homotopy_class_planner.h.

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 909 of file homotopy_class_planner.cpp.

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`
	)

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 46 of file homotopy_class_planner.hpp.

void teb_local_planner::HomotopyClassPlanner::clearGraph ( ) [inline]

Clear any existing graph of the homotopy class search.

Definition at line 455 of file homotopy_class_planner.h.

void teb_local_planner::HomotopyClassPlanner::clearPlanner ( ) [inline, virtual]

Reset the planner.

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

Implements teb_local_planner::PlannerInterface.

Definition at line 374 of file homotopy_class_planner.h.

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 945 of file homotopy_class_planner.cpp.

const TebConfig* teb_local_planner::HomotopyClassPlanner::config ( ) const [inline]

Access config (read-only)

Returns:: const pointer to the config instance

Definition at line 439 of file homotopy_class_planner.h.

void teb_local_planner::HomotopyClassPlanner::createGraph	(	const PoseSE2 &	start,
		const PoseSE2 &	goal,
		double	dist_to_obst,
		double	obstacle_heading_threshold,
		const geometry_msgs::Twist *	start_velocity
	)		`[protected]`

Create a graph containing points in the global frame that can be used to explore new possible paths between start and goal.

This version of the graph creation places a keypoint on the left and right side of each obstacle w.r.t to the goal heading.
All feasible paths between start and goal point are extracted using a Depth First Search afterwards.
This version works very well for small point obstacles. For more complex obstacles call the createProbRoadmapGraph() method that samples keypoints in a predefined area and hopefully finds all relevant alternative paths.

See also:: createProbRoadmapGraph

Parameters:

start	Start pose from wich to start on (e.g. the current robot pose).
goal	Goal pose to find paths to (e.g. the 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).
obstacle_heading_threshold	Value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account [0,1]
start_velocity	start velocity (optional)

Definition at line 201 of file homotopy_class_planner.cpp.

void teb_local_planner::HomotopyClassPlanner::createProbRoadmapGraph	(	const PoseSE2 &	start,
		const PoseSE2 &	goal,
		double	dist_to_obst,
		int	no_samples,
		double	obstacle_heading_threshold,
		const geometry_msgs::Twist *	start_velocity
	)		`[protected]`

Create a graph and sample points in the global frame that can be used to explore new possible paths between start and goal.

This version of the graph samples keypoints in a predefined area (config) in the current frame between start and goal.
Afterwards all feasible paths between start and goal point are extracted using a Depth First Search.
Use the sampling method for complex, non-point or huge obstacles.
You may call createGraph() instead.

See also:: createGraph

Parameters:

start	Start pose from wich to start on (e.g. the current robot pose).
goal	Goal pose to find paths to (e.g. the 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).
no_samples	number of random samples
obstacle_heading_threshold	Value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account [0,1]
start_velocity	start velocity (optional)

Find all paths between start and goal!

Definition at line 326 of file homotopy_class_planner.cpp.

void teb_local_planner::HomotopyClassPlanner::deleteTebDetours ( double threshold = 0.0 )

Check all available trajectories (TEBs) for detours and delete found ones.

See also:: TimedElasticBand::detectDetoursBackwards

Parameters:

threshold Threshold paramter for allowed orientation changes (below 0 -> greater than 90 deg)

Definition at line 734 of file homotopy_class_planner.cpp.

void teb_local_planner::HomotopyClassPlanner::DepthFirst	(	HcGraph &	g,
		std::vector< HcGraphVertexType > &	visited,
		const HcGraphVertexType &	goal,
		double	start_orientation,
		double	goal_orientation,
		const geometry_msgs::Twist *	start_velocity
	)		`[protected]`

Depth First Search implementation to find all paths between the start and the specified goal vertex.

Complete paths are stored to the internal path container.

See also:: http://www.technical-recipes.com/2011/a-recursive-algorithm-to-find-all-paths-between-two-given-nodes/

Parameters:

g	Graph on which the depth first should be performed
visited	A container that stores visited vertices (pass an empty container, it will be filled inside during recursion).
goal	Desired goal vertex
start_orientation	Orientation of the first trajectory pose, required to initialize the trajectory/TEB
goal_orientation	Orientation of the goal trajectory pose, required to initialize the trajectory/TEB
start_velocity	start velocity (optional)

Examine adjacent nodes

Recursion for all adjacent vertices

Definition at line 448 of file homotopy_class_planner.cpp.

void teb_local_planner::HomotopyClassPlanner::exploreEquivalenceClassesAndInitTebs	(	const PoseSE2 &	start,
		const PoseSE2 &	goal,
		double	dist_to_obst,
		const geometry_msgs::Twist *	start_vel
	)

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).
@param	start_velocity start velocity (optional)

Definition at line 645 of file homotopy_class_planner.cpp.

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 780 of file homotopy_class_planner.cpp.

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 407 of file homotopy_class_planner.h.

bool teb_local_planner::HomotopyClassPlanner::getVelocityCommand	(	double &	vx,
		double &	vy,
		double &	omega
	)		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]

Returns:: true if command is valid, false otherwise

Implements teb_local_planner::PlannerInterface.

Definition at line 149 of file homotopy_class_planner.cpp.

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 501 of file homotopy_class_planner.cpp.

void teb_local_planner::HomotopyClassPlanner::initialize	(	const TebConfig &	cfg,
		ObstContainer *	obstacles = `NULL`,
		RobotFootprintModelPtr	robot_model = `boost::make_shared<PointRobotFootprint>()`,
		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)
robot_model	Shared pointer to the robot shape model used for optimization (optional)
visualization	Shared pointer to the TebVisualization class (optional)
via_points	Container storing via-points (optional)

Definition at line 83 of file homotopy_class_planner.cpp.

bool teb_local_planner::HomotopyClassPlanner::isHorizonReductionAppropriate ( const std::vector< geometry_msgs::PoseStamped > & initial_plan ) const [virtual]

Check if the planner suggests a shorter horizon (e.g. to resolve problems)

This method is intendend to be called after determining that a trajectory provided by the planner is infeasible. In some cases a reduction of the horizon length might resolve problems. E.g. if a planned trajectory cut corners. Implemented cases: see TebOptimalPlanner

Parameters:

initial_plan The intial and transformed plan (part of the local map and pruned up to the robot position)

Returns:: true, if the planner suggests a shorter horizon, false otherwise.

Reimplemented from teb_local_planner::PlannerInterface.

Definition at line 936 of file homotopy_class_planner.cpp.

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

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 425 of file homotopy_class_planner.h.

bool teb_local_planner::HomotopyClassPlanner::isInitialized ( ) const [inline]

Returns true if the planner is initialized.

Definition at line 450 of file homotopy_class_planner.h.

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`
	)		`[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 926 of file homotopy_class_planner.cpp.

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 445 of file homotopy_class_planner.h.

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 710 of file homotopy_class_planner.cpp.

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 103 of file homotopy_class_planner.cpp.

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 119 of file homotopy_class_planner.cpp.

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 127 of file homotopy_class_planner.cpp.

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 by utilizing deleteTebDetours().

Definition at line 529 of file homotopy_class_planner.cpp.

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 824 of file homotopy_class_planner.cpp.

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 96 of file homotopy_class_planner.cpp.

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 689 of file homotopy_class_planner.cpp.

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 612 of file homotopy_class_planner.cpp.

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 166 of file homotopy_class_planner.cpp.

Member Data Documentation

TebOptimalPlannerPtr teb_local_planner::HomotopyClassPlanner::best_teb_ [protected]

Store the current best teb.

Definition at line 565 of file homotopy_class_planner.h.

const TebConfig* teb_local_planner::HomotopyClassPlanner::cfg_ [protected]

Config class that stores and manages all related parameters.

Definition at line 559 of file homotopy_class_planner.h.

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 577 of file homotopy_class_planner.h.

HcGraph teb_local_planner::HomotopyClassPlanner::graph_ [protected]

Store the graph that is utilized to find alternative homotopy classes.

Definition at line 574 of file homotopy_class_planner.h.

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 568 of file homotopy_class_planner.h.

EquivalenceClassPtr teb_local_planner::HomotopyClassPlanner::initial_plan_eq_class_ [protected]

Store the equivalence class of the initial plan.

Definition at line 569 of file homotopy_class_planner.h.

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 570 of file homotopy_class_planner.h.

bool teb_local_planner::HomotopyClassPlanner::initialized_ [protected]

Keeps track about the correct initialization of this class.

Definition at line 582 of file homotopy_class_planner.h.

ObstContainer* teb_local_planner::HomotopyClassPlanner::obstacles_ [protected]

Store obstacles that are relevant for planning.

Definition at line 560 of file homotopy_class_planner.h.

boost::random::mt19937 teb_local_planner::HomotopyClassPlanner::rnd_generator_ [protected]

Random number generator used by createProbRoadmapGraph to sample graph keypoints.

Definition at line 580 of file homotopy_class_planner.h.

RobotFootprintModelPtr teb_local_planner::HomotopyClassPlanner::robot_model_ [protected]

Robot model shared instance.