A class to control the performance of a planned footstep path on the NAO robot. More...

#include <FootstepNavigation.h>

Public Member Functions
	FootstepNavigation ()
void	goalPoseCallback (const geometry_msgs::PoseStampedConstPtr &goal_pose)
	Callback to set a simulated robot at a certain pose.
void	mapCallback (const nav_msgs::OccupancyGridConstPtr &occupancy_map)
	Callback to set the map.
bool	setGoal (const geometry_msgs::PoseStampedConstPtr goal_pose)
	Wrapper for FootstepPlanner::setGoal.
bool	setGoal (float x, float y, float theta)
	Wrapper for FootstepPlanner::setGoal.
virtual	~FootstepNavigation ()
Protected Member Functions
void	activeCallback ()
	Called from within ROS' actionlib at the start of a new goal request.
void	doneCallback (const actionlib::SimpleClientGoalState &state, const humanoid_nav_msgs::ExecFootstepsResultConstPtr &result)
	Called from within ROS' actionlib at the end of a goal request.
void	executeFootsteps ()
	Executes footsteps as boost::thread.
void	executeFootstepsFast ()
	Alternative (and more fluid) execution of footsteps using ROS' actionlib.
void	feedbackCallback (const humanoid_nav_msgs::ExecFootstepsFeedbackConstPtr &fb)
	Called from within ROS' actionlib during the execution of a goal request.
bool	getFootstep (const tf::Pose &from, const State &from_planned, const State &to, humanoid_nav_msgs::StepTarget *footstep)
	Calculates the footstep necessary to reach 'to' from within 'from'.
bool	getFootstepsFromPath (const State &current_support_leg, int starting_step_num, std::vector< humanoid_nav_msgs::StepTarget > &footsteps)
	Extracts the footsteps necessary to perform the calculated path.
bool	getFootTransform (const std::string &foot_id, const std::string &world_frame_id, const ros::Time &time, const ros::Duration &waiting_time, tf::Transform *foot)
	Obtains the pose of the robot's foot from tf.
bool	performable (const humanoid_nav_msgs::StepTarget &footstep)
bool	performable (float step_x, float step_y)
bool	performanceValid (const humanoid_nav_msgs::ClipFootstep &footstep)
bool	performanceValid (const State &planned, const State &executed)
bool	performanceValid (float a_x, float a_y, float a_theta, float b_x, float b_y, float b_theta)
bool	plan ()
	Starts the planning task from scratch discarding previous planning information.
bool	replan ()
	Starts the planning task. First FootstepPlanner::replan() is called to use planning information from previous tasks. If this fails FootstepPlanner::plan() is called to plan from scratch. Otherwise the planning task is unsuccessful.
void	startExecution ()
	Starts the execution of the calculated path.
bool	updateStart ()
	Updates the robot's current pose.
Protected Attributes
double	ivAccuracyTheta
double	ivAccuracyX
double	ivAccuracyY
double	ivCellSize
ros::ServiceClient	ivClipFootstepSrv
int	ivControlStepIdx
bool	ivExecutingFootsteps
	Used to lock the calculation and execution of footsteps.
boost::mutex	ivExecutionLock
const int	ivExecutionShift
	Fixed delay (=2) of the incoming footsteps.
double	ivFeedbackFrequency
	The rate the action server sends its feedback.
boost::shared_ptr< boost::thread >	ivFootstepExecutionPtr
actionlib::SimpleActionClient < humanoid_nav_msgs::ExecFootstepsAction >	ivFootstepsExecution
	Simple action client to control a footstep execution.
ros::ServiceClient	ivFootstepSrv
bool	ivForwardSearch
	Search direction.
ros::Subscriber	ivGoalPoseSub
ros::Subscriber	ivGridMapSub
std::string	ivIdFootLeft
std::string	ivIdFootRight
std::string	ivIdMapFrame
double	ivMaxInvStepTheta
double	ivMaxInvStepX
double	ivMaxInvStepY
double	ivMaxStepTheta
double	ivMaxStepX
double	ivMaxStepY
int	ivNumAngleBins
FootstepPlanner	ivPlanner
int	ivResetStepIdx
ros::Subscriber	ivRobotPoseSub
bool	ivSafeExecution
	Whether to use the slower but more cautious execution or not.
std::vector< std::pair< double, double > >	ivStepRange
tf::TransformListener	ivTransformListener

Detailed Description

A class to control the performance of a planned footstep path on the NAO robot.

Definition at line 50 of file FootstepNavigation.h.

Constructor & Destructor Documentation

footstep_planner::FootstepNavigation::FootstepNavigation ( )

Definition at line 26 of file FootstepNavigation.cpp.

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

Definition at line 108 of file FootstepNavigation.cpp.

Member Function Documentation

void footstep_planner::FootstepNavigation::activeCallback ( ) [protected]

Called from within ROS' actionlib at the start of a new goal request.

Definition at line 306 of file FootstepNavigation.cpp.

void footstep_planner::FootstepNavigation::doneCallback	(	const actionlib::SimpleClientGoalState &	state,
		const humanoid_nav_msgs::ExecFootstepsResultConstPtr &	result
	)		`[protected]`

Called from within ROS' actionlib at the end of a goal request.

Definition at line 316 of file FootstepNavigation.cpp.

void footstep_planner::FootstepNavigation::executeFootsteps ( ) [protected]

Executes footsteps as boost::thread.

Definition at line 183 of file FootstepNavigation.cpp.

void footstep_planner::FootstepNavigation::executeFootstepsFast ( ) [protected]

Alternative (and more fluid) execution of footsteps using ROS' actionlib.

Definition at line 264 of file FootstepNavigation.cpp.

void footstep_planner::FootstepNavigation::feedbackCallback ( const humanoid_nav_msgs::ExecFootstepsFeedbackConstPtr & fb ) [protected]

Called from within ROS' actionlib during the execution of a goal request.

Definition at line 334 of file FootstepNavigation.cpp.

bool footstep_planner::FootstepNavigation::getFootstep	(	const tf::Pose &	from,
		const State &	from_planned,
		const State &	to,
		humanoid_nav_msgs::StepTarget *	footstep
	)		`[protected]`

Calculates the footstep necessary to reach 'to' from within 'from'.

Returns:: True if the footstep can be performed by the NAO robot.

Definition at line 559 of file FootstepNavigation.cpp.

bool footstep_planner::FootstepNavigation::getFootstepsFromPath	(	const State &	current_support_leg,
		int	starting_step_num,
		std::vector< humanoid_nav_msgs::StepTarget > &	footsteps
	)		`[protected]`

Extracts the footsteps necessary to perform the calculated path.

Parameters:

current_support_leg	The current support leg of the robot. Used to calculate the footstep necessary to reach the calculated path.
starting_step_num	Index of the state in the path which has to be reached first. Usually this is 1 (since state 0 is the current support leg of the robot) but for readjustments indices different from 1 are necessary.

Returns:: False if an extracted footstep is invalid.

Definition at line 636 of file FootstepNavigation.cpp.

bool footstep_planner::FootstepNavigation::getFootTransform	(	const std::string &	foot_id,
		const std::string &	world_frame_id,
		const ros::Time &	time,
		const ros::Duration &	waiting_time,
		tf::Transform *	foot
	)		`[protected]`

Obtains the pose of the robot's foot from tf.

Returns:: True if transformation has been received.

Definition at line 670 of file FootstepNavigation.cpp.

void footstep_planner::FootstepNavigation::goalPoseCallback ( const geometry_msgs::PoseStampedConstPtr & goal_pose )

Callback to set a simulated robot at a certain pose.

Subscribed to 'goal'.

Definition at line 449 of file FootstepNavigation.cpp.

void footstep_planner::FootstepNavigation::mapCallback ( const nav_msgs::OccupancyGridConstPtr & occupancy_map )

Callback to set the map.

Subscribed to 'map'.

Definition at line 473 of file FootstepNavigation.cpp.

bool footstep_planner::FootstepNavigation::performable ( const humanoid_nav_msgs::StepTarget & footstep ) [protected]

Definition at line 732 of file FootstepNavigation.cpp.

bool footstep_planner::FootstepNavigation::performable	(	float	step_x,
		float	step_y
	)		`[protected]`

Definition at line 759 of file FootstepNavigation.cpp.

bool footstep_planner::FootstepNavigation::performanceValid ( const humanoid_nav_msgs::ClipFootstep & footstep ) [protected]

Parameters:

footstep The response from the clip footstep service (i.e. it contains the unclipped (calculated) step and the clipped (performable) step).

Returns:: True if the footstep can be performed by the robot (i.e. it is within the robot's max ranges).

Definition at line 709 of file FootstepNavigation.cpp.

bool footstep_planner::FootstepNavigation::performanceValid	(	const State &	planned,
		const State &	executed
	)		`[protected]`

Returns:: True if both states are equal upon some accuracy.

Definition at line 722 of file FootstepNavigation.cpp.

bool footstep_planner::FootstepNavigation::performanceValid	(	float	a_x,
		float	a_y,
		float	a_theta,
		float	b_x,
		float	b_y,
		float	b_theta
	)		`[protected]`

Returns:: True if both states are equal upon some accuracy.

Definition at line 698 of file FootstepNavigation.cpp.

bool footstep_planner::FootstepNavigation::plan ( ) [protected]

Starts the planning task from scratch discarding previous planning information.

Returns:: Success of the planning.

Definition at line 113 of file FootstepNavigation.cpp.

bool footstep_planner::FootstepNavigation::replan ( ) [protected]

Starts the planning task. First FootstepPlanner::replan() is called to use planning information from previous tasks. If this fails FootstepPlanner::plan() is called to plan from scratch. Otherwise the planning task is unsuccessful.