Public Types | Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
jsk_footstep_planner::FootstepPlanner Class Reference

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 104 of file footstep_planner.h.

Member Typedef Documentation

◆ Config

typedef FootstepPlannerConfig jsk_footstep_planner::FootstepPlanner::Config

Definition at line 108 of file footstep_planner.h.

◆ Ptr

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

Definition at line 107 of file footstep_planner.h.

Constructor & Destructor Documentation

◆ FootstepPlanner()

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

Definition at line 78 of file footstep_planner.cpp.

Member Function Documentation

◆ buildGraph()

void jsk_footstep_planner::FootstepPlanner::buildGraph ( )
protectedvirtual

Definition at line 1010 of file footstep_planner.cpp.

◆ collisionBoundingBoxInfoService()

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

Definition at line 268 of file footstep_planner.cpp.

◆ configCallback()

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

Definition at line 919 of file footstep_planner.cpp.

◆ followPathLineHeuristic()

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

Definition at line 811 of file footstep_planner.cpp.

◆ obstacleCallback()

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

Definition at line 143 of file footstep_planner.cpp.

◆ planCB()

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

Definition at line 443 of file footstep_planner.cpp.

◆ pointcloudCallback()

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

Definition at line 155 of file footstep_planner.cpp.

◆ profile()

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

Definition at line 765 of file footstep_planner.cpp.

◆ projectFootPrint()

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

◆ projectFootPrintService()

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

Definition at line 348 of file footstep_planner.cpp.

◆ projectFootPrintWithLocalSearchService()

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

Definition at line 208 of file footstep_planner.cpp.

◆ projectFootstepService()

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

Definition at line 280 of file footstep_planner.cpp.

◆ publishPointCloud()

void jsk_footstep_planner::FootstepPlanner::publishPointCloud ( const pcl::PointCloud< pcl::PointNormal > &  cloud,
ros::Publisher &  pub,
const std_msgs::Header &  header 
)
protectedvirtual

Definition at line 754 of file footstep_planner.cpp.

◆ publishText()

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

Definition at line 402 of file footstep_planner.cpp.

◆ readSuccessors()

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

◆ setHeuristicPathLine()

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

Definition at line 1030 of file footstep_planner.cpp.

◆ setHeuristicPathService()

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

Definition at line 381 of file footstep_planner.cpp.

◆ stepCostHeuristic()

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

Definition at line 786 of file footstep_planner.cpp.

◆ straightHeuristic()

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

Definition at line 799 of file footstep_planner.cpp.

◆ straightRotationHeuristic()

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

Definition at line 805 of file footstep_planner.cpp.

◆ zeroHeuristic()

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

Definition at line 793 of file footstep_planner.cpp.

Member Data Documentation

◆ as_

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

Definition at line 168 of file footstep_planner.h.

◆ close_list_theta_num_

int jsk_footstep_planner::FootstepPlanner::close_list_theta_num_
protected

Definition at line 208 of file footstep_planner.h.

◆ close_list_x_num_

int jsk_footstep_planner::FootstepPlanner::close_list_x_num_
protected

Definition at line 206 of file footstep_planner.h.

◆ close_list_y_num_

int jsk_footstep_planner::FootstepPlanner::close_list_y_num_
protected

Definition at line 207 of file footstep_planner.h.

◆ collision_bbox_offset_

Eigen::Affine3f jsk_footstep_planner::FootstepPlanner::collision_bbox_offset_
protected

Definition at line 186 of file footstep_planner.h.

◆ collision_bbox_size_

Eigen::Vector3f jsk_footstep_planner::FootstepPlanner::collision_bbox_size_
protected

Definition at line 185 of file footstep_planner.h.

◆ cost_weight_

double jsk_footstep_planner::FootstepPlanner::cost_weight_
protected

Definition at line 213 of file footstep_planner.h.

◆ footstep_size_x_

double jsk_footstep_planner::FootstepPlanner::footstep_size_x_
protected

Definition at line 204 of file footstep_planner.h.

◆ footstep_size_y_

double jsk_footstep_planner::FootstepPlanner::footstep_size_y_
protected

Definition at line 205 of file footstep_planner.h.

◆ graph_

FootstepGraph::Ptr jsk_footstep_planner::FootstepPlanner::graph_
protected

Definition at line 183 of file footstep_planner.h.

◆ heuristic_

std::string jsk_footstep_planner::FootstepPlanner::heuristic_
protected

Definition at line 210 of file footstep_planner.h.

◆ heuristic_first_rotation_weight_

double jsk_footstep_planner::FootstepPlanner::heuristic_first_rotation_weight_
protected

Definition at line 211 of file footstep_planner.h.

◆ heuristic_second_rotation_weight_

double jsk_footstep_planner::FootstepPlanner::heuristic_second_rotation_weight_
protected

Definition at line 212 of file footstep_planner.h.

◆ heuristic_weight_

double jsk_footstep_planner::FootstepPlanner::heuristic_weight_
protected

Definition at line 214 of file footstep_planner.h.

◆ inv_lleg_footstep_offset_

Eigen::Vector3f jsk_footstep_planner::FootstepPlanner::inv_lleg_footstep_offset_
protected

Definition at line 187 of file footstep_planner.h.

◆ inv_rleg_footstep_offset_

Eigen::Vector3f jsk_footstep_planner::FootstepPlanner::inv_rleg_footstep_offset_
protected

Definition at line 188 of file footstep_planner.h.

