Public Member Functions | Private Member Functions | Private Attributes
eband_local_planner::EBandPlanner Class Reference

Implements the Elastic Band Method for SE2-Manifold (mobile Base) More...

#include <eband_local_planner.h>

List of all members.

Public Member Functions

bool addFrames (const std::vector< geometry_msgs::PoseStamped > &robot_pose, const AddAtPosition &add_frames_at)
 converts robot_pose to a bubble and tries to connect to the path
 EBandPlanner ()
 Default constructor.
 EBandPlanner (std::string name, costmap_2d::Costmap2DROS *costmap_ros)
 Constructs the elastic band object.
bool getBand (std::vector< Bubble > &elastic_band)
 This outputs the elastic_band.
bool getPlan (std::vector< geometry_msgs::PoseStamped > &global_plan)
 This transforms the refined band to a path again and outputs it.
void initialize (std::string name, costmap_2d::Costmap2DROS *costmap_ros)
 Initializes the elastic band class by accesing costmap and loading parameters.
bool optimizeBand ()
 cycles over the elastic band set before and optimizes it locally by minimizing an energy-function
bool optimizeBand (std::vector< Bubble > &band)
 cycles over the elastic band checks it for validity and optimizes it locally by minimizing an energy-function
void reconfigure (EBandPlannerConfig &config)
 Reconfigures the parameters of the planner.
bool setPlan (const std::vector< geometry_msgs::PoseStamped > &global_plan)
 Set plan which shall be optimized to elastic band planner.
void setVisualization (boost::shared_ptr< EBandVisualization > eband_visual)
 passes a reference to the eband visualization object which can be used to visualize the band optimization
 ~EBandPlanner ()
 Destructor.

Private Member Functions

bool applyForces (int bubble_num, std::vector< Bubble > &band, std::vector< geometry_msgs::WrenchStamped > forces)
 Applies forces to move bubbles and recalculates expansion of bubbles.
bool calcBubbleDifference (geometry_msgs::Pose start_center_pose, geometry_msgs::Pose end_center_pose, geometry_msgs::Twist &difference)
 Calculates the difference between the pose of the center of two bubbles and outputs it as Twist (pointing from first to second bubble) [depends kinematic].
bool calcBubbleDistance (geometry_msgs::Pose start_center_pose, geometry_msgs::Pose end_center_pose, double &distance)
 This calculates the distance between two bubbles [depends kinematics, shape].
bool calcExternalForces (int bubble_num, Bubble curr_bubble, geometry_msgs::WrenchStamped &forces)
 Calculates external forces for bubbles along the band [depends shape, environment].
bool calcInternalForces (int bubble_num, std::vector< Bubble > band, Bubble curr_bubble, geometry_msgs::WrenchStamped &forces)
 Calculates internal forces for bubbles along the band [depends kinematic].
bool calcObstacleKinematicDistance (geometry_msgs::Pose center_pose, double &distance)
 Calculates the distance between the center of a bubble and the closest obstacle [depends kinematics, shape, environment].
bool checkOverlap (Bubble bubble1, Bubble bubble2)
 this checks whether two bubbles overlap
bool convertBandToPlan (std::vector< geometry_msgs::PoseStamped > &plan, std::vector< Bubble > band)
 This converts a band from a sequence of bubbles into a plan from, a sequence of stamped poses.
bool convertPlanToBand (std::vector< geometry_msgs::PoseStamped > plan, std::vector< Bubble > &band)
 This converts a plan from a sequence of stamped poses into a band, a sequence of bubbles. Therefore it calculates distances to the surrounding obstacles and derives the expansion or radius of the bubble.
bool fillGap (std::vector< Bubble > &band, std::vector< Bubble >::iterator &start_iter, std::vector< Bubble >::iterator &end_iter)
 recursively fills gaps between two bubbles (if possible)
bool getForcesAt (int bubble_num, std::vector< Bubble > band, Bubble curr_bubble, geometry_msgs::WrenchStamped &forces)
 Calculates all forces for a certain bubble at a specific position in the band [depends kinematic].
