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 More...

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. More...

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

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 More...

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. More...

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. More...

msgs::ErrorStatus	planPattern (msgs::StepPlanRequestService::Request &req)
	plans stepping More...

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. 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, bool ignore_collision=false)
	Sets the goal pose as position of left and right footsteps. More...

bool	setGoal (const msgs::StepPlanRequest &req, bool ignore_collision=false)
	Sets the goal pose. More...

void	setPlanner ()
	Sets the planning algorithm used by SBPL. More...

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. More...

bool	setStart (const msgs::StepPlanRequest &req, bool ignore_collision=false)
	Sets the start pose. More...

void	startPlanning (msgs::StepPlanRequestService::Request &req)
	: starts planning in a new thread More...

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

inlineprotected

Returns: Number of expanded states.

Definition at line 179 of file footstep_planner.h.

size_t vigir_footstep_planning::FootstepPlanner::getNumFootPoses ( ) const

inlineprotected

Returns: Number of planned foot poses.

Definition at line 185 of file footstep_planner.h.

state_iter_t vigir_footstep_planning::FootstepPlanner::getPathBegin ( ) const

inlineprotected

Definition at line 187 of file footstep_planner.h.

double vigir_footstep_planning::FootstepPlanner::getPathCosts ( ) const

inlineprotected

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

inlineprotected

Definition at line 188 of file footstep_planner.h.

int vigir_footstep_planning::FootstepPlanner::getPathSize ( )

inlineprotected

Returns: Size of the planned path.

Definition at line 191 of file footstep_planner.h.

State vigir_footstep_planning::FootstepPlanner::getStartFootLeft ( )

inlineprotected

Definition at line 193 of file footstep_planner.h.

State vigir_footstep_planning::FootstepPlanner::getStartFootRight ( )

inlineprotected

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 ( )

inlineprotected

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

Definition at line 316 of file footstep_planner.cpp.

Member Data Documentation

EnvironmentParameters::Ptr vigir_footstep_planning::FootstepPlanner::env_params

protected

Definition at line 257 of file footstep_planner.h.

FeedbackCB vigir_footstep_planning::FootstepPlanner::feedback_cb

protected

Definition at line 239 of file footstep_planner.h.

FootPoseTransformer vigir_footstep_planning::FootstepPlanner::foot_pose_transformer

protected

Definition at line 253 of file footstep_planner.h.

std::string vigir_footstep_planning::FootstepPlanner::frame_id

protected

Definition at line 256 of file footstep_planner.h.

bool vigir_footstep_planning::FootstepPlanner::goal_pose_set_up

protected

Definition at line 259 of file footstep_planner.h.

pcl::PointCloud<pcl::PointXYZI>::Ptr vigir_footstep_planning::FootstepPlanner::ivCheckedFootContactSupport

protected

Definition at line 270 of file footstep_planner.h.

ros::Publisher vigir_footstep_planning::FootstepPlanner::ivCheckedFootContactSupportPub

protected

Definition at line 233 of file footstep_planner.h.

State vigir_footstep_planning::FootstepPlanner::ivGoalFootLeft

protected

Definition at line 249 of file footstep_planner.h.

State vigir_footstep_planning::FootstepPlanner::ivGoalFootRight

protected

Definition at line 250 of file footstep_planner.h.

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

protected

Definition at line 245 of file footstep_planner.h.

double vigir_footstep_planning::FootstepPlanner::ivPathCost

protected

Definition at line 266 of file footstep_planner.h.

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

protected

Definition at line 242 of file footstep_planner.h.

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

protected

Definition at line 243 of file footstep_planner.h.

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

protected

Definition at line 268 of file footstep_planner.h.

State vigir_footstep_planning::FootstepPlanner::ivStartFootLeft

protected

Definition at line 247 of file footstep_planner.h.

State vigir_footstep_planning::FootstepPlanner::ivStartFootRight

protected

Definition at line 248 of file footstep_planner.h.

double vigir_footstep_planning::FootstepPlanner::max_number_steps

protected

Definition at line 261 of file footstep_planner.h.

double vigir_footstep_planning::FootstepPlanner::max_path_length_ratio

protected

Definition at line 264 of file footstep_planner.h.

boost::recursive_mutex vigir_footstep_planning::FootstepPlanner::planner_mutex

mutableprotected

Definition at line 235 of file footstep_planner.h.

boost::thread vigir_footstep_planning::FootstepPlanner::planning_thread

protected

Definition at line 236 of file footstep_planner.h.

PreemptCB vigir_footstep_planning::FootstepPlanner::preempt_cb

protected

Definition at line 240 of file footstep_planner.h.

ResultCB vigir_footstep_planning::FootstepPlanner::result_cb

protected

Definition at line 238 of file footstep_planner.h.

unsigned int vigir_footstep_planning::FootstepPlanner::start_foot_selection

protected

Definition at line 258 of file footstep_planner.h.

bool vigir_footstep_planning::FootstepPlanner::start_pose_set_up

protected

Definition at line 259 of file footstep_planner.h.

unsigned int vigir_footstep_planning::FootstepPlanner::step_plan_uid

protected

Definition at line 273 of file footstep_planner.h.

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

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation