#include <footstep_graph.h>

Inheritance diagram for jsk_footstep_planner::FootstepGraph:

Public Types
typedef boost::function< double(StatePtr, StatePtr, double) >	PathCostFunction

typedef boost::shared_ptr< FootstepGraph >	Ptr

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

Public Types inherited from jsk_footstep_planner::Graph< FootstepState >
typedef boost::shared_ptr< Graph >	Ptr

typedef boost::shared_ptr< StateT >	StatePtr

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::Ptr >	localMoveFootstepState (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< StatePtr >	successors (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< StatePtr >	nodes_

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.

typedef boost::shared_ptr<FootstepGraph> jsk_footstep_planner::FootstepGraph::Ptr

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:

Public Types

Public Member Functions

Protected Attributes

Private Attributes

Friends

Detailed Description

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Friends And Related Function Documentation

Member Data Documentation