footstep_state.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 
39 
43 
44 #define DEBUG 0
45 #if DEBUG
46 // debug
47 #include <ros/ros.h>
48 #include <visualization_msgs/Marker.h>
49 #include <visualization_msgs/MarkerArray.h>
50 #endif
51 
52 #define DEBUG_PRINT(proc) if (debug_print_) { std::cerr << proc << std::endl; }
53 
54 namespace jsk_footstep_planner
55 {
56 #if DEBUG
57  ros::Publisher pub_debug_marker;
58 #endif
59  std::string projectStateToString(unsigned int state)
60  {
61  if (state == projection_state::success) {
62  return "success";
63  }
64  else if (state == projection_state::no_pointcloud) {
65  return "no pointcloud";
66  }
67  else if (state == projection_state::no_enough_support) {
68  return "no enough support";
69  }
70  else if (state == projection_state::no_plane) {
71  return "no plane";
72  }
73  else if (state == projection_state::no_enough_inliers) {
74  return "no enough inliers";
75  }
76  else if (state == projection_state::close_to_success) {
77  return "close to success";
78  }
79  else {
80  return "unknown error";
81  }
82  }
83 
84  jsk_footstep_msgs::Footstep::Ptr
86  {
87  jsk_footstep_msgs::Footstep::Ptr ret(new jsk_footstep_msgs::Footstep);
88  tf::poseEigenToMsg(pose_, ret->pose);
89  ret->dimensions.x = dimensions_[0];
90  ret->dimensions.y = dimensions_[1];
91  ret->dimensions.z = dimensions_[2];
92  ret->leg = leg_;
93  return ret;
94  }
95  jsk_footstep_msgs::Footstep::Ptr
96  FootstepState::toROSMsg(const Eigen::Vector3f& ioffset)
97  {
98  jsk_footstep_msgs::Footstep::Ptr ret(new jsk_footstep_msgs::Footstep);
99  Eigen::Affine3f newpose = pose_ * Eigen::Translation3f(ioffset);
100  tf::poseEigenToMsg(newpose, ret->pose);
101  ret->dimensions.x = dimensions_[0];
102  ret->dimensions.y = dimensions_[1];
103  ret->dimensions.z = dimensions_[2];
104  ret->offset.x = - ioffset[0];
105  ret->offset.y = - ioffset[1];
106  ret->offset.z = - ioffset[2];
107  ret->leg = leg_;
108  return ret;
109  }
110 
111  pcl::PointIndices::Ptr
112  FootstepState::cropPointCloudExact(pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
113  pcl::PointIndices::Ptr near_indices,
114  double padding_x, double padding_y)
115  {
116  // Project vertices into 2d
117  Eigen::Vector3f z(0, 0, 1);
118  Eigen::Vector3f a = pose_ * Eigen::Vector3f( dimensions_[0]/2 + padding_x, dimensions_[1]/2 + padding_y, 0);
119  Eigen::Vector3f b = pose_ * Eigen::Vector3f(-dimensions_[0]/2 - padding_x, dimensions_[1]/2 + padding_y, 0);
120  Eigen::Vector3f c = pose_ * Eigen::Vector3f(-dimensions_[0]/2 - padding_x, -dimensions_[1]/2 - padding_y, 0);
121  Eigen::Vector3f d = pose_ * Eigen::Vector3f( dimensions_[0]/2 + padding_x, -dimensions_[1]/2 - padding_y, 0);
122  Eigen::Vector3f a_2d = a + (- z.dot(a)) * z;
123  Eigen::Vector3f b_2d = b + (- z.dot(b)) * z;
124  Eigen::Vector3f c_2d = c + (- z.dot(c)) * z;
125  Eigen::Vector3f d_2d = d + (- z.dot(d)) * z;
126 
127  Eigen::Vector2f a2d(a_2d[0], a_2d[1]);
128  Eigen::Vector2f b2d(b_2d[0], b_2d[1]);
129  Eigen::Vector2f c2d(c_2d[0], c_2d[1]);
130  Eigen::Vector2f d2d(d_2d[0], d_2d[1]);
131  //std::set<int> set_indices;
132  pcl::PointIndices::Ptr ret(new pcl::PointIndices);
133  ret->indices.reserve(near_indices->indices.size());
134  const std::vector<int> near_indices_indices = near_indices->indices;
135  for (size_t i = 0; i < near_indices->indices.size(); i++) {
136  const int index = near_indices_indices[i];
137  const pcl::PointNormal point = cloud->points[index];
138  const Eigen::Vector3f ep = point.getVector3fMap();
139  const Eigen::Vector3f point_2d = ep + (-z.dot(ep)) * z;
140  const Eigen::Vector2f p2d(point_2d[0], point_2d[1]);
141  if (cross2d((b2d - a2d), (p2d - a2d)) > 0 &&
142  cross2d((c2d - b2d), (p2d - b2d)) > 0 &&
143  cross2d((d2d - c2d), (p2d - c2d)) > 0 &&
144  cross2d((a2d - d2d), (p2d - d2d)) > 0) {
145  //set_indices.insert(index);
146  ret->indices.push_back(index);
147  }
148  }
149 
150  //ret->indices = std::vector<int>(set_indices.begin(), set_indices.end());
151  return ret;
152  }
153 
154  pcl::PointIndices::Ptr
155  FootstepState::cropPointCloud(pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
157  double padding_x, double padding_y)
158  {
159  pcl::PointIndices::Ptr near_indices(new pcl::PointIndices);
160  Eigen::Vector3f a = pose_ * Eigen::Vector3f( dimensions_[0]/2 + padding_x, dimensions_[1]/2 + padding_y, 0);
161  Eigen::Vector3f b = pose_ * Eigen::Vector3f(-dimensions_[0]/2 - padding_x, dimensions_[1]/2 + padding_y, 0);
162  Eigen::Vector3f c = pose_ * Eigen::Vector3f(-dimensions_[0]/2 - padding_x, -dimensions_[1]/2 - padding_y, 0);
163  Eigen::Vector3f d = pose_ * Eigen::Vector3f( dimensions_[0]/2 + padding_x, -dimensions_[1]/2 - padding_y, 0);
164  grid_search->approximateSearchInBox(a, b, c, d, *near_indices);
165  return cropPointCloudExact(cloud, near_indices, padding_x, padding_y);
166  }
167 
168 #if 0
169  pcl::PointIndices::Ptr
170  FootstepState::cropPointCloud(pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
171  pcl::search::Octree<pcl::PointNormal>& tree)
172  {
173  pcl::PointNormal center;
174  center.getVector3fMap() = Eigen::Vector3f(pose_.translation());
175  center.z = 0.0;
176  float r = 0.2;
177  pcl::PointIndices::Ptr near_indices(new pcl::PointIndices);
178  std::vector<float> distances;
179  tree.radiusSearch(center, r, near_indices->indices, distances);
180  return cropPointCloudExact(cloud, near_indices);
181  }
182 #endif
183  bool FootstepState::crossCheck(FootstepState::Ptr other, float collision_padding)
184  {
185  Eigen::Vector3f a0, a1, a2, a3;
186  Eigen::Vector3f b0, b1, b2, b3;
187  vertices(a0, a1, a2, a3, collision_padding);
188  other->vertices(b0, b1, b2, b3, collision_padding);
189  Line2D a_01(a0, a1), a_12(a1, a2), a_23(a2, a3), a_30(a3, a0);
190  Line2D b_01(b0, b1), b_12(b1, b2), b_23(b2, b3), b_30(b3, b0);
191  return !(a_01.isCrossing(b_01) ||
192  a_01.isCrossing(b_12) ||
193  a_01.isCrossing(b_23) ||
194  a_01.isCrossing(b_30) ||
195  a_12.isCrossing(b_01) ||
196  a_12.isCrossing(b_12) ||
197  a_12.isCrossing(b_23) ||
198  a_12.isCrossing(b_30) ||
199  a_23.isCrossing(b_01) ||
200  a_23.isCrossing(b_12) ||
201  a_23.isCrossing(b_23) ||
202  a_23.isCrossing(b_30) ||
203  a_30.isCrossing(b_01) ||
204  a_30.isCrossing(b_12) ||
205  a_30.isCrossing(b_23) ||
206  a_30.isCrossing(b_30));
207  }
208 
210  FootstepState::projectToCloud(pcl::KdTreeFLANN<pcl::PointNormal>& tree,
211  pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
213  pcl::search::Octree<pcl::PointNormal>& tree_2d,
214  pcl::PointCloud<pcl::PointNormal>::Ptr cloud_2d,
215  const Eigen::Vector3f& z,
216  unsigned int& error_state,
218  {
219  // TODO: z is ignored
220  // extract candidate points
221  //pcl::PointIndices::Ptr indices = cropPointCloud(cloud_2d, tree_2d);
222  // Before computing, check is it supported or not to omit recognition
223  DEBUG_PRINT(std::endl << "[FS state] projectToCloud");
224  pcl::PointIndices::Ptr indices;
225  FootstepSupportState presupport_state;
226  if (parameters.skip_cropping) {
227  presupport_state = isSupportedByPointCloudWithoutCropping(
228  pose_, cloud, tree, indices,
229  parameters.support_check_x_sampling,
230  parameters.support_check_y_sampling,
232  DEBUG_PRINT("[FS state] pre /(skip_cropping) projection state " << presupport_state);
233  }
234  indices = cropPointCloud(cloud, grid_search,
235  parameters.support_padding_x,
236  parameters.support_padding_y);
237  DEBUG_PRINT("[FS state] pre / indices " << indices->indices.size());
238  if (indices->indices.size() < parameters.plane_estimation_min_inliers) {
239  DEBUG_PRINT("[FS state] no enough inliners");
241  return FootstepState::Ptr();
242  }
243  if (!parameters.skip_cropping) {
244 #if DEBUG
245  double ax = 0.0, ay = 0.0, az = 0.0;
246  double xx = 0.0, yy = 0.0, zz = 0.0;
247  for (size_t i = 0; i < indices->indices.size(); i++) {
248  pcl::PointNormal pp = cloud->points[indices->indices[i]];
249  ROS_INFO("%d %f %f %f", indices->indices[i], pp.x, pp.y, pp.z);
250  ax += pp.x; ay += pp.y; az += pp.z;
251  xx += pp.x*pp.x; yy += pp.y*pp.y; zz += pp.z*pp.z;
252  }
253  int ss = indices->indices.size();
254  ROS_INFO("ave( %d ): %f %f %f, %f %f %f",
255  ss, ax/ss, ay/ss, az/ss,
256  sqrt(xx/ss - (ax/ss)*(ax/ss)),
257  sqrt(yy/ss - (ay/ss)*(ay/ss)),
258  sqrt(zz/ss - (az/ss)*(az/ss)));
259 #endif
260  presupport_state = isSupportedByPointCloud(
261  pose_, cloud, tree, indices,
262  parameters.support_check_x_sampling,
263  parameters.support_check_y_sampling,
265  DEBUG_PRINT("[FS state] pre / (!skip_cropping) projection state " << presupport_state);
266  }
267  if (presupport_state == projection_state::success) {
269  resolution_,
270  index_x_,
271  index_y_,
272  index_yaw_));
273  }
274  // estimate plane with ransac
275  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
276  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
277  if (!parameters.plane_estimation_use_normal) {
278  pcl::SACSegmentation<pcl::PointNormal> seg;
279  seg.setOptimizeCoefficients (true);
280  seg.setRadiusLimits(0.01, std::numeric_limits<double>::max ());
281  seg.setMethodType(pcl::SAC_RANSAC);
282  seg.setDistanceThreshold(parameters.plane_estimation_outlier_threshold);
283  seg.setModelType(pcl::SACMODEL_PLANE);
284  seg.setInputCloud(cloud);
285  //
286  seg.setIndices(indices);
287  seg.setMaxIterations(parameters.plane_estimation_max_iterations);
288  seg.segment(*inliers, *coefficients);
289  } else {
290  pcl::SACSegmentationFromNormals<pcl::PointNormal, pcl::PointNormal> seg;
291  seg.setOptimizeCoefficients (true);
292  seg.setRadiusLimits(0.01, std::numeric_limits<double>::max ());
293  seg.setMethodType(pcl::SAC_RANSAC);
294  seg.setDistanceThreshold(parameters.plane_estimation_outlier_threshold);
295  seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
296  seg.setInputCloud(cloud);
297  //
298  seg.setInputNormals(cloud);
299  seg.setNormalDistanceWeight(parameters.plane_estimation_normal_distance_weight);
300  seg.setMinMaxOpeningAngle(-parameters.plane_estimation_normal_opening_angle,
302  //
303  seg.setIndices(indices);
304  seg.setMaxIterations(parameters.plane_estimation_max_iterations);
305  seg.segment(*inliers, *coefficients);
306  }
307 
308  DEBUG_PRINT( "[FS state] inliers " << inliers->indices.size() );
309  if (inliers->indices.size() == 0) {
310  DEBUG_PRINT( "[FS state] no plane" );
311  error_state = projection_state::no_plane;
312  return FootstepState::Ptr();
313  }
314  else if (inliers->indices.size() < parameters.plane_estimation_min_inliers) {
315  DEBUG_PRINT( "[FS state] no enough inliners " << inliers->indices.size() );
317  return FootstepState::Ptr();
318  }
319  else {
320  jsk_recognition_utils::Plane plane(coefficients->values);
321  if (!plane.isSameDirection(z)) {
322  plane = plane.flip();
323  }
324  // plane
325  // DEBUG_PRINT( "[FS state] no enough inliners" );
326  Eigen::Vector3f n = plane.getNormal();
327  Eigen::Vector3f x = pose_.matrix().block<3, 3>(0, 0) * Eigen::Vector3f::UnitX();
328  if (acos(n.dot(x)) == 0) {
330  return FootstepState::Ptr();
331  }
332  Eigen::Vector3f rotation_axis = n.cross(x).normalized();
333  Eigen::Vector3f new_x = Eigen::AngleAxisf(M_PI / 2.0, rotation_axis) * n;
334  if (acos(new_x.dot(x)) > M_PI / 2.0) {
335  new_x = - new_x;
336  }
337  Eigen::Vector3f new_y = n.cross(new_x);
338  Eigen::Matrix3f new_rot_mat;
339  new_rot_mat << new_x, new_y, n;
340  Eigen::Quaternionf new_rot(new_rot_mat);
341  Eigen::Vector3f p(pose_.translation());
342  double alpha = (- plane.getD() - n.dot(p)) / (n.dot(z));
343  Eigen::Vector3f q = p + alpha * z;
344 
345  Eigen::Affine3f new_pose = Eigen::Translation3f(q) * new_rot;
346  // std::cout << "new_pose: " << std::endl << new_pose.matrix() << std::endl;
347  // std::cout << "q: " << std::endl << q << std::endl;
348  // std::cout << "new_rot_mat: " << std::endl << new_rot_mat << std::endl;
349  //Eigen::Affine3f new_pose = new_rot * Eigen::Translation3f(q);
350  // check is it enough points to support the footstep
351 #if DEBUG
352  visualization_msgs::Marker marker;
353  marker.header.frame_id = "map";
354  marker.header.stamp = ros::Time();
355  //marker.ns = "my_namespace";
356  marker.id = 0;
357  marker.type = visualization_msgs::Marker::POINTS;
358  marker.action = visualization_msgs::Marker::ADD;
359  marker.pose.position.x = 0;
360  marker.pose.position.y = 0;
361  marker.pose.position.z = 0;
362  marker.pose.orientation.x = 0.0;
363  marker.pose.orientation.y = 0.0;
364  marker.pose.orientation.z = 0.0;
365  marker.pose.orientation.w = 1.0;
366  marker.scale.x = 0.01;
367  marker.scale.y = 0.01;
368  marker.scale.z = 0.1;
369  marker.color.a = 1.0; // Don't forget to set the alpha!
370  marker.color.r = 1.0;
371  marker.color.g = 0.0;
372  marker.color.b = 0.0;
373  //marker.points.resize(inliers->indices.size());
374  for(int i; i < inliers->indices.size(); i++) {
375  geometry_msgs::Point pp;
376  pcl::PointNormal pt = cloud->points[inliers->indices[i]];
377  pp.x = pt.x;
378  pp.y = pt.y;
379  pp.z = pt.z;
380  marker.points.push_back(pp);
381  }
382  visualization_msgs::Marker marker_p;
383  marker_p.header.frame_id = "map";
384  marker_p.header.stamp = ros::Time();
385  //marker_p.ns = "my_namespace";
386  marker_p.id = 1;
387  marker_p.type = visualization_msgs::Marker::POINTS;
388  marker_p.action = visualization_msgs::Marker::ADD;
389  marker_p.pose.position.x = 0;
390  marker_p.pose.position.y = 0;
391  marker_p.pose.position.z = 0;
392  marker_p.pose.orientation.x = 0.0;
393  marker_p.pose.orientation.y = 0.0;
394  marker_p.pose.orientation.z = 0.0;
395  marker_p.pose.orientation.w = 1.0;
396  marker_p.scale.x = 0.01;
397  marker_p.scale.y = 0.01;
398  marker_p.scale.z = 0.1;
399  marker_p.color.a = 1.0; // Don't forget to set the alpha!
400  marker_p.color.r = 0.0;
401  marker_p.color.g = 0.0;
402  marker_p.color.b = 1.0;
403  //marker.points.resize(inliers->indices.size());
404  for(int i; i < inliers->indices.size(); i++) {
405  geometry_msgs::Point pp;
406  pcl::PointNormal pt = cloud->points[inliers->indices[i]];
407  Eigen::Vector3f ep(pt.x, pt.y, pt.z);
408  Eigen::Vector3f rt;
409  plane.project(ep, rt);
410  pp.x = rt(0);
411  pp.y = rt(1);
412  pp.z = rt(2);
413  marker_p.points.push_back(pp);
414  }
415 
416  //only if using a MESH_RESOURCE marker type:
417  visualization_msgs::MarkerArray arry;
418  arry.markers.push_back(marker);
419  arry.markers.push_back(marker_p);
420  pub_debug_marker.publish( arry );
421 #endif
422  FootstepSupportState support_state;
423  if (parameters.skip_cropping) {
425  new_pose, cloud, tree, inliers,
426  parameters.support_check_x_sampling,
427  parameters.support_check_y_sampling,
429  DEBUG_PRINT( "[FS state] (skip_cropping) projection state " << support_state );
430  }
431  else {
432  support_state = isSupportedByPointCloud(
433  new_pose, cloud, tree, inliers,
434  parameters.support_check_x_sampling,
435  parameters.support_check_y_sampling,
437  DEBUG_PRINT( "[FS state] (!skip_cropping) projection state " << support_state );
438  }
439  if (support_state == NOT_SUPPORTED) {
440  DEBUG_PRINT( "[FS state] NOT SUPPORTED" );
442  return FootstepState::Ptr();
443  }
444  else if (support_state == CLOSE_TO_SUPPORTED) {
445  DEBUG_PRINT( "[FS state] CLOSE TO SUPPORTED" );
447  return FootstepState::Ptr();
448  }
449  else if ((inliers->indices.size() / (double)indices->indices.size()) < parameters.plane_estimation_min_ratio_of_inliers ) {
450  DEBUG_PRINT( "[FS state] ratio of inliers " << (inliers->indices.size() / (double)indices->indices.size()) );
452  return FootstepState::Ptr();
453  }
454  else {
455  DEBUG_PRINT( "[FS state] success" );
456  error_state = projection_state::success;
457  return FootstepState::Ptr(new FootstepState(leg_, new_pose, dimensions_,
458  resolution_,
459  index_x_,
460  index_y_,
461  index_yaw_));
462  }
463  }
464  }
465 
467  FootstepState::isSupportedByPointCloud(const Eigen::Affine3f& pose,
468  pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
469  pcl::KdTreeFLANN<pcl::PointNormal>& tree,
470  pcl::PointIndices::Ptr inliers,
471  const int foot_x_sampling_num,
472  const int foot_y_sampling_num,
473  const double vertex_threshold)
474  {
475  const double dx = dimensions_[0] / foot_x_sampling_num;
476  const double dy = dimensions_[1] / foot_y_sampling_num;
477  // vertices
478  const Eigen::Vector3f ux = Eigen::Vector3f::UnitX();
479  const Eigen::Vector3f uy = Eigen::Vector3f::UnitY();
480  const Eigen::Affine3f new_origin = pose *
481  Eigen::Translation3f(- ux * dimensions_[0] / 2.0) *
482  Eigen::Translation3f(- uy * dimensions_[1] / 2.0);
483  const Eigen::Affine3f inv_pose = new_origin.inverse();
484 
485  bool occupiedp[foot_x_sampling_num][foot_y_sampling_num];
486  // Initialize by false
487  for (size_t i = 0; i < foot_x_sampling_num; i++) {
488  for (size_t j = 0; j < foot_y_sampling_num; j++) {
489  occupiedp[i][j] = false;
490  }
491  }
492 
493  for (size_t i = 0; i < inliers->indices.size(); i++) {
494  pcl::PointNormal pp = cloud->points[inliers->indices[i]];
495  const Eigen::Vector3f p = pp.getVector3fMap();
496  const Eigen::Vector3f local_p = inv_pose * p;
497  const int nx = floor(local_p[0] / dx);
498  const int ny = floor(local_p[1] / dy);
499  //std::cout << "abs_local_p2: " << std::abs(local_p[2]) << "/" << vertex_threshold << std::endl;
500  if (0 <= nx && nx < foot_x_sampling_num &&
501  0 <= ny && ny < foot_y_sampling_num &&
502  std::abs(local_p[2]) < vertex_threshold) {
503  occupiedp[nx][ny] = true;
504  }
505  }
506  for (size_t i = 0; i < foot_x_sampling_num; i++) {
507  for (size_t j = 0; j < foot_y_sampling_num; j++) {
508  if (!occupiedp[i][j]) {
509  return NOT_SUPPORTED;
510  }
511  }
512  }
513 
514  Eigen::Vector3f a((pose * Eigen::Translation3f(ux * dimensions_[0] / 2 + uy * dimensions_[1] / 2)).translation());
515  Eigen::Vector3f b((pose * Eigen::Translation3f(- ux * dimensions_[0] / 2 + uy * dimensions_[1] / 2)).translation());
516  Eigen::Vector3f c((pose * Eigen::Translation3f(- ux * dimensions_[0] / 2 - uy * dimensions_[1] / 2)).translation());
517  Eigen::Vector3f d((pose * Eigen::Translation3f(ux * dimensions_[0] / 2 - uy * dimensions_[1] / 2)).translation());
518  pcl::PointNormal pa, pb, pc, pd, p;
519  pa.getVector3fMap() = a;
520  pb.getVector3fMap() = b;
521  pc.getVector3fMap() = c;
522  pd.getVector3fMap() = d;
523  p.getVector3fMap() = Eigen::Vector3f(pose.translation());
524  std::vector<int> kdl_indices;
525  std::vector<float> kdl_distances;
526  if (tree.radiusSearch(p, vertex_threshold, kdl_indices, kdl_distances, 1) > 0 &&
527  tree.radiusSearch(pa, vertex_threshold, kdl_indices, kdl_distances, 1) > 0 &&
528  tree.radiusSearch(pb, vertex_threshold, kdl_indices, kdl_distances, 1) > 0 &&
529  tree.radiusSearch(pc, vertex_threshold, kdl_indices, kdl_distances, 1) > 0 &&
530  tree.radiusSearch(pd, vertex_threshold, kdl_indices, kdl_distances, 1) > 0) {
531  return SUPPORTED;
532  }
533  else {
534  return CLOSE_TO_SUPPORTED;
535  }
536  }
537 
540  const Eigen::Affine3f& pose,
541  pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
542  pcl::KdTreeFLANN<pcl::PointNormal>& tree,
543  pcl::PointIndices::Ptr inliers,
544  const int foot_x_sampling_num,
545  const int foot_y_sampling_num,
546  const double vertex_threshold)
547  {
548  const Eigen::Vector3f ux = Eigen::Vector3f::UnitX();
549  const Eigen::Vector3f uy = Eigen::Vector3f::UnitY();
550  Eigen::Vector3f a((pose * Eigen::Translation3f(ux * dimensions_[0] / 2 + uy * dimensions_[1] / 2)).translation());
551  Eigen::Vector3f b((pose * Eigen::Translation3f(- ux * dimensions_[0] / 2 + uy * dimensions_[1] / 2)).translation());
552  Eigen::Vector3f c((pose * Eigen::Translation3f(- ux * dimensions_[0] / 2 - uy * dimensions_[1] / 2)).translation());
553  Eigen::Vector3f d((pose * Eigen::Translation3f(ux * dimensions_[0] / 2 - uy * dimensions_[1] / 2)).translation());
554  pcl::PointNormal pa, pb, pc, pd, p;
555  pa.getVector3fMap() = a;
556  pb.getVector3fMap() = b;
557  pc.getVector3fMap() = c;
558  pd.getVector3fMap() = d;
559  p.getVector3fMap() = Eigen::Vector3f(pose.translation());
560  std::vector<int> kdl_indices;
561  std::vector<float> kdl_distances;
562  size_t success_count = 0;
563 
564  if (tree.radiusSearch(p, vertex_threshold, kdl_indices, kdl_distances, 1) > 0) {
565  ++success_count;
566  }
567  if (tree.radiusSearch(pa, vertex_threshold, kdl_indices, kdl_distances, 1) > 0) {
568  ++success_count;
569  }
570  if (tree.radiusSearch(pb, vertex_threshold, kdl_indices, kdl_distances, 1) > 0) {
571  ++success_count;
572  }
573  if (tree.radiusSearch(pc, vertex_threshold, kdl_indices, kdl_distances, 1) > 0) {
574  ++success_count;
575  }
576  if (tree.radiusSearch(pd, vertex_threshold, kdl_indices, kdl_distances, 1) > 0) {
577  ++success_count;
578  }
579  if (success_count == 5) {
580  return SUPPORTED;
581  }
582  // else if (success_count > 0) {
583  // return CLOSE_TO_SUPPORTED;
584  // }
585  else {
586  return NOT_SUPPORTED;
587  }
588  }
589 
590 
591 
592  FootstepState::Ptr FootstepState::fromROSMsg(const jsk_footstep_msgs::Footstep& f,
593  const Eigen::Vector3f& size,
594  const Eigen::Vector3f& resolution)
595  {
596  Eigen::Affine3f pose;
597  Eigen::Vector3f offset (f.offset.x, f.offset.y, f.offset.z);
598  tf::poseMsgToEigen(f.pose, pose);
599  pose *= Eigen::Translation3f(offset);
601  f.leg, pose,
602  size, resolution));
603  }
604 
605  Eigen::Affine3f FootstepState::midcoords(const FootstepState& other)
606  {
607  Eigen::Affine3f first = pose_;
608  Eigen::Affine3f second = other.pose_;
609  Eigen::Translation3f pos((Eigen::Vector3f(first.translation()) + Eigen::Vector3f(second.translation())) / 2.0);
610  Eigen::Quaternionf rot = Eigen::Quaternionf(first.matrix().block<3, 3>(0, 0)).slerp(0.5, Eigen::Quaternionf(second.matrix().block<3, 3>(0, 0)));
611  return pos * rot;
612  //return rot * pos;
613  }
614 
615 }
d
FootstepState(int leg, const Eigen::Affine3f &pose, const Eigen::Vector3f &dimensions, const Eigen::Vector3f &resolution)
pos
ANNGrid::Ptr grid_search
void publish(const boost::shared_ptr< M > &message) const
virtual bool crossCheck(FootstepState::Ptr other, float collision_padding=0)
return true if this and other are collision free.
virtual FootstepSupportState isSupportedByPointCloud(const Eigen::Affine3f &pose, pcl::PointCloud< pcl::PointNormal >::Ptr cloud, pcl::KdTreeFLANN< pcl::PointNormal > &tree, pcl::PointIndices::Ptr inliers, const int foot_x_sampling_num, const int foot_y_sampling_num, const double vertex_threshold)
FootstepParameters parameters
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
virtual bool isSameDirection(const Plane &another)
GLfloat n[6][3]
virtual bool isCrossing(Line2D &other)
Definition: line2d.cpp:45
alpha
coefficients
pcl::PointIndices::Ptr cropPointCloud(pcl::PointCloud< pcl::PointNormal >::Ptr cloud, ANNGrid::Ptr grid_search, double padding_x=0.0, double padding_y=0.0)
pose
pcl::KdTreeFLANN< pcl::PointNormal > tree
pa
q
virtual jsk_footstep_msgs::Footstep::Ptr toROSMsg()
unsigned int index
c
#define M_PI
std::string projectStateToString(unsigned int state)
double sqrt()
pcl::PointCloud< pcl::PointNormal >::Ptr cloud
const Eigen::Vector3f resolution(0.05, 0.05, 0.08)
#define ROS_INFO(...)
double z
rot
boost::shared_ptr< FootstepState > Ptr
virtual void project(const Eigen::Vector3f &p, Eigen::Vector3f &output)
virtual FootstepState::Ptr projectToCloud(pcl::KdTreeFLANN< pcl::PointNormal > &tree, pcl::PointCloud< pcl::PointNormal >::Ptr cloud, ANNGrid::Ptr grid_search, pcl::search::Octree< pcl::PointNormal > &tree_2d, pcl::PointCloud< pcl::PointNormal >::Ptr cloud_2d, const Eigen::Vector3f &z, unsigned int &error_state, FootstepParameters &parameters)
#define DEBUG_PRINT(proc)
virtual Eigen::Affine3f midcoords(const FootstepState &other)
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
r
virtual FootstepSupportState isSupportedByPointCloudWithoutCropping(const Eigen::Affine3f &pose, pcl::PointCloud< pcl::PointNormal >::Ptr cloud, pcl::KdTreeFLANN< pcl::PointNormal > &tree, pcl::PointIndices::Ptr inliers, const int foot_x_sampling_num, const int foot_y_sampling_num, const double vertex_threshold)
void vertices(Eigen::Vector3f &a, Eigen::Vector3f &b, Eigen::Vector3f &c, Eigen::Vector3f &d, double collision_padding=0)
static FootstepState::Ptr fromROSMsg(const jsk_footstep_msgs::Footstep &f, const Eigen::Vector3f &size, const Eigen::Vector3f &resolution)
float cross2d(const Eigen::Vector2f &a, const Eigen::Vector2f &b) const
double x
pcl::PointIndices::Ptr cropPointCloudExact(pcl::PointCloud< pcl::PointNormal >::Ptr cloud, pcl::PointIndices::Ptr near_indices, double padding_x=0.0, double padding_y=0.0)
virtual Eigen::Vector3f getNormal()
char a[26]


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Thu Nov 14 2019 03:53:28