footstep_state.h
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 
36 
37 #ifndef JSK_FOOTSTEP_PLANNER_FOOTSTEP_STATE_H_
38 #define JSK_FOOTSTEP_PLANNER_FOOTSTEP_STATE_H_
39 
40 #include <jsk_footstep_msgs/Footstep.h>
41 
42 #include <pcl/kdtree/kdtree_flann.h>
43 #include <pcl/filters/crop_box.h>
44 #include <pcl/sample_consensus/method_types.h>
45 #include <pcl/sample_consensus/model_types.h>
46 #include <pcl/segmentation/sac_segmentation.h>
47 #include <pcl/segmentation/sac_segmentation.h>
48 #include <pcl/filters/project_inliers.h>
49 #include <pcl/search/octree.h>
50 
54 
55 namespace jsk_footstep_planner
56 {
57 
58  namespace projection_state
59  {
60  const unsigned int success = 1;
61  const unsigned int no_pointcloud = 2;
62  const unsigned int no_enough_support = 4;
63  const unsigned int no_plane = 8;
64  const unsigned int no_enough_inliers = 16;
65  const unsigned int close_to_success = 32;
66  const unsigned int transition_limit = 64;
67  const unsigned int vertical_footstep = 128;
68  const unsigned int no_enough_inliers_ratio = 256;
69  }
70 
71  std::string projectStateToString(unsigned int state);
72 
74  {
78  };
79 
81  {
82  public:
84  FootstepState(int leg,
85  const Eigen::Affine3f& pose,
86  const Eigen::Vector3f& dimensions,
87  const Eigen::Vector3f& resolution):
88  leg_(leg), pose_(pose), dimensions_(dimensions), resolution_(resolution)
89  {
90  debug_print_ = false;
91  float x = pose_.translation()[0];
92  float y = pose_.translation()[1];
93  float roll, pitch, yaw;
94  pcl::getEulerAngles(pose_, roll, pitch, yaw);
95  index_x_ = x / resolution_[0];
96  index_y_ = y / resolution_[1];
97  index_yaw_ = yaw / resolution_[2];
98  }
99 
100  FootstepState(int leg,
101  const Eigen::Affine3f& pose,
102  const Eigen::Vector3f& dimensions,
103  const Eigen::Vector3f& resolution,
104  int index_x,
105  int index_y,
106  int index_yaw):
107  leg_(leg), pose_(pose), dimensions_(dimensions), resolution_(resolution),
108  index_x_(index_x), index_y_(index_y), index_yaw_(index_yaw)
109  {
110  debug_print_ = false;
111  }
112 
113  static
114  FootstepState::Ptr fromROSMsg(const jsk_footstep_msgs::Footstep& f,
115  const Eigen::Vector3f& size,
116  const Eigen::Vector3f& resolution);
117 
118  inline float cross2d(const Eigen::Vector2f& a, const Eigen::Vector2f& b) const
119  {
120  return a[0] * b[1] - a[1] * b[0];
121  }
122  virtual jsk_footstep_msgs::Footstep::Ptr toROSMsg();
123  virtual jsk_footstep_msgs::Footstep::Ptr toROSMsg(const Eigen::Vector3f& ioffset);
124 #if 0
125  virtual FootstepState::Ptr
126  projectToCloud(pcl::KdTreeFLANN<pcl::PointNormal>& tree,
127  pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
129  pcl::search::Octree<pcl::PointNormal>& tree_2d,
130  pcl::PointCloud<pcl::PointNormal>::Ptr cloud_2d,
131  const Eigen::Vector3f& z,
132  unsigned int& error_state,
133  double outlier_threshold,
134  int max_iterations,
135  int min_inliers,
136  int foot_x_sampling_num = 3,
137  int foot_y_sampling_num = 3,
138  double vertex_threshold = 0.02,
139  const bool skip_cropping = true,
140  const bool use_normal = false,
141  double normal_distance_weight = 0.2,
142  double normal_opening_angle = 0.2,
143  double min_ratio_of_inliers = 0.8);
144 #endif
145  virtual FootstepState::Ptr
146  projectToCloud(pcl::KdTreeFLANN<pcl::PointNormal>& tree,
147  pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
148  ANNGrid::Ptr grid_search,
149  pcl::search::Octree<pcl::PointNormal>& tree_2d,
150  pcl::PointCloud<pcl::PointNormal>::Ptr cloud_2d,
151  const Eigen::Vector3f& z,
152  unsigned int& error_state,
154 
155 #if 0
156  pcl::PointIndices::Ptr
157  cropPointCloud(pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
158  pcl::search::Octree<pcl::PointNormal>& tree);
159 #endif
160  pcl::PointIndices::Ptr
161  cropPointCloud(pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
162  ANNGrid::Ptr grid_search,
163  double padding_x = 0.0, double padding_y = 0.0);
164 
165  pcl::PointIndices::Ptr
166  cropPointCloudExact(pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
167  pcl::PointIndices::Ptr near_indices,
168  double padding_x = 0.0, double padding_y = 0.0);
169 
170  template <class PointT>
172  {
173  PointT p;
174  p.getVector3fMap() = Eigen::Vector3f(pose_.translation());
175  return p;
176  }
177 
178  inline void vertices(Eigen::Vector3f& a,
179  Eigen::Vector3f& b,
180  Eigen::Vector3f& c,
181  Eigen::Vector3f& d,
182  double collision_padding = 0)
183  {
184  const Eigen::Vector3f ux = Eigen::Vector3f::UnitX();
185  const Eigen::Vector3f uy = Eigen::Vector3f::UnitY();
186  double dim0 = dimensions_[0] + collision_padding;
187  double dim1 = dimensions_[1] + collision_padding;
188  a = Eigen::Vector3f((pose_ * Eigen::Translation3f( ux * dim0 / 2 + uy * dim1 / 2)).translation());
189  b = Eigen::Vector3f((pose_ * Eigen::Translation3f(- ux * dim0 / 2 + uy * dim1 / 2)).translation());
190  c = Eigen::Vector3f((pose_ * Eigen::Translation3f(- ux * dim0 / 2 - uy * dim1 / 2)).translation());
191  d = Eigen::Vector3f((pose_ * Eigen::Translation3f( ux * dim0 / 2 - uy * dim1 / 2)).translation());
192  }
193 
198  virtual bool crossCheck(FootstepState::Ptr other, float collision_padding = 0);
199 
200  virtual Eigen::Affine3f getPose() const { return pose_; }
201  virtual void setPose(const Eigen::Affine3f& pose)
202  {
203  pose_ = pose;
204  }
205 
206  virtual int getLeg() const { return leg_; }
207  virtual Eigen::Vector3f getDimensions() const { return dimensions_; }
209  {
210  return ((index_x_ == other.index_x_) &&
211  (index_y_ == other.index_y_) &&
212  (index_yaw_ == other.index_yaw_));
213  }
214 
215  virtual Eigen::Vector3f getResolution() const { return resolution_; }
216 
217 
218  inline virtual int indexX() { return index_x_; }
219  inline virtual int indexY() { return index_y_; }
220  inline virtual int indexT() { return index_yaw_; }
221 
222 
223  virtual FootstepSupportState
224  isSupportedByPointCloud(const Eigen::Affine3f& pose,
225  pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
226  pcl::KdTreeFLANN<pcl::PointNormal>& tree,
227  pcl::PointIndices::Ptr inliers,
228  const int foot_x_sampling_num,
229  const int foot_y_sampling_num,
230  const double vertex_threshold);
231  virtual FootstepSupportState
232  isSupportedByPointCloudWithoutCropping(const Eigen::Affine3f& pose,
233  pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
234  pcl::KdTreeFLANN<pcl::PointNormal>& tree,
235  pcl::PointIndices::Ptr inliers,
236  const int foot_x_sampling_num,
237  const int foot_y_sampling_num,
238  const double vertex_threshold);
239 
240  virtual Eigen::Affine3f midcoords(const FootstepState& other);
241  protected:
242  Eigen::Affine3f pose_;
243  const Eigen::Vector3f dimensions_;
244  const Eigen::Vector3f resolution_; // not memory efficient?
245  const int leg_;
246  int index_x_;
247  int index_y_;
250  private:
251 
252  };
253 
254  inline size_t hash_value(const FootstepState::Ptr& s)
255  {
256  return std::abs(s->indexX()) << (25 + 14) + std::abs(s->indexY()) << 14
257  + std::abs(s->indexT());
258  }
259 
260 }
261 
262 #endif
FootstepState(int leg, const Eigen::Affine3f &pose, const Eigen::Vector3f &dimensions, const Eigen::Vector3f &resolution)
virtual Eigen::Vector3f getResolution() const
ANNGrid::Ptr grid_search
FootstepParameters parameters
pitch
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
pose
pcl::KdTreeFLANN< pcl::PointNormal > tree
FootstepState(int leg, const Eigen::Affine3f &pose, const Eigen::Vector3f &dimensions, const Eigen::Vector3f &resolution, int index_x, int index_y, int index_yaw)
double y
yaw
size
virtual void setPose(const Eigen::Affine3f &pose)
virtual Eigen::Vector3f getDimensions() const
std::string projectStateToString(unsigned int state)
pcl::PointCloud< pcl::PointNormal >::Ptr cloud
const Eigen::Vector3f resolution(0.05, 0.05, 0.08)
double z
bool operator==(FootstepState &other)
boost::shared_ptr< FootstepState > Ptr
virtual Eigen::Affine3f getPose() const
pcl::PointXYZRGB PointT
const geometry_msgs::Pose * pose_
void cropPointCloud(const typename pcl::PointCloud< PointT >::Ptr &cloud, const jsk_recognition_msgs::BoundingBox &bbox_msg, std::vector< int > *indices, bool extract_removed_indices=false)
f
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
float cross2d(const Eigen::Vector2f &a, const Eigen::Vector2f &b) const
void vertices(Eigen::Vector3f &a, Eigen::Vector3f &b, Eigen::Vector3f &c, Eigen::Vector3f &d, double collision_padding=0)
roll
double x
size_t hash_value(const FootstepState::Ptr &s)


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