Actionlib server for footstep planning. More...

#include <footstep_planner.h>

Public Types
typedef FootstepPlannerConfig	Config
typedef boost::shared_ptr < FootstepPlanner >	Ptr
Public Member Functions
	FootstepPlanner (ros::NodeHandle &nh)
virtual void	setHeuristicPathLine (jsk_recognition_utils::PolyLine &path_line)
Protected Member Functions
virtual void	buildGraph ()
virtual bool	collisionBoundingBoxInfoService (jsk_footstep_planner::CollisionBoundingBoxInfo::Request &req, jsk_footstep_planner::CollisionBoundingBoxInfo::Response &res)
virtual void	configCallback (Config &config, uint32_t level)
virtual double	followPathLineHeuristic (SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
virtual void	obstacleCallback (const sensor_msgs::PointCloud2::ConstPtr &msg)
virtual void	planCB (const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr &goal)
virtual void	pointcloudCallback (const sensor_msgs::PointCloud2::ConstPtr &msg)
virtual void	profile (FootstepAStarSolver< FootstepGraph > &solver, FootstepGraph::Ptr graph)
virtual bool	projectFootPrint (const Eigen::Affine3f &center_pose, const Eigen::Affine3f &left_pose_trans, const Eigen::Affine3f &right_pose_trans, geometry_msgs::Pose &pose)
virtual bool	projectFootPrintService (jsk_interactive_marker::SnapFootPrint::Request &req, jsk_interactive_marker::SnapFootPrint::Response &res)
virtual bool	projectFootPrintWithLocalSearchService (jsk_interactive_marker::SnapFootPrint::Request &req, jsk_interactive_marker::SnapFootPrint::Response &res)
virtual bool	projectFootstepService (jsk_footstep_planner::ProjectFootstep::Request &req, jsk_footstep_planner::ProjectFootstep::Response &res)
virtual void	publishPointCloud (const pcl::PointCloud< pcl::PointNormal > &cloud, ros::Publisher &pub, const std_msgs::Header &header)
virtual void	publishText (ros::Publisher &pub, const std::string &text, PlanningStatus status)
virtual bool	readSuccessors (ros::NodeHandle &nh)
virtual bool	setHeuristicPathService (jsk_footstep_planner::SetHeuristicPath::Request &req, jsk_footstep_planner::SetHeuristicPath::Response &res)
virtual double	stepCostHeuristic (SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
virtual double	straightHeuristic (SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
virtual double	straightRotationHeuristic (SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
virtual double	zeroHeuristic (SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
Protected Attributes
actionlib::SimpleActionServer < jsk_footstep_msgs::PlanFootstepsAction >	as_
int	close_list_theta_num_
int	close_list_x_num_
int	close_list_y_num_
Eigen::Affine3f	collision_bbox_offset_
Eigen::Vector3f	collision_bbox_size_
double	cost_weight_
double	footstep_size_x_
double	footstep_size_y_
FootstepGraph::Ptr	graph_
std::string	heuristic_
double	heuristic_first_rotation_weight_
double	heuristic_second_rotation_weight_
double	heuristic_weight_
Eigen::Vector3f	inv_lleg_footstep_offset_
Eigen::Vector3f	inv_rleg_footstep_offset_
std_msgs::Header	latest_header_
boost::mutex	mutex_
pcl::PointCloud< pcl::PointXYZ > ::Ptr	obstacle_model_
std::string	obstacle_model_frame_id_
FootstepParameters	parameters_
double	planning_timeout_
pcl::PointCloud < pcl::PointNormal >::Ptr	pointcloud_model_
std::string	pointcloud_model_frame_id_
int	profile_period_
bool	project_goal_state_
bool	project_start_state_
ros::Publisher	pub_close_list_
ros::Publisher	pub_open_list_
ros::Publisher	pub_text_
double	resolution_theta_
double	resolution_x_
double	resolution_y_
jsk_footstep_msgs::PlanFootstepsResult	result_
bool	rich_profiling_
boost::shared_ptr < dynamic_reconfigure::Server < Config > >	srv_
ros::ServiceServer	srv_collision_bounding_box_info_
ros::ServiceServer	srv_project_footprint_
ros::ServiceServer	srv_project_footprint_with_local_search_
ros::ServiceServer	srv_project_footstep_
ros::ServiceServer	srv_set_heuristic_path_
ros::Subscriber	sub_obstacle_model_
ros::Subscriber	sub_pointcloud_model_
std::vector< Eigen::Affine3f >	successors_
bool	use_lazy_perception_
bool	use_local_movement_
bool	use_obstacle_model_
bool	use_pointcloud_model_

Detailed Description

Actionlib server for footstep planning.

Definition at line 72 of file footstep_planner.h.

Member Typedef Documentation

typedef FootstepPlannerConfig jsk_footstep_planner::FootstepPlanner::Config

Definition at line 76 of file footstep_planner.h.

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

Definition at line 75 of file footstep_planner.h.

Constructor & Destructor Documentation

jsk_footstep_planner::FootstepPlanner::FootstepPlanner ( ros::NodeHandle & nh )

Definition at line 45 of file footstep_planner.cpp.

Member Function Documentation

void jsk_footstep_planner::FootstepPlanner::buildGraph ( ) [protected, virtual]

Definition at line 976 of file footstep_planner.cpp.

bool jsk_footstep_planner::FootstepPlanner::collisionBoundingBoxInfoService	(	jsk_footstep_planner::CollisionBoundingBoxInfo::Request &	req,
		jsk_footstep_planner::CollisionBoundingBoxInfo::Response &	res
	)		`[protected, virtual]`

Definition at line 235 of file footstep_planner.cpp.

void jsk_footstep_planner::FootstepPlanner::configCallback	(	Config &	config,
		uint32_t	level
	)		`[protected, virtual]`

Definition at line 886 of file footstep_planner.cpp.

double jsk_footstep_planner::FootstepPlanner::followPathLineHeuristic	(	SolverNode< FootstepState, FootstepGraph >::Ptr	node,
		FootstepGraph::Ptr	graph
	)		`[protected, virtual]`

Definition at line 778 of file footstep_planner.cpp.

void jsk_footstep_planner::FootstepPlanner::obstacleCallback ( const sensor_msgs::PointCloud2::ConstPtr & msg ) [protected, virtual]

Definition at line 110 of file footstep_planner.cpp.

void jsk_footstep_planner::FootstepPlanner::planCB ( const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr & goal ) [protected, virtual]

Definition at line 410 of file footstep_planner.cpp.

void jsk_footstep_planner::FootstepPlanner::pointcloudCallback ( const sensor_msgs::PointCloud2::ConstPtr & msg ) [protected, virtual]

Definition at line 122 of file footstep_planner.cpp.

void jsk_footstep_planner::FootstepPlanner::profile	(	FootstepAStarSolver< FootstepGraph > &	solver,
		FootstepGraph::Ptr	graph
	)		`[protected, virtual]`

Definition at line 732 of file footstep_planner.cpp.

bool jsk_footstep_planner::FootstepPlanner::projectFootPrint	(	const Eigen::Affine3f &	center_pose,
		const Eigen::Affine3f &	left_pose_trans,
		const Eigen::Affine3f &	right_pose_trans,
		geometry_msgs::Pose &	pose
	)		`[protected, virtual]`

Definition at line 135 of file footstep_planner.cpp.

bool jsk_footstep_planner::FootstepPlanner::projectFootPrintService	(	jsk_interactive_marker::SnapFootPrint::Request &	req,
		jsk_interactive_marker::SnapFootPrint::Response &	res
	)		`[protected, virtual]`

Definition at line 315 of file footstep_planner.cpp.

bool jsk_footstep_planner::FootstepPlanner::projectFootPrintWithLocalSearchService	(	jsk_interactive_marker::SnapFootPrint::Request &	req,
		jsk_interactive_marker::SnapFootPrint::Response &	res
	)		`[protected, virtual]`

Definition at line 175 of file footstep_planner.cpp.

bool jsk_footstep_planner::FootstepPlanner::projectFootstepService	(	jsk_footstep_planner::ProjectFootstep::Request &	req,
		jsk_footstep_planner::ProjectFootstep::Response &	res
	)		`[protected, virtual]`

Definition at line 247 of file footstep_planner.cpp.

void jsk_footstep_planner::FootstepPlanner::publishPointCloud	(	const pcl::PointCloud< pcl::PointNormal > &	cloud,
		ros::Publisher &	pub,
		const std_msgs::Header &	header
	)		`[protected, virtual]`

Definition at line 721 of file footstep_planner.cpp.

void jsk_footstep_planner::FootstepPlanner::publishText	(	ros::Publisher &	pub,
		const std::string &	text,
		PlanningStatus	status
	)		`[protected, virtual]`

Definition at line 369 of file footstep_planner.cpp.

bool jsk_footstep_planner::FootstepPlanner::readSuccessors ( ros::NodeHandle & nh ) [protected, virtual]

format is successors:

x: 0 y: 0 theta: 0
x: 0 y: 0 theta: 0 ...

Definition at line 795 of file footstep_planner.cpp.

void jsk_footstep_planner::FootstepPlanner::setHeuristicPathLine ( jsk_recognition_utils::PolyLine & path_line ) [virtual]

Definition at line 996 of file footstep_planner.cpp.

bool jsk_footstep_planner::FootstepPlanner::setHeuristicPathService	(	jsk_footstep_planner::SetHeuristicPath::Request &	req,
		jsk_footstep_planner::SetHeuristicPath::Response &	res
	)		`[protected, virtual]`

Definition at line 348 of file footstep_planner.cpp.

double jsk_footstep_planner::FootstepPlanner::stepCostHeuristic	(	SolverNode< FootstepState, FootstepGraph >::Ptr	node,
		FootstepGraph::Ptr	graph
	)		`[protected, virtual]`

Definition at line 753 of file footstep_planner.cpp.

double jsk_footstep_planner::FootstepPlanner::straightHeuristic	(	SolverNode< FootstepState, FootstepGraph >::Ptr	node,
		FootstepGraph::Ptr	graph
	)		`[protected, virtual]`

Definition at line 766 of file footstep_planner.cpp.

double jsk_footstep_planner::FootstepPlanner::straightRotationHeuristic	(	SolverNode< FootstepState, FootstepGraph >::Ptr	node,
		FootstepGraph::Ptr	graph
	)		`[protected, virtual]`

Definition at line 772 of file footstep_planner.cpp.

double jsk_footstep_planner::FootstepPlanner::zeroHeuristic	(	SolverNode< FootstepState, FootstepGraph >::Ptr	node,
		FootstepGraph::Ptr	graph
	)		`[protected, virtual]`