grid_path_planner.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 
4 
5 //param
7 
9 
10 #include <visualization_msgs/MarkerArray.h>
12 
13 namespace jsk_footstep_planner
14 {
16  as_(nh, nh.getNamespace(),
17  boost::bind(&GridPathPlanner::planCB, this, _1), false),
18  plane_tree_(new pcl::KdTreeFLANN<pcl::PointNormal>),
19  obstacle_tree_(new pcl::KdTreeFLANN<pcl::PointXYZ>)
20  {
21  pub_text_ = nh.advertise<jsk_rviz_plugins::OverlayText> ("text", 1, true);
22  pub_marker_ = nh.advertise<visualization_msgs::MarkerArray> ("grid_graph_marker", 2, true);
23  pub_close_list_ = nh.advertise<sensor_msgs::PointCloud2> ("close_list", 1, true);
24  pub_open_list_ = nh.advertise<sensor_msgs::PointCloud2> ("open_list", 1, true);
25 
27  "collision_bounding_box_info", &GridPathPlanner::collisionBoundingBoxInfoService, this);
28 
29  sub_plane_points_ = nh.subscribe("plane_points", 1, &GridPathPlanner::pointcloudCallback, this);
30  sub_obstacle_points_ = nh.subscribe("obstacle_points", 1, &GridPathPlanner::obstacleCallback, this);
31 
32  std::vector<double> collision_bbox_size, collision_bbox_offset;
33  if (jsk_topic_tools::readVectorParameter(nh, "collision_bbox_size", collision_bbox_size)) {
34  collision_bbox_size_[0] = collision_bbox_size[0];
35  collision_bbox_size_[1] = collision_bbox_size[1];
36  collision_bbox_size_[2] = collision_bbox_size[2];
37  }
38  if (jsk_topic_tools::readVectorParameter(nh, "collision_bbox_offset", collision_bbox_offset)) {
39  collision_bbox_offset_ = Eigen::Affine3f::Identity() * Eigen::Translation3f(collision_bbox_offset[0],
40  collision_bbox_offset[1],
41  collision_bbox_offset[2]);
42  }
43 
44  nh.param("map_resolution", map_resolution_, 0.4);
45  ROS_INFO("map resolution: %f", map_resolution_);
46  nh.param("collision_circle_radius", collision_circle_radius_, 0.35);
47  nh.param("collision_circle_min_height", collision_circle_min_height_, 0.4);
48  nh.param("collision_circle_max_height", collision_circle_max_height_, 1.9);
49  use_obstacle_points_ = true;
50  use_plane_points_ = true;
51 
52  as_.start();
53  }
54 
56  {
57  //boost::mutex::scoped_lock lock(mutex_);
58  ROS_INFO("build plane %d", plane_points_->points.size());
59  Eigen::Vector4f minpt, maxpt;
60  pcl::getMinMax3D<pcl::PointNormal> (*plane_points_, minpt, maxpt);
61 
62  Eigen::Vector4f len = maxpt - minpt;
63  map_offset_[0] = minpt[0];
64  map_offset_[1] = minpt[1];
65  map_offset_[2] = 0.0;
66  int sizex = (int)(len[0] / map_resolution_);
67  int sizey = (int)(len[1] / map_resolution_);
68 
69  ROS_INFO("min_point: [%f, %f] / size: %d %d",
70  map_offset_[0], map_offset_[1], sizex, sizey);
71 
72  obstacle_tree_->setInputCloud(obstacle_points_);
73  plane_tree_->setInputCloud(plane_points_);
74 
75  gridmap_.reset(new GridMap(sizex, sizey));
76  gridmap_->setCostFunction(boost::bind(&GridPathPlanner::updateCost, this, _1));
77  graph_.reset(new Graph(gridmap_));
78  }
79 
80  void GridPathPlanner::obstacleCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
81  {
82  boost::mutex::scoped_lock lock(mutex_);
83  //ROS_DEBUG("obstacle points is updated");
84  ROS_INFO("obstacle points is updated");
85  obstacle_points_.reset(new pcl::PointCloud<pcl::PointXYZ>);
87  obstacle_points_frame_id_ = msg->header.frame_id; // check frame_id
88  }
89 
90  void GridPathPlanner::pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
91  {
92  boost::mutex::scoped_lock lock(mutex_);
93  //ROS_DEBUG("pointcloud points is updated");
94  ROS_INFO("pointcloud points is updated");
95  plane_points_.reset(new pcl::PointCloud<pcl::PointNormal>);
97  plane_points_frame_id_ = msg->header.frame_id; // check frame_id
98  }
99 
101  jsk_footstep_planner::CollisionBoundingBoxInfo::Request& req,
102  jsk_footstep_planner::CollisionBoundingBoxInfo::Response& res)
103  {
104  boost::mutex::scoped_lock lock(mutex_);
105  res.box_dimensions.x = collision_bbox_size_[0];
106  res.box_dimensions.y = collision_bbox_size_[1];
107  res.box_dimensions.z = collision_bbox_size_[2];
109  return true;
110  }
111 
113  const std::string& text,
114  GridPlanningStatus status)
115  {
116  std_msgs::ColorRGBA ok_color;
117  ok_color.r = 0.3568627450980392;
118  ok_color.g = 0.7529411764705882;
119  ok_color.b = 0.8705882352941177;
120  ok_color.a = 1.0;
121  std_msgs::ColorRGBA warn_color;
122  warn_color.r = 0.9411764705882353;
123  warn_color.g = 0.6784313725490196;
124  warn_color.b = 0.3058823529411765;
125  warn_color.a = 1.0;
126  std_msgs::ColorRGBA error_color;
127  error_color.r = 0.8509803921568627;
128  error_color.g = 0.3254901960784314;
129  error_color.b = 0.30980392156862746;
130  error_color.a = 1.0;
131  std_msgs::ColorRGBA color;
132  if (status == OK) {
133  color = ok_color;
134  }
135  else if (status == WARNING) {
136  color = warn_color;
137  }
138  else if (status == ERROR) {
139  color = error_color;
140  }
141  jsk_rviz_plugins::OverlayText msg;
142  msg.text = text;
143  msg.width = 1000;
144  msg.height = 1000;
145  msg.top = 10;
146  msg.left = 10;
147  msg.bg_color.a = 0.0;
148  msg.fg_color = color;
149  msg.text_size = 24;
150  pub.publish(msg);
151  }
152 
153  void GridPathPlanner::planCB(const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr& goal)
154  {
155  boost::mutex::scoped_lock lock(mutex_);
156  // latest_header_ = goal->goal_footstep.header;
157  ROS_INFO("planCB");
158 
159  // check frame_id sanity
160  std::string goal_frame_id = goal->initial_footstep.header.frame_id;
161 #if 0
162  if (use_plane_points_) {
163  // check perception cloud header
164  if (goal_frame_id != plane_points_frame_id_) {
165  ROS_ERROR("frame_id of goal and pointcloud do not match. goal: %s, pointcloud: %s.",
166  goal_frame_id.c_str(), plane_points_frame_id_.c_str());
167  as_.setPreempted();
168  return;
169  }
170  }
171  if (use_obstacle_points_) {
172  // check perception cloud header
173  if (goal_frame_id != obstacle_points_frame_id_) {
174  ROS_ERROR("frame_id of goal and obstacle pointcloud do not match. goal: %s, obstacle: %s.",
175  goal_frame_id.c_str(), obstacle_points_frame_id_.c_str());
176  as_.setPreempted();
177  return;
178  }
179  }
180 #endif
181  double initx = 0;
182  double inity = 0;
183  double goalx = 0;
184  double goaly = 0;
185  for(int i = 0; i < goal->initial_footstep.footsteps.size(); i++) {
186  initx += goal->initial_footstep.footsteps[i].pose.position.x;
187  inity += goal->initial_footstep.footsteps[i].pose.position.y;
188  }
189  initx /= goal->initial_footstep.footsteps.size();
190  inity /= goal->initial_footstep.footsteps.size();
191  for(int i = 0; i < goal->goal_footstep.footsteps.size(); i++) {
192  goalx += goal->goal_footstep.footsteps[i].pose.position.x;
193  goaly += goal->goal_footstep.footsteps[i].pose.position.y;
194  }
195  goalx /= goal->initial_footstep.footsteps.size();
196  goaly /= goal->initial_footstep.footsteps.size();
197 
198  ROS_INFO("start: %f %f, goal %f %f", initx, inity, goalx, goaly);
199 
200  ROS_INFO("build graph");
201  buildGraph();
202 
203  ROS_INFO("solve");
204  Eigen::Vector3f startp(initx, inity, 0);
205  Eigen::Vector3f goalp(goalx, goaly, 0);
206  int sx, sy, gx, gy;
207  pointToGrid(startp, sx, sy);
208  pointToGrid(goalp, gx, gy);
209 
210  if(!gridmap_->inRange(sx, sy)) {
211  ROS_ERROR("start is not in range %d %d", sx, sy);
212  as_.setPreempted();
213  return;
214  }
215 
216  if(!gridmap_->inRange(gx, gy)) {
217  ROS_ERROR("goal is not in range %d %d", gx, gy);
218  as_.setPreempted();
219  return;
220  }
221 
222  GridState::Ptr start_state = graph_->getState(sx, sy);
223  GridState::Ptr goal_state = graph_->getState(gx, gy);
224 
225  if(start_state->getOccupancy() != 0) {
226  ROS_ERROR("start state is occupied");
227  as_.setPreempted();
228  return;
229  }
230  if(goal_state->getOccupancy() != 0) {
231  ROS_ERROR("goal state is occupied");
232  as_.setPreempted();
233  return;
234  }
235 
236  graph_->setStartState(start_state);
237  graph_->setGoalState(goal_state);
238 
239  Solver solver(graph_);
240  //solver.setHeuristic(boost::bind(&gridPerceptionHeuristicDistance, _1, _2));
241  solver.setHeuristic(boost::bind(&GridPathPlanner::heuristicDistance, this, _1, _2));
242 
243  Solver::Path path = solver.solve();
244  if(path.size() < 1) {
245  ROS_ERROR("Failed to plan path");
246  ROS_INFO("pub marker"); //debug
247  publishMarker(); //debug
248  as_.setPreempted();
249  return;
250  }
251  ROS_INFO("solved path_size: %d", path.size());
252  std::vector<Eigen::Vector3f > points;
253  for(int i = 0; i < path.size(); i++) {
254  //Solver::StatePtr st = path[i]->getState();
255  int ix = path[i]->getState()->indexX();
256  int iy = path[i]->getState()->indexY();
257  Eigen::Vector3f p;
258  gridToPoint(ix, iy, p);
259  points.push_back(p);
260  ROS_INFO("path %d (%f %f) [%d - %d]", i, ix, iy, p[0], p[1]);
261  }
262  {
264  visualization_msgs::MarkerArray ma;
265  visualization_msgs::Marker m;
266  pl.toMarker(m);
267  m.header.frame_id = "map";
268  m.id = 101;
269  m.scale.x = 0.05;
270  ma.markers.push_back(m);
271  pub_marker_.publish(ma);
272  }
273  ROS_INFO("pub marker");
274  publishMarker();
275 
276  {
277  jsk_footstep_msgs::PlanFootstepsResult result_;
278  result_.result.header = goal->goal_footstep.header;
279 
280  std::vector<Eigen::Vector3f > points;
281  for(int i = 0; i < path.size(); i++) {
282  int ix = path[i]->getState()->indexX();
283  int iy = path[i]->getState()->indexY();
284  Eigen::Vector3f p;
285  gridToPoint(ix, iy, p);
286  jsk_footstep_msgs::Footstep fs;
287  fs.leg = jsk_footstep_msgs::Footstep::LLEG;
288  fs.dimensions.x = map_resolution_;
289  fs.dimensions.y = map_resolution_;
290  fs.dimensions.z = 0.01;
291  fs.pose.orientation.x = 0;
292  fs.pose.orientation.y = 0;
293  fs.pose.orientation.z = 0;
294  fs.pose.orientation.w = 1;
295  fs.pose.position.x = p[0];
296  fs.pose.position.y = p[1];
297  fs.pose.position.z = p[2];
298  result_.result.footsteps.push_back(fs);
299  }
300  as_.setSucceeded(result_);
301  }
302 
303 #if 0
304  pcl::PointCloud<pcl::PointXYZRGB> close_list_cloud, open_list_cloud;
305  //solver.openListToPointCloud(open_list_cloud);
306  //solver.closeListToPointCloud(close_list_cloud);
307  publishPointCloud(close_list_cloud, pub_close_list_, goal->goal_footstep.header);
308  publishPointCloud(open_list_cloud, pub_open_list_, goal->goal_footstep.header);
309 #endif
310 
311 #if 0
313  (boost::format("Took %f sec\nPerception took %f sec\nPlanning took %f sec\n%lu path\nopen list: %lu\nclose list:%lu")
314  % planning_duration
315  % graph_->getPerceptionDuration().toSec()
316  % (planning_duration - graph_->getPerceptionDuration().toSec())
317  % path.size()
318  % open_list_cloud.points.size()
319  % close_list_cloud.points.size()).str(),
320  OK);
321  ROS_INFO_STREAM("use_obstacle_points: " << graph_->useObstaclePoints());
322 #endif
323  }
324 
326  const pcl::PointCloud<pcl::PointXYZRGB>& cloud,
328  const std_msgs::Header& header)
329  {
330  sensor_msgs::PointCloud2 ros_cloud;
331  pcl::toROSMsg(cloud, ros_cloud);
332  ros_cloud.header = header;
333  pub.publish(ros_cloud);
334  }
335 
337  {
338  // determine occupancy
340  {
341  Eigen::Vector3f p;
342  gridToPoint(ptr->indexX(), ptr->indexY(), p);
343 
345  //double pos1 = collision_circle_max_height_ - collision_circle_radius_;
346  int size = 0;
347  std::vector<double> pos;
348  pos.push_back(pos0);
349 
350  for(std::vector<double>::iterator it = pos.begin();
351  it != pos.end(); it++) {
352  std::vector<int> near_indices;
353  std::vector<float> distances;
354  pcl::PointXYZ center;
355  p[2] = *it;
356  center.getVector3fMap() = p;
357 
358  obstacle_tree_->radiusSearch(center, collision_circle_radius_, near_indices, distances);
359 
360  //std::cerr << "oz: " << near_indices.size() << std::endl;
361  size += near_indices.size();
362  }
363  if(size > 1) { // TODO: occupancy_threshold
364  ptr->setOccupancy(1);
365  } else {
366  ptr->setOccupancy(0);
367  }
368  }
369 
370  // determine cost
372  {
373  Eigen::Vector3f p;
374  gridToPoint(ptr->indexX(), ptr->indexY(), p);
375 
376  std::vector<int> plane_indices;
377  std::vector<float> distances;
378  pcl::PointNormal center;
379  center.getVector3fMap() = p;
380  plane_tree_->radiusSearch(center, map_resolution_ * 1.4, plane_indices, distances);
381  //std::cerr << "ps: " << plane_indices.size() << std::endl;
382  // TODO: point num / point average / point deviation
383  double cost = 0.0;
384  if (plane_indices.size() < 50) { // TODO: plane threshold
385  ptr->setOccupancy(2 + ptr->getOccupancy());
386  }
387  ptr->setCost(cost);
388  }
389 
390  return false;
391  }
392 
394  {
395  visualization_msgs::MarkerArray ma;
396 
397  int sx = gridmap_->sizeX();
398  int sy = gridmap_->sizeY();
399 
400  { // state plane
401  visualization_msgs::Marker m;
402  m.header.frame_id = "map";
403  m.type = visualization_msgs::Marker::CUBE_LIST;
404  m.id = 100;
405  m.scale.x = map_resolution_;
406  m.scale.y = map_resolution_;
407  m.scale.z = 0.01;
408  m.color.r = 0.0;
409  m.color.g = 0.0;
410  m.color.b = 1.0;
411  m.color.a = 1.0;
412 
413  for(int y = 0; y < sy; y++) {
414  for(int x = 0; x < sx; x++) {
415  GridState::Ptr st = graph_->getState(x, y);
416  Eigen::Vector3f p;
417  gridToPoint(x, y, p);
418  p[2] = -0.005;
419  geometry_msgs::Point gp;
420  gp.x = p[0];
421  gp.y = p[1];
422  gp.z = p[2];
423  m.points.push_back(gp);
424 
425  std_msgs::ColorRGBA col;
426  if(st->getOccupancy() == 1) {
427  col.r = 0.0;
428  col.g = 0.15;
429  col.b = 0.0;
430  col.a = 1.0;
431  } else if(st->getOccupancy() == 2) {
432  col.r = 0.0;
433  col.g = 0.0;
434  col.b = 0.0;
435  col.a = 1.0;
436  } else if(st->getOccupancy() == 3) {
437  col.r = 0.15;
438  col.g = 0.0;
439  col.b = 0.0;
440  col.a = 1.0;
441  } else {
442  col.r = 0.0;
443  col.g = 0.0;
444  col.b = 1.0;
445  col.a = 1.0;
446  }
447 
448  m.colors.push_back(col);
449  }
450  }
451  ma.markers.push_back(m);
452  }
453 
454  pub_marker_.publish(ma);
455  }
456 }
msg
path
pos
void publish(const boost::shared_ptr< M > &message) const
void toMarker(visualization_msgs::Marker &marker) const
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
virtual void gridToPoint(const int ix, const int iy, Eigen::Vector3f &p)
virtual void pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)
virtual std::vector< typename SolverNode< State, GraphT >::Ptr > solve(const ros::WallDuration &timeout=ros::WallDuration(1000000000.0))
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual void obstacleCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
double y
virtual void publishPointCloud(const pcl::PointCloud< pcl::PointXYZRGB > &cloud, ros::Publisher &pub, const std_msgs::Header &header)
actionlib::SimpleActionServer< jsk_footstep_msgs::PlanFootstepsAction > as_
bool readVectorParameter(ros::NodeHandle &nh, const std::string &param_name, std::vector< double > &result)
size
virtual bool updateCost(GridState::Ptr ptr)
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
virtual void planCB(const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr &goal)
ROSCPP_DECL const std::string & getNamespace()
pcl::PointCloud< pcl::PointNormal >::Ptr cloud
virtual void setHeuristic(HeuristicFunction h)
Definition: astar_solver.h:71
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
string str
pcl::KdTreeFLANN< pcl::PointNormal >::Ptr plane_tree_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
pcl::PointCloud< pcl::PointNormal >::Ptr plane_points_
virtual bool collisionBoundingBoxInfoService(jsk_footstep_planner::CollisionBoundingBoxInfo::Request &req, jsk_footstep_planner::CollisionBoundingBoxInfo::Response &res)
void toROSMsg(const pcl::PointCloud< T > &cloud, sensor_msgs::Image &msg)
points
mutex_t * lock
pcl::KdTreeFLANN< pcl::PointXYZ >::Ptr obstacle_tree_
ros::ServiceServer srv_collision_bounding_box_info_
jsk_footstep_msgs::PlanFootstepsResult result_
#define ROS_INFO_STREAM(args)
std::vector< typename SolverNode< State, GraphT >::Ptr > Path
pcl::PointCloud< pcl::PointXYZ >::Ptr obstacle_points_
virtual void publishText(ros::Publisher &pub, const std::string &text, GridPlanningStatus status)
Actionlib server for footstep planning.
double x
#define ROS_ERROR(...)
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 Fri May 14 2021 02:51:52