00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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 ¶meters);
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_;
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