Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
footstep_planner::FootstepNavigation Class Reference

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_legThe current support leg of the robot. Used to calculate the footstep necessary to reach the calculated path.
starting_step_numIndex 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
footstepThe 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:


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Mon Jun 10 2019 13:38:25