Public Types | Public Member Functions | Static Public Member Functions | Protected Attributes | List of all members
teb_local_planner::HomotopyClassPlanner Class Reference

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:
Inheritance graph
[legend]

Public Types

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

Public Member Functions

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...
 
int bestTebIdx () const
 find the index of the currently best TEB in the container 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...
 
void clearGraph ()
 Clear any existing graph of the homotopy class search. More...
 
virtual void clearPlanner ()
 Reset the planner. 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)
 
const TebConfigconfig () const
 Access config (read-only) More...
 
const EquivalenceClassContainergetEquivalenceClassRef () const
 Return the current set of equivalence erelations (read-only) More...
 
const TebOptPlannerContainergetTrajectoryContainer () const
 Read-only access to the internal trajectory container. More...
 
 HomotopyClassPlanner ()
 Default constructor. More...
 
 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. More...
 
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. More...
 
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) More...
 
bool isInitialized () const
 Returns true if the planner is initialized. More...
 
const ObstContainerobstacles () const
 Access current obstacle container (read-only) More...
 
virtual void setPreferredTurningDir (RotType dir)
 Prefer a desired initial turning direction (by penalizing the opposing one) 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) 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)
 Check whether the planned trajectory is feasible or not. 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...
 
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. More...
 
void deleteTebDetours (double threshold=0.0)
 Check all available trajectories (TEBs) for detours and delete found ones. 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)
 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)
 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)
 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...
 
- Public Member Functions inherited from teb_local_planner::PlannerInterface
virtual void computeCurrentCost (std::vector< double > &cost, double obst_cost_scale=1.0, bool alternative_time_cost=false)
 
 PlannerInterface ()
 Default constructor. More...
 
virtual ~PlannerInterface ()
 Virtual destructor. More...
 

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

Protected Member Functions

Explore new paths and keep only a single one for each homotopy class
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...
 

Protected Attributes

TebOptimalPlannerPtr best_teb_
 Store the current best teb. More...
 
const TebConfigcfg_
 Config class that stores and manages all related parameters. 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< GraphSearchInterfacegraph_search_
 
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...
 
bool initialized_
 Keeps track about the correct initialization of this class. More...
 
ros::Time last_eq_class_switching_time_
 Store the time at which the equivalence class changed recently. More...
 
ObstContainerobstacles_
 Store obstacles that are relevant for planning. More...
 
RobotFootprintModelPtr robot_model_
 Robot model shared instance. More...
 
TebOptPlannerContainer tebs_
 Container that stores multiple local teb planners (for alternative equivalence classes) and their corresponding costs. More...
 
const ViaPointContainervia_points_
 Store the current list of via-points. More...
 
TebVisualizationPtr visualization_
 Instance of the visualization class (local/global plan, obstacles, ...) 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:

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

Member Typedef Documentation

Definition at line 111 of file homotopy_class_planner.h.

Constructor & Destructor Documentation

teb_local_planner::HomotopyClassPlanner::HomotopyClassPlanner ( )

Default constructor.

Definition at line 46 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
cfgConst reference to the TebConfig class for internal parameters
obstaclesContainer storing all relevant obstacles (see Obstacle)
robot_modelShared pointer to the robot shape model used for optimization (optional)
visualizationShared pointer to the TebVisualization class (optional)
via_pointsContainer storing via-points (optional)

Definition at line 50 of file homotopy_class_planner.cpp.

teb_local_planner::HomotopyClassPlanner::~HomotopyClassPlanner ( )
virtual

Destruct the HomotopyClassPlanner.

Definition at line 56 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, 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_startstart iterator of a generic 2d path
path_endend iterator of a generic 2d path
fun_positionunary function that returns the Eigen::Vector2d object
start_orientationOrientation of the first pose of the trajectory (optional, otherwise use goal heading)
goal_orientationOrientation of the last pose of the trajectory (optional, otherwise use goal heading)
start_velocitystart velocity (optional)
Template Parameters
BidirIterBidirectional iterator type
Fununyary function that transforms the dereferenced iterator into an Eigen::Vector2d
Returns
Shared pointer to the newly created teb optimal planner

Definition at line 66 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, if this teb constitutes a new equivalence class. Initialize it with a simple straight line between a given start and goal.

Parameters
startstart pose
goalgoal pose
start_velocitystart velocity (optional)
Returns
Shared pointer to the newly created teb optimal planner

Definition at line 350 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 , if this teb constitutes a new equivalence class. Initialize it using a PoseStamped container.

