footstep_planner::FootstepPlanner Class Reference

A class to control the interaction between ROS and the footstep planner. More...

#include <FootstepPlanner.h>

Public Member Functions
void	clearFootstepPathVis (unsigned num_footsteps=0)
	Clear the footstep path visualization from a previous planning task. More...
	FootstepPlanner ()
size_t	getNumExpandedStates () const
size_t	getNumFootPoses () const
state_iter_t	getPathBegin () const
double	getPathCosts () const
state_iter_t	getPathEnd () const
int	getPathSize ()
State	getStartFootLeft ()
State	getStartFootRight ()
void	goalPoseCallback (const geometry_msgs::PoseStampedConstPtr &goal_pose)
	Callback to set the goal pose as a robot pose centered between two feet. If the start pose has been set previously the planning is started. More...
void	mapCallback (const nav_msgs::OccupancyGridConstPtr &occupancy_map)
	Callback to set the map. More...
bool	pathExists ()
bool	plan (bool force_new_plan=true)
	Start a planning task from scratch (will delete information of previous planning tasks). Map and start, goal poses need to be set beforehand. More...
bool	plan (const geometry_msgs::PoseStampedConstPtr start, const geometry_msgs::PoseStampedConstPtr goal)
	Sets start, goal poses and calls FootstepPlanner::plan(). More...
bool	plan (float start_x, float start_y, float start_theta, float goal_x, float goal_y, float goal_theta)
	Sets start, goal poses and calls FootstepPlanner::plan(). More...
bool	planFeetService (humanoid_nav_msgs::PlanFootstepsBetweenFeet::Request &req, humanoid_nav_msgs::PlanFootstepsBetweenFeet::Response &resp)
	Service handle to plan footsteps. More...
bool	planService (humanoid_nav_msgs::PlanFootsteps::Request &req, humanoid_nav_msgs::PlanFootsteps::Response &resp)
	Service handle to plan footsteps. More...
bool	replan ()
	Starts a planning task based on previous planning information (note that this method can also be used when no previous planning was performed). Map and start, goal poses need to be set beforehand. More...
void	reset ()
	Reset the previous planning information. More...
void	resetTotally ()
	Reset and reinitialize the environment. More...
bool	setGoal (const State &left_foot, const State &right_foot)
	Sets the goal pose as two feet (left / right) More...
bool	setGoal (const geometry_msgs::PoseStampedConstPtr goal_pose)
	Sets the goal pose as a robot pose centered between two feet. More...
bool	setGoal (float x, float y, float theta)
	Sets the goal pose as a robot pose centered between two feet. More...
void	setMarkerNamespace (const std::string &ns)
void	setMaxSearchTime (int search_time)
	Set the maximal search time. More...
bool	setStart (const geometry_msgs::PoseStampedConstPtr start_pose)
	Sets the start pose as a robot pose centered between two feet. More...
bool	setStart (float x, float y, float theta)
	Sets the start pose as a robot pose centered between two feet. More...
bool	setStart (const State &left_foot, const State &right_foot)
	Sets the start pose as position of left and right footsteps. More...
void	startPoseCallback (const geometry_msgs::PoseWithCovarianceStampedConstPtr &start_pose)
	Callback to set the start pose as a robot pose centered between two feet. If the goal pose has been set previously the planning is started. More...
bool	updateMap (const gridmap_2d::GridMap2DPtr map)
	Updates the map in the planning environment. More...
virtual	~FootstepPlanner ()

Public Attributes
environment_params	ivEnvironmentParams
	Planning parameters. More...

Protected Member Functions
void	broadcastExpandedNodesVis ()
void	broadcastFootstepPathVis ()
void	broadcastHeuristicPathVis ()
void	broadcastPathVis ()
void	broadcastRandomNodesVis ()
void	extractFootstepsSrv (std::vector< humanoid_nav_msgs::StepTarget > &footsteps) const
	helper to create service response More...
bool	extractPath (const std::vector< int > &state_ids)
	Extracts the path (list of foot poses) from a list of state IDs calculated by the SBPL. More...
void	footPoseToMarker (const State &footstep, visualization_msgs::Marker *marker)
	Generates a visualization msgs for a foot pose. More...
State	getFootPose (const State &robot, Leg side)
	Returns the foot pose of a leg for a given robot pose. More...
