footstep_state.h
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 
00037 #ifndef JSK_FOOTSTEP_PLANNER_FOOTSTEP_STATE_H_
00038 #define JSK_FOOTSTEP_PLANNER_FOOTSTEP_STATE_H_
00039 
00040 #include <jsk_footstep_msgs/Footstep.h>
00041 
00042 #include <pcl/kdtree/kdtree_flann.h>
00043 #include <pcl/filters/crop_box.h>
00044 #include <pcl/sample_consensus/method_types.h>
00045 #include <pcl/sample_consensus/model_types.h>
00046 #include <pcl/segmentation/sac_segmentation.h>
00047 #include <pcl/segmentation/sac_segmentation.h>
00048 #include <pcl/filters/project_inliers.h>
00049 #include <pcl/search/octree.h>
00050 
00051 #include "jsk_footstep_planner/ann_grid.h"
00052 #include "jsk_footstep_planner/util.h"
00053 #include "jsk_footstep_planner/footstep_parameters.h"
00054 
00055 namespace jsk_footstep_planner
00056 {
00057 
00058   namespace projection_state
00059   {
00060     const unsigned int success = 1;
00061     const unsigned int no_pointcloud = 2;
00062     const unsigned int no_enough_support = 4;
00063     const unsigned int no_plane = 8;
00064     const unsigned int no_enough_inliers = 16;
00065     const unsigned int close_to_success = 32;
00066     const unsigned int transition_limit = 64;
00067     const unsigned int vertical_footstep = 128;
00068     const unsigned int no_enough_inliers_ratio = 256;
00069   }
00070 
00071   std::string projectStateToString(unsigned int state);
00072   
00073   enum FootstepSupportState
00074   {
00075     NOT_SUPPORTED,
00076     SUPPORTED,
00077     CLOSE_TO_SUPPORTED,
00078   };
00079   
00080   class FootstepState
00081   {
00082   public:
00083     typedef boost::shared_ptr<FootstepState> Ptr;
00084     FootstepState(int leg,
00085                   const Eigen::Affine3f& pose,
00086                   const Eigen::Vector3f& dimensions,
00087                   const Eigen::Vector3f& resolution):
00088       leg_(leg), pose_(pose), dimensions_(dimensions), resolution_(resolution)
00089     {
00090       debug_print_ = false;
00091       float x = pose_.translation()[0];
00092       float y = pose_.translation()[1];
00093       float roll, pitch, yaw;
00094       pcl::getEulerAngles(pose_, roll, pitch, yaw);
00095       index_x_ = x / resolution_[0];
00096       index_y_ = y / resolution_[1];
00097       index_yaw_ = yaw / resolution_[2];
00098     }
00099 
00100     FootstepState(int leg,
00101                   const Eigen::Affine3f& pose,
00102                   const Eigen::Vector3f& dimensions,
00103                   const Eigen::Vector3f& resolution,
00104                   int index_x,
00105                   int index_y,
00106                   int index_yaw):
00107       leg_(leg), pose_(pose), dimensions_(dimensions), resolution_(resolution),
00108       index_x_(index_x), index_y_(index_y), index_yaw_(index_yaw)
00109     {
00110       debug_print_ = false;
00111     }
00112 
00113     static
00114     FootstepState::Ptr fromROSMsg(const jsk_footstep_msgs::Footstep& f,
00115                                   const Eigen::Vector3f& size,
00116                                   const Eigen::Vector3f& resolution);
00117     
00118     inline float cross2d(const Eigen::Vector2f& a, const Eigen::Vector2f& b) const
00119     {
00120       return a[0] * b[1] - a[1] * b[0];
00121     }
00122     virtual jsk_footstep_msgs::Footstep::Ptr toROSMsg();
00123     virtual jsk_footstep_msgs::Footstep::Ptr toROSMsg(const Eigen::Vector3f& ioffset);
00124 #if 0
00125     virtual FootstepState::Ptr
00126     projectToCloud(pcl::KdTreeFLANN<pcl::PointNormal>& tree,
00127                    pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
00128                    ANNGrid::Ptr grid_search,
00129                    pcl::search::Octree<pcl::PointNormal>& tree_2d,
00130                    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_2d,
00131                    const Eigen::Vector3f& z,
00132                    unsigned int& error_state,
00133                    double outlier_threshold,
00134                    int max_iterations,
00135                    int min_inliers,
00136                    int foot_x_sampling_num = 3,
00137                    int foot_y_sampling_num = 3,
00138                    double vertex_threshold = 0.02,
00139                    const bool skip_cropping = true,
00140                    const bool use_normal = false,
00141                    double normal_distance_weight = 0.2,
00142                    double normal_opening_angle = 0.2,
00143                    double min_ratio_of_inliers = 0.8);
00144 #endif
00145     virtual FootstepState::Ptr
00146     projectToCloud(pcl::KdTreeFLANN<pcl::PointNormal>& tree,
00147                    pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
00148                    ANNGrid::Ptr grid_search,
00149                    pcl::search::Octree<pcl::PointNormal>& tree_2d,
00150                    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_2d,
00151                    const Eigen::Vector3f& z,
00152                    unsigned int& error_state,
00153                    FootstepParameters &parameters);
00154     
00155 #if 0
00156     pcl::PointIndices::Ptr
00157     cropPointCloud(pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
00158                    pcl::search::Octree<pcl::PointNormal>& tree);
00159 #endif
00160     pcl::PointIndices::Ptr
00161     cropPointCloud(pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
00162                    ANNGrid::Ptr grid_search,
00163                    double padding_x = 0.0, double padding_y = 0.0);
00164 
00165     pcl::PointIndices::Ptr
00166     cropPointCloudExact(pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
00167                         pcl::PointIndices::Ptr near_indices,
00168                         double padding_x = 0.0, double padding_y = 0.0);
00169 
00170     template <class PointT>
00171     PointT toPoint()
00172     {
00173       PointT p;
00174       p.getVector3fMap() = Eigen::Vector3f(pose_.translation());
00175       return p;
00176     }
00177 
00178     inline void vertices(Eigen::Vector3f& a,
00179                          Eigen::Vector3f& b,
00180                          Eigen::Vector3f& c,
00181                          Eigen::Vector3f& d,
00182                          double collision_padding = 0)
00183     {
00184       const Eigen::Vector3f ux = Eigen::Vector3f::UnitX();
00185       const Eigen::Vector3f uy = Eigen::Vector3f::UnitY();
00186       double dim0 = dimensions_[0] + collision_padding;
00187       double dim1 = dimensions_[1] + collision_padding;
00188       a = Eigen::Vector3f((pose_ * Eigen::Translation3f(  ux * dim0 / 2 + uy * dim1 / 2)).translation());
00189       b = Eigen::Vector3f((pose_ * Eigen::Translation3f(- ux * dim0 / 2 + uy * dim1 / 2)).translation());
00190       c = Eigen::Vector3f((pose_ * Eigen::Translation3f(- ux * dim0 / 2 - uy * dim1 / 2)).translation());
00191       d = Eigen::Vector3f((pose_ * Eigen::Translation3f(  ux * dim0 / 2 - uy * dim1 / 2)).translation());
00192     }
00193     
00198     virtual bool crossCheck(FootstepState::Ptr other, float collision_padding = 0);
00199     
00200     virtual Eigen::Affine3f getPose() const { return pose_; }
00201     virtual void setPose(const Eigen::Affine3f& pose)
00202     {
00203       pose_ = pose;
00204     }
00205     
00206     virtual int getLeg() const { return leg_; }
00207     virtual Eigen::Vector3f getDimensions() const { return dimensions_; }
00208     bool operator==(FootstepState& other)
00209     {
00210       return ((index_x_ == other.index_x_) &&
00211               (index_y_ == other.index_y_) &&
00212               (index_yaw_ == other.index_yaw_));
00213     }
00214 
00215     virtual Eigen::Vector3f getResolution() const { return resolution_; }
00216       
00217 
00218     inline virtual int indexX() { return index_x_; }
00219     inline virtual int indexY() { return index_y_; }
00220     inline virtual int indexT() { return index_yaw_; }
00221 
00222 
00223     virtual FootstepSupportState
00224     isSupportedByPointCloud(const Eigen::Affine3f& pose,
00225                             pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
00226                             pcl::KdTreeFLANN<pcl::PointNormal>& tree,
00227                             pcl::PointIndices::Ptr inliers,
00228                             const int foot_x_sampling_num,
00229                             const int foot_y_sampling_num,
00230                             const double vertex_threshold);
00231     virtual FootstepSupportState
00232     isSupportedByPointCloudWithoutCropping(const Eigen::Affine3f& pose,
00233                                            pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
00234                                            pcl::KdTreeFLANN<pcl::PointNormal>& tree,
00235                                            pcl::PointIndices::Ptr inliers,
00236                                            const int foot_x_sampling_num,
00237                                            const int foot_y_sampling_num,
00238                                            const double vertex_threshold);
00239     
00240     virtual Eigen::Affine3f midcoords(const FootstepState& other);
00241   protected:
00242     Eigen::Affine3f pose_;
00243     const Eigen::Vector3f dimensions_;
00244     const Eigen::Vector3f resolution_; // not memory efficient?
00245     const int leg_;
00246     int index_x_;
00247     int index_y_;
00248     int index_yaw_;
00249     bool debug_print_;
00250   private:
00251     
00252   };
00253 
00254   inline size_t hash_value(const FootstepState::Ptr& s)
00255   {
00256     return std::abs(s->indexX()) << (25 + 14) + std::abs(s->indexY()) << 14
00257       + std::abs(s->indexT());
00258   }
00259   
00260 }
00261 
00262 #endif


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Fri Apr 19 2019 03:45:28