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

void	mapCallback (const nav_msgs::OccupancyGridConstPtr &occupancy_map)
	Callback to set the map. More...

bool	setGoal (const geometry_msgs::PoseStampedConstPtr goal_pose)
	Wrapper for FootstepPlanner::setGoal. More...

bool	setGoal (float x, float y, float theta)
	Wrapper for FootstepPlanner::setGoal. More...

virtual	~FootstepNavigation ()

Protected Member Functions
void	activeCallback ()
	Called from within ROS' actionlib at the start of a new goal request. More...

void	doneCallback (const actionlib::SimpleClientGoalState &state, const humanoid_nav_msgs::ExecFootstepsResultConstPtr &result)
	Called from within ROS' actionlib at the end of a goal request. More...

void	executeFootsteps ()
	Executes footsteps as boost::thread. More...

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

void	feedbackCallback (const humanoid_nav_msgs::ExecFootstepsFeedbackConstPtr &fb)
	Called from within ROS' actionlib during the execution of a goal request. More...

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

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

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

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

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

void	startExecution ()
	Starts the execution of the calculated path. More...

bool	updateStart ()
	Updates the robot's current pose. More...

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

boost::mutex	ivExecutionLock

const int	ivExecutionShift
	Fixed delay (=2) of the incoming footsteps. More...

double	ivFeedbackFrequency
	The rate the action server sends its feedback. More...

boost::shared_ptr< boost::thread >	ivFootstepExecutionPtr

actionlib::SimpleActionClient< humanoid_nav_msgs::ExecFootstepsAction >	ivFootstepsExecution
	Simple action client to control a footstep execution. More...

ros::ServiceClient	ivFootstepSrv

bool	ivForwardSearch
	Search direction. More...

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

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.

Returns: Success of the planning.

Definition at line 132 of file FootstepNavigation.cpp.

bool footstep_planner::FootstepNavigation::setGoal ( const geometry_msgs::PoseStampedConstPtr goal_pose )

Wrapper for FootstepPlanner::setGoal.

Definition at line 503 of file FootstepNavigation.cpp.

bool footstep_planner::FootstepNavigation::setGoal	(	float	x,
		float	y,
		float	theta
	)

Wrapper for FootstepPlanner::setGoal.

Definition at line 512 of file FootstepNavigation.cpp.

void footstep_planner::FootstepNavigation::startExecution ( )

protected

Starts the execution of the calculated path.

Definition at line 166 of file FootstepNavigation.cpp.

bool footstep_planner::FootstepNavigation::updateStart ( )

protected

Updates the robot's current pose.

Definition at line 519 of file FootstepNavigation.cpp.

Member Data Documentation

double footstep_planner::FootstepNavigation::ivAccuracyTheta

protected

Definition at line 210 of file FootstepNavigation.h.

double footstep_planner::FootstepNavigation::ivAccuracyX

protected

Definition at line 208 of file FootstepNavigation.h.

double footstep_planner::FootstepNavigation::ivAccuracyY

protected

Definition at line 209 of file FootstepNavigation.h.

double footstep_planner::FootstepNavigation::ivCellSize

protected

Definition at line 211 of file FootstepNavigation.h.

ros::ServiceClient footstep_planner::FootstepNavigation::ivClipFootstepSrv

protected

Definition at line 196 of file FootstepNavigation.h.

int footstep_planner::FootstepNavigation::ivControlStepIdx

protected

Index to keep track of the currently executed footstep and the currently observed one.

Definition at line 234 of file FootstepNavigation.h.

bool footstep_planner::FootstepNavigation::ivExecutingFootsteps

protected

Used to lock the calculation and execution of footsteps.

Definition at line 218 of file FootstepNavigation.h.

boost::mutex footstep_planner::FootstepNavigation::ivExecutionLock

protected

Definition at line 200 of file FootstepNavigation.h.

const int footstep_planner::FootstepNavigation::ivExecutionShift

protected

Fixed delay (=2) of the incoming footsteps.

Definition at line 228 of file FootstepNavigation.h.

double footstep_planner::FootstepNavigation::ivFeedbackFrequency

protected

The rate the action server sends its feedback.

Definition at line 221 of file FootstepNavigation.h.

boost::shared_ptr<boost::thread> footstep_planner::FootstepNavigation::ivFootstepExecutionPtr

protected

Definition at line 202 of file FootstepNavigation.h.

actionlib::SimpleActionClient< humanoid_nav_msgs::ExecFootstepsAction> footstep_planner::FootstepNavigation::ivFootstepsExecution

protected

Simple action client to control a footstep execution.

Definition at line 225 of file FootstepNavigation.h.

ros::ServiceClient footstep_planner::FootstepNavigation::ivFootstepSrv

protected

Definition at line 195 of file FootstepNavigation.h.

bool footstep_planner::FootstepNavigation::ivForwardSearch

protected

Search direction.

Definition at line 215 of file FootstepNavigation.h.

ros::Subscriber footstep_planner::FootstepNavigation::ivGoalPoseSub

protected

Definition at line 193 of file FootstepNavigation.h.

ros::Subscriber footstep_planner::FootstepNavigation::ivGridMapSub

protected

Definition at line 191 of file FootstepNavigation.h.

std::string footstep_planner::FootstepNavigation::ivIdFootLeft

protected

Definition at line 205 of file FootstepNavigation.h.

std::string footstep_planner::FootstepNavigation::ivIdFootRight

protected

Definition at line 204 of file FootstepNavigation.h.

std::string footstep_planner::FootstepNavigation::ivIdMapFrame

protected

Definition at line 206 of file FootstepNavigation.h.

double footstep_planner::FootstepNavigation::ivMaxInvStepTheta

protected

Definition at line 250 of file FootstepNavigation.h.

double footstep_planner::FootstepNavigation::ivMaxInvStepX

protected

Definition at line 248 of file FootstepNavigation.h.

double footstep_planner::FootstepNavigation::ivMaxInvStepY

protected

Definition at line 249 of file FootstepNavigation.h.

double footstep_planner::FootstepNavigation::ivMaxStepTheta

protected

Definition at line 247 of file FootstepNavigation.h.

double footstep_planner::FootstepNavigation::ivMaxStepX

protected

Definition at line 245 of file FootstepNavigation.h.

double footstep_planner::FootstepNavigation::ivMaxStepY

protected

Definition at line 246 of file FootstepNavigation.h.

int footstep_planner::FootstepNavigation::ivNumAngleBins

protected

Definition at line 212 of file FootstepNavigation.h.

FootstepPlanner footstep_planner::FootstepNavigation::ivPlanner

protected

Definition at line 189 of file FootstepNavigation.h.

int footstep_planner::FootstepNavigation::ivResetStepIdx

protected

Index used to keep track of the currently observed footstep after replanning.

Definition at line 240 of file FootstepNavigation.h.

ros::Subscriber footstep_planner::FootstepNavigation::ivRobotPoseSub

protected

Definition at line 192 of file FootstepNavigation.h.

bool footstep_planner::FootstepNavigation::ivSafeExecution

protected

Whether to use the slower but more cautious execution or not.

Definition at line 243 of file FootstepNavigation.h.

std::vector<std::pair<double, double> > footstep_planner::FootstepNavigation::ivStepRange

protected

Definition at line 252 of file FootstepNavigation.h.

tf::TransformListener footstep_planner::FootstepNavigation::ivTransformListener

protected

Definition at line 198 of file FootstepNavigation.h.

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

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation