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.
	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.
void	mapCallback (const nav_msgs::OccupancyGridConstPtr &occupancy_map)
	Callback to set the map.
bool	pathExists ()
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().
bool	plan (const geometry_msgs::PoseStampedConstPtr &start, const geometry_msgs::PoseStampedConstPtr &goal)
	Sets start, goal poses and calls FootstepPlanner::plan().
bool	plan ()
	Start a planning task from scratch (will delete information of previous planning tasks). Map and start, goal poses need to be set beforehand.
bool	planService (humanoid_nav_msgs::PlanFootsteps::Request &req, humanoid_nav_msgs::PlanFootsteps::Response &resp)
	Service handle to plan footsteps.
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.
void	reset ()
	Reset the previous planning information.
void	resetTotally ()
	Reset and reinitialize the environment.
bool	setGoal (float x, float y, float theta)
	Sets the goal pose as a robot pose centered between two feet.
bool	setGoal (const geometry_msgs::PoseStampedConstPtr &goal_pose)
	Sets the goal pose as a robot pose centered between two feet.
void	setMarkerNamespace (const std::string &ns)
void	setMaxSearchTime (int search_time)
	Set the maximal search time.
bool	setStart (const State &left_foot, const State &right_foot)
	Sets the start pose as position of left and right footsteps.
bool	setStart (float x, float y, float theta)
	Sets the start pose as a robot pose centered between two feet.
bool	setStart (const geometry_msgs::PoseStampedConstPtr &start_pose)
	Sets the start pose as a robot pose centered between two feet.
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.
bool	updateMap (const gridmap_2d::GridMap2DPtr &map)
	Updates the map in the planning environment.
virtual	~FootstepPlanner ()
Protected Member Functions
void	broadcastExpandedNodesVis ()
void	broadcastFootstepPathVis ()
void	broadcastHeuristicPathVis ()
void	broadcastPathVis ()
void	broadcastRandomNodesVis ()
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.
void	footPoseToMarker (const State &footstep, visualization_msgs::Marker *marker)
	Generates a visualization msgs for a foot pose.
State	getFootPose (const State &robot, Leg side)
	Returns the foot pose of a leg for a given robot pose.
bool	pathIsNew (const std::vector< int > &new_path)
bool	run ()
	Starts the planning task in the underlying SBPL.
void	setPlanner ()
	Sets the planning algorithm used by SBPL.
void	updateEnvironment (const gridmap_2d::GridMap2DPtr &old_map)
	Updates the environment in case of a changed map.
Protected Attributes
int	ivChangedCellsLimit
	If limit of changed cells is reached the planner starts a new task from the scratch.
int	ivCollisionCheckAccuracy
environment_params	ivEnvironmentParams
ros::Publisher	ivExpandedStatesVisPub
double	ivFootSeparation
ros::Publisher	ivFootstepPathVisPub
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
bool	ivPathExists
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 58 of file FootstepPlanner.h.

Constructor & Destructor Documentation

footstep_planner::FootstepPlanner::FootstepPlanner ( )

Definition at line 36 of file FootstepPlanner.cpp.

footstep_planner::FootstepPlanner::~FootstepPlanner ( ) [virtual]

Definition at line 303 of file FootstepPlanner.cpp.

Member Function Documentation

void footstep_planner::FootstepPlanner::broadcastExpandedNodesVis ( ) [protected]

Definition at line 955 of file FootstepPlanner.cpp.

void footstep_planner::FootstepPlanner::broadcastFootstepPathVis ( ) [protected]

Definition at line 987 of file FootstepPlanner.cpp.

void footstep_planner::FootstepPlanner::broadcastHeuristicPathVis ( ) [protected]

void footstep_planner::FootstepPlanner::broadcastPathVis ( ) [protected]

Definition at line 1073 of file FootstepPlanner.cpp.

void footstep_planner::FootstepPlanner::broadcastRandomNodesVis ( ) [protected]

Definition at line 1031 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 929 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 436 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 1101 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 898 of file FootstepPlanner.cpp.

size_t footstep_planner::FootstepPlanner::getNumExpandedStates ( ) const [inline]

Returns:: Number of expanded states.

Definition at line 188 of file FootstepPlanner.h.

size_t footstep_planner::FootstepPlanner::getNumFootPoses ( ) const [inline]

Returns:: Number of planned foot poses.

Definition at line 194 of file FootstepPlanner.h.

state_iter_t footstep_planner::FootstepPlanner::getPathBegin ( ) const [inline]

Definition at line 196 of file FootstepPlanner.h.

double footstep_planner::FootstepPlanner::getPathCosts ( ) const [inline]

Returns:: Costs of the planned footstep path.

Definition at line 185 of file FootstepPlanner.h.

state_iter_t footstep_planner::FootstepPlanner::getPathEnd ( ) const [inline]

Definition at line 197 of file FootstepPlanner.h.

int footstep_planner::FootstepPlanner::getPathSize ( ) [inline]

Returns:: Size of the planned path.

Definition at line 200 of file FootstepPlanner.h.

State footstep_planner::FootstepPlanner::getStartFootLeft ( ) [inline]

Definition at line 202 of file FootstepPlanner.h.

State footstep_planner::FootstepPlanner::getStartFootRight ( ) [inline]

Definition at line 203 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 600 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 641 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 212 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 915 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 545 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 534 of file FootstepPlanner.cpp.

bool footstep_planner::FootstepPlanner::plan ( )

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 488 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 559 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.