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_

size_t	num_finalize_steps_

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 46 of file footstep_planner.cpp.

Member Function Documentation

void jsk_footstep_planner::FootstepPlanner::buildGraph ( )

protectedvirtual

Definition at line 978 of file footstep_planner.cpp.

bool jsk_footstep_planner::FootstepPlanner::collisionBoundingBoxInfoService	(	jsk_footstep_planner::CollisionBoundingBoxInfo::Request &	req,
		jsk_footstep_planner::CollisionBoundingBoxInfo::Response &	res
	)

protectedvirtual

Definition at line 236 of file footstep_planner.cpp.

void jsk_footstep_planner::FootstepPlanner::configCallback	(	Config &	config,
		uint32_t	level
	)

protectedvirtual

Definition at line 887 of file footstep_planner.cpp.

double jsk_footstep_planner::FootstepPlanner::followPathLineHeuristic	(	SolverNode< FootstepState, FootstepGraph >::Ptr	node,
		FootstepGraph::Ptr	graph
	)

protectedvirtual

Definition at line 779 of file footstep_planner.cpp.

void jsk_footstep_planner::FootstepPlanner::obstacleCallback ( const sensor_msgs::PointCloud2::ConstPtr & msg )

protectedvirtual

Definition at line 111 of file footstep_planner.cpp.

void jsk_footstep_planner::FootstepPlanner::planCB ( const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr & goal )

protectedvirtual

Definition at line 411 of file footstep_planner.cpp.

void jsk_footstep_planner::FootstepPlanner::pointcloudCallback ( const sensor_msgs::PointCloud2::ConstPtr & msg )

protectedvirtual

Definition at line 123 of file footstep_planner.cpp.

void jsk_footstep_planner::FootstepPlanner::profile	(	FootstepAStarSolver< FootstepGraph > &	solver,
		FootstepGraph::Ptr	graph
	)

protectedvirtual

Definition at line 733 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
	)

protectedvirtual

Definition at line 136 of file footstep_planner.cpp.

bool jsk_footstep_planner::FootstepPlanner::projectFootPrintService	(	jsk_interactive_marker::SnapFootPrint::Request &	req,
		jsk_interactive_marker::SnapFootPrint::Response &	res
	)

protectedvirtual

Definition at line 316 of file footstep_planner.cpp.

bool jsk_footstep_planner::FootstepPlanner::projectFootPrintWithLocalSearchService	(	jsk_interactive_marker::SnapFootPrint::Request &	req,
		jsk_interactive_marker::SnapFootPrint::Response &	res
	)

protectedvirtual

Definition at line 176 of file footstep_planner.cpp.

bool jsk_footstep_planner::FootstepPlanner::projectFootstepService	(	jsk_footstep_planner::ProjectFootstep::Request &	req,
		jsk_footstep_planner::ProjectFootstep::Response &	res
	)

protectedvirtual

Definition at line 248 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
	)

protectedvirtual

Definition at line 722 of file footstep_planner.cpp.

void jsk_footstep_planner::FootstepPlanner::publishText	(	ros::Publisher &	pub,
		const std::string &	text,
		PlanningStatus	status
	)

protectedvirtual

Definition at line 370 of file footstep_planner.cpp.

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

protectedvirtual

format is successors:

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

Definition at line 796 of file footstep_planner.cpp.

void jsk_footstep_planner::FootstepPlanner::setHeuristicPathLine ( jsk_recognition_utils::PolyLine & path_line )

virtual

Definition at line 998 of file footstep_planner.cpp.

bool jsk_footstep_planner::FootstepPlanner::setHeuristicPathService	(	jsk_footstep_planner::SetHeuristicPath::Request &	req,
		jsk_footstep_planner::SetHeuristicPath::Response &	res
	)

protectedvirtual

Definition at line 349 of file footstep_planner.cpp.

double jsk_footstep_planner::FootstepPlanner::stepCostHeuristic	(	SolverNode< FootstepState, FootstepGraph >::Ptr	node,
		FootstepGraph::Ptr	graph
	)

protectedvirtual

Definition at line 754 of file footstep_planner.cpp.

double jsk_footstep_planner::FootstepPlanner::straightHeuristic	(	SolverNode< FootstepState, FootstepGraph >::Ptr	node,
		FootstepGraph::Ptr	graph
	)

protectedvirtual

Definition at line 767 of file footstep_planner.cpp.

double jsk_footstep_planner::FootstepPlanner::straightRotationHeuristic	(	SolverNode< FootstepState, FootstepGraph >::Ptr	node,
		FootstepGraph::Ptr	graph
	)

protectedvirtual

Definition at line 773 of file footstep_planner.cpp.

double jsk_footstep_planner::FootstepPlanner::zeroHeuristic	(	SolverNode< FootstepState, FootstepGraph >::Ptr	node,
		FootstepGraph::Ptr	graph
	)

protectedvirtual

Definition at line 761 of file footstep_planner.cpp.

Member Data Documentation

actionlib::SimpleActionServer<jsk_footstep_msgs::PlanFootstepsAction> jsk_footstep_planner::FootstepPlanner::as_

protected

Definition at line 136 of file footstep_planner.h.

int jsk_footstep_planner::FootstepPlanner::close_list_theta_num_

protected

Definition at line 176 of file footstep_planner.h.

int jsk_footstep_planner::FootstepPlanner::close_list_x_num_

protected

Definition at line 174 of file footstep_planner.h.

int jsk_footstep_planner::FootstepPlanner::close_list_y_num_

protected

Definition at line 175 of file footstep_planner.h.

Eigen::Affine3f jsk_footstep_planner::FootstepPlanner::collision_bbox_offset_

protected

Definition at line 154 of file footstep_planner.h.

Eigen::Vector3f jsk_footstep_planner::FootstepPlanner::collision_bbox_size_

protected

Definition at line 153 of file footstep_planner.h.

double jsk_footstep_planner::FootstepPlanner::cost_weight_

protected

Definition at line 181 of file footstep_planner.h.

double jsk_footstep_planner::FootstepPlanner::footstep_size_x_

protected

Definition at line 172 of file footstep_planner.h.

double jsk_footstep_planner::FootstepPlanner::footstep_size_y_

protected

Definition at line 173 of file footstep_planner.h.

FootstepGraph::Ptr jsk_footstep_planner::FootstepPlanner::graph_

protected

Definition at line 151 of file footstep_planner.h.

std::string jsk_footstep_planner::FootstepPlanner::heuristic_

protected

Definition at line 178 of file footstep_planner.h.

double jsk_footstep_planner::FootstepPlanner::heuristic_first_rotation_weight_

protected

Definition at line 179 of file footstep_planner.h.

double jsk_footstep_planner::FootstepPlanner::heuristic_second_rotation_weight_

protected

Definition at line 180 of file footstep_planner.h.

double jsk_footstep_planner::FootstepPlanner::heuristic_weight_

protected

Definition at line 182 of file footstep_planner.h.

Eigen::Vector3f jsk_footstep_planner::FootstepPlanner::inv_lleg_footstep_offset_

protected

Definition at line 155 of file footstep_planner.h.

Eigen::Vector3f jsk_footstep_planner::FootstepPlanner::inv_rleg_footstep_offset_

protected

Definition at line 156 of file footstep_planner.h.

std_msgs::Header jsk_footstep_planner::FootstepPlanner::latest_header_

protected

Definition at line 157 of file footstep_planner.h.

boost::mutex jsk_footstep_planner::FootstepPlanner::mutex_

protected

Definition at line 135 of file footstep_planner.h.

size_t jsk_footstep_planner::FootstepPlanner::num_finalize_steps_

protected

Definition at line 185 of file footstep_planner.h.

pcl::PointCloud<pcl::PointXYZ>::Ptr jsk_footstep_planner::FootstepPlanner::obstacle_model_

protected

Definition at line 150 of file footstep_planner.h.

std::string jsk_footstep_planner::FootstepPlanner::obstacle_model_frame_id_

protected

Definition at line 183 of file footstep_planner.h.

FootstepParameters jsk_footstep_planner::FootstepPlanner::parameters_

protected

Definition at line 159 of file footstep_planner.h.

double jsk_footstep_planner::FootstepPlanner::planning_timeout_

protected

Definition at line 184 of file footstep_planner.h.

pcl::PointCloud<pcl::PointNormal>::Ptr jsk_footstep_planner::FootstepPlanner::pointcloud_model_

protected

Definition at line 149 of file footstep_planner.h.

std::string jsk_footstep_planner::FootstepPlanner::pointcloud_model_frame_id_

protected

Definition at line 183 of file footstep_planner.h.

int jsk_footstep_planner::FootstepPlanner::profile_period_

protected

Definition at line 177 of file footstep_planner.h.

bool jsk_footstep_planner::FootstepPlanner::project_goal_state_

protected

Definition at line 164 of file footstep_planner.h.

bool jsk_footstep_planner::FootstepPlanner::project_start_state_

protected

Definition at line 163 of file footstep_planner.h.

ros::Publisher jsk_footstep_planner::FootstepPlanner::pub_close_list_

protected

Definition at line 139 of file footstep_planner.h.

ros::Publisher jsk_footstep_planner::FootstepPlanner::pub_open_list_

protected

Definition at line 140 of file footstep_planner.h.

ros::Publisher jsk_footstep_planner::FootstepPlanner::pub_text_

protected

Definition at line 141 of file footstep_planner.h.

double jsk_footstep_planner::FootstepPlanner::resolution_theta_

protected

Definition at line 171 of file footstep_planner.h.

double jsk_footstep_planner::FootstepPlanner::resolution_x_

protected

Definition at line 169 of file footstep_planner.h.

double jsk_footstep_planner::FootstepPlanner::resolution_y_

protected

Definition at line 170 of file footstep_planner.h.

jsk_footstep_msgs::PlanFootstepsResult jsk_footstep_planner::FootstepPlanner::result_

protected

Definition at line 137 of file footstep_planner.h.

bool jsk_footstep_planner::FootstepPlanner::rich_profiling_

protected

Definition at line 162 of file footstep_planner.h.

boost::shared_ptr<dynamic_reconfigure::Server<Config> > jsk_footstep_planner::FootstepPlanner::srv_

protected

Definition at line 138 of file footstep_planner.h.

ros::ServiceServer jsk_footstep_planner::FootstepPlanner::srv_collision_bounding_box_info_

protected

Definition at line 146 of file footstep_planner.h.

ros::ServiceServer jsk_footstep_planner::FootstepPlanner::srv_project_footprint_

protected

Definition at line 144 of file footstep_planner.h.

ros::ServiceServer jsk_footstep_planner::FootstepPlanner::srv_project_footprint_with_local_search_

protected

Definition at line 145 of file footstep_planner.h.

ros::ServiceServer jsk_footstep_planner::FootstepPlanner::srv_project_footstep_

protected

Definition at line 147 of file footstep_planner.h.

ros::ServiceServer jsk_footstep_planner::FootstepPlanner::srv_set_heuristic_path_

protected

Definition at line 148 of file footstep_planner.h.

ros::Subscriber jsk_footstep_planner::FootstepPlanner::sub_obstacle_model_

protected

Definition at line 143 of file footstep_planner.h.

ros::Subscriber jsk_footstep_planner::FootstepPlanner::sub_pointcloud_model_

protected

Definition at line 142 of file footstep_planner.h.

std::vector<Eigen::Affine3f> jsk_footstep_planner::FootstepPlanner::successors_

protected

Definition at line 152 of file footstep_planner.h.

bool jsk_footstep_planner::FootstepPlanner::use_lazy_perception_

protected

Definition at line 166 of file footstep_planner.h.

bool jsk_footstep_planner::FootstepPlanner::use_local_movement_

protected

Definition at line 167 of file footstep_planner.h.

bool jsk_footstep_planner::FootstepPlanner::use_obstacle_model_

protected

Definition at line 168 of file footstep_planner.h.

bool jsk_footstep_planner::FootstepPlanner::use_pointcloud_model_

protected

Definition at line 165 of file footstep_planner.h.

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

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation