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 (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.
bool	plan (const geometry_msgs::PoseStampedConstPtr start, const geometry_msgs::PoseStampedConstPtr goal)
	Sets start, goal poses and calls FootstepPlanner::plan().
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	planFeetService (humanoid_nav_msgs::PlanFootstepsBetweenFeet::Request &req, humanoid_nav_msgs::PlanFootstepsBetweenFeet::Response &resp)
	Service handle to plan footsteps.
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 (const State &left_foot, const State &right_foot)
	Sets the goal pose as two feet (left / right)
bool	setGoal (const geometry_msgs::PoseStampedConstPtr goal_pose)
	Sets the goal pose as a robot pose centered between two feet.
bool	setGoal (float x, float y, float theta)
	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 geometry_msgs::PoseStampedConstPtr start_pose)
	Sets the start pose as a robot pose centered between two feet.
bool	setStart (float x, float y, float theta)
	Sets the start pose as a robot pose centered between two feet.
bool	setStart (const State &left_foot, const State &right_foot)
	Sets the start pose as position of left and right footsteps.
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 ()
Public Attributes
environment_params	ivEnvironmentParams
	Planning parameters.
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
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
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.