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

#include <footstep_planner.h>

Public Types
typedef boost::shared_ptr < const FootstepPlanner >	ConstPtr
typedef boost::function< void(msgs::PlanningFeedback)>	FeedbackCB
typedef boost::function< void()>	PreemptCB
typedef boost::shared_ptr < FootstepPlanner >	Ptr
typedef boost::function< void(msgs::StepPlanRequestService::Response)>	ResultCB
Public Member Functions
	FootstepPlanner (ros::NodeHandle &nh)
bool	isPlanning () const
void	preemptPlanning ()
	stops thread running planning
bool	setParams (const vigir_generic_params::ParameterSet &params)
msgs::ErrorStatus	stepPlanRequest (msgs::StepPlanRequestService::Request &req, ResultCB result_cb=ResultCB(), FeedbackCB feedback_cb=FeedbackCB(), PreemptCB preempt_cb=PreemptCB())
bool	stepPlanRequestService (msgs::StepPlanRequestService::Request &req, msgs::StepPlanRequestService::Response &resp)
	Service handle to plan footsteps.
msgs::ErrorStatus	updateFeet (msgs::Feet &feet, uint8_t mode, bool transform=true) const
msgs::ErrorStatus	updateFoot (msgs::Foot &foot, uint8_t mode, bool transform=true) const
msgs::ErrorStatus	updateStepPlan (msgs::StepPlan &result, uint8_t mode, const std::string &param_set_name=std::string(), bool transform=true) const
virtual	~FootstepPlanner ()
Protected Member Functions
bool	checkRobotCollision (const State &left_foot, const State &right_foot, bool &left, bool &right) const
void	doPlanning (msgs::StepPlanRequestService::Request &req)
	: method used in seperate thread
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.
msgs::ErrorStatus	finalizeAndAddStepToPlan (State &s, const msgs::PatternParameters &env_params, bool change_z=true)
bool	finalizeStepPlan (msgs::StepPlanRequestService::Request &req, msgs::StepPlanRequestService::Response &resp, bool post_process)
	extracts step plan response from planning result
bool	findNearestValidState (State &s) const
State	getFootPose (const State &robot, Leg leg, double dx, double dy, double dyaw)
	Returns the foot pose of a leg for a given robot pose.
State	getFootPose (const State &robot, Leg leg)
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 ()
bool	pathExists ()
bool	pathIsNew (const std::vector< int > &new_path)
bool	plan (ReplanParams &params)
	Starts the planning task in the underlying SBPL.
msgs::ErrorStatus	planPattern (msgs::StepPlanRequestService::Request &req)
	plans stepping
msgs::ErrorStatus	planSteps (msgs::StepPlanRequestService::Request &req)
	Start a planning task from scratch (will delete information of previous planning tasks). 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, bool ignore_collision=false)
	Sets the goal pose as position of left and right footsteps.
bool	setGoal (const msgs::StepPlanRequest &req, bool ignore_collision=false)
	Sets the goal pose.
void	setPlanner ()
	Sets the planning algorithm used by SBPL.
bool	setStart (const State &left_foot, const State &right_foot, bool ignore_collision=false)
	Sets the start pose as position of left and right footsteps.
bool	setStart (const msgs::StepPlanRequest &req, bool ignore_collision=false)
	Sets the start pose.
void	startPlanning (msgs::StepPlanRequestService::Request &req)
	: starts planning in a new thread
Protected Attributes
EnvironmentParameters::Ptr	env_params
FeedbackCB	feedback_cb
FootPoseTransformer	foot_pose_transformer
std::string	frame_id
bool	goal_pose_set_up
pcl::PointCloud < pcl::PointXYZI >::Ptr	ivCheckedFootContactSupport
ros::Publisher	ivCheckedFootContactSupportPub
State	ivGoalFootLeft
State	ivGoalFootRight
std::vector< State >	ivPath
double	ivPathCost
boost::shared_ptr < FootstepPlannerEnvironment >	ivPlannerEnvironmentPtr
boost::shared_ptr< SBPLPlanner >	ivPlannerPtr
std::vector< int >	ivPlanningStatesIds
State	ivStartFootLeft
State	ivStartFootRight
double	max_number_steps
double	max_path_length_ratio
boost::recursive_mutex	planner_mutex
boost::thread	planning_thread
PreemptCB	preempt_cb
ResultCB	result_cb
unsigned int	start_foot_selection
bool	start_pose_set_up
unsigned int	step_plan_uid

Detailed Description

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

Definition at line 92 of file footstep_planner.h.

Member Typedef Documentation

typedef boost::shared_ptr<const FootstepPlanner> vigir_footstep_planning::FootstepPlanner::ConstPtr

Definition at line 119 of file footstep_planner.h.

typedef boost::function<void (msgs::PlanningFeedback)> vigir_footstep_planning::FootstepPlanner::FeedbackCB

Definition at line 96 of file footstep_planner.h.

typedef boost::function<void ()> vigir_footstep_planning::FootstepPlanner::PreemptCB

Definition at line 97 of file footstep_planner.h.

typedef boost::shared_ptr<FootstepPlanner> vigir_footstep_planning::FootstepPlanner::Ptr

Definition at line 118 of file footstep_planner.h.

typedef boost::function<void (msgs::StepPlanRequestService::Response)> vigir_footstep_planning::FootstepPlanner::ResultCB

Definition at line 95 of file footstep_planner.h.

Constructor & Destructor Documentation

vigir_footstep_planning::FootstepPlanner::FootstepPlanner ( ros::NodeHandle & nh )

Definition at line 5 of file footstep_planner.cpp.

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

Definition at line 28 of file footstep_planner.cpp.

Member Function Documentation

bool vigir_footstep_planning::FootstepPlanner::checkRobotCollision	(	const State &	left_foot,
		const State &	right_foot,
		bool &	left,
		bool &	right
	)		const `[protected]`

Definition at line 1209 of file footstep_planner.cpp.

void vigir_footstep_planning::FootstepPlanner::doPlanning ( msgs::StepPlanRequestService::Request & req ) [protected]

: method used in seperate thread

Definition at line 1096 of file footstep_planner.cpp.

bool vigir_footstep_planning::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 184 of file footstep_planner.cpp.

msgs::ErrorStatus vigir_footstep_planning::FootstepPlanner::finalizeAndAddStepToPlan	(	State &	s,
		const msgs::PatternParameters &	env_params,
		bool	change_z = `true`
	)		`[protected]`

Definition at line 843 of file footstep_planner.cpp.

bool vigir_footstep_planning::FootstepPlanner::finalizeStepPlan	(	msgs::StepPlanRequestService::Request &	req,
		msgs::StepPlanRequestService::Response &	resp,
		bool	post_process
	)		`[protected]`

extracts step plan response from planning result

Definition at line 865 of file footstep_planner.cpp.

bool vigir_footstep_planning::FootstepPlanner::findNearestValidState ( State & s ) const [protected]

Definition at line 1149 of file footstep_planner.cpp.

State vigir_footstep_planning::FootstepPlanner::getFootPose	(	const State &	robot,
		Leg	leg,
		double	dx,
		double	dy,
		double	dyaw
	)		`[protected]`

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

Definition at line 1322 of file footstep_planner.cpp.

State vigir_footstep_planning::FootstepPlanner::getFootPose	(	const State &	robot,
		Leg	leg
	)		`[protected]`

Definition at line 1346 of file footstep_planner.cpp.

size_t vigir_footstep_planning::FootstepPlanner::getNumExpandedStates ( ) const [inline, protected]

Returns:: Number of expanded states.

Definition at line 179 of file footstep_planner.h.

size_t vigir_footstep_planning::FootstepPlanner::getNumFootPoses ( ) const [inline, protected]

Returns:: Number of planned foot poses.

Definition at line 185 of file footstep_planner.h.

state_iter_t vigir_footstep_planning::FootstepPlanner::getPathBegin ( ) const [inline, protected]

Definition at line 187 of file footstep_planner.h.

double vigir_footstep_planning::FootstepPlanner::getPathCosts ( ) const [inline, protected]

Returns:: Costs of the planned footstep path.

Definition at line 176 of file footstep_planner.h.

state_iter_t vigir_footstep_planning::FootstepPlanner::getPathEnd ( ) const [inline, protected]

Definition at line 188 of file footstep_planner.h.

int vigir_footstep_planning::FootstepPlanner::getPathSize ( ) [inline, protected]

Returns:: Size of the planned path.

Definition at line 191 of file footstep_planner.h.

State vigir_footstep_planning::FootstepPlanner::getStartFootLeft ( ) [inline, protected]

Definition at line 193 of file footstep_planner.h.

State vigir_footstep_planning::FootstepPlanner::getStartFootRight ( ) [inline, protected]

Definition at line 194 of file footstep_planner.h.

bool vigir_footstep_planning::FootstepPlanner::isPlanning ( ) const

Definition at line 85 of file footstep_planner.cpp.

bool vigir_footstep_planning::FootstepPlanner::pathExists ( ) [inline, protected]

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

Definition at line 203 of file footstep_planner.h.

bool vigir_footstep_planning::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 1351 of file footstep_planner.cpp.

bool vigir_footstep_planning::FootstepPlanner::plan ( ReplanParams & params ) [protected]

Starts the planning task in the underlying SBPL.

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

: updateGoal adds goal states to list so planner may use it independend from costs...

Definition at line 90 of file footstep_planner.cpp.

msgs::ErrorStatus vigir_footstep_planning::FootstepPlanner::planPattern ( msgs::StepPlanRequestService::Request & req ) [protected]

plans stepping

Definition at line 465 of file footstep_planner.cpp.

msgs::ErrorStatus vigir_footstep_planning::FootstepPlanner::planSteps ( msgs::StepPlanRequestService::Request & req ) [protected]

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.

: Hack to disable collision check for start pose

Definition at line 439 of file footstep_planner.cpp.

void vigir_footstep_planning::FootstepPlanner::preemptPlanning ( )

stops thread running planning

Definition at line 1136 of file footstep_planner.cpp.

void vigir_footstep_planning::FootstepPlanner::reset ( ) [protected]

Reset the previous planning information.

Definition at line 395 of file footstep_planner.cpp.

void vigir_footstep_planning::FootstepPlanner::resetTotally ( ) [protected]

Reset and reinitialize the environment.

Definition at line 415 of file footstep_planner.cpp.

bool vigir_footstep_planning::FootstepPlanner::setGoal	(	const State &	left_foot,
		const State &	right_foot,
		bool	ignore_collision = `false`
	)		`[protected]`

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

Returns:: True if the two foot poses have been set successfully.

Definition at line 1269 of file footstep_planner.cpp.

bool vigir_footstep_planning::FootstepPlanner::setGoal	(	const msgs::StepPlanRequest &	req,
		bool	ignore_collision = `false`
	)		`[protected]`

Sets the goal pose.

Returns:: True if the two foot poses have been set successfully.

Definition at line 1300 of file footstep_planner.cpp.

bool vigir_footstep_planning::FootstepPlanner::setParams ( const vigir_generic_params::ParameterSet & params )

Definition at line 208 of file footstep_planner.cpp.

void vigir_footstep_planning::FootstepPlanner::setPlanner ( ) [protected]

Sets the planning algorithm used by SBPL.

Definition at line 36 of file footstep_planner.cpp.

bool vigir_footstep_planning::FootstepPlanner::setStart	(	const State &	left_foot,
		const State &	right_foot,
		bool	ignore_collision = `false`
	)		`[protected]`

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 1223 of file footstep_planner.cpp.

bool vigir_footstep_planning::FootstepPlanner::setStart	(	const msgs::StepPlanRequest &	req,
		bool	ignore_collision = `false`
	)		`[protected]`

Sets the start pose.

Returns:: True if the two foot poses have been set successfully.

Definition at line 1254 of file footstep_planner.cpp.

void vigir_footstep_planning::FootstepPlanner::startPlanning ( msgs::StepPlanRequestService::Request & req ) [protected]

: starts planning in a new thread

Definition at line 1090 of file footstep_planner.cpp.

msgs::ErrorStatus vigir_footstep_planning::FootstepPlanner::stepPlanRequest	(	msgs::StepPlanRequestService::Request &	req,
		ResultCB	result_cb = `ResultCB()`,
		FeedbackCB	feedback_cb = `FeedbackCB()`,
		PreemptCB	preempt_cb = `PreemptCB()`
	)

Definition at line 1021 of file footstep_planner.cpp.

bool vigir_footstep_planning::FootstepPlanner::stepPlanRequestService	(	msgs::StepPlanRequestService::Request &	req,
		msgs::StepPlanRequestService::Response &	resp
	)

Service handle to plan footsteps.

Definition at line 1073 of file footstep_planner.cpp.

msgs::ErrorStatus vigir_footstep_planning::FootstepPlanner::updateFeet	(	msgs::Feet &	feet,
		uint8_t	mode,
		bool	transform = `true`
	)		const

Definition at line 291 of file footstep_planner.cpp.

msgs::ErrorStatus vigir_footstep_planning::FootstepPlanner::updateFoot	(	msgs::Foot &	foot,
		uint8_t	mode,
		bool	transform = `true`
	)		const

Definition at line 255 of file footstep_planner.cpp.

msgs::ErrorStatus vigir_footstep_planning::FootstepPlanner::updateStepPlan	(	msgs::StepPlan &	result,
		uint8_t	mode,
		const std::string &	param_set_name = `std::string()`,
		bool	transform = `true`
	)		const