◆ latest_header_

std_msgs::Header jsk_footstep_planner::FootstepPlanner::latest_header_
protected

Definition at line 189 of file footstep_planner.h.

◆ mutex_

boost::mutex jsk_footstep_planner::FootstepPlanner::mutex_
protected

Definition at line 167 of file footstep_planner.h.

◆ num_finalize_steps_

size_t jsk_footstep_planner::FootstepPlanner::num_finalize_steps_
protected

Definition at line 217 of file footstep_planner.h.

◆ obstacle_model_

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

Definition at line 182 of file footstep_planner.h.

◆ obstacle_model_frame_id_

std::string jsk_footstep_planner::FootstepPlanner::obstacle_model_frame_id_
protected

Definition at line 215 of file footstep_planner.h.

◆ parameters_

FootstepParameters jsk_footstep_planner::FootstepPlanner::parameters_
protected

Definition at line 191 of file footstep_planner.h.

◆ planning_timeout_

double jsk_footstep_planner::FootstepPlanner::planning_timeout_
protected

Definition at line 216 of file footstep_planner.h.

◆ pointcloud_model_

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

Definition at line 181 of file footstep_planner.h.

◆ pointcloud_model_frame_id_

std::string jsk_footstep_planner::FootstepPlanner::pointcloud_model_frame_id_
protected

Definition at line 215 of file footstep_planner.h.

◆ profile_period_

int jsk_footstep_planner::FootstepPlanner::profile_period_
protected

Definition at line 209 of file footstep_planner.h.

◆ project_goal_state_

bool jsk_footstep_planner::FootstepPlanner::project_goal_state_
protected

Definition at line 196 of file footstep_planner.h.

◆ project_start_state_

bool jsk_footstep_planner::FootstepPlanner::project_start_state_
protected

Definition at line 195 of file footstep_planner.h.

◆ pub_close_list_

ros::Publisher jsk_footstep_planner::FootstepPlanner::pub_close_list_
protected

Definition at line 171 of file footstep_planner.h.

◆ pub_open_list_

ros::Publisher jsk_footstep_planner::FootstepPlanner::pub_open_list_
protected

Definition at line 172 of file footstep_planner.h.

◆ pub_text_

ros::Publisher jsk_footstep_planner::FootstepPlanner::pub_text_
protected

Definition at line 173 of file footstep_planner.h.

◆ resolution_theta_

double jsk_footstep_planner::FootstepPlanner::resolution_theta_
protected

Definition at line 203 of file footstep_planner.h.

◆ resolution_x_

double jsk_footstep_planner::FootstepPlanner::resolution_x_
protected

Definition at line 201 of file footstep_planner.h.

◆ resolution_y_

double jsk_footstep_planner::FootstepPlanner::resolution_y_
protected

Definition at line 202 of file footstep_planner.h.

◆ result_

jsk_footstep_msgs::PlanFootstepsResult jsk_footstep_planner::FootstepPlanner::result_
protected

Definition at line 169 of file footstep_planner.h.

◆ rich_profiling_

bool jsk_footstep_planner::FootstepPlanner::rich_profiling_
protected

Definition at line 194 of file footstep_planner.h.

◆ srv_

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

Definition at line 170 of file footstep_planner.h.

◆ srv_collision_bounding_box_info_

ros::ServiceServer jsk_footstep_planner::FootstepPlanner::srv_collision_bounding_box_info_
protected

Definition at line 178 of file footstep_planner.h.

◆ srv_project_footprint_

ros::ServiceServer jsk_footstep_planner::FootstepPlanner::srv_project_footprint_
protected

Definition at line 176 of file footstep_planner.h.

◆ srv_project_footprint_with_local_search_

ros::ServiceServer jsk_footstep_planner::FootstepPlanner::srv_project_footprint_with_local_search_
protected

Definition at line 177 of file footstep_planner.h.

◆ srv_project_footstep_

ros::ServiceServer jsk_footstep_planner::FootstepPlanner::srv_project_footstep_
protected

Definition at line 179 of file footstep_planner.h.

◆ srv_set_heuristic_path_

ros::ServiceServer jsk_footstep_planner::FootstepPlanner::srv_set_heuristic_path_
protected

Definition at line 180 of file footstep_planner.h.

◆ sub_obstacle_model_

ros::Subscriber jsk_footstep_planner::FootstepPlanner::sub_obstacle_model_
protected

Definition at line 175 of file footstep_planner.h.

◆ sub_pointcloud_model_

ros::Subscriber jsk_footstep_planner::FootstepPlanner::sub_pointcloud_model_
protected

Definition at line 174 of file footstep_planner.h.

◆ successors_

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

Definition at line 184 of file footstep_planner.h.

◆ use_lazy_perception_

bool jsk_footstep_planner::FootstepPlanner::use_lazy_perception_
protected

Definition at line 198 of file footstep_planner.h.

◆ use_local_movement_

bool jsk_footstep_planner::FootstepPlanner::use_local_movement_
protected

Definition at line 199 of file footstep_planner.h.

◆ use_obstacle_model_

bool jsk_footstep_planner::FootstepPlanner::use_obstacle_model_
protected

Definition at line 200 of file footstep_planner.h.

◆ use_pointcloud_model_

bool jsk_footstep_planner::FootstepPlanner::use_pointcloud_model_
protected

Definition at line 197 of file footstep_planner.h.

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

jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Wed Jan 24 2024 04:05:30