footstep_planner.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
40 #include <pcl/common/angles.h>
41 #include <boost/format.hpp>
42 
43 namespace jsk_footstep_planner
44 {
46  as_(nh, nh.getNamespace(),
47  boost::bind(&FootstepPlanner::planCB, this, _1), false)
48  {
49  srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (nh);
50  typename dynamic_reconfigure::Server<Config>::CallbackType f =
51  boost::bind (&FootstepPlanner::configCallback, this, _1, _2);
52  srv_->setCallback (f);
53  pub_text_ = nh.advertise<jsk_rviz_plugins::OverlayText>(
54  "text", 1, true);
55  pub_close_list_ = nh.advertise<sensor_msgs::PointCloud2>(
56  "close_list", 1, true);
57  pub_open_list_ = nh.advertise<sensor_msgs::PointCloud2>(
58  "open_list", 1, true);
60  "project_footprint", &FootstepPlanner::projectFootPrintService, this);
62  "project_footprint_with_local_search", &FootstepPlanner::projectFootPrintWithLocalSearchService, this);
64  "collision_bounding_box_info", &FootstepPlanner::collisionBoundingBoxInfoService, this);
66  "project_footstep", &FootstepPlanner::projectFootstepService, this);
68  "set_heuristic_path", &FootstepPlanner::setHeuristicPathService, this);
69  std::vector<double> lleg_footstep_offset, rleg_footstep_offset;
70  if (jsk_topic_tools::readVectorParameter(nh, "lleg_footstep_offset", lleg_footstep_offset)) {
71  inv_lleg_footstep_offset_ = Eigen::Vector3f(- lleg_footstep_offset[0],
72  - lleg_footstep_offset[1],
73  - lleg_footstep_offset[2]);
74  } else {
75  inv_lleg_footstep_offset_ = Eigen::Vector3f(0, 0, 0);
76  }
77  if (jsk_topic_tools::readVectorParameter(nh, "rleg_footstep_offset", rleg_footstep_offset)) {
78  inv_rleg_footstep_offset_ = Eigen::Vector3f(- rleg_footstep_offset[0],
79  - rleg_footstep_offset[1],
80  - rleg_footstep_offset[2]);
81  } else {
82  inv_rleg_footstep_offset_ = Eigen::Vector3f(0, 0, 0);
83  }
84  {
85  boost::mutex::scoped_lock lock(mutex_);
86  if (!readSuccessors(nh)) {
87  return;
88  }
89 
90  ROS_INFO("building graph");
91  buildGraph();
92  ROS_INFO("build graph done");
93  }
94  sub_pointcloud_model_ = nh.subscribe("pointcloud_model", 1, &FootstepPlanner::pointcloudCallback, this);
95  sub_obstacle_model_ = nh.subscribe("obstacle_model", 1, &FootstepPlanner::obstacleCallback, this);
96  std::vector<double> collision_bbox_size, collision_bbox_offset;
97  if (jsk_topic_tools::readVectorParameter(nh, "collision_bbox_size", collision_bbox_size)) {
98  collision_bbox_size_[0] = collision_bbox_size[0];
99  collision_bbox_size_[1] = collision_bbox_size[1];
100  collision_bbox_size_[2] = collision_bbox_size[2];
101  }
102  if (jsk_topic_tools::readVectorParameter(nh, "collision_bbox_offset", collision_bbox_offset)) {
103  collision_bbox_offset_ = Eigen::Affine3f::Identity() * Eigen::Translation3f(collision_bbox_offset[0],
104  collision_bbox_offset[1],
105  collision_bbox_offset[2]);
106  }
107  as_.start();
108  }
109 
111  const sensor_msgs::PointCloud2::ConstPtr& msg)
112  {
113  boost::mutex::scoped_lock lock(mutex_);
114  obstacle_model_.reset(new pcl::PointCloud<pcl::PointXYZ>);
116  obstacle_model_frame_id_ = msg->header.frame_id;
117  if (graph_ && use_obstacle_model_) {
118  graph_->setObstacleModel(obstacle_model_);
119  }
120  }
121 
123  const sensor_msgs::PointCloud2::ConstPtr& msg)
124  {
125  boost::mutex::scoped_lock lock(mutex_);
126  ROS_DEBUG("pointcloud model is updated");
127  pointcloud_model_.reset(new pcl::PointCloud<pcl::PointNormal>);
129  pointcloud_model_frame_id_ = msg->header.frame_id;
130  if (graph_ && use_pointcloud_model_) {
131  graph_->setPointCloudModel(pointcloud_model_);
132  }
133  }
134 
136  const Eigen::Affine3f& center_pose,
137  const Eigen::Affine3f& left_pose_trans,
138  const Eigen::Affine3f& right_pose_trans,
139  geometry_msgs::Pose& pose)
140  {
141  const Eigen::Vector3f resolution(resolution_x_,
144  const Eigen::Vector3f footstep_size(footstep_size_x_,
146  0.000001);
147  Eigen::Affine3f left_pose = center_pose * left_pose_trans;
148  Eigen::Affine3f right_pose = center_pose * right_pose_trans;
149  FootstepState::Ptr left_input(new FootstepState(
150  jsk_footstep_msgs::Footstep::LEFT,
151  left_pose,
152  footstep_size,
153  resolution));
154  FootstepState::Ptr right_input(new FootstepState(
155  jsk_footstep_msgs::Footstep::RIGHT,
156  right_pose,
157  footstep_size,
158  resolution));
159  FootstepState::Ptr projected_left = graph_->projectFootstep(left_input);
160  FootstepState::Ptr projected_right = graph_->projectFootstep(right_input);
161  if (!projected_left || !projected_right) {
162  return false;
163  }
164  Eigen::Affine3f projected_left_pose = projected_left->getPose();
165  Eigen::Affine3f projected_right_pose = projected_right->getPose();
166  Eigen::Quaternionf rot = Eigen::Quaternionf(projected_left_pose.rotation()).slerp(
167  0.5, Eigen::Quaternionf(projected_right_pose.rotation()));
168  Eigen::Vector3f pos = (Eigen::Vector3f(projected_right_pose.translation()) +
169  Eigen::Vector3f(projected_left_pose.translation())) / 2.0;
170  Eigen::Affine3f mid = Eigen::Translation3f(pos) * rot;
171  tf::poseEigenToMsg(mid, pose);
172  return true;
173  }
174 
176  jsk_interactive_marker::SnapFootPrint::Request& req,
177  jsk_interactive_marker::SnapFootPrint::Response& res)
178  {
179  boost::mutex::scoped_lock lock(mutex_);
180  if (!graph_ ) {
181  return false;
182  }
184  ROS_ERROR("No pointcloud model is yet available");
186  "No pointcloud model is yet available",
187  ERROR);
188  return false;
189  }
190  Eigen::Affine3f center_pose, left_pose_trans, right_pose_trans;
191  std::vector<Eigen::Affine3f> center_poses;
192  tf::poseMsgToEigen(req.lleg_pose, left_pose_trans);
193  tf::poseMsgToEigen(req.rleg_pose, right_pose_trans);
194  tf::poseMsgToEigen(req.input_pose.pose, center_pose);
195  const double dx = 0.05;
196  const double dy = 0.05;
197  const double dtheta = pcl::deg2rad(5.0);
198  for (int xi = 0; xi < 3; xi++) {
199  for (int yi = 0; yi < 3; yi++) {
200  for (int thetai = 0; thetai < 3; thetai++) {
201  Eigen::Affine3f transppp = affineFromXYYaw(xi * dx, yi * dy, thetai * dtheta);
202  Eigen::Affine3f transppm = affineFromXYYaw(xi * dx, yi * dy, - thetai * dtheta);
203  Eigen::Affine3f transpmp = affineFromXYYaw(xi * dx, - yi * dy, thetai * dtheta);
204  Eigen::Affine3f transpmm = affineFromXYYaw(xi * dx, - yi * dy, -thetai * dtheta);
205  Eigen::Affine3f transmpp = affineFromXYYaw(- xi * dx, yi * dy, thetai * dtheta);
206  Eigen::Affine3f transmpm = affineFromXYYaw(- xi * dx, yi * dy, - thetai * dtheta);
207  Eigen::Affine3f transmmp = affineFromXYYaw(- xi * dx, - yi * dy, thetai * dtheta);
208  Eigen::Affine3f transmmm = affineFromXYYaw(- xi * dx, - yi * dy, - thetai * dtheta);
209  center_poses.push_back(center_pose * transppp);
210  center_poses.push_back(center_pose * transppm);
211  center_poses.push_back(center_pose * transpmp);
212  center_poses.push_back(center_pose * transpmm);
213  center_poses.push_back(center_pose * transmpp);
214  center_poses.push_back(center_pose * transmpm);
215  center_poses.push_back(center_pose * transmmp);
216  center_poses.push_back(center_pose * transmmm);
217  }
218  }
219  }
220  for (size_t i = 0; i < center_poses.size(); i++) {
221  if (projectFootPrint(center_poses[i], left_pose_trans, right_pose_trans,
222  res.snapped_pose.pose)) {
223  res.success = true;
224  res.snapped_pose.header = req.input_pose.header;
225  return true;
226  }
227  }
228  ROS_ERROR("Failed to project footprint");
230  "Failed to project goal",
231  ERROR);
232  return false;
233  }
234 
236  jsk_footstep_planner::CollisionBoundingBoxInfo::Request& req,
237  jsk_footstep_planner::CollisionBoundingBoxInfo::Response& res)
238  {
239  boost::mutex::scoped_lock lock(mutex_);
240  res.box_dimensions.x = collision_bbox_size_[0];
241  res.box_dimensions.y = collision_bbox_size_[1];
242  res.box_dimensions.z = collision_bbox_size_[2];
244  return true;
245  }
246 
248  jsk_footstep_planner::ProjectFootstep::Request& req,
249  jsk_footstep_planner::ProjectFootstep::Response& res)
250  {
251  boost::mutex::scoped_lock lock(mutex_);
252  if (!graph_) {
253  return false;
254  }
255  if (!pointcloud_model_) {
256  ROS_ERROR("No pointcloud model is yet available");
257  //publishText(pub_text_,
258  //"No pointcloud model is yet available",
259  //ERROR);
260  return false;
261  }
262 
263  const Eigen::Vector3f resolution(resolution_x_,
266  const Eigen::Vector3f footstep_size(footstep_size_x_,
268  0.000001);
269 
270  for (std::vector<jsk_footstep_msgs::Footstep>::iterator it = req.input.footsteps.begin();
271  it != req.input.footsteps.end(); it++) {
272  if (it->offset.x == 0.0 &&
273  it->offset.y == 0.0 &&
274  it->offset.z == 0.0 ) {
275  if (it->leg == jsk_footstep_msgs::Footstep::LEFT) {
276  it->offset.x = - inv_lleg_footstep_offset_[0];
277  it->offset.y = - inv_lleg_footstep_offset_[1];
278  it->offset.z = - inv_lleg_footstep_offset_[2];
279  } else {
280  it->offset.x = - inv_rleg_footstep_offset_[0];
281  it->offset.y = - inv_rleg_footstep_offset_[1];
282  it->offset.z = - inv_rleg_footstep_offset_[2];
283  }
284  }
285  if(it->dimensions.x == 0 &&
286  it->dimensions.y == 0 &&
287  it->dimensions.z == 0 ) {
288  it->dimensions.x = footstep_size_x_;
289  it->dimensions.y = footstep_size_y_;
290  it->dimensions.z = 0.000001;
291  }
292  FootstepState::Ptr step = FootstepState::fromROSMsg(*it, footstep_size, resolution);
293  FootstepState::Ptr projected = graph_->projectFootstep(step);
294  if(!!projected) {
295  res.success.push_back(true);
296  jsk_footstep_msgs::Footstep::Ptr p;
297  if (it->leg == jsk_footstep_msgs::Footstep::LEFT) {
298  p = projected->toROSMsg(inv_lleg_footstep_offset_);
299  } else if (it->leg == jsk_footstep_msgs::Footstep::RIGHT) {
300  p = projected->toROSMsg(inv_rleg_footstep_offset_);
301  } else {
302  p = projected->toROSMsg();
303  }
304  res.result.footsteps.push_back(*p);
305  } else {
306  res.success.push_back(false);
307  res.result.footsteps.push_back(*it); // return the same step as in input
308  }
309  }
310  res.result.header = req.input.header;
311 
312  return true;
313  }
314 
316  jsk_interactive_marker::SnapFootPrint::Request& req,
317  jsk_interactive_marker::SnapFootPrint::Response& res)
318  {
319  boost::mutex::scoped_lock lock(mutex_);
320  if (!graph_) {
321  return false;
322  }
323  if (!pointcloud_model_) {
324  ROS_ERROR("No pointcloud model is yet available");
326  "No pointcloud model is yet available",
327  ERROR);
328  return false;
329  }
330  Eigen::Affine3f center_pose, left_pose_trans, right_pose_trans;
331  tf::poseMsgToEigen(req.lleg_pose, left_pose_trans);
332  tf::poseMsgToEigen(req.rleg_pose, right_pose_trans);
333  tf::poseMsgToEigen(req.input_pose.pose, center_pose);
334  if (projectFootPrint(center_pose, left_pose_trans, right_pose_trans,
335  res.snapped_pose.pose)) {
336  res.success = true;
337  res.snapped_pose.header = req.input_pose.header;
338  return true;
339  }
340  else {
341  ROS_ERROR("Failed to project footprint");
343  "Failed to project goal",
344  ERROR);
345  return false;
346  }
347  }
349  jsk_footstep_planner::SetHeuristicPath::Request& req,
350  jsk_footstep_planner::SetHeuristicPath::Response& res)
351  {
352  boost::mutex::scoped_lock lock(mutex_);
353  if (!graph_) {
354  return false;
355  }
356  std::vector<Eigen::Vector3f> points;
357  for(int i = 0; i < req.segments.size(); i++) {
358  Eigen::Vector3f p(req.segments[i].x,
359  req.segments[i].y,
360  req.segments[i].z);
361  points.push_back(p);
362  }
365 
366  return true;
367  }
368 
370  const std::string& text,
371  PlanningStatus status)
372  {
373  std_msgs::ColorRGBA ok_color;
374  ok_color.r = 0.3568627450980392;
375  ok_color.g = 0.7529411764705882;
376  ok_color.b = 0.8705882352941177;
377  ok_color.a = 1.0;
378  std_msgs::ColorRGBA warn_color;
379  warn_color.r = 0.9411764705882353;
380  warn_color.g = 0.6784313725490196;
381  warn_color.b = 0.3058823529411765;
382  warn_color.a = 1.0;
383  std_msgs::ColorRGBA error_color;
384  error_color.r = 0.8509803921568627;
385  error_color.g = 0.3254901960784314;
386  error_color.b = 0.30980392156862746;
387  error_color.a = 1.0;
388  std_msgs::ColorRGBA color;
389  if (status == OK) {
390  color = ok_color;
391  }
392  else if (status == WARNING) {
393  color = warn_color;
394  }
395  else if (status == ERROR) {
396  color = error_color;
397  }
398  jsk_rviz_plugins::OverlayText msg;
399  msg.text = text;
400  msg.width = 1000;
401  msg.height = 1000;
402  msg.top = 10;
403  msg.left = 10;
404  msg.bg_color.a = 0.0;
405  msg.fg_color = color;
406  msg.text_size = 24;
407  pub.publish(msg);
408  }
409 
411  const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr& goal)
412  {
413  boost::mutex::scoped_lock lock(mutex_);
414  latest_header_ = goal->goal_footstep.header;
415  ROS_INFO("planCB");
416  // check message sanity
417  if (goal->initial_footstep.footsteps.size() == 0) {
418  ROS_ERROR("no initial footstep is specified");
419  as_.setPreempted();
420  return;
421  }
422  if (goal->goal_footstep.footsteps.size() != 2) {
423  ROS_ERROR("Need to specify 2 goal footsteps");
424  as_.setPreempted();
425  return;
426  }
428  ROS_ERROR("No pointcloud model is yet available");
429  as_.setPreempted();
430  return;
431  }
432  // check frame_id sanity
433  std::string goal_frame_id = goal->initial_footstep.header.frame_id;
434  if (use_pointcloud_model_) {
435  // check perception cloud header
436  if (goal_frame_id != pointcloud_model_frame_id_) {
437  ROS_ERROR("frame_id of goal and pointcloud do not match. goal: %s, pointcloud: %s.",
438  goal_frame_id.c_str(), pointcloud_model_frame_id_.c_str());
439  as_.setPreempted();
440  return;
441  }
442  }
443  if (use_obstacle_model_) {
444  // check perception cloud header
445  if (goal_frame_id != obstacle_model_frame_id_) {
446  ROS_ERROR("frame_id of goal and obstacle pointcloud do not match. goal: %s, obstacle: %s.",
447  goal_frame_id.c_str(), obstacle_model_frame_id_.c_str());
448  as_.setPreempted();
449  return;
450  }
451  }
452  Eigen::Vector3f footstep_size(footstep_size_x_, footstep_size_y_, 0.000001);
453  Eigen::Vector3f search_resolution(resolution_x_, resolution_y_, resolution_theta_);
454  // check goal is whether collision free
455  // conevrt goal footstep into FootstepState::Ptr instance.
456  if (goal->goal_footstep.footsteps.size() != 2) {
457  ROS_ERROR("goal footstep should be a pair of footsteps");
458  as_.setPreempted();
459  return;
460  }
461  std::vector<jsk_footstep_msgs::Footstep > goal_ros;
462  goal_ros.push_back(goal->goal_footstep.footsteps[0]);
463  goal_ros.push_back(goal->goal_footstep.footsteps[1]);
464  for (int i = 0; i < 2; i++) {
465  if (goal_ros[i].offset.x == 0.0 &&
466  goal_ros[i].offset.y == 0.0 &&
467  goal_ros[i].offset.z == 0.0 ) {
468  if (goal_ros[i].leg == jsk_footstep_msgs::Footstep::LEFT) {
469  goal_ros[i].offset.x = - inv_lleg_footstep_offset_[0];
470  goal_ros[i].offset.y = - inv_lleg_footstep_offset_[1];
471  goal_ros[i].offset.z = - inv_lleg_footstep_offset_[2];
472  } else {
473  goal_ros[i].offset.x = - inv_rleg_footstep_offset_[0];
474  goal_ros[i].offset.y = - inv_rleg_footstep_offset_[1];
475  goal_ros[i].offset.z = - inv_rleg_footstep_offset_[2];
476  }
477  }
478  if(goal_ros[i].dimensions.x == 0 &&
479  goal_ros[i].dimensions.y == 0 &&
480  goal_ros[i].dimensions.z == 0 ) {
481  goal_ros[i].dimensions.x = footstep_size_x_;
482  goal_ros[i].dimensions.y = footstep_size_y_;
483  goal_ros[i].dimensions.z = 0.000001;
484  }
485  }
486 
487  FootstepState::Ptr first_goal = FootstepState::fromROSMsg(goal_ros[0],
488  footstep_size,
489  search_resolution);
490  FootstepState::Ptr second_goal = FootstepState::fromROSMsg(goal_ros[1],
491  footstep_size,
492  search_resolution);
493  if (!graph_->isSuccessable(second_goal, first_goal)) {
494  ROS_ERROR("goal is non-realistic");
495  as_.setPreempted();
496  return;
497  }
498  ros::WallDuration timeout;
499  if(goal->timeout.toSec() == 0.0) {
501  } else {
502  timeout = ros::WallDuration(goal->timeout.toSec());
503  }
504 
505 
507  // set start state
508  // 0 is always start
509  jsk_footstep_msgs::Footstep start_ros = goal->initial_footstep.footsteps[0];
510  if (start_ros.offset.x == 0.0 &&
511  start_ros.offset.y == 0.0 &&
512  start_ros.offset.z == 0.0 ) {
513  if (start_ros.leg == jsk_footstep_msgs::Footstep::LEFT) {
514  start_ros.offset.x = - inv_lleg_footstep_offset_[0];
515  start_ros.offset.y = - inv_lleg_footstep_offset_[1];
516  start_ros.offset.z = - inv_lleg_footstep_offset_[2];
517  } else {
518  start_ros.offset.x = - inv_rleg_footstep_offset_[0];
519  start_ros.offset.y = - inv_rleg_footstep_offset_[1];
520  start_ros.offset.z = - inv_rleg_footstep_offset_[2];
521  }
522  }
524  start_ros,
525  footstep_size,
526  search_resolution));
527  graph_->setStartState(start);
528  if (project_start_state_) {
529  if (!graph_->projectStart()) {
530  ROS_ERROR("Failed to project start state");
532  "Failed to project start",
533  ERROR);
534 
535  as_.setPreempted();
536  return;
537  }
538  }
539 
541  // set goal state
542  jsk_footstep_msgs::Footstep left_goal, right_goal;
543  for (size_t i = 0; i < goal_ros.size(); i++) {
545  goal_ros[i],
546  footstep_size,
547  Eigen::Vector3f(resolution_x_,
550  if (goal_state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
551  graph_->setLeftGoalState(goal_state);
552  left_goal = goal_ros[i];
553  }
554  else if (goal_state->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
555  graph_->setRightGoalState(goal_state);
556  right_goal = goal_ros[i];
557  }
558  else {
559  ROS_ERROR("unknown goal leg");
560  as_.setPreempted();
561  return;
562  }
563  }
564  if (project_goal_state_) {
565  if (!graph_->projectGoal()) {
566  ROS_ERROR("Failed to project goal");
567  as_.setPreempted();
569  "Failed to project goal",
570  ERROR);
571  return;
572  }
573  }
574  // set parameters
576  graph_->setTransitionLimit(
584  }
585  else {
586  graph_->setTransitionLimit(TransitionLimitXYZRPY::Ptr());
587  }
588  if (use_obstacle_model_) {
589  graph_->setCollisionBBoxSize(collision_bbox_size_);
590  graph_->setCollisionBBoxOffset(collision_bbox_offset_);
591  }
593  graph_->setGlobalTransitionLimit(
597 
598  }
599  else {
600  graph_->setGlobalTransitionLimit(TransitionLimitRP::Ptr());
601  }
602  graph_->setParameters(parameters_);
603 
604  graph_->setSuccessorFunction(boost::bind(&FootstepGraph::successors_original, graph_, _1, _2));
605  graph_->setPathCostFunction(boost::bind(&FootstepGraph::path_cost_original, graph_, _1, _2, _3));
606 
607  //ROS_INFO_STREAM(graph_->infoString());
608  // Solver setup
614  cost_weight_,
616  if (heuristic_ == "step_cost") {
617  solver.setHeuristic(boost::bind(&FootstepPlanner::stepCostHeuristic, this, _1, _2));
618  }
619  else if (heuristic_ == "zero") {
620  solver.setHeuristic(boost::bind(&FootstepPlanner::zeroHeuristic, this, _1, _2));
621  }
622  else if (heuristic_ == "straight") {
623  solver.setHeuristic(boost::bind(&FootstepPlanner::straightHeuristic, this, _1, _2));
624  }
625  else if (heuristic_ == "straight_rotation") {
626  solver.setHeuristic(boost::bind(&FootstepPlanner::straightRotationHeuristic, this, _1, _2));
627  }
628  else if (heuristic_ == "follow_path") {
629  solver.setHeuristic(boost::bind(&FootstepPlanner::followPathLineHeuristic, this, _1, _2));
630  }
631  else {
632  ROS_ERROR("Unknown heuristics");
633  as_.setPreempted();
634  return;
635  }
636  graph_->clearPerceptionDuration();
637  solver.setProfileFunction(boost::bind(&FootstepPlanner::profile, this, _1, _2));
638  ROS_INFO("start_solver timeout: %f", timeout.toSec());
639  ros::WallTime start_time = ros::WallTime::now();
640  std::vector<SolverNode<FootstepState, FootstepGraph>::Ptr> path = solver.solve(timeout);
641  ros::WallTime end_time = ros::WallTime::now();
642  double planning_duration = (end_time - start_time).toSec();
643  //ROS_INFO_STREAM("took " << planning_duration << " sec");
644  //ROS_INFO_STREAM("path: " << path.size());
645  if (path.size() == 0) {
646  pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
647  solver.openListToPointCloud(open_list_cloud);
648  solver.closeListToPointCloud(close_list_cloud);
649  std::string info_str
650  = (boost::format("Failed to plan path / Took %f sec\nPerception took %f sec\nPlanning took %f sec\n%lu path\nopen list: %lu\nclose list:%lu")
651  % planning_duration
652  % graph_->getPerceptionDuration().toSec()
653  % (planning_duration - graph_->getPerceptionDuration().toSec())
654  % path.size()
655  % open_list_cloud.points.size()
656  % close_list_cloud.points.size()).str();
657  ROS_ERROR_STREAM(info_str);
658  // pub open/close list
659  publishPointCloud(close_list_cloud, pub_close_list_, goal->goal_footstep.header);
660  publishPointCloud(open_list_cloud, pub_open_list_, goal->goal_footstep.header);
661  // pub text
663  info_str,
664  ERROR);
665  //
666  as_.setPreempted();
667  return;
668  }
669  // finalize in graph
670  std::vector <FootstepState::Ptr> finalizeSteps;
671  if (! (graph_->finalizeSteps((path.size() >1 ? path[path.size()-2]->getState() : FootstepState::Ptr()),
672  path[path.size()-1]->getState(),
673  finalizeSteps))) {
674  ROS_ERROR("Failed to finalize path");
676  "Failed to finalize path",
677  ERROR);
678  as_.setPreempted();
679  return;
680  }
681  // Convert path to FootstepArray
682  jsk_footstep_msgs::FootstepArray ros_path;
683  ros_path.header = goal->goal_footstep.header;
684  for (size_t i = 0; i < path.size(); i++) {
685  const FootstepState::Ptr st = path[i]->getState();
686  if (st->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
687  ros_path.footsteps.push_back(*(st->toROSMsg(inv_lleg_footstep_offset_)));
688  } else {
689  ros_path.footsteps.push_back(*(st->toROSMsg(inv_rleg_footstep_offset_)));
690  }
691  }
692  for (size_t i = 0; i < finalizeSteps.size(); i++) {
693  const FootstepState::Ptr st = finalizeSteps[i];
694  if (st->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
695  ros_path.footsteps.push_back(*(st->toROSMsg(inv_lleg_footstep_offset_)));
696  } else {
697  ros_path.footsteps.push_back(*(st->toROSMsg(inv_rleg_footstep_offset_)));
698  }
699  }
700  result_.result = ros_path;
702 
703  pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
704  solver.openListToPointCloud(open_list_cloud);
705  solver.closeListToPointCloud(close_list_cloud);
706  publishPointCloud(close_list_cloud, pub_close_list_, goal->goal_footstep.header);
707  publishPointCloud(open_list_cloud, pub_open_list_, goal->goal_footstep.header);
708  std::string info_str
709  = (boost::format("Took %f sec\nPerception took %f sec\nPlanning took %f sec\n%lu path\nopen list: %lu\nclose list:%lu")
710  % planning_duration
711  % graph_->getPerceptionDuration().toSec()
712  % (planning_duration - graph_->getPerceptionDuration().toSec())
713  % path.size()
714  % open_list_cloud.points.size()
715  % close_list_cloud.points.size()).str();
716  ROS_INFO_STREAM(info_str);
717  publishText(pub_text_, info_str, OK);
718  ROS_INFO_STREAM("use_obstacle_model: " << graph_->useObstacleModel());
719  }
720 
722  const pcl::PointCloud<pcl::PointNormal>& cloud,
724  const std_msgs::Header& header)
725  {
726  sensor_msgs::PointCloud2 ros_cloud;
727  pcl::toROSMsg(cloud, ros_cloud);
728  ros_cloud.header = header;
729  pub.publish(ros_cloud);
730  }
731 
733  {
734  if (as_.isPreemptRequested()) {
735  solver.cancelSolve();
736  ROS_WARN("cancelled!");
737  }
738  // ROS_INFO("open list: %lu", solver.getOpenList().size());
739  // ROS_INFO("close list: %lu", solver.getCloseList().size());
741  (boost::format("open_list: %lu\nclose list:%lu")
742  % (solver.getOpenList().size()) % (solver.getCloseList().size())).str(),
743  OK);
744  if (rich_profiling_) {
745  pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
746  solver.openListToPointCloud(open_list_cloud);
747  solver.closeListToPointCloud(close_list_cloud);
750  }
751  }
752 
755  {
758  }
759 
762  {
763  return footstepHeuristicZero(node, graph);
764  }
765 
768  {
769  return footstepHeuristicStraight(node, graph);
770  }
771 
774  {
775  return footstepHeuristicStraightRotation(node, graph);
776  }
777 
780  {
781  return footstepHeuristicFollowPathLine(node, graph);
782  }
783 
796  {
797  successors_.clear();
798  if (!nh.hasParam("successors")) {
799  ROS_FATAL("no successors are specified");
800  return false;
801  }
802  // read default translation from right foot to left foot
803  double default_x = 0.0;
804  double default_y = 0.0;
805  double default_theta = 0.0;
806  if (nh.hasParam("default_lfoot_to_rfoot_offset")) {
807  std::vector<double> default_offset;
808  if (jsk_topic_tools::readVectorParameter(nh, "default_lfoot_to_rfoot_offset", default_offset)) {
809  default_x = default_offset[0];
810  default_y = default_offset[1];
811  default_theta = default_offset[2];
812  }
813  }
814  // read successors
815  XmlRpc::XmlRpcValue successors_xml;
816  nh.param("successors", successors_xml, successors_xml);
817  if (successors_xml.getType() != XmlRpc::XmlRpcValue::TypeArray)
818  {
819  ROS_FATAL("successors should be an array");
820  return false;
821  }
822  for (size_t i_successors = 0; i_successors < successors_xml.size(); i_successors++) {
823  XmlRpc::XmlRpcValue successor_xml;
824  successor_xml = successors_xml[i_successors];
825  if (successor_xml.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
826  ROS_FATAL("element of successors should be an dictionary");
827  return false;
828  }
829  double x = 0;
830  double y = 0;
831  double theta = 0;
832  if (successor_xml.hasMember("x")) {
833  x = jsk_topic_tools::getXMLDoubleValue(successor_xml["x"]);
834  x += default_x;
835  }
836  if (successor_xml.hasMember("y")) {
837  y = jsk_topic_tools::getXMLDoubleValue(successor_xml["y"]);
838  y += default_y;
839  }
840  if (successor_xml.hasMember("theta")) {
841  theta = jsk_topic_tools::getXMLDoubleValue(successor_xml["theta"]);
842  theta += default_theta;
843  }
844  // successors written in parameters should not include offset
845  // successors using in graph include offset(between the center of the cube and the end-coords)
846  // Footstep returned to a client, its pose indicates end-coords
847  Eigen::Affine3f successor =
848  Eigen::Translation3f(inv_lleg_footstep_offset_[0],
851  affineFromXYYaw(x, y, theta) *
852  Eigen::Translation3f(-inv_rleg_footstep_offset_[0],
855  successors_.push_back(successor);
856  }
857  ROS_INFO("%lu successors are defined", successors_.size());
858  if ((default_x != 0.0) || (default_y != 0.0) || (default_theta != 0.0)) {
859  ROS_INFO("default_offset: #f(%f %f %f)", default_x, default_y, default_theta);
860  }
861  if ((inv_lleg_footstep_offset_[0] != 0) ||
862  (inv_lleg_footstep_offset_[1] != 0) ||
863  (inv_lleg_footstep_offset_[2] != 0) ) {
864  ROS_INFO("left_leg_offset: #f(%f %f %f)",
868  }
869  if ((inv_rleg_footstep_offset_[0] != 0) ||
870  (inv_rleg_footstep_offset_[1] != 0) ||
871  (inv_rleg_footstep_offset_[2] != 0) ) {
872  ROS_INFO("right_leg_offset: #f(%f %f %f)",
876  }
877  for (size_t i = 0; i < successors_.size(); i++) {
878  Eigen::Vector3f tr = successors_[i].translation();
879  float roll, pitch, yaw;
880  pcl::getEulerAngles(successors_[i], roll, pitch, yaw);
881  ROS_INFO("successor_%2.2d: (make-coords :pos (scale 1000 #f(%f %f 0)) :rpy (list %f 0 0))", i, tr[0], tr[1], yaw);
882  }
883  return true;
884  }
885 
886  void FootstepPlanner::configCallback(Config &config, uint32_t level)
887  {
888  boost::mutex::scoped_lock lock(mutex_);
889  bool need_to_rebuild_graph = false;
890  if (use_pointcloud_model_ != config.use_pointcloud_model) {
891  use_pointcloud_model_ = config.use_pointcloud_model;
892  need_to_rebuild_graph = true;
893  }
894  if (use_lazy_perception_ != config.use_lazy_perception) {
895  use_lazy_perception_ = config.use_lazy_perception;
896  need_to_rebuild_graph = true;
897  }
898  if (use_local_movement_ != config.use_local_movement) {
899  use_local_movement_ = config.use_local_movement;
900  need_to_rebuild_graph = true;
901  }
902  if (resolution_x_ != config.resolution_x) {
903  resolution_x_ = config.resolution_x;
904  need_to_rebuild_graph = true;
905  }
906  if (resolution_y_ != config.resolution_y) {
907  resolution_y_ = config.resolution_y;
908  need_to_rebuild_graph = true;
909  }
910  if (resolution_theta_ != config.resolution_theta) {
911  resolution_theta_ = config.resolution_theta;
912  need_to_rebuild_graph = true;
913  }
914  planning_timeout_ = config.planning_timeout;
915  rich_profiling_ = config.rich_profiling;
916  parameters_.use_transition_limit = config.use_transition_limit;
917  parameters_.use_global_transition_limit = config.use_global_transition_limit;
918  parameters_.local_move_x = config.local_move_x;
919  parameters_.local_move_y = config.local_move_y;
920  parameters_.local_move_theta = config.local_move_theta;
921  parameters_.local_move_x_num = config.local_move_x_num;
922  parameters_.local_move_y_num = config.local_move_y_num;
923  parameters_.local_move_theta_num = config.local_move_theta_num;
924  parameters_.local_move_x_offset = config.local_move_x_offset;
925  parameters_.local_move_y_offset = config.local_move_y_offset;
926  parameters_.local_move_theta_offset = config.local_move_theta_offset;
927  parameters_.transition_limit_x = config.transition_limit_x;
928  parameters_.transition_limit_y = config.transition_limit_y;
929  parameters_.transition_limit_z = config.transition_limit_z;
930  parameters_.transition_limit_roll = config.transition_limit_roll;
931  parameters_.transition_limit_pitch = config.transition_limit_pitch;
932  parameters_.transition_limit_yaw = config.transition_limit_yaw;
933  parameters_.global_transition_limit_roll = config.global_transition_limit_roll;
934  parameters_.global_transition_limit_pitch = config.global_transition_limit_pitch;
935  parameters_.goal_pos_thr = config.goal_pos_thr;
936  parameters_.goal_rot_thr = config.goal_rot_thr;
937  parameters_.plane_estimation_use_normal = config.plane_estimation_use_normal;
938  parameters_.plane_estimation_normal_distance_weight = config.plane_estimation_normal_distance_weight;
939  parameters_.plane_estimation_normal_opening_angle = config.plane_estimation_normal_opening_angle;
940  parameters_.plane_estimation_min_ratio_of_inliers = config.plane_estimation_min_ratio_of_inliers;
941  parameters_.plane_estimation_max_iterations = config.plane_estimation_max_iterations;
942  parameters_.plane_estimation_min_inliers = config.plane_estimation_min_inliers;
943  parameters_.plane_estimation_outlier_threshold = config.plane_estimation_outlier_threshold;
944  parameters_.support_check_x_sampling = config.support_check_x_sampling;
945  parameters_.support_check_y_sampling = config.support_check_y_sampling;
946  parameters_.support_check_vertex_neighbor_threshold = config.support_check_vertex_neighbor_threshold;
947  parameters_.support_padding_x = config.support_padding_x;
948  parameters_.support_padding_y = config.support_padding_y;
949  parameters_.skip_cropping = config.skip_cropping;
950  footstep_size_x_ = config.footstep_size_x;
951  footstep_size_y_ = config.footstep_size_y;
952  project_start_state_ = config.project_start_state;
953  project_goal_state_ = config.project_goal_state;
954  close_list_x_num_ = config.close_list_x_num;
955  close_list_y_num_ = config.close_list_y_num;
956  close_list_theta_num_ = config.close_list_theta_num;
957  profile_period_ = config.profile_period;
958  heuristic_ = config.heuristic;
959  heuristic_first_rotation_weight_ = config.heuristic_first_rotation_weight;
960  heuristic_second_rotation_weight_ = config.heuristic_second_rotation_weight;
961  cost_weight_ = config.cost_weight;
962  heuristic_weight_ = config.heuristic_weight;
963  if (use_obstacle_model_ != config.use_obstacle_model) {
964  use_obstacle_model_ = config.use_obstacle_model;
965  need_to_rebuild_graph = true;
966  }
967  parameters_.obstacle_resolution = config.obstacle_resolution;
968  if (need_to_rebuild_graph) {
969  if (graph_) { // In order to skip first initialization
970  ROS_INFO("re-building graph");
971  buildGraph();
972  }
973  }
974  }
975 
977  {
978  graph_.reset(new FootstepGraph(Eigen::Vector3f(resolution_x_,
986  graph_->setPointCloudModel(pointcloud_model_);
987  }
989  graph_->setObstacleModel(obstacle_model_);
990  }
991  //graph_->setObstacleResolution(parameters_.obstacle_resolution);
992  graph_->setParameters(parameters_);
993  graph_->setBasicSuccessors(successors_);
994  }
995 
997  {
998  graph_->setHeuristicPathLine(path_line); // copy ???
999  }
1000 }
msg
pcl::PointCloud< pcl::PointXYZ >::Ptr obstacle_model_
Actionlib server for footstep planning.
virtual void setProfileFunction(ProfileFunction f)
double footstepHeuristicStepCost(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph, double first_rotation_weight, double second_rotation_weight)
#define ROS_FATAL(...)
path
virtual void obstacleCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)
pos
virtual bool projectFootstepService(jsk_footstep_planner::ProjectFootstep::Request &req, jsk_footstep_planner::ProjectFootstep::Response &res)
void publish(const boost::shared_ptr< M > &message) 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())
Eigen::Affine3f affineFromXYYaw(double x, double y, double yaw)
Definition: util.h:42
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
virtual double stepCostHeuristic(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
int size() const
pitch
virtual FootstepStateDiscreteCloseList getCloseList()
double footstepHeuristicStraightRotation(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
void closeListToPointCloud(pcl::PointCloud< PointT > &output_cloud)
virtual void configCallback(Config &config, uint32_t level)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual void profile(FootstepAStarSolver< FootstepGraph > &solver, FootstepGraph::Ptr graph)
virtual bool projectFootPrintService(jsk_interactive_marker::SnapFootPrint::Request &req, jsk_interactive_marker::SnapFootPrint::Response &res)
pcl::PointCloud< pcl::PointNormal >::Ptr pointcloud_model_
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
Type const & getType() const
#define ROS_WARN(...)
void openListToPointCloud(pcl::PointCloud< PointT > &output_cloud)
double getXMLDoubleValue(XmlRpc::XmlRpcValue val)
double path_cost_original(StatePtr from, StatePtr to, double prev_cost)
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
double y
virtual double zeroHeuristic(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
virtual bool projectFootPrintWithLocalSearchService(jsk_interactive_marker::SnapFootPrint::Request &req, jsk_interactive_marker::SnapFootPrint::Response &res)
bool readVectorParameter(ros::NodeHandle &nh, const std::string &param_name, std::vector< double > &result)
virtual bool projectFootPrint(const Eigen::Affine3f &center_pose, const Eigen::Affine3f &left_pose_trans, const Eigen::Affine3f &right_pose_trans, geometry_msgs::Pose &pose)
yaw
double footstepHeuristicStraight(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
ros::ServiceServer srv_project_footprint_with_local_search_
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
ROSCPP_DECL const std::string & getNamespace()
virtual std::vector< typename SolverNode< State, GraphT >::Ptr > solve(const ros::WallDuration &timeout=ros::WallDuration(1000000000.0))
pcl::PointCloud< pcl::PointNormal >::Ptr cloud
const Eigen::Vector3f resolution(0.05, 0.05, 0.08)
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
FootstepGraph::Ptr graph
start
virtual void publishPointCloud(const pcl::PointCloud< pcl::PointNormal > &cloud, ros::Publisher &pub, const std_msgs::Header &header)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual bool collisionBoundingBoxInfoService(jsk_footstep_planner::CollisionBoundingBoxInfo::Request &req, jsk_footstep_planner::CollisionBoundingBoxInfo::Response &res)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
Class to provide limit of transition of footstep about Roll and Pitch. This class is designed for glo...
virtual bool setHeuristicPathService(jsk_footstep_planner::SetHeuristicPath::Request &req, jsk_footstep_planner::SetHeuristicPath::Response &res)
rot
boost::shared_ptr< FootstepState > Ptr
double footstepHeuristicZero(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
void toROSMsg(const pcl::PointCloud< T > &cloud, sensor_msgs::Image &msg)
const Eigen::Vector3f footstep_size(0.2, 0.1, 0.000001)
points
bool hasMember(const std::string &name) const
mutex_t * lock
virtual double straightHeuristic(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
#define ROS_INFO_STREAM(args)
static WallTime now()
ros::ServiceServer srv_collision_bounding_box_info_
std::vector< Eigen::Affine3f > successors_
virtual bool readSuccessors(ros::NodeHandle &nh)
bool successors_original(StatePtr target_state, std::vector< FootstepGraph::StatePtr > &ret)
class to provide limit of transition of footstep with 6 Full parameters.
f
actionlib::SimpleActionServer< jsk_footstep_msgs::PlanFootstepsAction > as_
step
virtual void planCB(const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr &goal)
static FootstepState::Ptr fromROSMsg(const jsk_footstep_msgs::Footstep &f, const Eigen::Vector3f &size, const Eigen::Vector3f &resolution)
bool hasParam(const std::string &key) const
#define ROS_ERROR_STREAM(args)
virtual void publishText(ros::Publisher &pub, const std::string &text, PlanningStatus status)
double footstepHeuristicFollowPathLine(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
virtual double followPathLineHeuristic(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
virtual void pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)
roll
double x
#define ROS_ERROR(...)
virtual double straightRotationHeuristic(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
virtual void setHeuristicPathLine(jsk_recognition_utils::PolyLine &path_line)
jsk_footstep_msgs::PlanFootstepsResult result_
#define ROS_DEBUG(...)


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Fri Jul 26 2019 03:54:32