Public Types | Public Member Functions | Protected Attributes | Private Attributes | Friends | List of all members
jsk_footstep_planner::FootstepGraph Class Reference

#include <footstep_graph.h>

Inheritance diagram for jsk_footstep_planner::FootstepGraph:
Inheritance graph
[legend]

Public Types

typedef boost::function< double(StatePtr, StatePtr, double) > PathCostFunction
 
typedef boost::shared_ptr< FootstepGraphPtr
 
typedef boost::function< bool(StatePtr target_state, std::vector< StatePtr > &) > SuccessorFunction
 
- Public Types inherited from jsk_footstep_planner::Graph< FootstepState >
typedef boost::shared_ptr< GraphPtr
 
typedef boost::shared_ptr< StateTStatePtr
 
typedef FootstepState StateT
 

Public Member Functions

virtual void clearPerceptionDuration ()
 
bool finalizeSteps (const StatePtr &last_1_Step, const StatePtr &lastStep, std::vector< StatePtr > &finalizeSteps)
 
 FootstepGraph (const Eigen::Vector3f &resolution, const bool use_pointcloud_model=false, const bool lazy_projection=true, const bool local_movement=false, const bool use_obstacle_model=false)
 
virtual TransitionLimit::Ptr getGlobalTransitionLimit ()
 
virtual FootstepState::Ptr getGoal (int leg)
 
virtual ros::WallDuration getPerceptionDuration ()
 
virtual pcl::PointIndices::Ptr getPointIndicesCollidingSphere (const Eigen::Affine3f &c)
 
virtual Eigen::Affine3f getRobotCoords (StatePtr current_state, StatePtr previous_state) const
 Compute robot coords from current footstep and previous footstep. R_robot = midcoords(F_current, F_previous) * R_offset;. More...
 
virtual TransitionLimit::Ptr getTransitionLimit ()
 
virtual std::string infoString () const
 return string about graph information. More...
 
virtual bool isColliding (StatePtr current_state, StatePtr previous_state)
 return True if current_state collides with obstacle. More...
 
virtual bool isCollidingBox (const Eigen::Affine3f &c, pcl::PointIndices::Ptr candidates) const
 
virtual bool isGoal (StatePtr state)
 
virtual bool isSuccessable (StatePtr current_state, StatePtr previous_state)
 
virtual bool lazyProjection () const
 
virtual std::vector< FootstepState::PtrlocalMoveFootstepState (FootstepState::Ptr in)
 
virtual bool localMovement () const
 
virtual double maxSuccessorDistance ()
 
virtual double maxSuccessorRotation ()
 
double path_cost_original (StatePtr from, StatePtr to, double prev_cost)
 
virtual double pathCost (StatePtr from, StatePtr to, double prev_cost)
 
virtual FootstepState::Ptr projectFootstep (FootstepState::Ptr in)
 
virtual FootstepState::Ptr projectFootstep (FootstepState::Ptr in, unsigned int &state)
 
virtual bool projectGoal ()
 
virtual bool projectStart ()
 
virtual void setBasicSuccessors (std::vector< Eigen::Affine3f > left_to_right_successors)
 
virtual void setCollisionBBoxOffset (const Eigen::Affine3f &offset)
 
virtual void setCollisionBBoxSize (const Eigen::Vector3f &size)
 
virtual void setGlobalTransitionLimit (TransitionLimit::Ptr limit)
 
virtual void setGoalState (FootstepState::Ptr left, FootstepState::Ptr right)
 
void setHeuristicPathLine (jsk_recognition_utils::PolyLine &path_line)
 
virtual void setLeftGoalState (FootstepState::Ptr goal)
 
virtual void setObstacleModel (pcl::PointCloud< pcl::PointXYZ >::Ptr model)
 
virtual void setParameters (FootstepParameters &p)
 
virtual void setPathCostFunction (PathCostFunction p)
 
virtual void setPointCloudModel (pcl::PointCloud< pcl::PointNormal >::Ptr model)
 
virtual void setProgressPublisher (ros::NodeHandle &nh, std::string topic)
 
virtual void setRightGoalState (FootstepState::Ptr goal)
 
virtual void setSuccessorFunction (SuccessorFunction s)
 
virtual void setTransitionLimit (TransitionLimit::Ptr limit)
 
virtual std::vector< StatePtrsuccessors (StatePtr target_state)
 
bool successors_original (StatePtr target_state, std::vector< FootstepGraph::StatePtr > &ret)
 
virtual bool useObstacleModel () const
 
virtual bool usePointCloudModel () const
 
- Public Member Functions inherited from jsk_footstep_planner::Graph< FootstepState >
virtual void addNode (StatePtr state)
 
virtual StatePtr getGoalState ()
 
virtual StatePtr getStartState ()
 
 Graph ()
 
virtual size_t numNodes ()
 
virtual void setGoalState (StatePtr goal)
 
virtual void setStartState (StatePtr start)
 

Protected Attributes

Eigen::Affine3f collision_bbox_offset_
 
Eigen::Vector3f collision_bbox_size_
 
TransitionLimit::Ptr global_transition_limit_
 
ANNGrid::Ptr grid_search_
 
jsk_recognition_utils::PolyLine::Ptr heuristic_path_
 
const bool lazy_projection_
 
FootstepState::Ptr left_goal_state_
 
const bool local_movement_
 
double max_successor_distance_
 
double max_successor_rotation_
 
pcl::PointCloud< pcl::PointXYZ >::Ptr obstacle_model_
 
pcl::KdTreeFLANN< pcl::PointXYZ >::Ptr obstacle_tree_model_
 
FootstepParameters parameters_
 
ros::WallDuration perception_duration_
 
pcl::PointCloud< pcl::PointNormal >::Ptr pointcloud_model_
 
pcl::PointCloud< pcl::PointNormal >::Ptr pointcloud_model_2d_
 
ros::Publisher pub_progress_
 
bool publish_progress_
 
const Eigen::Vector3f resolution_
 
FootstepState::Ptr right_goal_state_
 
std::vector< Eigen::Affine3f > successors_from_left_to_right_
 
std::vector< Eigen::Affine3f > successors_from_right_to_left_
 
TransitionLimit::Ptr transition_limit_
 
pcl::KdTreeFLANN< pcl::PointNormal >::Ptr tree_model_
 
pcl::search::Octree< pcl::PointNormal >::Ptr tree_model_2d_
 
const bool use_obstacle_model_
 
const bool use_pointcloud_model_
 
FootstepState::Ptr zero_state_
 zero_state is used only for global transition limit More...
 
- Protected Attributes inherited from jsk_footstep_planner::Graph< FootstepState >
StatePtr goal_state_
 
std::vector< StatePtrnodes_
 
StatePtr start_state_
 

Private Attributes

PathCostFunction path_cost_func_
 
SuccessorFunction successor_func_
 

Friends

double footstepHeuristicFollowPathLine (SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
 

Detailed Description

Definition at line 54 of file footstep_graph.h.

Member Typedef Documentation

typedef boost::function< double(StatePtr, StatePtr, double) > jsk_footstep_planner::FootstepGraph::PathCostFunction

Definition at line 59 of file footstep_graph.h.

Definition at line 57 of file footstep_graph.h.

typedef boost::function< bool(StatePtr target_state, std::vector<StatePtr> &) > jsk_footstep_planner::FootstepGraph::SuccessorFunction

Definition at line 58 of file footstep_graph.h.

Constructor & Destructor Documentation

jsk_footstep_planner::FootstepGraph::FootstepGraph ( const Eigen::Vector3f &  resolution,
const bool  use_pointcloud_model = false,
const bool  lazy_projection = true,
const bool  local_movement = false,
const bool  use_obstacle_model = false 
)
inline

Definition at line 63 of file footstep_graph.h.

Member Function Documentation

virtual void jsk_footstep_planner::FootstepGraph::clearPerceptionDuration ( )
inlinevirtual

Definition at line 211 of file footstep_graph.h.

bool jsk_footstep_planner::FootstepGraph::finalizeSteps ( const StatePtr last_1_Step,
const StatePtr lastStep,
std::vector< StatePtr > &  finalizeSteps 
)

Definition at line 154 of file footstep_graph.cpp.

virtual TransitionLimit::Ptr jsk_footstep_planner::FootstepGraph::getGlobalTransitionLimit ( )
inlinevirtual

Definition at line 207 of file footstep_graph.h.

virtual FootstepState::Ptr jsk_footstep_planner::FootstepGraph::getGoal ( int  leg)
inlinevirtual

Definition at line 143 of file footstep_graph.h.

virtual ros::WallDuration jsk_footstep_planner::FootstepGraph::getPerceptionDuration ( )
inlinevirtual

Definition at line 210 of file footstep_graph.h.

pcl::PointIndices::Ptr jsk_footstep_planner::FootstepGraph::getPointIndicesCollidingSphere ( const Eigen::Affine3f &  c)
virtual

Definition at line 107 of file footstep_graph.cpp.

Eigen::Affine3f jsk_footstep_planner::FootstepGraph::getRobotCoords ( StatePtr  current_state,
StatePtr  previous_state 
) const
virtual

Compute robot coords from current footstep and previous footstep. R_robot = midcoords(F_current, F_previous) * R_offset;.

Definition at line 101 of file footstep_graph.cpp.

virtual TransitionLimit::Ptr jsk_footstep_planner::FootstepGraph::getTransitionLimit ( )
inlinevirtual

Definition at line 205 of file footstep_graph.h.

std::string jsk_footstep_planner::FootstepGraph::infoString ( ) const
virtual

return string about graph information.

Definition at line 168 of file footstep_graph.cpp.

bool jsk_footstep_planner::FootstepGraph::isColliding ( StatePtr  current_state,
StatePtr  previous_state 
)
virtual

return True if current_state collides with obstacle.

Definition at line 137 of file footstep_graph.cpp.

bool jsk_footstep_planner::FootstepGraph::isCollidingBox ( const Eigen::Affine3f &  c,
pcl::PointIndices::Ptr  candidates 
) const
virtual

Definition at line 118 of file footstep_graph.cpp.

bool jsk_footstep_planner::FootstepGraph::isGoal ( StatePtr  state)
virtual

Implements jsk_footstep_planner::Graph< FootstepState >.

Definition at line 71 of file footstep_graph.cpp.

bool jsk_footstep_planner::FootstepGraph::isSuccessable ( StatePtr  current_state,
StatePtr  previous_state 
)
virtual

Definition at line 214 of file footstep_graph.cpp.

virtual bool jsk_footstep_planner::FootstepGraph::lazyProjection ( ) const
inlinevirtual

Definition at line 200 of file footstep_graph.h.

std::vector< FootstepState::Ptr > jsk_footstep_planner::FootstepGraph::localMoveFootstepState ( FootstepState::Ptr  in)
virtual

Definition at line 233 of file footstep_graph.cpp.

virtual bool jsk_footstep_planner::FootstepGraph::localMovement ( ) const
inlinevirtual

Definition at line 201 of file footstep_graph.h.

virtual double jsk_footstep_planner::FootstepGraph::maxSuccessorDistance ( )
inlinevirtual

Definition at line 156 of file footstep_graph.h.

virtual double jsk_footstep_planner::FootstepGraph::maxSuccessorRotation ( )
inlinevirtual

Definition at line 160 of file footstep_graph.h.

double jsk_footstep_planner::FootstepGraph::path_cost_original ( StatePtr  from,
StatePtr  to,
double  prev_cost 
)
inline

Definition at line 223 of file footstep_graph.h.

virtual double jsk_footstep_planner::FootstepGraph::pathCost ( StatePtr  from,
StatePtr  to,
double  prev_cost 
)
inlinevirtual

Reimplemented from jsk_footstep_planner::Graph< FootstepState >.

Definition at line 96 of file footstep_graph.h.

FootstepState::Ptr jsk_footstep_planner::FootstepGraph::projectFootstep ( FootstepState::Ptr  in)
virtual

Definition at line 324 of file footstep_graph.cpp.

FootstepState::Ptr jsk_footstep_planner::FootstepGraph::projectFootstep ( FootstepState::Ptr  in,
unsigned int &  state 
)
virtual

Definition at line 330 of file footstep_graph.cpp.

bool jsk_footstep_planner::FootstepGraph::projectGoal ( )
virtual

Definition at line 350 of file footstep_graph.cpp.

bool jsk_footstep_planner::FootstepGraph::projectStart ( )
virtual

Definition at line 372 of file footstep_graph.cpp.

void jsk_footstep_planner::FootstepGraph::setBasicSuccessors ( std::vector< Eigen::Affine3f >  left_to_right_successors)
virtual

Definition at line 41 of file footstep_graph.cpp.

virtual void jsk_footstep_planner::FootstepGraph::setCollisionBBoxOffset ( const Eigen::Affine3f &  offset)
inlinevirtual

Definition at line 213 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setCollisionBBoxSize ( const Eigen::Vector3f &  size)
inlinevirtual

Definition at line 214 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setGlobalTransitionLimit ( TransitionLimit::Ptr  limit)
inlinevirtual

Definition at line 206 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setGoalState ( FootstepState::Ptr  left,
FootstepState::Ptr  right 
)
inlinevirtual

Definition at line 117 of file footstep_graph.h.

void jsk_footstep_planner::FootstepGraph::setHeuristicPathLine ( jsk_recognition_utils::PolyLine path_line)
inline

Definition at line 227 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setLeftGoalState ( FootstepState::Ptr  goal)
inlinevirtual

Definition at line 124 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setObstacleModel ( pcl::PointCloud< pcl::PointXYZ >::Ptr  model)
inlinevirtual

Definition at line 189 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setParameters ( FootstepParameters p)
inlinevirtual

Definition at line 203 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setPathCostFunction ( PathCostFunction  p)
inlinevirtual

Definition at line 219 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setPointCloudModel ( pcl::PointCloud< pcl::PointNormal >::Ptr  model)
inlinevirtual

Definition at line 171 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setProgressPublisher ( ros::NodeHandle nh,
std::string  topic 
)
inlinevirtual

Definition at line 165 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setRightGoalState ( FootstepState::Ptr  goal)
inlinevirtual

Definition at line 129 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setSuccessorFunction ( SuccessorFunction  s)
inlinevirtual

Definition at line 216 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setTransitionLimit ( TransitionLimit::Ptr  limit)
inlinevirtual

Definition at line 204 of file footstep_graph.h.

virtual std::vector<StatePtr> jsk_footstep_planner::FootstepGraph::successors ( StatePtr  target_state)
inlinevirtual

Implements jsk_footstep_planner::Graph< FootstepState >.

Definition at line 88 of file footstep_graph.h.

bool jsk_footstep_planner::FootstepGraph::successors_original ( StatePtr  target_state,
std::vector< FootstepGraph::StatePtr > &  ret 
)

Definition at line 274 of file footstep_graph.cpp.

virtual bool jsk_footstep_planner::FootstepGraph::useObstacleModel ( ) const
inlinevirtual

Definition at line 198 of file footstep_graph.h.

virtual bool jsk_footstep_planner::FootstepGraph::usePointCloudModel ( ) const
inlinevirtual

Definition at line 199 of file footstep_graph.h.

Friends And Related Function Documentation

double footstepHeuristicFollowPathLine ( SolverNode< FootstepState, FootstepGraph >::Ptr  node,
FootstepGraph::Ptr  graph 
)
friend

Member Data Documentation

Eigen::Affine3f jsk_footstep_planner::FootstepGraph::collision_bbox_offset_
protected

Definition at line 249 of file footstep_graph.h.

Eigen::Vector3f jsk_footstep_planner::FootstepGraph::collision_bbox_size_
protected

Definition at line 250 of file footstep_graph.h.

TransitionLimit::Ptr jsk_footstep_planner::FootstepGraph::global_transition_limit_
protected

Definition at line 263 of file footstep_graph.h.

ANNGrid::Ptr jsk_footstep_planner::FootstepGraph::grid_search_
protected

Definition at line 239 of file footstep_graph.h.

jsk_recognition_utils::PolyLine::Ptr jsk_footstep_planner::FootstepGraph::heuristic_path_
protected

Definition at line 269 of file footstep_graph.h.

const bool jsk_footstep_planner::FootstepGraph::lazy_projection_
protected

Definition at line 257 of file footstep_graph.h.

FootstepState::Ptr jsk_footstep_planner::FootstepGraph::left_goal_state_
protected

Definition at line 242 of file footstep_graph.h.

const bool jsk_footstep_planner::FootstepGraph::local_movement_
protected

Definition at line 258 of file footstep_graph.h.

double jsk_footstep_planner::FootstepGraph::max_successor_distance_
protected

Definition at line 251 of file footstep_graph.h.

double jsk_footstep_planner::FootstepGraph::max_successor_rotation_
protected

Definition at line 252 of file footstep_graph.h.

pcl::PointCloud<pcl::PointXYZ>::Ptr jsk_footstep_planner::FootstepGraph::obstacle_model_
protected

Definition at line 233 of file footstep_graph.h.

pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr jsk_footstep_planner::FootstepGraph::obstacle_tree_model_
protected

Definition at line 236 of file footstep_graph.h.

FootstepParameters jsk_footstep_planner::FootstepGraph::parameters_
protected

Definition at line 264 of file footstep_graph.h.

PathCostFunction jsk_footstep_planner::FootstepGraph::path_cost_func_
private

Definition at line 272 of file footstep_graph.h.

ros::WallDuration jsk_footstep_planner::FootstepGraph::perception_duration_
protected

Definition at line 266 of file footstep_graph.h.

pcl::PointCloud<pcl::PointNormal>::Ptr jsk_footstep_planner::FootstepGraph::pointcloud_model_
protected

Definition at line 232 of file footstep_graph.h.

pcl::PointCloud<pcl::PointNormal>::Ptr jsk_footstep_planner::FootstepGraph::pointcloud_model_2d_
protected

Definition at line 234 of file footstep_graph.h.

ros::Publisher jsk_footstep_planner::FootstepGraph::pub_progress_
protected

Definition at line 254 of file footstep_graph.h.

bool jsk_footstep_planner::FootstepGraph::publish_progress_
protected

Definition at line 253 of file footstep_graph.h.

const Eigen::Vector3f jsk_footstep_planner::FootstepGraph::resolution_
protected

Definition at line 260 of file footstep_graph.h.

FootstepState::Ptr jsk_footstep_planner::FootstepGraph::right_goal_state_
protected

Definition at line 243 of file footstep_graph.h.

SuccessorFunction jsk_footstep_planner::FootstepGraph::successor_func_
private

Definition at line 271 of file footstep_graph.h.

std::vector<Eigen::Affine3f> jsk_footstep_planner::FootstepGraph::successors_from_left_to_right_
protected

Definition at line 240 of file footstep_graph.h.

std::vector<Eigen::Affine3f> jsk_footstep_planner::FootstepGraph::successors_from_right_to_left_
protected

Definition at line 241 of file footstep_graph.h.

TransitionLimit::Ptr jsk_footstep_planner::FootstepGraph::transition_limit_
protected

Definition at line 262 of file footstep_graph.h.

pcl::KdTreeFLANN<pcl::PointNormal>::Ptr jsk_footstep_planner::FootstepGraph::tree_model_
protected

Definition at line 235 of file footstep_graph.h.

pcl::search::Octree<pcl::PointNormal>::Ptr jsk_footstep_planner::FootstepGraph::tree_model_2d_
protected

Definition at line 238 of file footstep_graph.h.

const bool jsk_footstep_planner::FootstepGraph::use_obstacle_model_
protected

Definition at line 259 of file footstep_graph.h.

const bool jsk_footstep_planner::FootstepGraph::use_pointcloud_model_
protected

Definition at line 256 of file footstep_graph.h.

FootstepState::Ptr jsk_footstep_planner::FootstepGraph::zero_state_
protected

zero_state is used only for global transition limit

Definition at line 248 of file footstep_graph.h.


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


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Thu Nov 14 2019 03:53:28