grid_path_planner.h
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 
3 #ifndef JSK_FOOTSTEP_PLANNER_GRID_PATH_PLANNER_H_
4 #define JSK_FOOTSTEP_PLANNER_GRID_PATH_PLANNER_H_
5 
6 #include <ros/ros.h>
8 
9 // ros
10 #include <sensor_msgs/PointCloud2.h>
11 //#include <dynamic_reconfigure/server.h>
12 #include <jsk_rviz_plugins/OverlayText.h>
13 #include <jsk_footstep_msgs/FootstepArray.h>
14 #include <jsk_footstep_msgs/PlanFootstepsAction.h>
15 //#include <jsk_footstep_planner/FootstepPlannerConfig.h>
16 #include <jsk_footstep_planner/CollisionBoundingBoxInfo.h>
17 
18 // pcl
19 #include <pcl/kdtree/kdtree_flann.h>
20 
21 // grid path planning
23 
24 namespace jsk_footstep_planner
25 {
27  {
29  OK,
30  WARNING,
31  ERROR
32  };
38  {
39  public:
43 
45 
46  protected:
47  virtual void pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
48  virtual void obstacleCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
49  virtual void planCB(const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr& goal);
50 
52  jsk_footstep_planner::CollisionBoundingBoxInfo::Request& req,
53  jsk_footstep_planner::CollisionBoundingBoxInfo::Response& res);
54  // buildGraph is not thread safe, it is responsible for caller to take care
55  // of mutex
56  virtual void buildGraph();
57 
58  // virtual void profile(FootstepAStarSolver<FootstepGraph>& solver, FootstepGraph::Ptr graph);
59 
60  virtual void publishPointCloud(
61  const pcl::PointCloud<pcl::PointXYZRGB>& cloud,
63  const std_msgs::Header& header);
64 
65  virtual void publishText(ros::Publisher& pub,
66  const std::string& text,
67  GridPlanningStatus status);
68 
69  virtual bool updateCost(GridState::Ptr ptr);
70  //solver.openListToPointCloud(open_list_cloud);
71  //solver.closeListToPointCloud(close_list_cloud);
72 
73  virtual void publishMarker();
74 
75  inline virtual void gridToPoint(const int ix, const int iy, Eigen::Vector3f &p)
76  {
77  Eigen::Vector3f add(map_resolution_ * (ix + 0.5), map_resolution_ * (iy + 0.5), 0);
78  p = map_offset_ + add;
79  }
80 
81  inline virtual void pointToGrid(const Eigen::Vector3f &p, int &ix, int &iy)
82  {
83  Eigen::Vector3f ipos = p - map_offset_;
84  ix = (int)(ipos[0] / map_resolution_);
85  iy = (int)(ipos[1] / map_resolution_);
86  }
87  // profile
88 
90  PerceptionGridGraph>::Ptr node,
92  int ix = node->getState()->indexX();
93  int iy = node->getState()->indexY();
94  double gx = graph->getGoalState()->indexX() - ix;
95  double gy = graph->getGoalState()->indexY() - iy;
96 
97  return std::sqrt(gx * gx + gy * gy);
98  }
99 
102  jsk_footstep_msgs::PlanFootstepsResult result_;
103 
108 
109  ros::Subscriber sub_plane_points_; // plane for stepping
110  ros::Subscriber sub_obstacle_points_; // collision detection with environment
111 
113 
114  pcl::PointCloud<pcl::PointNormal>::Ptr plane_points_;
115  pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_points_;
116  pcl::KdTreeFLANN<pcl::PointNormal>::Ptr plane_tree_;
117  pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr obstacle_tree_;
118 
121 
124 
125  Eigen::Vector3f map_offset_;
126 
127  // Parameters
128  Eigen::Vector3f collision_bbox_size_;
129  Eigen::Affine3f collision_bbox_offset_;
130 
135 
138  private:
139  };
140 }
141 
142 #endif
jsk_footstep_planner::OK
@ OK
Definition: footstep_planner.h:130
jsk_footstep_planner::PerceptionGridMap
Definition: grid_perception.h:33
jsk_footstep_planner::GridPathPlanner::sub_obstacle_points_
ros::Subscriber sub_obstacle_points_
Definition: grid_path_planner.h:110
jsk_footstep_planner::GridPathPlanner::Graph
PerceptionGridGraph Graph
Definition: grid_path_planner.h:40
jsk_footstep_planner::GridPathPlanner::plane_points_
pcl::PointCloud< pcl::PointNormal >::Ptr plane_points_
Definition: grid_path_planner.h:114
ros::Publisher
jsk_footstep_planner::GridPathPlanner::collision_circle_min_height_
double collision_circle_min_height_
Definition: grid_path_planner.h:133
jsk_footstep_planner::GridPlanningStatus
GridPlanningStatus
Definition: grid_path_planner.h:26
boost::shared_ptr
grid_perception.h
jsk_footstep_planner::NONE
@ NONE
Definition: grid_path_planner.h:28
jsk_footstep_planner::GridPathPlanner::as_
actionlib::SimpleActionServer< jsk_footstep_msgs::PlanFootstepsAction > as_
Definition: grid_path_planner.h:101
ros.h
cloud
pcl::PointCloud< pcl::PointNormal >::Ptr cloud
Definition: footstep_projection_demo.cpp:49
footstep_array_to_bounding_box_array.pub
pub
Definition: footstep_array_to_bounding_box_array.py:24
jsk_footstep_planner::GridPathPlanner::heuristicDistance
double heuristicDistance(SolverNode< PerceptionGridGraph::State, PerceptionGridGraph >::Ptr node, PerceptionGridGraph::Ptr graph)
Definition: grid_path_planner.h:89
jsk_footstep_planner::GridPathPlanner::use_obstacle_points_
bool use_obstacle_points_
Definition: grid_path_planner.h:136
jsk_footstep_planner::GridPathPlanner::graph_
PerceptionGridGraph::Ptr graph_
Definition: grid_path_planner.h:123
jsk_footstep_planner::SolverNode
Definition: solver_node.h:82
simple_action_server.h
jsk_footstep_planner::GridPathPlanner::result_
jsk_footstep_msgs::PlanFootstepsResult result_
Definition: grid_path_planner.h:102
jsk_footstep_planner::GridPathPlanner::publishMarker
virtual void publishMarker()
Definition: grid_path_planner.cpp:393
jsk_footstep_planner::GridPathPlanner::use_plane_points_
bool use_plane_points_
Definition: grid_path_planner.h:137
jsk_footstep_planner::ERROR
@ ERROR
Definition: footstep_planner.h:130
jsk_footstep_planner::GridPathPlanner::publishText
virtual void publishText(ros::Publisher &pub, const std::string &text, GridPlanningStatus status)
Definition: grid_path_planner.cpp:112
jsk_footstep_planner::GridPathPlanner::GridPathPlanner
GridPathPlanner(ros::NodeHandle &nh)
Definition: grid_path_planner.cpp:15
mutex
boost::mutex mutex
Definition: pointcloud_model_generator_node.cpp:46
jsk_footstep_planner::GridPathPlanner::obstacleCallback
virtual void obstacleCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)
Definition: grid_path_planner.cpp:80
jsk_footstep_planner::GridPathPlanner::pub_marker_
ros::Publisher pub_marker_
Definition: grid_path_planner.h:107
jsk_footstep_planner::GridPathPlanner::buildGraph
virtual void buildGraph()
Definition: grid_path_planner.cpp:55
ros::ServiceServer
jsk_footstep_planner::GridPathPlanner::mutex_
boost::mutex mutex_
Definition: grid_path_planner.h:100
jsk_footstep_planner::GridPathPlanner::srv_collision_bounding_box_info_
ros::ServiceServer srv_collision_bounding_box_info_
Definition: grid_path_planner.h:112
jsk_footstep_planner::WARNING
@ WARNING
Definition: footstep_planner.h:130
jsk_footstep_planner
Definition: ann_grid.h:50
jsk_footstep_planner::GridPathPlanner::map_resolution_
double map_resolution_
Definition: grid_path_planner.h:131
jsk_footstep_planner::GridPathPlanner::pub_open_list_
ros::Publisher pub_open_list_
Definition: grid_path_planner.h:105
pose_array.p
p
Definition: pose_array.py:21
graph
FootstepGraph::Ptr graph
Definition: footstep_planning_2d_interactive_demo.cpp:51
jsk_footstep_planner::GridPathPlanner::obstacle_points_
pcl::PointCloud< pcl::PointXYZ >::Ptr obstacle_points_
Definition: grid_path_planner.h:115
jsk_footstep_planner::GridPathPlanner::gridmap_
PerceptionGridMap::Ptr gridmap_
Definition: grid_path_planner.h:122
jsk_footstep_planner::GridGraph::State
Graph< GridState >::StateT State
Definition: grid_graph.h:91
jsk_footstep_planner::GridPathPlanner::pub_close_list_
ros::Publisher pub_close_list_
Definition: grid_path_planner.h:104
jsk_footstep_planner::GridPathPlanner::pointToGrid
virtual void pointToGrid(const Eigen::Vector3f &p, int &ix, int &iy)
Definition: grid_path_planner.h:81
jsk_footstep_planner::GridPathPlanner::collision_circle_radius_
double collision_circle_radius_
Definition: grid_path_planner.h:132
jsk_footstep_planner::GridPathPlanner::collisionBoundingBoxInfoService
virtual bool collisionBoundingBoxInfoService(jsk_footstep_planner::CollisionBoundingBoxInfo::Request &req, jsk_footstep_planner::CollisionBoundingBoxInfo::Response &res)
Definition: grid_path_planner.cpp:100
jsk_footstep_planner::GridPathPlanner::GridMap
PerceptionGridMap GridMap
Definition: grid_path_planner.h:41
add
bool add(const actionlib::TwoIntsGoal &req, actionlib::TwoIntsResult &res)
jsk_footstep_planner::GridPathPlanner::publishPointCloud
virtual void publishPointCloud(const pcl::PointCloud< pcl::PointXYZRGB > &cloud, ros::Publisher &pub, const std_msgs::Header &header)
Definition: grid_path_planner.cpp:325
jsk_footstep_planner::GridPathPlanner::collision_bbox_size_
Eigen::Vector3f collision_bbox_size_
Definition: grid_path_planner.h:128
jsk_footstep_planner::GridPathPlanner::planCB
virtual void planCB(const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr &goal)
Definition: grid_path_planner.cpp:153
jsk_footstep_planner::GridPathPlanner::pub_text_
ros::Publisher pub_text_
Definition: grid_path_planner.h:106
jsk_footstep_planner::GridPathPlanner::obstacle_points_frame_id_
std::string obstacle_points_frame_id_
Definition: grid_path_planner.h:120
jsk_footstep_planner::GridPathPlanner::gridToPoint
virtual void gridToPoint(const int ix, const int iy, Eigen::Vector3f &p)
Definition: grid_path_planner.h:75
actionlib::SimpleActionServer< jsk_footstep_msgs::PlanFootstepsAction >
jsk_footstep_planner::GridPathPlanner::updateCost
virtual bool updateCost(GridState::Ptr ptr)
Definition: grid_path_planner.cpp:336
jsk_footstep_planner::GridPathPlanner::map_offset_
Eigen::Vector3f map_offset_
Definition: grid_path_planner.h:125
jsk_footstep_planner::GridPathPlanner::Solver
GridAStarSolver< PerceptionGridGraph > Solver
Definition: grid_path_planner.h:42
jsk_footstep_planner::GridPathPlanner
Actionlib server for footstep planning.
Definition: grid_path_planner.h:37
jsk_footstep_planner::GridPathPlanner::plane_tree_
pcl::KdTreeFLANN< pcl::PointNormal >::Ptr plane_tree_
Definition: grid_path_planner.h:116
jsk_footstep_planner::GridAStarSolver
Definition: grid_astar_solver.h:11
jsk_footstep_planner::GridGraph
Definition: grid_graph.h:84
jsk_footstep_planner::GridPathPlanner::plane_points_frame_id_
std::string plane_points_frame_id_
Definition: grid_path_planner.h:119
jsk_footstep_planner::GridPathPlanner::collision_bbox_offset_
Eigen::Affine3f collision_bbox_offset_
Definition: grid_path_planner.h:129
jsk_footstep_planner::GridPathPlanner::obstacle_tree_
pcl::KdTreeFLANN< pcl::PointXYZ >::Ptr obstacle_tree_
Definition: grid_path_planner.h:117
jsk_footstep_planner::GridPathPlanner::sub_plane_points_
ros::Subscriber sub_plane_points_
Definition: grid_path_planner.h:109
ros::NodeHandle
ros::Subscriber
jsk_footstep_planner::GridPathPlanner::pointcloudCallback
virtual void pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)
Definition: grid_path_planner.cpp:90
jsk_footstep_planner::GridPathPlanner::collision_circle_max_height_
double collision_circle_max_height_
Definition: grid_path_planner.h:134


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