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 
37 #include <jsk_topic_tools/log_utils.h>
38 #include <jsk_topic_tools/rosparam_utils.h>
40 #include <pcl/common/angles.h>
41 #include <algorithm>
42 #include <boost/format.hpp>
43 
44 namespace jsk_footstep_planner
45 {
47  as_(nh, nh.getNamespace(),
48  boost::bind(&FootstepPlanner::planCB, this, _1), false)
49  {
50  srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (nh);
51  typename dynamic_reconfigure::Server<Config>::CallbackType f =
52  boost::bind (&FootstepPlanner::configCallback, this, _1, _2);
53  srv_->setCallback (f);
54  pub_text_ = nh.advertise<jsk_rviz_plugins::OverlayText>(
55  "text", 1, true);
56  pub_close_list_ = nh.advertise<sensor_msgs::PointCloud2>(
57  "close_list", 1, true);
58  pub_open_list_ = nh.advertise<sensor_msgs::PointCloud2>(
59  "open_list", 1, true);
60  srv_project_footprint_ = nh.advertiseService(
61  "project_footprint", &FootstepPlanner::projectFootPrintService, this);
62  srv_project_footprint_with_local_search_ = nh.advertiseService(
63  "project_footprint_with_local_search", &FootstepPlanner::projectFootPrintWithLocalSearchService, this);
64  srv_collision_bounding_box_info_ = nh.advertiseService(
65  "collision_bounding_box_info", &FootstepPlanner::collisionBoundingBoxInfoService, this);
66  srv_project_footstep_ = nh.advertiseService(
67  "project_footstep", &FootstepPlanner::projectFootstepService, this);
68  srv_set_heuristic_path_ = nh.advertiseService(
69  "set_heuristic_path", &FootstepPlanner::setHeuristicPathService, this);
70  std::vector<double> lleg_footstep_offset, rleg_footstep_offset;
71  if (jsk_topic_tools::readVectorParameter(nh, "lleg_footstep_offset", lleg_footstep_offset)) {
72  inv_lleg_footstep_offset_ = Eigen::Vector3f(- lleg_footstep_offset[0],
73  - lleg_footstep_offset[1],
74  - lleg_footstep_offset[2]);
75  } else {
76  inv_lleg_footstep_offset_ = Eigen::Vector3f(0, 0, 0);
77  }
78  if (jsk_topic_tools::readVectorParameter(nh, "rleg_footstep_offset", rleg_footstep_offset)) {
79  inv_rleg_footstep_offset_ = Eigen::Vector3f(- rleg_footstep_offset[0],
80  - rleg_footstep_offset[1],
81  - rleg_footstep_offset[2]);
82  } else {
83  inv_rleg_footstep_offset_ = Eigen::Vector3f(0, 0, 0);
84  }
85  {
86  boost::mutex::scoped_lock lock(mutex_);
87  if (!readSuccessors(nh)) {
88  return;
89  }
90 
91  ROS_INFO("building graph");
92  buildGraph();
93  ROS_INFO("build graph done");
94  }
95  sub_pointcloud_model_ = nh.subscribe("pointcloud_model", 1, &FootstepPlanner::pointcloudCallback, this);
96  sub_obstacle_model_ = nh.subscribe("obstacle_model", 1, &FootstepPlanner::obstacleCallback, this);
97  std::vector<double> collision_bbox_size, collision_bbox_offset;
98  if (jsk_topic_tools::readVectorParameter(nh, "collision_bbox_size", collision_bbox_size)) {
99  collision_bbox_size_[0] = collision_bbox_size[0];
100  collision_bbox_size_[1] = collision_bbox_size[1];
101  collision_bbox_size_[2] = collision_bbox_size[2];
102  }
103  if (jsk_topic_tools::readVectorParameter(nh, "collision_bbox_offset", collision_bbox_offset)) {
104  collision_bbox_offset_ = Eigen::Affine3f::Identity() * Eigen::Translation3f(collision_bbox_offset[0],
105  collision_bbox_offset[1],
106  collision_bbox_offset[2]);
107  }
108  as_.start();
109  }
110 
112  const sensor_msgs::PointCloud2::ConstPtr& msg)
113  {
114  boost::mutex::scoped_lock lock(mutex_);
115  obstacle_model_.reset(new pcl::PointCloud<pcl::PointXYZ>);
117  obstacle_model_frame_id_ = msg->header.frame_id;
118  if (graph_ && use_obstacle_model_) {
119  graph_->setObstacleModel(obstacle_model_);
120  }
121  }
122 
124  const sensor_msgs::PointCloud2::ConstPtr& msg)
125  {
126  boost::mutex::scoped_lock lock(mutex_);
127  ROS_DEBUG("pointcloud model is updated");
128  pointcloud_model_.reset(new pcl::PointCloud<pcl::PointNormal>);
130  pointcloud_model_frame_id_ = msg->header.frame_id;
131  if (graph_ && use_pointcloud_model_) {
132  graph_->setPointCloudModel(pointcloud_model_);
133  }
134  }
135 
137  const Eigen::Affine3f& center_pose,
138  const Eigen::Affine3f& left_pose_trans,
139  const Eigen::Affine3f& right_pose_trans,
140  geometry_msgs::Pose& pose)
141  {
142  const Eigen::Vector3f resolution(resolution_x_,
145  const Eigen::Vector3f footstep_size(footstep_size_x_,
147  0.000001);
148  Eigen::Affine3f left_pose = center_pose * left_pose_trans;
149  Eigen::Affine3f right_pose = center_pose * right_pose_trans;
150  FootstepState::Ptr left_input(new FootstepState(
151  jsk_footstep_msgs::Footstep::LEFT,
152  left_pose,
154  resolution));
156  jsk_footstep_msgs::Footstep::RIGHT,
157  right_pose,
159  resolution));
160  FootstepState::Ptr projected_left = graph_->projectFootstep(left_input);
161  FootstepState::Ptr projected_right = graph_->projectFootstep(right_input);
162  if (!projected_left || !projected_right) {
163  return false;
164  }
165  Eigen::Affine3f projected_left_pose = projected_left->getPose();
166  Eigen::Affine3f projected_right_pose = projected_right->getPose();
167  Eigen::Quaternionf rot = Eigen::Quaternionf(projected_left_pose.rotation()).slerp(
168  0.5, Eigen::Quaternionf(projected_right_pose.rotation()));
169  Eigen::Vector3f pos = (Eigen::Vector3f(projected_right_pose.translation()) +
170  Eigen::Vector3f(projected_left_pose.translation())) / 2.0;
171  Eigen::Affine3f mid = Eigen::Translation3f(pos) * rot;
172  tf::poseEigenToMsg(mid, pose);
173  return true;
174  }
175 
177  jsk_interactive_marker::SnapFootPrint::Request& req,
178  jsk_interactive_marker::SnapFootPrint::Response& res)
179  {
180  boost::mutex::scoped_lock lock(mutex_);
181  if (!graph_ ) {
182  return false;
183  }
185  ROS_ERROR("No pointcloud model is yet available");
187  "No pointcloud model is yet available",
188  ERROR);
189  return false;
190  }
191  Eigen::Affine3f center_pose, left_pose_trans, right_pose_trans;
192  std::vector<Eigen::Affine3f> center_poses;
193  tf::poseMsgToEigen(req.lleg_pose, left_pose_trans);
194  tf::poseMsgToEigen(req.rleg_pose, right_pose_trans);
195  tf::poseMsgToEigen(req.input_pose.pose, center_pose);
196  const double dx = 0.05;
197  const double dy = 0.05;
198  const double dtheta = pcl::deg2rad(5.0);
199  for (int xi = 0; xi < 3; xi++) {
200  for (int yi = 0; yi < 3; yi++) {
201  for (int thetai = 0; thetai < 3; thetai++) {
202  Eigen::Affine3f transppp = affineFromXYYaw(xi * dx, yi * dy, thetai * dtheta);
203  Eigen::Affine3f transppm = affineFromXYYaw(xi * dx, yi * dy, - thetai * dtheta);
204  Eigen::Affine3f transpmp = affineFromXYYaw(xi * dx, - yi * dy, thetai * dtheta);
205  Eigen::Affine3f transpmm = affineFromXYYaw(xi * dx, - yi * dy, -thetai * dtheta);
206  Eigen::Affine3f transmpp = affineFromXYYaw(- xi * dx, yi * dy, thetai * dtheta);
207  Eigen::Affine3f transmpm = affineFromXYYaw(- xi * dx, yi * dy, - thetai * dtheta);
208  Eigen::Affine3f transmmp = affineFromXYYaw(- xi * dx, - yi * dy, thetai * dtheta);
209  Eigen::Affine3f transmmm = affineFromXYYaw(- xi * dx, - yi * dy, - thetai * dtheta);
210  center_poses.push_back(center_pose * transppp);
211  center_poses.push_back(center_pose * transppm);
212  center_poses.push_back(center_pose * transpmp);
213  center_poses.push_back(center_pose * transpmm);
214  center_poses.push_back(center_pose * transmpp);
215  center_poses.push_back(center_pose * transmpm);
216  center_poses.push_back(center_pose * transmmp);
217  center_poses.push_back(center_pose * transmmm);
218  }
219  }
220  }
221  for (size_t i = 0; i < center_poses.size(); i++) {
222  if (projectFootPrint(center_poses[i], left_pose_trans, right_pose_trans,
223  res.snapped_pose.pose)) {
224  res.success = true;
225  res.snapped_pose.header = req.input_pose.header;
226  return true;
227  }
228  }
229  ROS_ERROR("Failed to project footprint");
231  "Failed to project goal",
232  ERROR);
233  return false;
234  }
235 
237  jsk_footstep_planner::CollisionBoundingBoxInfo::Request& req,
238  jsk_footstep_planner::CollisionBoundingBoxInfo::Response& res)
239  {
240  boost::mutex::scoped_lock lock(mutex_);
241  res.box_dimensions.x = collision_bbox_size_[0];
242  res.box_dimensions.y = collision_bbox_size_[1];
243  res.box_dimensions.z = collision_bbox_size_[2];
245  return true;
246  }
247 
249  jsk_footstep_planner::ProjectFootstep::Request& req,
250  jsk_footstep_planner::ProjectFootstep::Response& res)
251  {
252  boost::mutex::scoped_lock lock(mutex_);
253  if (!graph_) {
254  return false;
255  }
256  if (!pointcloud_model_) {
257  ROS_ERROR("No pointcloud model is yet available");
258  //publishText(pub_text_,
259  //"No pointcloud model is yet available",
260  //ERROR);
261  return false;
262  }
263 
264  const Eigen::Vector3f resolution(resolution_x_,
267  const Eigen::Vector3f footstep_size(footstep_size_x_,
269  0.000001);
270 
271  for (std::vector<jsk_footstep_msgs::Footstep>::iterator it = req.input.footsteps.begin();
272  it != req.input.footsteps.end(); it++) {
273  if (it->offset.x == 0.0 &&
274  it->offset.y == 0.0 &&
275  it->offset.z == 0.0 ) {
276  if (it->leg == jsk_footstep_msgs::Footstep::LEFT) {
277  it->offset.x = - inv_lleg_footstep_offset_[0];
278  it->offset.y = - inv_lleg_footstep_offset_[1];
279  it->offset.z = - inv_lleg_footstep_offset_[2];
280  } else {
281  it->offset.x = - inv_rleg_footstep_offset_[0];
282  it->offset.y = - inv_rleg_footstep_offset_[1];
283  it->offset.z = - inv_rleg_footstep_offset_[2];
284  }
285  }
286  if(it->dimensions.x == 0 &&
287  it->dimensions.y == 0 &&
288  it->dimensions.z == 0 ) {
289  it->dimensions.x = footstep_size_x_;
290  it->dimensions.y = footstep_size_y_;
291  it->dimensions.z = 0.000001;
292  }
294  FootstepState::Ptr projected = graph_->projectFootstep(step);
295  if(!!projected) {
296  res.success.push_back(true);
297  jsk_footstep_msgs::Footstep::Ptr p;
298  if (it->leg == jsk_footstep_msgs::Footstep::LEFT) {
299  p = projected->toROSMsg(inv_lleg_footstep_offset_);
300  } else if (it->leg == jsk_footstep_msgs::Footstep::RIGHT) {
301  p = projected->toROSMsg(inv_rleg_footstep_offset_);
302  } else {
303  p = projected->toROSMsg();
304  }
305  res.result.footsteps.push_back(*p);
306  } else {
307  res.success.push_back(false);
308  res.result.footsteps.push_back(*it); // return the same step as in input
309  }
310  }
311  res.result.header = req.input.header;
312 
313  return true;
314  }
315 
317  jsk_interactive_marker::SnapFootPrint::Request& req,
318  jsk_interactive_marker::SnapFootPrint::Response& res)
319  {
320  boost::mutex::scoped_lock lock(mutex_);
321  if (!graph_) {
322  return false;
323  }
324  if (!pointcloud_model_) {
325  ROS_ERROR("No pointcloud model is yet available");
327  "No pointcloud model is yet available",
328  ERROR);
329  return false;
330  }
331  Eigen::Affine3f center_pose, left_pose_trans, right_pose_trans;
332  tf::poseMsgToEigen(req.lleg_pose, left_pose_trans);
333  tf::poseMsgToEigen(req.rleg_pose, right_pose_trans);
334  tf::poseMsgToEigen(req.input_pose.pose, center_pose);
335  if (projectFootPrint(center_pose, left_pose_trans, right_pose_trans,
336  res.snapped_pose.pose)) {
337  res.success = true;
338  res.snapped_pose.header = req.input_pose.header;
339  return true;
340  }
341  else {
342  ROS_ERROR("Failed to project footprint");
344  "Failed to project goal",
345  ERROR);
346  return false;
347  }
348  }
350  jsk_footstep_planner::SetHeuristicPath::Request& req,
351  jsk_footstep_planner::SetHeuristicPath::Response& res)
352  {
353  boost::mutex::scoped_lock lock(mutex_);
354  if (!graph_) {
355  return false;
356  }
357  std::vector<Eigen::Vector3f> points;
358  for(int i = 0; i < req.segments.size(); i++) {
359  Eigen::Vector3f p(req.segments[i].x,
360  req.segments[i].y,
361  req.segments[i].z);
362  points.push_back(p);
363  }
366 
367  return true;
368  }
369 
371  const std::string& text,
372  PlanningStatus status)
373  {
374  std_msgs::ColorRGBA ok_color;
375  ok_color.r = 0.3568627450980392;
376  ok_color.g = 0.7529411764705882;
377  ok_color.b = 0.8705882352941177;
378  ok_color.a = 1.0;
379  std_msgs::ColorRGBA warn_color;
380  warn_color.r = 0.9411764705882353;
381  warn_color.g = 0.6784313725490196;
382  warn_color.b = 0.3058823529411765;
383  warn_color.a = 1.0;
384  std_msgs::ColorRGBA error_color;
385  error_color.r = 0.8509803921568627;
386  error_color.g = 0.3254901960784314;
387  error_color.b = 0.30980392156862746;
388  error_color.a = 1.0;
389  std_msgs::ColorRGBA color;
390  if (status == OK) {
391  color = ok_color;
392  }
393  else if (status == WARNING) {
394  color = warn_color;
395  }
396  else if (status == ERROR) {
397  color = error_color;
398  }
399  jsk_rviz_plugins::OverlayText msg;
400  msg.text = text;
401  msg.width = 1000;
402  msg.height = 1000;
403  msg.top = 10;
404  msg.left = 10;
405  msg.bg_color.a = 0.0;
406  msg.fg_color = color;
407  msg.text_size = 24;
408  pub.publish(msg);
409  }
410 
412  const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr& goal)
413  {
414  boost::mutex::scoped_lock lock(mutex_);
415  latest_header_ = goal->goal_footstep.header;
416  ROS_INFO("planCB");
417  // check message sanity
418  if (goal->initial_footstep.footsteps.size() == 0) {
419  ROS_ERROR("no initial footstep is specified");
420  as_.setPreempted();
421  return;
422  }
423  if (goal->goal_footstep.footsteps.size() != 2) {
424  ROS_ERROR("Need to specify 2 goal footsteps");
425  as_.setPreempted();
426  return;
427  }
429  ROS_ERROR("No pointcloud model is yet available");
430  as_.setPreempted();
431  return;
432  }
433  // check frame_id sanity
434  std::string goal_frame_id = goal->initial_footstep.header.frame_id;
435  if (use_pointcloud_model_) {
436  // check perception cloud header
437  if (goal_frame_id != pointcloud_model_frame_id_) {
438  ROS_ERROR("frame_id of goal and pointcloud do not match. goal: %s, pointcloud: %s.",
439  goal_frame_id.c_str(), pointcloud_model_frame_id_.c_str());
440  as_.setPreempted();
441  return;
442  }
443  }
444  if (use_obstacle_model_) {
445  // check perception cloud header
446  if (goal_frame_id != obstacle_model_frame_id_) {
447  ROS_ERROR("frame_id of goal and obstacle pointcloud do not match. goal: %s, obstacle: %s.",
448  goal_frame_id.c_str(), obstacle_model_frame_id_.c_str());
449  as_.setPreempted();
450  return;
451  }
452  }
453  Eigen::Vector3f footstep_size(footstep_size_x_, footstep_size_y_, 0.000001);
454  Eigen::Vector3f search_resolution(resolution_x_, resolution_y_, resolution_theta_);
455  // check goal is whether collision free
456  // conevrt goal footstep into FootstepState::Ptr instance.
457  if (goal->goal_footstep.footsteps.size() != 2) {
458  ROS_ERROR("goal footstep should be a pair of footsteps");
459  as_.setPreempted();
460  return;
461  }
462  std::vector<jsk_footstep_msgs::Footstep > goal_ros;
463  goal_ros.push_back(goal->goal_footstep.footsteps[0]);
464  goal_ros.push_back(goal->goal_footstep.footsteps[1]);
465  for (int i = 0; i < 2; i++) {
466  if (goal_ros[i].offset.x == 0.0 &&
467  goal_ros[i].offset.y == 0.0 &&
468  goal_ros[i].offset.z == 0.0 ) {
469  if (goal_ros[i].leg == jsk_footstep_msgs::Footstep::LEFT) {
470  goal_ros[i].offset.x = - inv_lleg_footstep_offset_[0];
471  goal_ros[i].offset.y = - inv_lleg_footstep_offset_[1];
472  goal_ros[i].offset.z = - inv_lleg_footstep_offset_[2];
473  } else {
474  goal_ros[i].offset.x = - inv_rleg_footstep_offset_[0];
475  goal_ros[i].offset.y = - inv_rleg_footstep_offset_[1];
476  goal_ros[i].offset.z = - inv_rleg_footstep_offset_[2];
477  }
478  }
479  if(goal_ros[i].dimensions.x == 0 &&
480  goal_ros[i].dimensions.y == 0 &&
481  goal_ros[i].dimensions.z == 0 ) {
482  goal_ros[i].dimensions.x = footstep_size_x_;
483  goal_ros[i].dimensions.y = footstep_size_y_;
484  goal_ros[i].dimensions.z = 0.000001;
485  }
486  }
487 
488  FootstepState::Ptr first_goal = FootstepState::fromROSMsg(goal_ros[0],
490  search_resolution);
491  FootstepState::Ptr second_goal = FootstepState::fromROSMsg(goal_ros[1],
493  search_resolution);
494  if (!graph_->isSuccessable(second_goal, first_goal)) {
495  ROS_ERROR("goal is non-realistic");
496  as_.setPreempted();
497  return;
498  }
499  ros::WallDuration timeout;
500  if(goal->timeout.toSec() == 0.0) {
502  } else {
503  timeout = ros::WallDuration(goal->timeout.toSec());
504  }
505 
506 
508  // set start state
509  // 0 is always start
510  jsk_footstep_msgs::Footstep start_ros = goal->initial_footstep.footsteps[0];
511  if (start_ros.offset.x == 0.0 &&
512  start_ros.offset.y == 0.0 &&
513  start_ros.offset.z == 0.0 ) {
514  if (start_ros.leg == jsk_footstep_msgs::Footstep::LEFT) {
515  start_ros.offset.x = - inv_lleg_footstep_offset_[0];
516  start_ros.offset.y = - inv_lleg_footstep_offset_[1];
517  start_ros.offset.z = - inv_lleg_footstep_offset_[2];
518  } else {
519  start_ros.offset.x = - inv_rleg_footstep_offset_[0];
520  start_ros.offset.y = - inv_rleg_footstep_offset_[1];
521  start_ros.offset.z = - inv_rleg_footstep_offset_[2];
522  }
523  }
525  start_ros,
527  search_resolution));
528  graph_->setStartState(start);
529  if (project_start_state_) {
530  if (!graph_->projectStart()) {
531  ROS_ERROR("Failed to project start state");
533  "Failed to project start",
534  ERROR);
535 
536  as_.setPreempted();
537  return;
538  }
539  }
540 
542  // set goal state
543  jsk_footstep_msgs::Footstep left_goal, right_goal;
544  for (size_t i = 0; i < goal_ros.size(); i++) {
546  goal_ros[i],
548  Eigen::Vector3f(resolution_x_,
551  if (goal_state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
552  graph_->setLeftGoalState(goal_state);
553  left_goal = goal_ros[i];
554  }
555  else if (goal_state->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
556  graph_->setRightGoalState(goal_state);
557  right_goal = goal_ros[i];
558  }
559  else {
560  ROS_ERROR("unknown goal leg");
561  as_.setPreempted();
562  return;
563  }
564  }
565  if (project_goal_state_) {
566  if (!graph_->projectGoal()) {
567  ROS_ERROR("Failed to project goal");
568  as_.setPreempted();
570  "Failed to project goal",
571  ERROR);
572  return;
573  }
574  }
575  // set parameters
577  graph_->setTransitionLimit(
578  TransitionLimitXYZRPY::Ptr(new TransitionLimitXYZRPY(
585  }
586  else {
587  graph_->setTransitionLimit(TransitionLimitXYZRPY::Ptr());
588  }
589  if (use_obstacle_model_) {
590  graph_->setCollisionBBoxSize(collision_bbox_size_);
591  graph_->setCollisionBBoxOffset(collision_bbox_offset_);
592  }
594  graph_->setGlobalTransitionLimit(
595  TransitionLimitRP::Ptr(new TransitionLimitRP(
598 
599  }
600  else {
601  graph_->setGlobalTransitionLimit(TransitionLimitRP::Ptr());
602  }
603  graph_->setParameters(parameters_);
604 
605  graph_->setSuccessorFunction(boost::bind(&FootstepGraph::successors_original, graph_, _1, _2));
606  graph_->setPathCostFunction(boost::bind(&FootstepGraph::path_cost_original, graph_, _1, _2, _3));
607 
608  //ROS_INFO_STREAM(graph_->infoString());
609  // Solver setup
610  FootstepAStarSolver<FootstepGraph> solver(graph_,
615  cost_weight_,
617  if (heuristic_ == "step_cost") {
618  solver.setHeuristic(boost::bind(&FootstepPlanner::stepCostHeuristic, this, _1, _2));
619  }
620  else if (heuristic_ == "zero") {
621  solver.setHeuristic(boost::bind(&FootstepPlanner::zeroHeuristic, this, _1, _2));
622  }
623  else if (heuristic_ == "straight") {
624  solver.setHeuristic(boost::bind(&FootstepPlanner::straightHeuristic, this, _1, _2));
625  }
626  else if (heuristic_ == "straight_rotation") {
627  solver.setHeuristic(boost::bind(&FootstepPlanner::straightRotationHeuristic, this, _1, _2));
628  }
629  else if (heuristic_ == "follow_path") {
630  solver.setHeuristic(boost::bind(&FootstepPlanner::followPathLineHeuristic, this, _1, _2));
631  }
632  else {
633  ROS_ERROR("Unknown heuristics");
634  as_.setPreempted();
635  return;
636  }
637  graph_->clearPerceptionDuration();
638  solver.setProfileFunction(boost::bind(&FootstepPlanner::profile, this, _1, _2));
639  ROS_INFO("start_solver timeout: %f", timeout.toSec());
641  std::vector<SolverNode<FootstepState, FootstepGraph>::Ptr> path = solver.solve(timeout);
642  ros::WallTime end_time = ros::WallTime::now();
643  double planning_duration = (end_time - start_time).toSec();
644  //ROS_INFO_STREAM("took " << planning_duration << " sec");
645  //ROS_INFO_STREAM("path: " << path.size());
646  if (path.size() == 0) {
647  pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
648  solver.openListToPointCloud(open_list_cloud);
649  solver.closeListToPointCloud(close_list_cloud);
650  std::string info_str
651  = (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")
652  % planning_duration
653  % graph_->getPerceptionDuration().toSec()
654  % (planning_duration - graph_->getPerceptionDuration().toSec())
655  % path.size()
656  % open_list_cloud.points.size()
657  % close_list_cloud.points.size()).str();
658  ROS_ERROR_STREAM(info_str);
659  // pub open/close list
660  publishPointCloud(close_list_cloud, pub_close_list_, goal->goal_footstep.header);
661  publishPointCloud(open_list_cloud, pub_open_list_, goal->goal_footstep.header);
662  // pub text
664  info_str,
665  ERROR);
666  //
667  as_.setPreempted();
668  return;
669  }
670  // finalize in graph
671  std::vector <FootstepState::Ptr> finalizeSteps;
672  if (! (graph_->finalizeSteps((path.size() >1 ? path[path.size()-2]->getState() : FootstepState::Ptr()),
673  path[path.size()-1]->getState(),
674  finalizeSteps))) {
675  ROS_ERROR("Failed to finalize path");
677  "Failed to finalize path",
678  ERROR);
679  as_.setPreempted();
680  return;
681  }
682  // Convert path to FootstepArray
683  jsk_footstep_msgs::FootstepArray ros_path;
684  ros_path.header = goal->goal_footstep.header;
685  for (size_t i = 0; i < path.size(); i++) {
686  const FootstepState::Ptr st = path[i]->getState();
687  if (st->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
688  ros_path.footsteps.push_back(*(st->toROSMsg(inv_lleg_footstep_offset_)));
689  } else {
690  ros_path.footsteps.push_back(*(st->toROSMsg(inv_rleg_footstep_offset_)));
691  }
692  }
693  for (size_t i = 0; i < std::min(num_finalize_steps_, finalizeSteps.size()); i++) {
694  const FootstepState::Ptr st = finalizeSteps[i];
695  if (st->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
696  ros_path.footsteps.push_back(*(st->toROSMsg(inv_lleg_footstep_offset_)));
697  } else {
698  ros_path.footsteps.push_back(*(st->toROSMsg(inv_rleg_footstep_offset_)));
699  }
700  }
701  result_.result = ros_path;
703 
704  pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
705  solver.openListToPointCloud(open_list_cloud);
706  solver.closeListToPointCloud(close_list_cloud);
707  publishPointCloud(close_list_cloud, pub_close_list_, goal->goal_footstep.header);
708  publishPointCloud(open_list_cloud, pub_open_list_, goal->goal_footstep.header);
709  std::string info_str
710  = (boost::format("Took %f sec\nPerception took %f sec\nPlanning took %f sec\n%lu path\nopen list: %lu\nclose list:%lu")
711  % planning_duration
712  % graph_->getPerceptionDuration().toSec()
713  % (planning_duration - graph_->getPerceptionDuration().toSec())
714  % path.size()
715  % open_list_cloud.points.size()
716  % close_list_cloud.points.size()).str();
717  ROS_INFO_STREAM(info_str);
718  publishText(pub_text_, info_str, OK);
719  ROS_INFO_STREAM("use_obstacle_model: " << graph_->useObstacleModel());
720  }
721 
723  const pcl::PointCloud<pcl::PointNormal>& cloud,
725  const std_msgs::Header& header)
726  {
727  sensor_msgs::PointCloud2 ros_cloud;
728  pcl::toROSMsg(cloud, ros_cloud);
729  ros_cloud.header = header;
730  pub.publish(ros_cloud);
731  }
732 
733  void FootstepPlanner::profile(FootstepAStarSolver<FootstepGraph>& solver, FootstepGraph::Ptr graph)
734  {
735  if (as_.isPreemptRequested()) {
736  solver.cancelSolve();
737  ROS_WARN("cancelled!");
738  }
739  // ROS_INFO("open list: %lu", solver.getOpenList().size());
740  // ROS_INFO("close list: %lu", solver.getCloseList().size());
742  (boost::format("open_list: %lu\nclose list:%lu")
743  % (solver.getOpenList().size()) % (solver.getCloseList().size())).str(),
744  OK);
745  if (rich_profiling_) {
746  pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
747  solver.openListToPointCloud(open_list_cloud);
748  solver.closeListToPointCloud(close_list_cloud);
751  }
752  }
753 
756  {
759  }
760 
763  {
764  return footstepHeuristicZero(node, graph);
765  }
766 
769  {
770  return footstepHeuristicStraight(node, graph);
771  }
772 
775  {
777  }
778 
781  {
783  }
784 
797  {
798  successors_.clear();
799  if (!nh.hasParam("successors")) {
800  ROS_FATAL("no successors are specified");
801  return false;
802  }
803  // read default translation from right foot to left foot
804  double default_x = 0.0;
805  double default_y = 0.0;
806  double default_theta = 0.0;
807  if (nh.hasParam("default_lfoot_to_rfoot_offset")) {
808  std::vector<double> default_offset;
809  if (jsk_topic_tools::readVectorParameter(nh, "default_lfoot_to_rfoot_offset", default_offset)) {
810  default_x = default_offset[0];
811  default_y = default_offset[1];
812  default_theta = default_offset[2];
813  }
814  }
815  // read successors
816  XmlRpc::XmlRpcValue successors_xml;
817  nh.param("successors", successors_xml, successors_xml);
818  if (successors_xml.getType() != XmlRpc::XmlRpcValue::TypeArray)
819  {
820  ROS_FATAL("successors should be an array");
821  return false;
822  }
823  for (size_t i_successors = 0; i_successors < successors_xml.size(); i_successors++) {
824  XmlRpc::XmlRpcValue successor_xml;
825  successor_xml = successors_xml[i_successors];
826  if (successor_xml.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
827  ROS_FATAL("element of successors should be an dictionary");
828  return false;
829  }
830  double x = 0;
831  double y = 0;
832  double theta = 0;
833  if (successor_xml.hasMember("x")) {
834  x = jsk_topic_tools::getXMLDoubleValue(successor_xml["x"]);
835  x += default_x;
836  }
837  if (successor_xml.hasMember("y")) {
838  y = jsk_topic_tools::getXMLDoubleValue(successor_xml["y"]);
839  y += default_y;
840  }
841  if (successor_xml.hasMember("theta")) {
842  theta = jsk_topic_tools::getXMLDoubleValue(successor_xml["theta"]);
843  theta += default_theta;
844  }
845  // successors written in parameters should not include offset
846  // successors using in graph include offset(between the center of the cube and the end-coords)
847  // Footstep returned to a client, its pose indicates end-coords
848  Eigen::Affine3f successor =
849  Eigen::Translation3f(inv_lleg_footstep_offset_[0],
852  affineFromXYYaw(x, y, theta) *
853  Eigen::Translation3f(-inv_rleg_footstep_offset_[0],
856  successors_.push_back(successor);
857  }
858  ROS_INFO("%lu successors are defined", successors_.size());
859  if ((default_x != 0.0) || (default_y != 0.0) || (default_theta != 0.0)) {
860  ROS_INFO("default_offset: #f(%f %f %f)", default_x, default_y, default_theta);
861  }
862  if ((inv_lleg_footstep_offset_[0] != 0) ||
863  (inv_lleg_footstep_offset_[1] != 0) ||
864  (inv_lleg_footstep_offset_[2] != 0) ) {
865  ROS_INFO("left_leg_offset: #f(%f %f %f)",
869  }
870  if ((inv_rleg_footstep_offset_[0] != 0) ||
871  (inv_rleg_footstep_offset_[1] != 0) ||
872  (inv_rleg_footstep_offset_[2] != 0) ) {
873  ROS_INFO("right_leg_offset: #f(%f %f %f)",
877  }
878  for (size_t i = 0; i < successors_.size(); i++) {
879  Eigen::Vector3f tr = successors_[i].translation();
880  float roll, pitch, yaw;
881  pcl::getEulerAngles(successors_[i], roll, pitch, yaw);
882  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);
883  }
884  return true;
885  }
886 
887  void FootstepPlanner::configCallback(Config &config, uint32_t level)
888  {
889  boost::mutex::scoped_lock lock(mutex_);
890  bool need_to_rebuild_graph = false;
891  if (use_pointcloud_model_ != config.use_pointcloud_model) {
892  use_pointcloud_model_ = config.use_pointcloud_model;
893  need_to_rebuild_graph = true;
894  }
895  if (use_lazy_perception_ != config.use_lazy_perception) {
896  use_lazy_perception_ = config.use_lazy_perception;
897  need_to_rebuild_graph = true;
898  }
899  if (use_local_movement_ != config.use_local_movement) {
900  use_local_movement_ = config.use_local_movement;
901  need_to_rebuild_graph = true;
902  }
903  if (resolution_x_ != config.resolution_x) {
904  resolution_x_ = config.resolution_x;
905  need_to_rebuild_graph = true;
906  }
907  if (resolution_y_ != config.resolution_y) {
908  resolution_y_ = config.resolution_y;
909  need_to_rebuild_graph = true;
910  }
911  if (resolution_theta_ != config.resolution_theta) {
912  resolution_theta_ = config.resolution_theta;
913  need_to_rebuild_graph = true;
914  }
915  planning_timeout_ = config.planning_timeout;
916  rich_profiling_ = config.rich_profiling;
917  parameters_.use_transition_limit = config.use_transition_limit;
918  parameters_.use_global_transition_limit = config.use_global_transition_limit;
920  parameters_.local_move_y = config.local_move_y;
921  parameters_.local_move_theta = config.local_move_theta;
922  parameters_.local_move_x_num = config.local_move_x_num;
923  parameters_.local_move_y_num = config.local_move_y_num;
924  parameters_.local_move_theta_num = config.local_move_theta_num;
925  parameters_.local_move_x_offset = config.local_move_x_offset;
926  parameters_.local_move_y_offset = config.local_move_y_offset;
927  parameters_.local_move_theta_offset = config.local_move_theta_offset;
928  parameters_.transition_limit_x = config.transition_limit_x;
929  parameters_.transition_limit_y = config.transition_limit_y;
930  parameters_.transition_limit_z = config.transition_limit_z;
931  parameters_.transition_limit_roll = config.transition_limit_roll;
932  parameters_.transition_limit_pitch = config.transition_limit_pitch;
933  parameters_.transition_limit_yaw = config.transition_limit_yaw;
934  parameters_.global_transition_limit_roll = config.global_transition_limit_roll;
935  parameters_.global_transition_limit_pitch = config.global_transition_limit_pitch;
936  parameters_.goal_pos_thr = config.goal_pos_thr;
937  parameters_.goal_rot_thr = config.goal_rot_thr;
938  parameters_.plane_estimation_use_normal = config.plane_estimation_use_normal;
939  parameters_.plane_estimation_normal_distance_weight = config.plane_estimation_normal_distance_weight;
940  parameters_.plane_estimation_normal_opening_angle = config.plane_estimation_normal_opening_angle;
941  parameters_.plane_estimation_min_ratio_of_inliers = config.plane_estimation_min_ratio_of_inliers;
942  parameters_.plane_estimation_max_iterations = config.plane_estimation_max_iterations;
943  parameters_.plane_estimation_min_inliers = config.plane_estimation_min_inliers;
944  parameters_.plane_estimation_outlier_threshold = config.plane_estimation_outlier_threshold;
945  parameters_.support_check_x_sampling = config.support_check_x_sampling;
946  parameters_.support_check_y_sampling = config.support_check_y_sampling;
947  parameters_.support_check_vertex_neighbor_threshold = config.support_check_vertex_neighbor_threshold;
948  parameters_.support_padding_x = config.support_padding_x;
949  parameters_.support_padding_y = config.support_padding_y;
950  parameters_.skip_cropping = config.skip_cropping;
951  footstep_size_x_ = config.footstep_size_x;
952  footstep_size_y_ = config.footstep_size_y;
953  project_start_state_ = config.project_start_state;
954  project_goal_state_ = config.project_goal_state;
955  close_list_x_num_ = config.close_list_x_num;
956  close_list_y_num_ = config.close_list_y_num;
957  close_list_theta_num_ = config.close_list_theta_num;
958  profile_period_ = config.profile_period;
959  heuristic_ = config.heuristic;
960  heuristic_first_rotation_weight_ = config.heuristic_first_rotation_weight;
961  heuristic_second_rotation_weight_ = config.heuristic_second_rotation_weight;
962  cost_weight_ = config.cost_weight;
963  heuristic_weight_ = config.heuristic_weight;
964  if (use_obstacle_model_ != config.use_obstacle_model) {
965  use_obstacle_model_ = config.use_obstacle_model;
966  need_to_rebuild_graph = true;
967  }
968  parameters_.obstacle_resolution = config.obstacle_resolution;
969  num_finalize_steps_ = config.num_finalize_steps;
970  if (need_to_rebuild_graph) {
971  if (graph_) { // In order to skip first initialization
972  ROS_INFO("re-building graph");
973  buildGraph();
974  }
975  }
976  }
977 
979  {
980  graph_.reset(new FootstepGraph(Eigen::Vector3f(resolution_x_,
988  graph_->setPointCloudModel(pointcloud_model_);
989  }
991  graph_->setObstacleModel(obstacle_model_);
992  }
993  //graph_->setObstacleResolution(parameters_.obstacle_resolution);
994  graph_->setParameters(parameters_);
995  graph_->setBasicSuccessors(successors_);
996  }
997 
999  {
1000  graph_->setHeuristicPathLine(path_line); // copy ???
1001  }
1002 }
jsk_footstep_planner::OK
@ OK
Definition: footstep_planner.h:130
tf::poseMsgToEigen
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
XmlRpc::XmlRpcValue::size
int size() const
jsk_footstep_planner::FootstepPlanner::heuristic_first_rotation_weight_
double heuristic_first_rotation_weight_
Definition: footstep_planner.h:211
rot
rot
jsk_footstep_planner::FootstepPlanner::profile
virtual void profile(FootstepAStarSolver< FootstepGraph > &solver, FootstepGraph::Ptr graph)
Definition: footstep_planner.cpp:765
jsk_footstep_planner::FootstepPlanner::projectFootPrint
virtual bool projectFootPrint(const Eigen::Affine3f &center_pose, const Eigen::Affine3f &left_pose_trans, const Eigen::Affine3f &right_pose_trans, geometry_msgs::Pose &pose)
Definition: footstep_planner.cpp:168
jsk_footstep_planner::FootstepPlanner::pub_open_list_
ros::Publisher pub_open_list_
Definition: footstep_planner.h:172
jsk_footstep_planner::FootstepPlanner::num_finalize_steps_
size_t num_finalize_steps_
Definition: footstep_planner.h:217
step
step
footstep_size
const Eigen::Vector3f footstep_size(0.2, 0.1, 0.000001)
getNamespace
const ROSCPP_DECL std::string & getNamespace()
jsk_footstep_planner::TransitionLimitRP::Ptr
boost::shared_ptr< TransitionLimitRP > Ptr
Definition: transition_limit.h:101
msg
msg
ros::Publisher
jsk_footstep_planner::FootstepParameters::goal_pos_thr
double goal_pos_thr
Definition: footstep_parameters.h:152
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
jsk_footstep_planner::FootstepPlanner::successors_
std::vector< Eigen::Affine3f > successors_
Definition: footstep_planner.h:184
jsk_footstep_planner::FootstepPlanner::project_start_state_
bool project_start_state_
Definition: footstep_planner.h:195
jsk_footstep_planner::FootstepPlanner::resolution_y_
double resolution_y_
Definition: footstep_planner.h:202
boost::shared_ptr< FootstepState >
jsk_footstep_planner::FootstepParameters::local_move_x_num
int local_move_x_num
Definition: footstep_parameters.h:130
i
int i
jsk_footstep_planner::FootstepPlanner::pointcloud_model_frame_id_
std::string pointcloud_model_frame_id_
Definition: footstep_planner.h:215
jsk_footstep_planner::footstepHeuristicStraightRotation
double footstepHeuristicStraightRotation(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
Definition: footstep_graph.cpp:438
pos
pos
header
header
start
start
jsk_footstep_planner::FootstepPlanner::use_obstacle_model_
bool use_obstacle_model_
Definition: footstep_planner.h:200
jsk_footstep_planner::FootstepParameters::plane_estimation_max_iterations
int plane_estimation_max_iterations
Definition: footstep_parameters.h:133
plot_bench.theta
theta
Definition: plot_bench.py:74
jsk_footstep_planner::FootstepPlanner::planCB
virtual void planCB(const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr &goal)
Definition: footstep_planner.cpp:443
cloud
pcl::PointCloud< pcl::PointNormal >::Ptr cloud
Definition: footstep_projection_demo.cpp:49
jsk_footstep_planner::FootstepParameters::local_move_y
double local_move_y
Definition: footstep_parameters.h:138
footstep_array_to_bounding_box_array.pub
pub
Definition: footstep_array_to_bounding_box_array.py:24
footstep_planner.h
jsk_footstep_planner::FootstepPlanner::mutex_
boost::mutex mutex_
Definition: footstep_planner.h:167
jsk_footstep_planner::SolverNode::Ptr
boost::shared_ptr< SolverNode > Ptr
Definition: solver_node.h:117
ROS_DEBUG
#define ROS_DEBUG(...)
jsk_footstep_planner::FootstepParameters::transition_limit_y
double transition_limit_y
Definition: footstep_parameters.h:144
jsk_footstep_planner::FootstepPlanner::resolution_x_
double resolution_x_
Definition: footstep_planner.h:201
jsk_footstep_planner::FootstepParameters::local_move_theta
double local_move_theta
Definition: footstep_parameters.h:139
plot_bench.dy
int dy
Definition: plot_bench.py:23
str
string str
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
jsk_footstep_planner::FootstepPlanner::pointcloudCallback
virtual void pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)
Definition: footstep_planner.cpp:155
resolution
const Eigen::Vector3f resolution(0.05, 0.05, 0.08)
jsk_footstep_planner::FootstepPlanner::straightHeuristic
virtual double straightHeuristic(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
Definition: footstep_planner.cpp:799
boost
jsk_footstep_planner::ERROR
@ ERROR
Definition: footstep_planner.h:130
jsk_footstep_planner::FootstepParameters::local_move_theta_num
int local_move_theta_num
Definition: footstep_parameters.h:132
jsk_footstep_planner::FootstepParameters::local_move_theta_offset
double local_move_theta_offset
Definition: footstep_parameters.h:142
pcl::fromROSMsg
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
jsk_footstep_planner::FootstepPlanner::configCallback
virtual void configCallback(Config &config, uint32_t level)
Definition: footstep_planner.cpp:919
actionlib::SimpleActionServer::isPreemptRequested
bool isPreemptRequested()
jsk_footstep_planner::FootstepPlanner::projectFootPrintService
virtual bool projectFootPrintService(jsk_interactive_marker::SnapFootPrint::Request &req, jsk_interactive_marker::SnapFootPrint::Response &res)
Definition: footstep_planner.cpp:348
plot_bench.dx
int dx
Definition: plot_bench.py:22
jsk_footstep_planner::FootstepPlanner::publishText
virtual void publishText(ros::Publisher &pub, const std::string &text, PlanningStatus status)
Definition: footstep_planner.cpp:402
jsk_footstep_planner::FootstepParameters::transition_limit_yaw
double transition_limit_yaw
Definition: footstep_parameters.h:148
actionlib::SimpleActionServer::setSucceeded
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
jsk_footstep_planner::footstepHeuristicStepCost
double footstepHeuristicStepCost(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph, double first_rotation_weight, double second_rotation_weight)
Definition: footstep_graph.cpp:449
jsk_footstep_planner::FootstepParameters::support_check_vertex_neighbor_threshold
double support_check_vertex_neighbor_threshold
Definition: footstep_parameters.h:158
jsk_footstep_planner::FootstepPlanner::collision_bbox_size_
Eigen::Vector3f collision_bbox_size_
Definition: footstep_planner.h:185
jsk_footstep_planner::FootstepPlanner::latest_header_
std_msgs::Header latest_header_
Definition: footstep_planner.h:189
XmlRpc::XmlRpcValue::TypeStruct
TypeStruct
jsk_footstep_planner::FootstepPlanner::as_
actionlib::SimpleActionServer< jsk_footstep_msgs::PlanFootstepsAction > as_
Definition: footstep_planner.h:168
jsk_footstep_planner::FootstepParameters::global_transition_limit_pitch
double global_transition_limit_pitch
Definition: footstep_parameters.h:150
jsk_footstep_planner::FootstepParameters::goal_rot_thr
double goal_rot_thr
Definition: footstep_parameters.h:153
text
text
jsk_footstep_planner::FootstepPlanner::obstacle_model_frame_id_
std::string obstacle_model_frame_id_
Definition: footstep_planner.h:215
jsk_footstep_planner::FootstepPlanner::use_pointcloud_model_
bool use_pointcloud_model_
Definition: footstep_planner.h:197
jsk_footstep_planner::FootstepPlanner::heuristic_
std::string heuristic_
Definition: footstep_planner.h:210
jsk_footstep_planner::FootstepState::Ptr
boost::shared_ptr< FootstepState > Ptr
Definition: footstep_state.h:115
jsk_footstep_planner::FootstepPlanner::close_list_theta_num_
int close_list_theta_num_
Definition: footstep_planner.h:208
jsk_footstep_planner::WARNING
@ WARNING
Definition: footstep_planner.h:130
jsk_footstep_planner
Definition: ann_grid.h:50
jsk_footstep_planner::FootstepParameters::plane_estimation_min_ratio_of_inliers
double plane_estimation_min_ratio_of_inliers
Definition: footstep_parameters.h:156
jsk_footstep_planner::FootstepPlanner::heuristic_second_rotation_weight_
double heuristic_second_rotation_weight_
Definition: footstep_planner.h:212
jsk_footstep_planner::FootstepPlanner::publishPointCloud
virtual void publishPointCloud(const pcl::PointCloud< pcl::PointNormal > &cloud, ros::Publisher &pub, const std_msgs::Header &header)
Definition: footstep_planner.cpp:754
jsk_footstep_planner::affineFromXYYaw
Eigen::Affine3f affineFromXYYaw(double x, double y, double yaw)
Definition: util.h:74
jsk_footstep_planner::FootstepPlanner::buildGraph
virtual void buildGraph()
Definition: footstep_planner.cpp:1010
jsk_footstep_planner::FootstepPlanner::collisionBoundingBoxInfoService
virtual bool collisionBoundingBoxInfoService(jsk_footstep_planner::CollisionBoundingBoxInfo::Request &req, jsk_footstep_planner::CollisionBoundingBoxInfo::Response &res)
Definition: footstep_planner.cpp:268
jsk_footstep_planner::FootstepPlanner::projectFootstepService
virtual bool projectFootstepService(jsk_footstep_planner::ProjectFootstep::Request &req, jsk_footstep_planner::ProjectFootstep::Response &res)
Definition: footstep_planner.cpp:280
jsk_footstep_planner::FootstepState
Definition: footstep_state.h:112
jsk_footstep_planner::FootstepGraph::Ptr
boost::shared_ptr< FootstepGraph > Ptr
Definition: footstep_graph.h:121
jsk_footstep_planner::FootstepPlanner::project_goal_state_
bool project_goal_state_
Definition: footstep_planner.h:196
jsk_footstep_planner::FootstepParameters::plane_estimation_min_inliers
int plane_estimation_min_inliers
Definition: footstep_parameters.h:134
y
double y
ros::WallTime::now
static WallTime now()
jsk_footstep_planner::FootstepPlanner::close_list_x_num_
int close_list_x_num_
Definition: footstep_planner.h:206
jsk_footstep_planner::FootstepParameters::support_padding_y
double support_padding_y
Definition: footstep_parameters.h:160
pose_array.p
p
Definition: pose_array.py:21
ROS_ERROR
#define ROS_ERROR(...)
graph
FootstepGraph::Ptr graph
Definition: footstep_planning_2d_interactive_demo.cpp:51
jsk_footstep_planner::FootstepParameters::use_transition_limit
bool use_transition_limit
Definition: footstep_parameters.h:125
jsk_footstep_planner::FootstepPlanner::readSuccessors
virtual bool readSuccessors(ros::NodeHandle &nh)
Definition: footstep_planner.cpp:828
jsk_footstep_planner::FootstepState::fromROSMsg
static FootstepState::Ptr fromROSMsg(const jsk_footstep_msgs::Footstep &f, const Eigen::Vector3f &size, const Eigen::Vector3f &resolution)
Definition: footstep_state.cpp:592
jsk_footstep_planner::FootstepGraph::path_cost_original
double path_cost_original(StatePtr from, StatePtr to, double prev_cost)
Definition: footstep_graph.h:287
jsk_footstep_planner::FootstepParameters::support_check_y_sampling
int support_check_y_sampling
Definition: footstep_parameters.h:136
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
_1
boost::arg< 1 > _1
ROS_WARN
#define ROS_WARN(...)
jsk_footstep_planner::FootstepPlanner::setHeuristicPathService
virtual bool setHeuristicPathService(jsk_footstep_planner::SetHeuristicPath::Request &req, jsk_footstep_planner::SetHeuristicPath::Response &res)
Definition: footstep_planner.cpp:381
jsk_footstep_planner::FootstepParameters::plane_estimation_normal_opening_angle
double plane_estimation_normal_opening_angle
Definition: footstep_parameters.h:155
jsk_footstep_planner::FootstepPlanner::close_list_y_num_
int close_list_y_num_
Definition: footstep_planner.h:207
jsk_footstep_planner::FootstepPlanner::planning_timeout_
double planning_timeout_
Definition: footstep_planner.h:216
jsk_footstep_planner::FootstepParameters::local_move_x_offset
double local_move_x_offset
Definition: footstep_parameters.h:140
tf::poseEigenToMsg
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
jsk_footstep_planner::FootstepParameters::transition_limit_z
double transition_limit_z
Definition: footstep_parameters.h:145
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
jsk_footstep_planner::FootstepPlanner::resolution_theta_
double resolution_theta_
Definition: footstep_planner.h:203
lock
mutex_t * lock
start_time
start_time
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
points
def points
ros::WallTime
jsk_footstep_planner::FootstepPlanner::heuristic_weight_
double heuristic_weight_
Definition: footstep_planner.h:214
actionlib::SimpleActionServer::setPreempted
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
jsk_footstep_planner::FootstepParameters::transition_limit_roll
double transition_limit_roll
Definition: footstep_parameters.h:146
jsk_footstep_planner::FootstepPlanner::footstep_size_x_
double footstep_size_x_
Definition: footstep_planner.h:204
pcl_conversion_util.h
XmlRpc::XmlRpcValue::getType
const Type & getType() const
ROS_FATAL
#define ROS_FATAL(...)
jsk_footstep_planner::FootstepPlanner::pointcloud_model_
pcl::PointCloud< pcl::PointNormal >::Ptr pointcloud_model_
Definition: footstep_planner.h:181
jsk_recognition_utils::PolyLine
jsk_footstep_planner::FootstepPlanner::inv_lleg_footstep_offset_
Eigen::Vector3f inv_lleg_footstep_offset_
Definition: footstep_planner.h:187
XmlRpc::XmlRpcValue::TypeArray
TypeArray
jsk_footstep_planner::FootstepParameters::support_check_x_sampling
int support_check_x_sampling
Definition: footstep_parameters.h:135
XmlRpc::XmlRpcValue::hasMember
bool hasMember(const std::string &name) const
jsk_footstep_planner::FootstepPlanner::FootstepPlanner
FootstepPlanner(ros::NodeHandle &nh)
Definition: footstep_planner.cpp:78
jsk_footstep_planner::FootstepPlanner::obstacle_model_
pcl::PointCloud< pcl::PointXYZ >::Ptr obstacle_model_
Definition: footstep_planner.h:182
jsk_footstep_planner::FootstepPlanner::inv_rleg_footstep_offset_
Eigen::Vector3f inv_rleg_footstep_offset_
Definition: footstep_planner.h:188
jsk_footstep_planner::FootstepPlanner::profile_period_
int profile_period_
Definition: footstep_planner.h:209
jsk_footstep_planner::FootstepPlanner::rich_profiling_
bool rich_profiling_
Definition: footstep_planner.h:194
jsk_footstep_planner::FootstepParameters::global_transition_limit_roll
double global_transition_limit_roll
Definition: footstep_parameters.h:149
jsk_footstep_planner::FootstepParameters::transition_limit_x
double transition_limit_x
Definition: footstep_parameters.h:143
jsk_footstep_planner::FootstepParameters::plane_estimation_normal_distance_weight
double plane_estimation_normal_distance_weight
Definition: footstep_parameters.h:154
jsk_footstep_planner::FootstepParameters::support_padding_x
double support_padding_x
Definition: footstep_parameters.h:159
jsk_footstep_planner::FootstepPlanner::followPathLineHeuristic
virtual double followPathLineHeuristic(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
Definition: footstep_planner.cpp:811
jsk_footstep_planner::FootstepParameters::local_move_x
double local_move_x
Definition: footstep_parameters.h:137
jsk_footstep_planner::FootstepPlanner::result_
jsk_footstep_msgs::PlanFootstepsResult result_
Definition: footstep_planner.h:169
f
f
jsk_footstep_planner::FootstepPlanner::straightRotationHeuristic
virtual double straightRotationHeuristic(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
Definition: footstep_planner.cpp:805
jsk_footstep_planner::FootstepParameters::skip_cropping
bool skip_cropping
Definition: footstep_parameters.h:129
jsk_footstep_planner::FootstepPlanner::zeroHeuristic
virtual double zeroHeuristic(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
Definition: footstep_planner.cpp:793
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
pcl::toROSMsg
void toROSMsg(const pcl::PointCloud< T > &cloud, sensor_msgs::Image &msg)
jsk_footstep_planner::FootstepPlanner::projectFootPrintWithLocalSearchService
virtual bool projectFootPrintWithLocalSearchService(jsk_interactive_marker::SnapFootPrint::Request &req, jsk_interactive_marker::SnapFootPrint::Response &res)
Definition: footstep_planner.cpp:208
jsk_footstep_planner::TransitionLimitXYZRPY::Ptr
boost::shared_ptr< TransitionLimitXYZRPY > Ptr
Definition: transition_limit.h:120
jsk_footstep_planner::footstepHeuristicStraight
double footstepHeuristicStraight(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
Definition: footstep_graph.cpp:427
x
double x
jsk_footstep_planner::FootstepPlanner::collision_bbox_offset_
Eigen::Affine3f collision_bbox_offset_
Definition: footstep_planner.h:186
jsk_footstep_planner::FootstepPlanner::graph_
FootstepGraph::Ptr graph_
Definition: footstep_planner.h:183
jsk_footstep_planner::FootstepParameters::local_move_y_offset
double local_move_y_offset
Definition: footstep_parameters.h:141
jsk_footstep_planner::FootstepPlanner::pub_text_
ros::Publisher pub_text_
Definition: footstep_planner.h:173
jsk_footstep_planner::FootstepParameters::transition_limit_pitch
double transition_limit_pitch
Definition: footstep_parameters.h:147
jsk_footstep_planner::FootstepPlanner::obstacleCallback
virtual void obstacleCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)
Definition: footstep_planner.cpp:143
DurationBase< WallDuration >::toSec
double toSec() const
jsk_footstep_planner::FootstepPlanner::use_lazy_perception_
bool use_lazy_perception_
Definition: footstep_planner.h:198
jsk_footstep_planner::FootstepParameters::use_global_transition_limit
bool use_global_transition_limit
Definition: footstep_parameters.h:127
ros::WallDuration
jsk_footstep_planner::FootstepPlanner::cost_weight_
double cost_weight_
Definition: footstep_planner.h:213
jsk_footstep_planner::FootstepPlanner::use_local_movement_
bool use_local_movement_
Definition: footstep_planner.h:199
jsk_footstep_planner::FootstepParameters::obstacle_resolution
double obstacle_resolution
Definition: footstep_parameters.h:151
ROS_INFO
#define ROS_INFO(...)
jsk_footstep_planner::FootstepPlanner::setHeuristicPathLine
virtual void setHeuristicPathLine(jsk_recognition_utils::PolyLine &path_line)
Definition: footstep_planner.cpp:1030
jsk_footstep_planner::FootstepGraph::successors_original
bool successors_original(StatePtr target_state, std::vector< FootstepGraph::StatePtr > &ret)
Definition: footstep_graph.cpp:306
config
config
jsk_footstep_planner::FootstepPlanner::stepCostHeuristic
virtual double stepCostHeuristic(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
Definition: footstep_planner.cpp:786
jsk_footstep_planner::footstepHeuristicZero
double footstepHeuristicZero(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
Definition: footstep_graph.cpp:420
jsk_footstep_planner::FootstepPlanner::pub_close_list_
ros::Publisher pub_close_list_
Definition: footstep_planner.h:171
req
req
jsk_footstep_planner::FootstepParameters::plane_estimation_outlier_threshold
double plane_estimation_outlier_threshold
Definition: footstep_parameters.h:157
jsk_footstep_planner::FootstepPlanner::footstep_size_y_
double footstep_size_y_
Definition: footstep_planner.h:205
XmlRpc::XmlRpcValue
jsk_footstep_planner::PlanningStatus
PlanningStatus
Definition: footstep_planner.h:96
res
res
ros::NodeHandle
jsk_footstep_planner::FootstepParameters::plane_estimation_use_normal
bool plane_estimation_use_normal
Definition: footstep_parameters.h:128
jsk_footstep_planner::FootstepParameters::local_move_y_num
int local_move_y_num
Definition: footstep_parameters.h:131
jsk_footstep_planner::footstepHeuristicFollowPathLine
double footstepHeuristicFollowPathLine(SolverNode< FootstepState, FootstepGraph >::Ptr node, FootstepGraph::Ptr graph)
Definition: footstep_graph.cpp:488
jsk_footstep_planner::FootstepPlanner::parameters_
FootstepParameters parameters_
Definition: footstep_planner.h:191


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