Parameters
initial_plancontainer of poses (start and goal orientation should be valid!)
start_velocitystart velocity (optional)
Returns
Shared pointer to the newly created teb optimal planner

Definition at line 373 of file homotopy_class_planner.cpp.

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_classequivalence class that should be tested
lockif 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 192 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 208 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 637 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,
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_startIterator to the first element in the path
path_endIterator to the last element in the path
obstaclesobstacle container
fun_cplx_pointfunction accepting the dereference iterator type and that returns the position as complex number.
Template Parameters
BidirIterBidirectional iterator type
Funfunction of the form std::complex< long double > (const T& point_type)
Returns
pointer to the equivalence class base type

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

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 352 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]costcurrent cost value for each trajectory [for a planner with just a single trajectory: size=1, vector will not be cleared]
obst_cost_scaleSpecify extra scaling for obstacle costs
viapoint_cost_scaleSpecify extra scaling for via points.
alternative_time_costReplace the cost for the time optimal objective by the actual (weighted) transition time

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

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
thresholdThreshold paramter for allowed orientation changes (below 0 -> greater than 90 deg)

Definition at line 441 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
startCurrent start pose (e.g. pose of the robot)
goalGoal pose (e.g. robot's goal)
dist_to_obstAllowed distance to obstacles: if not satisfying, the path will be rejected (note, this is not the distance used for optimization).

Definition at line 329 of file homotopy_class_planner.cpp.

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

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 487 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 397 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]vxtranslational velocity [m/s]
[out]vystrafing velocity which can be nonzero for holonomic robots [m/s]
[out]omegarotational velocity [rad/s]
Returns
true if command is valid, false otherwise

Implements teb_local_planner::PlannerInterface.

Definition at line 130 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_classequivalence class that should be tested
Returns
true if the h-signature is found, false otherwise

Definition at line 181 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
cfgConst reference to the TebConfig class for internal parameters
obstaclesContainer storing all relevant obstacles (see Obstacle)
robot_modelShared pointer to the robot shape model used for optimization (optional)
visualizationShared pointer to the TebVisualization class (optional)
via_pointsContainer storing via-points (optional)

Definition at line 60 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_planThe 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 673 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 
)
inlinestatic

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

Parameters
h1first h-signature
h2second h-signature
Returns
true if both h-signatures are similar, false otherwise.

Definition at line 415 of file homotopy_class_planner.h.

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

Returns true if the planner is initialized.

Definition at line 440 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_modelPointer to the costmap model
footprint_specThe specification of the footprint of the robot in world coordinates
inscribed_radiusThe radius of the inscribed circle of the robot
circumscribed_radiusThe radius of the circumscribed circle of the robot
look_ahead_idxNumber 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 654 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 435 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_innerloopNumber of inner iterations (see TebOptimalPlanner::optimizeTEB())
iter_outerloopNumber of outer iterations (see TebOptimalPlanner::optimizeTEB())

Definition at line 417 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_planvector of geometry_msgs::PoseStamped (must be valid until clearPlanner() is called!)
start_velCurrent start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)
free_goal_velif 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 86 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
starttf::Pose containing the start pose of the trajectory
goaltf::Pose containing the goal pose of the trajectory
start_velCurrent start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)
free_goal_velif 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 100 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
startPoseSE2 containing the start pose of the trajectory
goalPoseSE2 containing the goal pose of the trajectory
start_velInitial velocity at the start pose (twist message containing the translational and angular velocity).
free_goal_velif 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 108 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_detoursif this param is true, all existing TEBs are cleared from detour-candidates by utilizing deleteTebDetours().

Definition at line 212 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 531 of file homotopy_class_planner.cpp.

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
dirThis 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 664 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
visualizationshared pointer to a TebVisualization instance
See also
visualizeTeb

Definition at line 79 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
startNew start pose (optional)
goalNew goal pose (optional)
start_velocitystart velocity (optional)

Definition at line 396 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_trajectoriessee method description

Definition at line 296 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 147 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 514 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 508 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 523 of file homotopy_class_planner.h.

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

Definition at line 526 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 517 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 518 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 519 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 530 of file homotopy_class_planner.h.

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

ObstContainer* teb_local_planner::HomotopyClassPlanner::obstacles_
protected

Store obstacles that are relevant for planning.

Definition at line 509 of file homotopy_class_planner.h.

RobotFootprintModelPtr teb_local_planner::HomotopyClassPlanner::robot_model_
protected

Robot model shared instance.

Definition at line 515 of file homotopy_class_planner.h.

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 521 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 510 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 513 of file homotopy_class_planner.h.


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


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Jun 5 2019 19:25:10