bool interpolateBubbles (geometry_msgs::PoseStamped start_center, geometry_msgs::PoseStamped end_center, geometry_msgs::PoseStamped &interpolated_center)
 interpolates between two bubbles by calculating the pose for the center of a bubble in the middle of the path connecting the bubbles [depends kinematics]
bool modifyBandArtificialForce (std::vector< Bubble > &band)
 calculates internal and external forces and applies changes to the band
bool moveApproximateEquilibrium (const int &bubble_num, const std::vector< Bubble > &band, Bubble &curr_bubble, const geometry_msgs::WrenchStamped &curr_bubble_force, geometry_msgs::Twist &curr_step_width, const int &curr_recursion_depth)
 Checks for zero-crossings and large changes in force-vector after moving of bubble - if detected it tries to approximate the equilibrium point.
bool refineBand (std::vector< Bubble > &band)
 Cycles over an band and searches for gaps and redundant bubbles. Tries to close gaps and remove redundant bubbles.
bool removeAndFill (std::vector< Bubble > &band, std::vector< Bubble >::iterator &start_iter, std::vector< Bubble >::iterator &end_iter)
 recursively checks an intervall of a band whether gaps need to be filled or bubbles may be removed and fills or removes if possible. This exploits the sequential structure of the band. Therefore it can not detect redundant cycles which are closed over several on its own not redundant bubbles.
bool suppressTangentialForces (int bubble_num, std::vector< Bubble > band, geometry_msgs::WrenchStamped &forces)
 Calculates tangential portion of forces.

Private Attributes

std::vector< double > acc_lim_
 acceleration limits for translational and rotational motion
costmap_2d::Costmap2Dcostmap_
costmap_2d::Costmap2DROScostmap_ros_
 pointer to costmap
double costmap_weight_
 lower bound for absolute value of force below which it is treated as insignificant (no recursive approximation)
boost::shared_ptr
< EBandVisualization
eband_visual_
std::vector< Bubbleelastic_band_
double equilibrium_relative_overshoot_
 maximum depth for recursive approximation to constrain computational burden
double external_force_gain_
 gain for external forces ("Penalty on low distance to abstacles")
std::vector< geometry_msgs::Pointfootprint_spec_
std::vector
< geometry_msgs::PoseStamped > 
global_plan_
bool initialized_
double internal_force_gain_
 gain for internal forces ("Elasticity of Band")
int max_recursion_depth_approx_equi_
double min_bubble_overlap_
 minimum relative overlap two bubbles must have to be treated as connected
int num_optim_iterations_
 maximal number of iteration steps during optimization of band
double significant_force_
 percentage of old force for which a new force is considered significant when higher as this value
double tiny_bubble_distance_
 internal forces between two bubbles are only calc. if there distance is bigger than this lower bound
double tiny_bubble_expansion_
 lower bound for bubble expansion. below this bound bubble is considered as "in collision"
bool visualization_
base_local_planner::CostmapModelworld_model_

Detailed Description

Implements the Elastic Band Method for SE2-Manifold (mobile Base)

Definition at line 79 of file eband_local_planner.h.


Constructor & Destructor Documentation

Default constructor.

Definition at line 44 of file eband_local_planner.cpp.

Constructs the elastic band object.

Parameters:
nameThe name to give this instance of the elastic band local planner
costmapThe cost map to use for assigning costs to trajectories

Definition at line 47 of file eband_local_planner.cpp.

Destructor.

Definition at line 55 of file eband_local_planner.cpp.


Member Function Documentation

bool eband_local_planner::EBandPlanner::addFrames ( const std::vector< geometry_msgs::PoseStamped > &  robot_pose,
const AddAtPosition add_frames_at 
)

converts robot_pose to a bubble and tries to connect to the path

Parameters:
poseof the robot which shall be connected to the band
Returns:
true if pose could be connected

