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

#include <eband_local_planner.h>

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

	EBandPlanner ()
	Default constructor. More...

	EBandPlanner (std::string name, costmap_2d::Costmap2DROS *costmap_ros)
	Constructs the elastic band object. More...

bool	getBand (std::vector< Bubble > &elastic_band)
	This outputs the elastic_band. More...

bool	getPlan (std::vector< geometry_msgs::PoseStamped > &global_plan)
	This transforms the refined band to a path again and outputs it. More...

void	initialize (std::string name, costmap_2d::Costmap2DROS *costmap_ros)
	Initializes the elastic band class by accesing costmap and loading parameters. More...

bool	optimizeBand ()
	cycles over the elastic band set before and optimizes it locally by minimizing an energy-function More...

bool	optimizeBand (std::vector< Bubble > &band)
	cycles over the elastic band checks it for validity and optimizes it locally by minimizing an energy-function More...

void	reconfigure (EBandPlannerConfig &config)
	Reconfigures the parameters of the planner. More...

bool	setPlan (const std::vector< geometry_msgs::PoseStamped > &global_plan)
	Set plan which shall be optimized to elastic band planner. More...

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

	~EBandPlanner ()
	Destructor. More...

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

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

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

bool	calcExternalForces (int bubble_num, Bubble curr_bubble, geometry_msgs::WrenchStamped &forces)
	Calculates external forces for bubbles along the band [depends shape, environment]. More...

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

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

bool	checkOverlap (Bubble bubble1, Bubble bubble2)
	this checks whether two bubbles overlap More...

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

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

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

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

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

bool	modifyBandArtificialForce (std::vector< Bubble > &band)
	calculates internal and external forces and applies changes to the band More...

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

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

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

bool	suppressTangentialForces (int bubble_num, std::vector< Bubble > band, geometry_msgs::WrenchStamped &forces)
	Calculates tangential portion of forces. More...

Private Attributes
std::vector< double >	acc_lim_
	acceleration limits for translational and rotational motion More...

costmap_2d::Costmap2D *	costmap_

costmap_2d::Costmap2DROS *	costmap_ros_
	pointer to costmap More...

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

boost::shared_ptr< EBandVisualization >	eband_visual_

std::vector< Bubble >	elastic_band_

double	equilibrium_relative_overshoot_
	maximum depth for recursive approximation to constrain computational burden More...

double	external_force_gain_
	gain for external forces ("Penalty on low distance to abstacles") More...

std::vector< geometry_msgs::Point >	footprint_spec_

std::vector< geometry_msgs::PoseStamped >	global_plan_

bool	initialized_

double	internal_force_gain_
	gain for internal forces ("Elasticity of Band") More...

int	max_recursion_depth_approx_equi_

double	min_bubble_overlap_
	minimum relative overlap two bubbles must have to be treated as connected More...

int	num_optim_iterations_
	maximal number of iteration steps during optimization of band More...

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

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

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

bool	visualization_

base_local_planner::CostmapModel *	world_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

eband_local_planner::EBandPlanner::EBandPlanner ( )

Default constructor.

Definition at line 44 of file eband_local_planner.cpp.

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

Constructs the elastic band object.

Parameters

name	The name to give this instance of the elastic band local planner
costmap	The cost map to use for assigning costs to trajectories

Definition at line 47 of file eband_local_planner.cpp.

eband_local_planner::EBandPlanner::~EBandPlanner ( )

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

pose	of 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

reference	to number of bubble which shall be modified - number might be modified if additional bubbles are introduced to fill band
reference	to band which shall be modified
forces	and 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

pose	of the first bubble
pose	of the second bubble
reference	to 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

pose	of the center of first bubbles
pose	of the center of second bubbles
refernce	to 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

position	in band for which internal forces shall be calculated
Bubble	for which external forces shall be calculated
reference	to 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

position	in band for which internal forces shall be calculated
band	for which forces shall be calculated
Bubble	for which internal forces shall be calculated
reference	to 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

center	of bubble as Pose
reference	to 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

band	on which we want to check
iterator	to first bubble
iterator	to 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

band	is the sequence of bubbles which shall be converted
plan	is 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

plan	is the sequence of stamped Poses which shall be converted
band	is 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

reference	to the elastic band that is worked on
reference	to the iterator pointing to the start of the intervall that shall be filled
reference	to 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

reference via 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

position	in band for which internal forces shall be calculated
band	for which forces shall be calculated
Bubble	for which internal forces shall be calculated
reference	to 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

reference via 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

name	The name to give this instance of the trajectory planner
costmap	The 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

center	of first of the two bubbles between which shall be interpolated
center	of second of the two bubbles between which shall be interpolated
reference	to 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

reference to 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

position	of the checked bubble within the band
band	within which equilibrium position shall be approximated
force	calculated for last bubble pose
current	step width
current	recursion depth to constrain number of recurions

Returns: true if recursion successfully

Definition at line 1198 of file eband_local_planner.cpp.

bool eband_local_planner::EBandPlanner::optimizeBand ( )

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.

bool eband_local_planner::EBandPlanner::optimizeBand ( std::vector< Bubble > & band )

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

Parameters

reference to 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

config The 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

band	is 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

reference	to the elastic band that shall be checked
reference	to the iterator pointing to the start of the intervall that shall be checked
reference	to 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_plan The 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

pointer to 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

number	of bubble for which forces shall be recalculated
reference	to 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.

costmap_2d::Costmap2D* eband_local_planner::EBandPlanner::costmap_

private

Definition at line 185 of file eband_local_planner.h.

costmap_2d::Costmap2DROS* eband_local_planner::EBandPlanner::costmap_ros_

private

pointer to costmap

Definition at line 161 of file eband_local_planner.h.

double eband_local_planner::EBandPlanner::costmap_weight_

private

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.

boost::shared_ptr<EBandVisualization> eband_local_planner::EBandPlanner::eband_visual_

private

Definition at line 178 of file eband_local_planner.h.

std::vector<Bubble> eband_local_planner::EBandPlanner::elastic_band_

private

Definition at line 187 of file eband_local_planner.h.

double eband_local_planner::EBandPlanner::equilibrium_relative_overshoot_

private

maximum depth for recursive approximation to constrain computational burden

Definition at line 172 of file eband_local_planner.h.

double eband_local_planner::EBandPlanner::external_force_gain_

private

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

Definition at line 167 of file eband_local_planner.h.

std::vector<geometry_msgs::Point> eband_local_planner::EBandPlanner::footprint_spec_

private

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.

bool eband_local_planner::EBandPlanner::initialized_

private

Definition at line 181 of file eband_local_planner.h.

double eband_local_planner::EBandPlanner::internal_force_gain_

private

gain for internal forces ("Elasticity of Band")

Definition at line 166 of file eband_local_planner.h.

int eband_local_planner::EBandPlanner::max_recursion_depth_approx_equi_

private

Definition at line 171 of file eband_local_planner.h.

double eband_local_planner::EBandPlanner::min_bubble_overlap_

private

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

Definition at line 170 of file eband_local_planner.h.

int eband_local_planner::EBandPlanner::num_optim_iterations_

private

maximal number of iteration steps during optimization of band

Definition at line 165 of file eband_local_planner.h.

double eband_local_planner::EBandPlanner::significant_force_

private

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.

double eband_local_planner::EBandPlanner::tiny_bubble_distance_

private

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.

double eband_local_planner::EBandPlanner::tiny_bubble_expansion_

private

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

Definition at line 169 of file eband_local_planner.h.

bool eband_local_planner::EBandPlanner::visualization_

private

Definition at line 181 of file eband_local_planner.h.

base_local_planner::CostmapModel* eband_local_planner::EBandPlanner::world_model_

private

Definition at line 177 of file eband_local_planner.h.

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

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation