Public Member Functions | Static Public Member Functions | Protected Attributes
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]

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 TebConfigconfig () const
 Access config (read-only)
const TebOptPlannerContainergetTrajectoryContainer () 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 ObstContainerobstacles () 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 TebConfigcfg_
 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.
ObstContainerobstacles_
 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 ViaPointContainervia_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:

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

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

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_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 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:
startstart pose
goalgoal pose
start_velocitystart 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_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 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_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 512 of file homotopy_class_planner.cpp.

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.

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

Clear any existing graph of the homotopy class search.

Definition at line 455 of file homotopy_class_planner.h.

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

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:
startStart pose from wich to start on (e.g. the current robot pose).
goalGoal pose to find paths to (e.g. the 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).
obstacle_heading_thresholdValue of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account [0,1]
start_velocitystart 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:
startStart pose from wich to start on (e.g. the current robot pose).
goalGoal pose to find paths to (e.g. the 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).
no_samplesnumber of random samples
obstacle_heading_thresholdValue of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account [0,1]
start_velocitystart velocity (optional)

Find all paths between start and goal!

Definition at line 326 of file homotopy_class_planner.cpp.

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 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:
gGraph on which the depth first should be performed
visitedA container that stores visited vertices (pass an empty container, it will be filled inside during recursion).
goalDesired goal vertex
start_orientationOrientation of the first trajectory pose, required to initialize the trajectory/TEB
goal_orientationOrientation of the goal trajectory pose, required to initialize the trajectory/TEB
start_velocitystart 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:
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).
@paramstart_velocity start velocity (optional)

Definition at line 645 of file homotopy_class_planner.cpp.

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.

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]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 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_classequivalence 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:
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 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_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 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:
h1first h-signature
h2second h-signature
Returns:
true if both h-signatures are similar, false otherwise.

Definition at line 425 of file homotopy_class_planner.h.

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

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_innerloopNumber of inner iterations (see TebOptimalPlanner::optimizeTEB())
iter_outerloopNumber 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_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 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:
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 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:
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 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_detoursif 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.

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.

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 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:
startNew start pose (optional)
goalNew goal pose (optional)
start_velocitystart velocity (optional)

Definition at line 689 of file homotopy_class_planner.cpp.

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

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

Store the current best teb.

Definition at line 565 of file homotopy_class_planner.h.

Config class that stores and manages all related parameters.

Definition at line 559 of file homotopy_class_planner.h.

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.

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.

Store the equivalence class of the initial plan.

Definition at line 569 of file homotopy_class_planner.h.

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.

Keeps track about the correct initialization of this class.

Definition at line 582 of file homotopy_class_planner.h.

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.

Robot model shared instance.

Definition at line 566 of file homotopy_class_planner.h.

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

Definition at line 572 of file homotopy_class_planner.h.

Store the current list of via-points.

Definition at line 561 of file homotopy_class_planner.h.

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

Definition at line 564 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 Sat Jun 8 2019 20:21:34