#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 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;.
virtual TransitionLimit::Ptr	getTransitionLimit ()
virtual std::string	infoString () const
	return string about graph information.
virtual bool	isColliding (StatePtr current_state, StatePtr previous_state)
	return True if current_state collides with obstacle.
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
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
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

Reimplemented from jsk_footstep_planner::Graph< FootstepState >.

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 ( ) [inline, virtual]

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 ( ) [inline, virtual]

Definition at line 207 of file footstep_graph.h.

virtual FootstepState::Ptr jsk_footstep_planner::FootstepGraph::getGoal ( int leg ) [inline, virtual]

Definition at line 143 of file footstep_graph.h.

virtual ros::WallDuration jsk_footstep_planner::FootstepGraph::getPerceptionDuration ( ) [inline, virtual]

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 ( ) [inline, virtual]

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 [inline, virtual]

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 [inline, virtual]

Definition at line 201 of file footstep_graph.h.

virtual double jsk_footstep_planner::FootstepGraph::maxSuccessorDistance ( ) [inline, virtual]

Definition at line 156 of file footstep_graph.h.

virtual double jsk_footstep_planner::FootstepGraph::maxSuccessorRotation ( ) [inline, virtual]

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
	)		`[inline, virtual]`

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 ) [inline, virtual]

Definition at line 213 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setCollisionBBoxSize ( const Eigen::Vector3f & size ) [inline, virtual]

Definition at line 214 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setGlobalTransitionLimit ( TransitionLimit::Ptr limit ) [inline, virtual]

Definition at line 206 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setGoalState	(	FootstepState::Ptr	left,
		FootstepState::Ptr	right
	)		`[inline, virtual]`

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 ) [inline, virtual]

Definition at line 124 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setObstacleModel ( pcl::PointCloud< pcl::PointXYZ >::Ptr model ) [inline, virtual]

Definition at line 189 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setParameters ( FootstepParameters & p ) [inline, virtual]

Definition at line 203 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setPathCostFunction ( PathCostFunction p ) [inline, virtual]

Definition at line 219 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setPointCloudModel ( pcl::PointCloud< pcl::PointNormal >::Ptr model ) [inline, virtual]

Definition at line 171 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setProgressPublisher	(	ros::NodeHandle &	nh,
		std::string	topic
	)		`[inline, virtual]`

Definition at line 165 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setRightGoalState ( FootstepState::Ptr goal ) [inline, virtual]

Definition at line 129 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setSuccessorFunction ( SuccessorFunction s ) [inline, virtual]

Definition at line 216 of file footstep_graph.h.

virtual void jsk_footstep_planner::FootstepGraph::setTransitionLimit ( TransitionLimit::Ptr limit ) [inline, virtual]

Definition at line 204 of file footstep_graph.h.

virtual std::vector<StatePtr> jsk_footstep_planner::FootstepGraph::successors ( StatePtr target_state ) [inline, virtual]

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 [inline, virtual]

Definition at line 198 of file footstep_graph.h.

virtual bool jsk_footstep_planner::FootstepGraph::usePointCloudModel ( ) const [inline, virtual]

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