bool	pathIsNew (const std::vector< int > &new_path)
bool	run ()
	Starts the planning task in the underlying SBPL. More...
void	setPlanner ()
	Sets the planning algorithm used by SBPL. More...
void	updateEnvironment (const gridmap_2d::GridMap2DPtr old_map)
	Updates the environment in case of a changed map. More...

Protected Attributes
int	ivChangedCellsLimit
	If limit of changed cells is reached the planner starts a new task from the scratch. More...
int	ivCollisionCheckAccuracy
ros::Publisher	ivExpandedStatesVisPub
double	ivFootSeparation
ros::Publisher	ivFootstepPathVisPub
ros::ServiceServer	ivFootstepPlanFeetService
ros::ServiceServer	ivFootstepPlanService
State	ivGoalFootLeft
State	ivGoalFootRight
bool	ivGoalPoseSetUp
ros::Subscriber	ivGridMapSub
ros::Publisher	ivHeuristicPathVisPub
double	ivInitialEpsilon
int	ivLastMarkerMsgSize
gridmap_2d::GridMap2DPtr	ivMapPtr
std::string	ivMarkerNamespace
double	ivMaxSearchTime
double	ivMaxStepWidth
std::vector< State >	ivPath
double	ivPathCost
boost::shared_ptr< const PathCostHeuristic >	ivPathCostHeuristicPtr
ros::Publisher	ivPathVisPub
boost::shared_ptr< FootstepPlannerEnvironment >	ivPlannerEnvironmentPtr
boost::shared_ptr< SBPLPlanner >	ivPlannerPtr
std::string	ivPlannerType
std::vector< int >	ivPlanningStatesIds
ros::Publisher	ivRandomStatesVisPub
bool	ivSearchUntilFirstSolution
State	ivStartFootLeft
State	ivStartFootRight
bool	ivStartPoseSetUp
ros::Publisher	ivStartPoseVisPub

Detailed Description

A class to control the interaction between ROS and the footstep planner.

Definition at line 57 of file FootstepPlanner.h.

Constructor & Destructor Documentation

footstep_planner::FootstepPlanner::FootstepPlanner

(

)

Definition at line 31 of file FootstepPlanner.cpp.

footstep_planner::FootstepPlanner::~FootstepPlanner ( )

virtual

Definition at line 270 of file FootstepPlanner.cpp.

Member Function Documentation

void footstep_planner::FootstepPlanner::broadcastExpandedNodesVis ( )

protected

Definition at line 982 of file FootstepPlanner.cpp.

void footstep_planner::FootstepPlanner::broadcastFootstepPathVis ( )

protected

Definition at line 1014 of file FootstepPlanner.cpp.

void footstep_planner::FootstepPlanner::broadcastHeuristicPathVis ( )

protected

void footstep_planner::FootstepPlanner::broadcastPathVis ( )

protected

Definition at line 1100 of file FootstepPlanner.cpp.

void footstep_planner::FootstepPlanner::broadcastRandomNodesVis ( )

protected

Definition at line 1058 of file FootstepPlanner.cpp.

void footstep_planner::FootstepPlanner::clearFootstepPathVis

(

unsigned

num_footsteps = 0

)

Clear the footstep path visualization from a previous planning task.

Definition at line 956 of file FootstepPlanner.cpp.

void footstep_planner::FootstepPlanner::extractFootstepsSrv ( std::vector< humanoid_nav_msgs::StepTarget > &  footsteps ) const

protected

helper to create service response

Definition at line 595 of file FootstepPlanner.cpp.

bool footstep_planner::FootstepPlanner::extractPath ( const std::vector< int > &  state_ids )

protected

Extracts the path (list of foot poses) from a list of state IDs calculated by the SBPL.

Definition at line 410 of file FootstepPlanner.cpp.

void footstep_planner::FootstepPlanner::footPoseToMarker ( const State &  footstep, visualization_msgs::Marker *  marker  )

protected

Generates a visualization msgs for a foot pose.

Definition at line 1128 of file FootstepPlanner.cpp.

State footstep_planner::FootstepPlanner::getFootPose ( const State &  robot, Leg  side  )

protected

Returns the foot pose of a leg for a given robot pose.

Definition at line 925 of file FootstepPlanner.cpp.

size_t footstep_planner::FootstepPlanner::getNumExpandedStates ( ) const

inline

Returns

Number of expanded states.

Definition at line 198 of file FootstepPlanner.h.

size_t footstep_planner::FootstepPlanner::getNumFootPoses ( ) const

inline

Returns

Number of planned foot poses.

Definition at line 204 of file FootstepPlanner.h.

state_iter_t footstep_planner::FootstepPlanner::getPathBegin ( ) const

inline

Definition at line 206 of file FootstepPlanner.h.

double footstep_planner::FootstepPlanner::getPathCosts ( ) const

inline

Returns

Costs of the planned footstep path.

Definition at line 195 of file FootstepPlanner.h.

state_iter_t footstep_planner::FootstepPlanner::getPathEnd ( ) const

inline

Definition at line 207 of file FootstepPlanner.h.

int footstep_planner::FootstepPlanner::getPathSize ( )

inline

Returns

Size of the planned path.

Definition at line 210 of file FootstepPlanner.h.

State footstep_planner::FootstepPlanner::getStartFootLeft ( )

inline

Definition at line 212 of file FootstepPlanner.h.

State footstep_planner::FootstepPlanner::getStartFootRight ( )

inline

Definition at line 213 of file FootstepPlanner.h.

void footstep_planner::FootstepPlanner::goalPoseCallback

(

const geometry_msgs::PoseStampedConstPtr &

goal_pose

)

Callback to set the goal pose as a robot pose centered between two feet. If the start pose has been set previously the planning is started.

Subscribed to 'goal'.

Returns

True if the two foot poses have been set successfully.

Definition at line 622 of file FootstepPlanner.cpp.

void footstep_planner::FootstepPlanner::mapCallback

(

const nav_msgs::OccupancyGridConstPtr &

occupancy_map

)

Callback to set the map.

Subscribed to 'map'.

Definition at line 655 of file FootstepPlanner.cpp.

bool footstep_planner::FootstepPlanner::pathExists ( )

inline

Returns

True if for the current start and goal pose a path exists.

Definition at line 222 of file FootstepPlanner.h.

bool footstep_planner::FootstepPlanner::pathIsNew ( const std::vector< int > &  new_path )

protected

Returns

True if the newly calculated path is different from the existing one (if one exists).

Definition at line 942 of file FootstepPlanner.cpp.

bool footstep_planner::FootstepPlanner::plan

(

bool

force_new_plan = true

)

Start a planning task from scratch (will delete information of previous planning tasks). Map and start, goal poses need to be set beforehand.

Returns

Success of planning.

Definition at line 492 of file FootstepPlanner.cpp.

bool footstep_planner::FootstepPlanner::plan	(	const geometry_msgs::PoseStampedConstPtr	start,
		const geometry_msgs::PoseStampedConstPtr	goal
	)

Sets start, goal poses and calls FootstepPlanner::plan().

Definition at line 524 of file FootstepPlanner.cpp.

bool footstep_planner::FootstepPlanner::plan	(	float	start_x,
		float	start_y,
		float	start_theta,
		float	goal_x,
		float	goal_y,
		float	goal_theta
	)

Sets start, goal poses and calls FootstepPlanner::plan().

Definition at line 535 of file FootstepPlanner.cpp.

bool footstep_planner::FootstepPlanner::planFeetService	(	humanoid_nav_msgs::PlanFootstepsBetweenFeet::Request &	req,
		humanoid_nav_msgs::PlanFootstepsBetweenFeet::Response &	resp
	)

Service handle to plan footsteps.

Definition at line 570 of file FootstepPlanner.cpp.

bool footstep_planner::FootstepPlanner::planService	(	humanoid_nav_msgs::PlanFootsteps::Request &	req,
		humanoid_nav_msgs::PlanFootsteps::Response &	resp
	)

Service handle to plan footsteps.

Definition at line 549 of file FootstepPlanner.cpp.

bool footstep_planner::FootstepPlanner::replan

(

)

Starts a planning task based on previous planning information (note that this method can also be used when no previous planning was performed). Map and start, goal poses need to be set beforehand.

Returns

Success of planning.

Definition at line 517 of file FootstepPlanner.cpp.

void footstep_planner::FootstepPlanner::reset

(

)

Reset the previous planning information.

Definition at line 461 of file FootstepPlanner.cpp.

void footstep_planner::FootstepPlanner::resetTotally

(

)

Reset and reinitialize the environment.

Definition at line 478 of file FootstepPlanner.cpp.

bool footstep_planner::FootstepPlanner::run ( )

protected

Starts the planning task in the underlying SBPL.

NOTE: Never call this directly. Always use either plan() or replan() to invoke this method.

Definition at line 306 of file FootstepPlanner.cpp.

bool footstep_planner::FootstepPlanner::setGoal	(	const State &	left_foot,
		const State &	right_foot
	)

Sets the goal pose as two feet (left / right)

Returns

True if the two foot poses have been set successfully.

Definition at line 709 of file FootstepPlanner.cpp.

bool footstep_planner::FootstepPlanner::setGoal

(

const geometry_msgs::PoseStampedConstPtr

goal_pose

)

Sets the goal pose as a robot pose centered between two feet.

Returns

True if the two foot poses have been set successfully.

Definition at line 671 of file FootstepPlanner.cpp.

bool footstep_planner::FootstepPlanner::setGoal	(	float	x,
		float	y,
		float	theta
	)

Sets the goal pose as a robot pose centered between two feet.

Returns

True if the two foot poses have been set successfully.

Definition at line 680 of file FootstepPlanner.cpp.

void footstep_planner::FootstepPlanner::setMarkerNamespace ( const std::string &  ns )

inline

Definition at line 147 of file FootstepPlanner.h.

void footstep_planner::FootstepPlanner::setMaxSearchTime ( int  search_time )

inline

Set the maximal search time.

Definition at line 153 of file FootstepPlanner.h.

void footstep_planner::FootstepPlanner::setPlanner ( )

protected

Sets the planning algorithm used by SBPL.

Definition at line 275 of file FootstepPlanner.cpp.

bool footstep_planner::FootstepPlanner::setStart

(

const geometry_msgs::PoseStampedConstPtr

start_pose

)

Sets the start pose as a robot pose centered between two feet.

Returns

True if the two foot poses have been set successfully.

Definition at line 727 of file FootstepPlanner.cpp.

bool footstep_planner::FootstepPlanner::setStart	(	float	x,
		float	y,
		float	theta
	)

Sets the start pose as a robot pose centered between two feet.

Returns

True if the two foot poses have been set successfully.

Definition at line 754 of file FootstepPlanner.cpp.

bool footstep_planner::FootstepPlanner::setStart	(	const State &	left_foot,
		const State &	right_foot
	)

Sets the start pose as position of left and right footsteps.

Returns

True if the two foot poses have been set successfully.

Definition at line 736 of file FootstepPlanner.cpp.

void footstep_planner::FootstepPlanner::startPoseCallback

(

const geometry_msgs::PoseWithCovarianceStampedConstPtr &

start_pose

)

Callback to set the start pose as a robot pose centered between two feet. If the goal pose has been set previously the planning is started.

Subscribed to 'initialpose'.

Returns

True if the two foot poses have been set successfully.

Definition at line 638 of file FootstepPlanner.cpp.

void footstep_planner::FootstepPlanner::updateEnvironment ( const gridmap_2d::GridMap2DPtr  old_map )

protected

Updates the environment in case of a changed map.

Definition at line 809 of file FootstepPlanner.cpp.

bool footstep_planner::FootstepPlanner::updateMap

(

const gridmap_2d::GridMap2DPtr

map

)

Updates the map in the planning environment.

Returns

True if a replanning is necessary, i.e. the old path is not valid any more.

Definition at line 787 of file FootstepPlanner.cpp.

Member Data Documentation

int footstep_planner::FootstepPlanner::ivChangedCellsLimit

protected

If limit of changed cells is reached the planner starts a new task from the scratch.

Definition at line 308 of file FootstepPlanner.h.

int footstep_planner::FootstepPlanner::ivCollisionCheckAccuracy

protected

Definition at line 295 of file FootstepPlanner.h.

environment_params footstep_planner::FootstepPlanner::ivEnvironmentParams

Planning parameters.

Definition at line 225 of file FootstepPlanner.h.

ros::Publisher footstep_planner::FootstepPlanner::ivExpandedStatesVisPub

protected

Definition at line 283 of file FootstepPlanner.h.

double footstep_planner::FootstepPlanner::ivFootSeparation

protected

Definition at line 293 of file FootstepPlanner.h.

ros::Publisher footstep_planner::FootstepPlanner::ivFootstepPathVisPub

protected

Definition at line 284 of file FootstepPlanner.h.

ros::ServiceServer footstep_planner::FootstepPlanner::ivFootstepPlanFeetService

protected

Definition at line 291 of file FootstepPlanner.h.

ros::ServiceServer footstep_planner::FootstepPlanner::ivFootstepPlanService

protected

Definition at line 290 of file FootstepPlanner.h.

State footstep_planner::FootstepPlanner::ivGoalFootLeft

protected

Definition at line 280 of file FootstepPlanner.h.

State footstep_planner::FootstepPlanner::ivGoalFootRight

protected

Definition at line 281 of file FootstepPlanner.h.

bool footstep_planner::FootstepPlanner::ivGoalPoseSetUp

protected

Definition at line 297 of file FootstepPlanner.h.

ros::Subscriber footstep_planner::FootstepPlanner::ivGridMapSub

protected

Definition at line 286 of file FootstepPlanner.h.

ros::Publisher footstep_planner::FootstepPlanner::ivHeuristicPathVisPub

protected

Definition at line 287 of file FootstepPlanner.h.

double footstep_planner::FootstepPlanner::ivInitialEpsilon

protected

Definition at line 302 of file FootstepPlanner.h.

int footstep_planner::FootstepPlanner::ivLastMarkerMsgSize

protected

Definition at line 298 of file FootstepPlanner.h.

gridmap_2d::GridMap2DPtr footstep_planner::FootstepPlanner::ivMapPtr

protected

Definition at line 271 of file FootstepPlanner.h.

std::string footstep_planner::FootstepPlanner::ivMarkerNamespace

protected

Definition at line 311 of file FootstepPlanner.h.

double footstep_planner::FootstepPlanner::ivMaxSearchTime

protected

Definition at line 301 of file FootstepPlanner.h.

double footstep_planner::FootstepPlanner::ivMaxStepWidth

protected

Definition at line 294 of file FootstepPlanner.h.

std::vector<State> footstep_planner::FootstepPlanner::ivPath

protected

Definition at line 276 of file FootstepPlanner.h.

double footstep_planner::FootstepPlanner::ivPathCost

protected

Definition at line 299 of file FootstepPlanner.h.

boost::shared_ptr<const PathCostHeuristic> footstep_planner::FootstepPlanner::ivPathCostHeuristicPtr

protected

Definition at line 274 of file FootstepPlanner.h.

ros::Publisher footstep_planner::FootstepPlanner::ivPathVisPub

protected

Definition at line 288 of file FootstepPlanner.h.

boost::shared_ptr<FootstepPlannerEnvironment> footstep_planner::FootstepPlanner::ivPlannerEnvironmentPtr

protected

Definition at line 270 of file FootstepPlanner.h.

boost::shared_ptr<SBPLPlanner> footstep_planner::FootstepPlanner::ivPlannerPtr

protected

Definition at line 272 of file FootstepPlanner.h.

std::string footstep_planner::FootstepPlanner::ivPlannerType

protected

Definition at line 310 of file FootstepPlanner.h.

std::vector<int> footstep_planner::FootstepPlanner::ivPlanningStatesIds

protected

Definition at line 313 of file FootstepPlanner.h.

ros::Publisher footstep_planner::FootstepPlanner::ivRandomStatesVisPub

protected

Definition at line 285 of file FootstepPlanner.h.

bool footstep_planner::FootstepPlanner::ivSearchUntilFirstSolution

protected

Definition at line 300 of file FootstepPlanner.h.

State footstep_planner::FootstepPlanner::ivStartFootLeft

protected

Definition at line 278 of file FootstepPlanner.h.

State footstep_planner::FootstepPlanner::ivStartFootRight

protected

Definition at line 279 of file FootstepPlanner.h.

bool footstep_planner::FootstepPlanner::ivStartPoseSetUp

protected

Definition at line 297 of file FootstepPlanner.h.

ros::Publisher footstep_planner::FootstepPlanner::ivStartPoseVisPub

protected

Definition at line 289 of file FootstepPlanner.h.

---

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

• FootstepPlanner.h
• FootstepPlanner.cpp