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

Actionlib server for footstep planning. More...

#include <grid_path_planner.h>

Public Types

typedef PerceptionGridGraph Graph
 
typedef PerceptionGridMap GridMap
 
typedef GridAStarSolver< PerceptionGridGraphSolver
 

Public Member Functions

 GridPathPlanner (ros::NodeHandle &nh)
 

Protected Member Functions

virtual void buildGraph ()
 
virtual bool collisionBoundingBoxInfoService (jsk_footstep_planner::CollisionBoundingBoxInfo::Request &req, jsk_footstep_planner::CollisionBoundingBoxInfo::Response &res)
 
virtual void gridToPoint (const int ix, const int iy, Eigen::Vector3f &p)
 
double heuristicDistance (SolverNode< PerceptionGridGraph::State, PerceptionGridGraph >::Ptr node, PerceptionGridGraph::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 pointToGrid (const Eigen::Vector3f &p, int &ix, int &iy)
 
virtual void publishMarker ()
 
virtual void publishPointCloud (const pcl::PointCloud< pcl::PointXYZRGB > &cloud, ros::Publisher &pub, const std_msgs::Header &header)
 
virtual void publishText (ros::Publisher &pub, const std::string &text, GridPlanningStatus status)
 
virtual bool updateCost (GridState::Ptr ptr)
 

Protected Attributes

actionlib::SimpleActionServer< jsk_footstep_msgs::PlanFootstepsAction > as_
 
Eigen::Affine3f collision_bbox_offset_
 
Eigen::Vector3f collision_bbox_size_
 
double collision_circle_max_height_
 
double collision_circle_min_height_
 
double collision_circle_radius_
 
PerceptionGridGraph::Ptr graph_
 
PerceptionGridMap::Ptr gridmap_
 
Eigen::Vector3f map_offset_
 
double map_resolution_
 
boost::mutex mutex_
 
pcl::PointCloud< pcl::PointXYZ >::Ptr obstacle_points_
 
std::string obstacle_points_frame_id_
 
pcl::KdTreeFLANN< pcl::PointXYZ >::Ptr obstacle_tree_
 
pcl::PointCloud< pcl::PointNormal >::Ptr plane_points_
 
std::string plane_points_frame_id_
 
pcl::KdTreeFLANN< pcl::PointNormal >::Ptr plane_tree_
 
ros::Publisher pub_close_list_
 
ros::Publisher pub_marker_
 
ros::Publisher pub_open_list_
 
ros::Publisher pub_text_
 
jsk_footstep_msgs::PlanFootstepsResult result_
 
ros::ServiceServer srv_collision_bounding_box_info_
 
ros::Subscriber sub_obstacle_points_
 
ros::Subscriber sub_plane_points_
 
bool use_obstacle_points_
 
bool use_plane_points_
 

Detailed Description

Actionlib server for footstep planning.

Definition at line 37 of file grid_path_planner.h.

Member Typedef Documentation

◆ Graph

Definition at line 40 of file grid_path_planner.h.

◆ GridMap

Definition at line 41 of file grid_path_planner.h.

◆ Solver

Definition at line 42 of file grid_path_planner.h.

Constructor & Destructor Documentation

◆ GridPathPlanner()

jsk_footstep_planner::GridPathPlanner::GridPathPlanner ( ros::NodeHandle nh)

Definition at line 15 of file grid_path_planner.cpp.

Member Function Documentation

◆ buildGraph()

void jsk_footstep_planner::GridPathPlanner::buildGraph ( )
protectedvirtual

Definition at line 55 of file grid_path_planner.cpp.

◆ collisionBoundingBoxInfoService()

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

Definition at line 100 of file grid_path_planner.cpp.

◆ gridToPoint()

virtual void jsk_footstep_planner::GridPathPlanner::gridToPoint ( const int  ix,
const int  iy,
Eigen::Vector3f &  p 
)
inlineprotectedvirtual

Definition at line 75 of file grid_path_planner.h.

◆ heuristicDistance()

double jsk_footstep_planner::GridPathPlanner::heuristicDistance ( SolverNode< PerceptionGridGraph::State, PerceptionGridGraph >::Ptr  node,
PerceptionGridGraph::Ptr  graph 
)
inlineprotected

Definition at line 89 of file grid_path_planner.h.

◆ obstacleCallback()

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

Definition at line 80 of file grid_path_planner.cpp.

◆ planCB()

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

Definition at line 153 of file grid_path_planner.cpp.

◆ pointcloudCallback()

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

Definition at line 90 of file grid_path_planner.cpp.

◆ pointToGrid()

virtual void jsk_footstep_planner::GridPathPlanner::pointToGrid ( const Eigen::Vector3f &  p,
int &  ix,
int &  iy 
)
inlineprotectedvirtual

Definition at line 81 of file grid_path_planner.h.

◆ publishMarker()

void jsk_footstep_planner::GridPathPlanner::publishMarker ( )
protectedvirtual

Definition at line 393 of file grid_path_planner.cpp.

◆ publishPointCloud()

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

Definition at line 325 of file grid_path_planner.cpp.

◆ publishText()

void jsk_footstep_planner::GridPathPlanner::publishText ( ros::Publisher pub,
const std::string text,
GridPlanningStatus  status 
)
protectedvirtual

Definition at line 112 of file grid_path_planner.cpp.

◆ updateCost()

bool jsk_footstep_planner::GridPathPlanner::updateCost ( GridState::Ptr  ptr)
protectedvirtual

Definition at line 336 of file grid_path_planner.cpp.

Member Data Documentation

◆ as_

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

Definition at line 101 of file grid_path_planner.h.

◆ collision_bbox_offset_

Eigen::Affine3f jsk_footstep_planner::GridPathPlanner::collision_bbox_offset_
protected

Definition at line 129 of file grid_path_planner.h.

◆ collision_bbox_size_

Eigen::Vector3f jsk_footstep_planner::GridPathPlanner::collision_bbox_size_
protected

Definition at line 128 of file grid_path_planner.h.

◆ collision_circle_max_height_

double jsk_footstep_planner::GridPathPlanner::collision_circle_max_height_
protected

Definition at line 134 of file grid_path_planner.h.

◆ collision_circle_min_height_

double jsk_footstep_planner::GridPathPlanner::collision_circle_min_height_
protected

Definition at line 133 of file grid_path_planner.h.

◆ collision_circle_radius_

double jsk_footstep_planner::GridPathPlanner::collision_circle_radius_
protected

Definition at line 132 of file grid_path_planner.h.

◆ graph_

PerceptionGridGraph::Ptr jsk_footstep_planner::GridPathPlanner::graph_
protected

Definition at line 123 of file grid_path_planner.h.

◆ gridmap_

PerceptionGridMap::Ptr jsk_footstep_planner::GridPathPlanner::gridmap_
protected

Definition at line 122 of file grid_path_planner.h.

◆ map_offset_

Eigen::Vector3f jsk_footstep_planner::GridPathPlanner::map_offset_
protected

Definition at line 125 of file grid_path_planner.h.

◆ map_resolution_

double jsk_footstep_planner::GridPathPlanner::map_resolution_
protected

Definition at line 131 of file grid_path_planner.h.

◆ mutex_

boost::mutex jsk_footstep_planner::GridPathPlanner::mutex_
protected

Definition at line 100 of file grid_path_planner.h.

◆ obstacle_points_

pcl::PointCloud<pcl::PointXYZ>::Ptr jsk_footstep_planner::GridPathPlanner::obstacle_points_
protected

Definition at line 115 of file grid_path_planner.h.

◆ obstacle_points_frame_id_

std::string jsk_footstep_planner::GridPathPlanner::obstacle_points_frame_id_
protected

Definition at line 120 of file grid_path_planner.h.

◆ obstacle_tree_

pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr jsk_footstep_planner::GridPathPlanner::obstacle_tree_
protected

Definition at line 117 of file grid_path_planner.h.

◆ plane_points_

pcl::PointCloud<pcl::PointNormal>::Ptr jsk_footstep_planner::GridPathPlanner::plane_points_
protected

Definition at line 114 of file grid_path_planner.h.

◆ plane_points_frame_id_

std::string jsk_footstep_planner::GridPathPlanner::plane_points_frame_id_
protected

Definition at line 119 of file grid_path_planner.h.

◆ plane_tree_

pcl::KdTreeFLANN<pcl::PointNormal>::Ptr jsk_footstep_planner::GridPathPlanner::plane_tree_
protected

Definition at line 116 of file grid_path_planner.h.

◆ pub_close_list_

ros::Publisher jsk_footstep_planner::GridPathPlanner::pub_close_list_
protected

Definition at line 104 of file grid_path_planner.h.

◆ pub_marker_

ros::Publisher jsk_footstep_planner::GridPathPlanner::pub_marker_
protected

Definition at line 107 of file grid_path_planner.h.

◆ pub_open_list_

ros::Publisher jsk_footstep_planner::GridPathPlanner::pub_open_list_
protected

Definition at line 105 of file grid_path_planner.h.

◆ pub_text_

ros::Publisher jsk_footstep_planner::GridPathPlanner::pub_text_
protected

Definition at line 106 of file grid_path_planner.h.

◆ result_

jsk_footstep_msgs::PlanFootstepsResult jsk_footstep_planner::GridPathPlanner::result_
protected

Definition at line 102 of file grid_path_planner.h.

◆ srv_collision_bounding_box_info_

ros::ServiceServer jsk_footstep_planner::GridPathPlanner::srv_collision_bounding_box_info_
protected

Definition at line 112 of file grid_path_planner.h.

◆ sub_obstacle_points_

ros::Subscriber jsk_footstep_planner::GridPathPlanner::sub_obstacle_points_
protected

Definition at line 110 of file grid_path_planner.h.

◆ sub_plane_points_

ros::Subscriber jsk_footstep_planner::GridPathPlanner::sub_plane_points_
protected

Definition at line 109 of file grid_path_planner.h.

◆ use_obstacle_points_

bool jsk_footstep_planner::GridPathPlanner::use_obstacle_points_
protected

Definition at line 136 of file grid_path_planner.h.

◆ use_plane_points_

bool jsk_footstep_planner::GridPathPlanner::use_plane_points_
protected

Definition at line 137 of file grid_path_planner.h.


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


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Sun May 28 2023 03:03:20