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
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 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)
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 Eigen::Vector2d &start_vel, bool free_goal_vel=false)
	Plan a trajectory between a given start and goal pose.
virtual bool	getVelocityCommand (double &v, 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	exploreHomotopyClassesAndInitTebs (const PoseSE2 &start, const PoseSE2 &goal, double dist_to_obst, boost::optional< const Eigen::Vector2d & > start_vel)
	Explore paths in new 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 >
void	addAndInitNewTeb (BidirIter path_start, BidirIter path_end, Fun fun_position, double start_orientation, double goal_orientation, boost::optional< const Eigen::Vector2d & > start_velocity)
	Add a new Teb to the internal trajectory container and initialize it using a generic 2D reference path.
void	addAndInitNewTeb (const PoseSE2 &start, const PoseSE2 &goal, boost::optional< const Eigen::Vector2d & > 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.
void	addAndInitNewTeb (const std::vector< geometry_msgs::PoseStamped > &initial_plan, boost::optional< const Eigen::Vector2d & > start_velocity)
	Add a new Teb to the internal trajectory container and initialize it using a PoseStamped container.
void	updateAllTEBs (boost::optional< const PoseSE2 & > start, boost::optional< const PoseSE2 & > goal, boost::optional< const Eigen::Vector2d & > start_velocity)
	Update TEBs with new pose, goal and current velocity.
void	optimizeAllTEBs (unsigned int iter_innerloop, unsigned int iter_outerloop)
	Optimize all available trajectories by invoking the optimizer on each one.
TebOptimalPlannerPtr	selectBestTeb ()
	In case of multiple, internally stored, alternative trajectories, select the best one according to their cost values.
Static Public Member Functions
template<typename BidirIter , typename Fun >
static std::complex< long double >	calculateHSignature (BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer *obstacles=NULL, double prescaler=1)
	Calculate the H-Signature of a path.
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, boost::optional< const Eigen::Vector2d & > 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, boost::optional< const Eigen::Vector2d & > 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	hasHSignature (const std::complex< long double > &H) const
	Check if a h-signature exists already.
bool	addHSignatureIfNew (const std::complex< long double > &H, bool lock=false)
	Internal helper function that adds a h-signature to the list of known h-signatures 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, boost::optional< const Eigen::Vector2d & > start_velocity)
	Depth First Search implementation to find all paths between the start and the specified goal vertex.
void	clearGraph ()
	Clear any existing graph of the homotopy class search.
int	bestTebIdx () const
	find the index of the currently best TEB in the container
Protected Attributes
TebOptimalPlannerPtr	best_teb_
	Store the current best teb.
const TebConfig *	cfg_
	Config class that stores and manages all related parameters.
HcGraph	graph_
	Store the graph that is utilized to find alternative homotopy classes.
std::vector< std::pair < std::complex< long double > , bool > >	h_signatures_
	Store all known h-signatures to allow checking for duplicates after finding and adding new ones.
const std::vector < geometry_msgs::PoseStamped > *	initial_plan_
	Store the initial plan if available for a better trajectory initialization.
std::complex< long double >	initial_plan_h_sig_
	Store the h_signature of the initial plan.
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 homotopy 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)

Homotopy classes are explored using the help 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 130 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 74 of file homotopy_class_planner.cpp.

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

Destruct the HomotopyClassPlanner.

Definition at line 80 of file homotopy_class_planner.cpp.

Member Function Documentation

template<typename BidirIter , typename Fun >

void teb_local_planner::HomotopyClassPlanner::addAndInitNewTeb	(	BidirIter	path_start,
		BidirIter	path_end,
		Fun	fun_position,
		double	start_orientation,
		double	goal_orientation,
		boost::optional< const Eigen::Vector2d & >	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

Definition at line 138 of file homotopy_class_planner.hpp.

void teb_local_planner::HomotopyClassPlanner::addAndInitNewTeb	(	const PoseSE2 &	start,
		const PoseSE2 &	goal,
		boost::optional< const Eigen::Vector2d & >	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)

Definition at line 667 of file homotopy_class_planner.cpp.

void teb_local_planner::HomotopyClassPlanner::addAndInitNewTeb	(	const std::vector< geometry_msgs::PoseStamped > &	initial_plan,
		boost::optional< const Eigen::Vector2d & >	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)

Definition at line 676 of file homotopy_class_planner.cpp.

bool teb_local_planner::HomotopyClassPlanner::addHSignatureIfNew	(	const std::complex< long double > &	H,
		bool	lock = `false`
	)		`[protected]`

Internal helper function that adds a h-signature to the list of known h-signatures only if it is unique.

Parameters:

H	h-signature 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 511 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 229 of file homotopy_class_planner.h.

int teb_local_planner::HomotopyClassPlanner::bestTebIdx ( ) const [protected]

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

template<typename BidirIter , typename Fun >

std::complex< long double > teb_local_planner::HomotopyClassPlanner::calculateHSignature	(	BidirIter	path_start,
		BidirIter	path_end,
		Fun	fun_cplx_point,
		const ObstContainer *	obstacles = `NULL`,
		double	prescaler = `1`
	)		`[static]`

Calculate the H-Signature of a path.

The H-Signature depends on the obstacle configuration and can be utilized to check whether two trajectores rely to the same homotopy class. Refer to:

S. Bhattacharya et al.: Search-based Path Planning with Homotopy Class Constraints, AAAI, 2010

The implemented function accepts generic path descriptions that are restricted to the following structure:
The path is composed of points T and is represented by a std::vector< T > or similar type (std::list, std::deque, ...).
Provide a unary function with the following signature std::complex< long double > (const T& point_type) that returns a complex value for the position (Re(*)=x, Im(*)=y).

T could also be a pointer type, if the passed function also accepts a const T* point_Type.

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.
prescaler	Change this value only if you observe problems with an huge amount of obstacles: interval (0,1]

Template Parameters:

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

Returns:: complex H-Signature value

Definition at line 46 of file homotopy_class_planner.hpp.

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

Clear any existing graph of the homotopy class search.

Definition at line 529 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 364 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 831 of file homotopy_class_planner.cpp.

void teb_local_planner::HomotopyClassPlanner::createGraph	(	const PoseSE2 &	start,
		const PoseSE2 &	goal,
		double	dist_to_obst,
		double	obstacle_heading_threshold,
		boost::optional< const Eigen::Vector2d & >	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 202 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,
		boost::optional< const Eigen::Vector2d & >	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 327 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 730 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,
		boost::optional< const Eigen::Vector2d & >	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 446 of file homotopy_class_planner.cpp.

void teb_local_planner::HomotopyClassPlanner::exploreHomotopyClassesAndInitTebs	(	const PoseSE2 &	start,
		const PoseSE2 &	goal,
		double	dist_to_obst,
		boost::optional< const Eigen::Vector2d & >	start_vel
	)

Explore paths in new 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 homotopy class.
Each time a new homotopy class is explored (by means of no previous trajectory/TEB remain in that homotopy 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 650 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 408 of file homotopy_class_planner.h.

bool teb_local_planner::HomotopyClassPlanner::getVelocityCommand	(	double &	v,
		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]	v	translational velocity [m/s]
[out]	omega	rotational velocity [rad/s]

Returns:: true if command is valid, false otherwise

Implements teb_local_planner::PlannerInterface.

Definition at line 151 of file homotopy_class_planner.cpp.

bool teb_local_planner::HomotopyClassPlanner::hasHSignature ( const std::complex< long double > & H ) const [protected]

Check if a h-signature exists already.

Parameters:

H	h-signature that should be tested

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

Definition at line 500 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 84 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 822 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 426 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 812 of file homotopy_class_planner.cpp.

void teb_local_planner::HomotopyClassPlanner::optimizeAllTEBs	(	unsigned int	iter_innerloop,
		unsigned 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 706 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 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 104 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 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 120 of file homotopy_class_planner.cpp.

bool teb_local_planner::HomotopyClassPlanner::plan	(	const PoseSE2 &	start,
		const PoseSE2 &	goal,
		const Eigen::Vector2d &	start_vel,
		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 (2D vector 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 129 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 534 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 765 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 97 of file homotopy_class_planner.cpp.

void teb_local_planner::HomotopyClassPlanner::updateAllTEBs	(	boost::optional< const PoseSE2 & >	start,
		boost::optional< const PoseSE2 & >	goal,
		boost::optional< const Eigen::Vector2d & >	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 685 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 617 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 167 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 548 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 544 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 556 of file homotopy_class_planner.h.

std::vector< std::pair<std::complex<long double>, bool> > teb_local_planner::HomotopyClassPlanner::h_signatures_ [protected]

Store all known h-signatures to allow checking for duplicates after finding and adding new ones.

Definition at line 558 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 551 of file homotopy_class_planner.h.

std::complex<long double> teb_local_planner::HomotopyClassPlanner::initial_plan_h_sig_ [protected]

Store the h_signature of the initial plan.

Definition at line 552 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 563 of file homotopy_class_planner.h.

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

Store obstacles that are relevant for planning.

Definition at line 542 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 561 of file homotopy_class_planner.h.

RobotFootprintModelPtr teb_local_planner::HomotopyClassPlanner::robot_model_ [protected]

Robot model shared instance.

Definition at line 549 of file homotopy_class_planner.h.

TebOptPlannerContainer teb_local_planner::HomotopyClassPlanner::tebs_ [protected]

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

Definition at line 554 of file homotopy_class_planner.h.

const ViaPointContainer* teb_local_planner::HomotopyClassPlanner::via_points_ [protected]

Store the current list of via-points.

Definition at line 543 of file homotopy_class_planner.h.

TebVisualizationPtr teb_local_planner::HomotopyClassPlanner::visualization_ [protected]

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

Definition at line 547 of file homotopy_class_planner.h.

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

Public Member Functions

Static Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation