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
virtual void gridToPoint(const int ix, const int iy, Eigen::Vector3f &p)
virtual void pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)
virtual void obstacleCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)
bool add(const actionlib::TwoIntsGoal &req, actionlib::TwoIntsResult &res)
virtual void publishPointCloud(const pcl::PointCloud< pcl::PointXYZRGB > &cloud, ros::Publisher &pub, const std_msgs::Header &header)
actionlib::SimpleActionServer< jsk_footstep_msgs::PlanFootstepsAction > as_
virtual bool updateCost(GridState::Ptr ptr)
virtual void planCB(const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr &goal)
pcl::PointCloud< pcl::PointNormal >::Ptr cloud
GridAStarSolver< PerceptionGridGraph > Solver
FootstepGraph::Ptr graph
pcl::KdTreeFLANN< pcl::PointNormal >::Ptr plane_tree_
pcl::PointCloud< pcl::PointNormal >::Ptr plane_points_
virtual bool collisionBoundingBoxInfoService(jsk_footstep_planner::CollisionBoundingBoxInfo::Request &req, jsk_footstep_planner::CollisionBoundingBoxInfo::Response &res)
Graph< GridState >::StateT State
Definition: grid_graph.h:91
pcl::KdTreeFLANN< pcl::PointXYZ >::Ptr obstacle_tree_
ros::ServiceServer srv_collision_bounding_box_info_
jsk_footstep_msgs::PlanFootstepsResult result_
pcl::PointCloud< pcl::PointXYZ >::Ptr obstacle_points_
virtual void publishText(ros::Publisher &pub, const std::string &text, GridPlanningStatus status)
boost::mutex mutex
Actionlib server for footstep planning.
virtual void pointToGrid(const Eigen::Vector3f &p, int &ix, int &iy)
double heuristicDistance(SolverNode< PerceptionGridGraph::State, PerceptionGridGraph >::Ptr node, PerceptionGridGraph::Ptr graph)


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