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
	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
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::Costmap2D *	costmap_
costmap_2d::Costmap2DROS *	costmap_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< Bubble >	elastic_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::Point >	footprint_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::CostmapModel *	world_model_

Detailed Description

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

Definition at line 78 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 228 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 986 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 1749 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 1714 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 1446 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 1368 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 1787 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 1686 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 1907 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 1849 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 685 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 206 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 1330 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 179 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 1642 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 831 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 1196 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 371 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 400 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 480 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 522 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 126 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 118 of file eband_local_planner.cpp.

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