Definition at line 230 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::applyForces ( int  bubble_num,
std::vector< Bubble > &  band,
std::vector< geometry_msgs::WrenchStamped >  forces 
) [private]

Applies forces to move bubbles and recalculates expansion of bubbles.

Parameters:
referenceto number of bubble which shall be modified - number might be modified if additional bubbles are introduced to fill band
referenceto band which shall be modified
forcesand torques which shall be applied
Returns:
true if modified band valid, false if band is broken (one or more bubbles in collision)

Definition at line 988 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::calcBubbleDifference ( geometry_msgs::Pose  start_center_pose,
geometry_msgs::Pose  end_center_pose,
geometry_msgs::Twist &  difference 
) [private]

Calculates the difference between the pose of the center of two bubbles and outputs it as Twist (pointing from first to second bubble) [depends kinematic].

Parameters:
poseof the first bubble
poseof the second bubble
referenceto variable in wich the difference is stored as twist
Returns:
true if difference was successfully calculated

Definition at line 1751 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::calcBubbleDistance ( geometry_msgs::Pose  start_center_pose,
geometry_msgs::Pose  end_center_pose,
double &  distance 
) [private]

This calculates the distance between two bubbles [depends kinematics, shape].

Parameters:
poseof the center of first bubbles
poseof the center of second bubbles
refernceto variable to pass distance to caller function
Returns:
true if distance was successfully calculated

Definition at line 1716 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::calcExternalForces ( int  bubble_num,
Bubble  curr_bubble,
geometry_msgs::WrenchStamped &  forces 
) [private]

Calculates external forces for bubbles along the band [depends shape, environment].

Parameters:
positionin band for which internal forces shall be calculated
Bubblefor which external forces shall be calculated
referenceto variable via which forces and torques are given back
Returns:
true if forces were calculated successfully

Definition at line 1448 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::calcInternalForces ( int  bubble_num,
std::vector< Bubble band,
Bubble  curr_bubble,
geometry_msgs::WrenchStamped &  forces 
) [private]

Calculates internal forces for bubbles along the band [depends kinematic].

Parameters:
positionin band for which internal forces shall be calculated
bandfor which forces shall be calculated
Bubblefor which internal forces shall be calculated
referenceto variable via which forces and torques are given back
Returns:
true if forces were calculated successfully

Definition at line 1370 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::calcObstacleKinematicDistance ( geometry_msgs::Pose  center_pose,
double &  distance 
) [private]

Calculates the distance between the center of a bubble and the closest obstacle [depends kinematics, shape, environment].

Parameters:
centerof bubble as Pose
referenceto distance variable
Returns:
True if successfully calculated distance false otherwise

Definition at line 1789 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::checkOverlap ( Bubble  bubble1,
Bubble  bubble2 
) [private]

this checks whether two bubbles overlap

Parameters:
bandon which we want to check
iteratorto first bubble
iteratorto second bubble
Returns:
true if bubbles overlap

Definition at line 1688 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::convertBandToPlan ( std::vector< geometry_msgs::PoseStamped > &  plan,
std::vector< Bubble band 
) [private]

This converts a band from a sequence of bubbles into a plan from, a sequence of stamped poses.

Parameters:
bandis the sequence of bubbles which shall be converted
planis the resulting sequence of stamped Poses
Returns:
true if path was successfully converted - band did not break

Definition at line 1909 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::convertPlanToBand ( std::vector< geometry_msgs::PoseStamped >  plan,
std::vector< Bubble > &  band 
) [private]

This converts a plan from a sequence of stamped poses into a band, a sequence of bubbles. Therefore it calculates distances to the surrounding obstacles and derives the expansion or radius of the bubble.

Parameters:
planis the sequence of stamped Poses which shall be converted
bandis the resulting sequence of bubbles
Returns:
true if path was successfully converted - band did not break

Definition at line 1851 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::fillGap ( std::vector< Bubble > &  band,
std::vector< Bubble >::iterator &  start_iter,
std::vector< Bubble >::iterator &  end_iter 
) [private]

recursively fills gaps between two bubbles (if possible)

Parameters:
referenceto the elastic band that is worked on
referenceto the iterator pointing to the start of the intervall that shall be filled
referenceto the iterator pointing to the end of the intervall that shall be filled
Returns:
true if gap was successfully filled otherwise false (band broken)

Definition at line 687 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::getBand ( std::vector< Bubble > &  elastic_band)

This outputs the elastic_band.

Parameters:
referencevia which the band is passed to the caller
Returns:
true if there was a band to pass

Definition at line 208 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::getForcesAt ( int  bubble_num,
std::vector< Bubble band,
Bubble  curr_bubble,
geometry_msgs::WrenchStamped &  forces 
) [private]

Calculates all forces for a certain bubble at a specific position in the band [depends kinematic].

Parameters:
positionin band for which internal forces shall be calculated
bandfor which forces shall be calculated
Bubblefor which internal forces shall be calculated
referenceto variable via which forces and torques are given back
Returns:
true if forces were calculated successfully

Definition at line 1332 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::getPlan ( std::vector< geometry_msgs::PoseStamped > &  global_plan)

This transforms the refined band to a path again and outputs it.

Parameters:
referencevia which the path is passed to the caller
Returns:
true if path could be handed over

Definition at line 181 of file eband_local_planner.cpp.

void eband_local_planner::EBandPlanner::initialize ( std::string  name,
costmap_2d::Costmap2DROS costmap_ros 
)

Initializes the elastic band class by accesing costmap and loading parameters.

Parameters:
nameThe name to give this instance of the trajectory planner
costmapThe cost map to use for assigning costs to trajectories

Definition at line 61 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::interpolateBubbles ( geometry_msgs::PoseStamped  start_center,
geometry_msgs::PoseStamped  end_center,
geometry_msgs::PoseStamped &  interpolated_center 
) [private]

interpolates between two bubbles by calculating the pose for the center of a bubble in the middle of the path connecting the bubbles [depends kinematics]

Parameters:
centerof first of the two bubbles between which shall be interpolated
centerof second of the two bubbles between which shall be interpolated
referenceto hand back the interpolated bubble's center
Returns:
true if interpolation was successful

Definition at line 1644 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::modifyBandArtificialForce ( std::vector< Bubble > &  band) [private]

calculates internal and external forces and applies changes to the band

Parameters:
referenceto band which shall be modified
Returns:
true if band could be modified, all steps finished cleanly

Definition at line 833 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::moveApproximateEquilibrium ( const int &  bubble_num,
const std::vector< Bubble > &  band,
Bubble curr_bubble,
const geometry_msgs::WrenchStamped &  curr_bubble_force,
geometry_msgs::Twist &  curr_step_width,
const int &  curr_recursion_depth 
) [private]

Checks for zero-crossings and large changes in force-vector after moving of bubble - if detected it tries to approximate the equilibrium point.

Parameters:
positionof the checked bubble within the band
bandwithin which equilibrium position shall be approximated
forcecalculated for last bubble pose
currentstep width
currentrecursion depth to constrain number of recurions
Returns:
true if recursion successfully

Definition at line 1198 of file eband_local_planner.cpp.

cycles over the elastic band set before and optimizes it locally by minimizing an energy-function

Returns:
true if band is valid

Definition at line 373 of file eband_local_planner.cpp.

cycles over the elastic band checks it for validity and optimizes it locally by minimizing an energy-function

Parameters:
referenceto band which shall be optimized
Returns:
true if band is valid

Definition at line 402 of file eband_local_planner.cpp.

void eband_local_planner::EBandPlanner::reconfigure ( EBandPlannerConfig &  config)

Reconfigures the parameters of the planner.

Parameters:
configThe dynamic reconfigure configuration

Definition at line 96 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::refineBand ( std::vector< Bubble > &  band) [private]

Cycles over an band and searches for gaps and redundant bubbles. Tries to close gaps and remove redundant bubbles.

Parameters:
bandis a reference to the band that shall be refined
Returns:
true if all gaps could be closed and band is valid

Definition at line 482 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::removeAndFill ( std::vector< Bubble > &  band,
std::vector< Bubble >::iterator &  start_iter,
std::vector< Bubble >::iterator &  end_iter 
) [private]

recursively checks an intervall of a band whether gaps need to be filled or bubbles may be removed and fills or removes if possible. This exploits the sequential structure of the band. Therefore it can not detect redundant cycles which are closed over several on its own not redundant bubbles.

Parameters:
referenceto the elastic band that shall be checked
referenceto the iterator pointing to the start of the intervall that shall be checked
referenceto the iterator pointing to the end of the intervall that shall be checked
Returns:
true if segment is valid (all gaps filled), falls if band is broken within this segment

Definition at line 524 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::setPlan ( const std::vector< geometry_msgs::PoseStamped > &  global_plan)

Set plan which shall be optimized to elastic band planner.

Parameters:
global_planThe plan which shall be optimized
Returns:
True if the plan was set successfully

Definition at line 128 of file eband_local_planner.cpp.

void eband_local_planner::EBandPlanner::setVisualization ( boost::shared_ptr< EBandVisualization eband_visual)

passes a reference to the eband visualization object which can be used to visualize the band optimization

Parameters:
pointerto visualization object

Definition at line 120 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::suppressTangentialForces ( int  bubble_num,
std::vector< Bubble band,
geometry_msgs::WrenchStamped &  forces 
) [private]

Calculates tangential portion of forces.

Parameters:
numberof bubble for which forces shall be recalculated
referenceto variable via which forces and torques are given back
Returns:
true if forces were calculated successfully

Definition at line 1587 of file eband_local_planner.cpp.


Member Data Documentation

std::vector<double> eband_local_planner::EBandPlanner::acc_lim_ [private]

acceleration limits for translational and rotational motion

Definition at line 164 of file eband_local_planner.h.

Definition at line 185 of file eband_local_planner.h.

pointer to costmap

Definition at line 161 of file eband_local_planner.h.

lower bound for absolute value of force below which it is treated as insignificant (no recursive approximation)

Definition at line 174 of file eband_local_planner.h.

Definition at line 178 of file eband_local_planner.h.

Definition at line 187 of file eband_local_planner.h.

maximum depth for recursive approximation to constrain computational burden

Definition at line 172 of file eband_local_planner.h.

gain for external forces ("Penalty on low distance to abstacles")

Definition at line 167 of file eband_local_planner.h.

Definition at line 184 of file eband_local_planner.h.

std::vector<geometry_msgs::PoseStamped> eband_local_planner::EBandPlanner::global_plan_ [private]

Definition at line 186 of file eband_local_planner.h.

Definition at line 181 of file eband_local_planner.h.

gain for internal forces ("Elasticity of Band")

Definition at line 166 of file eband_local_planner.h.

Definition at line 171 of file eband_local_planner.h.

minimum relative overlap two bubbles must have to be treated as connected

Definition at line 170 of file eband_local_planner.h.

maximal number of iteration steps during optimization of band

Definition at line 165 of file eband_local_planner.h.

percentage of old force for which a new force is considered significant when higher as this value

Definition at line 173 of file eband_local_planner.h.

internal forces between two bubbles are only calc. if there distance is bigger than this lower bound

Definition at line 168 of file eband_local_planner.h.

lower bound for bubble expansion. below this bound bubble is considered as "in collision"

Definition at line 169 of file eband_local_planner.h.

Definition at line 181 of file eband_local_planner.h.

Definition at line 177 of file eband_local_planner.h.


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


eband_local_planner
Author(s): Christian Connette, Bhaskara Marthi, Piyush Khandelwal
autogenerated on Thu Jun 6 2019 18:50:52