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
00042
00043 #include <ros/ros.h>
00044
00045 #include <visualization_msgs/Marker.h>
00046 #include <sensor_msgs/PointCloud2.h>
00047
00048 #include <pcl/point_types.h>
00049 #include <pcl/point_cloud.h>
00050 #include <pcl/ros/conversions.h>
00051 #include "pcl/sample_consensus/method_types.h"
00052 #include "pcl/sample_consensus/model_types.h"
00053 #include <pcl/segmentation/sac_segmentation.h>
00054 #include <pcl/filters/voxel_grid.h>
00055 #include <pcl/filters/passthrough.h>
00056 #include <pcl/filters/project_inliers.h>
00057 #include <pcl/surface/convex_hull.h>
00058 #include "pcl/filters/extract_indices.h"
00059 #include "pcl/segmentation/extract_polygonal_prism_data.h"
00060 #include "pcl/common/common.h"
00061 #include <pcl/io/pcd_io.h>
00062 #include "pcl/segmentation/extract_clusters.h"
00063 #include <pcl/features/normal_3d.h>
00064 #include <pcl/common/angles.h>
00065 #include "pcl/common/common.h"
00066 #include <pcl_ros/publisher.h>
00067
00068 #include <tf/transform_broadcaster.h>
00069 #include <tf/transform_listener.h>
00070 #include <tf/message_filter.h>
00071
00072
00073 #include <actionlib/server/simple_action_server.h>
00074 #include <handle_detection/HandleDetectionAction.h>
00075
00076 typedef pcl::PointXYZ Point;
00077 typedef pcl::PointCloud<Point> PointCloud;
00078 typedef PointCloud::Ptr PointCloudPtr;
00079 typedef PointCloud::Ptr PointCloudPtr;
00080 typedef PointCloud::ConstPtr PointCloudConstPtr;
00081 typedef pcl::KdTree<Point>::Ptr KdTreePtr;
00082
00083 const tf::Vector3 wp_normal(1, 0, 0);
00084 const double wp_offset = -1.45;
00085
00086
00088 class DrawerHandlesDetector {
00089 public:
00091 DrawerHandlesDetector(const ros::NodeHandle &nh) :
00092 nh_(nh), as_(nh, "detect",
00093 boost::bind(&DrawerHandlesDetector::executeCB, this, _1),
00094 false) {
00095
00096 as_.start();
00097
00098 nh_.param("z_min_limit", z_min_limit_, 0.1);
00099 nh_.param("z_max_limit", z_max_limit_, 3.0);
00100 nh_.param("y_min_limit", y_min_limit_, -0.5);
00101 nh_.param("y_max_limit", y_max_limit_, 0.5);
00102 nh_.param("x_min_limit", x_min_limit_, 0.0);
00103 nh_.param("x_max_limit", x_max_limit_, 1.0);
00104 nh_.param("publish_largest_handle_pose", publish_largest_handle_pose_,
00105 false);
00106
00107 nh_.param("k", k_, 30);
00108 normals_tree_ = boost::make_shared<pcl::KdTreeFLANN<Point> >();
00109 n3d_.setKSearch(k_);
00110 n3d_.setSearchMethod(normals_tree_);
00111
00112 nh_.param("sac_distance", sac_distance_, 0.02);
00113 nh_.param("normal_distance_weight", normal_distance_weight_, 0.05);
00114 nh_.param("max_iter", max_iter_, 500);
00115 nh_.param("eps_angle", eps_angle_, 20.0);
00116 nh_.param("seg_prob", seg_prob_, 0.99);
00117 seg_.setModelType(pcl::SACMODEL_NORMAL_PARALLEL_PLANE);
00118 seg_.setMethodType(pcl::SAC_RANSAC);
00119 seg_.setDistanceThreshold(sac_distance_);
00120 seg_.setNormalDistanceWeight(normal_distance_weight_);
00121 seg_.setOptimizeCoefficients(true);
00122 btVector3 axis(0.0, 0.0, 1.0);
00123 seg_.setAxis(
00124 Eigen::Vector3f(fabs(axis.getX()), fabs(axis.getY()),
00125 fabs(axis.getZ())));
00126 seg_.setEpsAngle(pcl::deg2rad(eps_angle_));
00127 seg_.setMaxIterations(max_iter_);
00128 seg_.setProbability(seg_prob_);
00129
00130 nh_.param("object_cluster_tolerance", object_cluster_tolerance_, 0.03);
00131 nh_.param("object_cluster_min_size", object_cluster_min_size_, 200);
00132 cluster_.setClusterTolerance(object_cluster_tolerance_);
00133 cluster_.setSpatialLocator(0);
00134 cluster_.setMinClusterSize(object_cluster_min_size_);
00135
00136 clusters_tree_ = boost::make_shared<pcl::KdTreeFLANN<Point> >();
00137 clusters_tree_->setEpsilon(1);
00138 cluster_.setSearchMethod(clusters_tree_);
00139
00140 nh_.param("nr_cluster", nr_cluster_, 1);
00141
00142 nh_.param("cluster_min_height", cluster_min_height_, 0.03);
00143 nh_.param("cluster_max_height", cluster_max_height_, 0.1);
00144
00145 nh_.param("handle_cluster_tolerance", handle_cluster_tolerance_, 0.02);
00146 nh_.param("handle_cluster_min_size", handle_cluster_min_size_, 40);
00147 nh_.param("handle_cluster_max_size", handle_cluster_max_size_, 500);
00148 handle_cluster_.setClusterTolerance(handle_cluster_tolerance_);
00149 handle_cluster_.setSpatialLocator(0);
00150 handle_cluster_.setMinClusterSize(handle_cluster_min_size_);
00151 handle_cluster_.setMaxClusterSize(handle_cluster_max_size_);
00152 cluster_.setSearchMethod(clusters_tree_);
00153
00154 seg_line_.setModelType(pcl::SACMODEL_LINE);
00155 seg_line_.setMethodType(pcl::SAC_RANSAC);
00156 seg_line_.setDistanceThreshold(0.05);
00157
00158 seg_line_.setOptimizeCoefficients(true);
00159 seg_line_.setMaxIterations(max_iter_);
00160 seg_line_.setProbability(seg_prob_);
00161
00162 nh_.param("min_table_inliers", min_table_inliers_, 100);
00163 nh_.param("voxel_size", voxel_size_, 0.01);
00164 nh_.param("point_cloud_topic", point_cloud_topic_,
00165 std::string("/shoulder_cloud2"));
00166 nh_.param("output_handle_topic", output_handle_topic_,
00167 std::string("handle_projected_inliers/output"));
00168 nh_.param("handle_pose_topic", handle_pose_topic_,
00169 std::string("handle_pose"));
00170 cloud_pub_.advertise(nh_, "debug_cloud", 1);
00171
00172 cloud_handle_pub_.advertise(nh_, output_handle_topic_, 10);
00173 handle_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>(
00174 handle_pose_topic_, 1);
00175 }
00176
00178 virtual ~DrawerHandlesDetector() {
00179 }
00180
00181 private:
00183 void executeCB(const handle_detection::HandleDetectionGoalConstPtr &goal) {
00184
00185 if (goal->number_of_handles < 1) {
00186 ROS_ERROR(
00187 "[DrawerHandlesDetector] Negativ or zero number given as desired number of handles.");
00188 as_.setAborted();
00189 return;
00190 }
00191
00192
00193 sensor_msgs::PointCloud2ConstPtr cloud_in;
00194 cloud_in = ros::topic::waitForMessage<sensor_msgs::PointCloud2>(
00195 point_cloud_topic_, nh_);
00196
00197 ROS_INFO_STREAM(
00198 "[" << getName ().c_str () << "] Received cloud: cloud time " << cloud_in->header.stamp);
00199 PointCloud cloud_raw, cloud;
00200
00201 pcl::fromROSMsg(*cloud_in, cloud_raw);
00202
00203 PointCloudPtr cloud_raw_ptr(new PointCloud(cloud_raw));
00204 PointCloudPtr cloud_x_ptr(new PointCloud());
00205 PointCloudPtr cloud_y_ptr(new PointCloud());
00206 PointCloudPtr cloud_z_ptr(new PointCloud());
00207
00208
00209 vgrid_.setInputCloud(cloud_raw_ptr);
00210 vgrid_.setLeafSize(voxel_size_, voxel_size_, voxel_size_);
00211
00212 vgrid_.setFilterFieldName("x");
00213 vgrid_.setFilterLimits(x_min_limit_, x_max_limit_);
00214 vgrid_.filter(*cloud_x_ptr);
00215
00216 vgrid_.setInputCloud(cloud_x_ptr);
00217 vgrid_.setFilterFieldName("y");
00218 vgrid_.setFilterLimits(y_min_limit_, y_max_limit_);
00219 vgrid_.filter(*cloud_y_ptr);
00220
00221 vgrid_.setInputCloud(cloud_y_ptr);
00222 vgrid_.setFilterFieldName("z");
00223 vgrid_.setFilterLimits(z_min_limit_, z_max_limit_);
00224 vgrid_.filter(*cloud_z_ptr);
00225
00226
00227
00228
00229
00230 pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(
00231 new pcl::PointCloud<pcl::Normal>());
00232 n3d_.setInputCloud(cloud_z_ptr);
00233 n3d_.compute(*cloud_normals);
00234
00235
00236 pcl::ModelCoefficients::Ptr table_coeff(new pcl::ModelCoefficients());
00237 pcl::PointIndices::Ptr table_inliers(new pcl::PointIndices());
00238 seg_.setInputCloud(cloud_z_ptr);
00239 seg_.setInputNormals(cloud_normals);
00240 seg_.segment(*table_inliers, *table_coeff);
00241 ROS_INFO(
00242 "[%s] Table model: [%f, %f, %f, %f] with %d inliers.", getName ().c_str (), table_coeff->values[0], table_coeff->values[1], table_coeff->values[2], table_coeff->values[3], (int)table_inliers->indices.size ());
00243
00244 if ((int) table_inliers->indices.size() <= min_table_inliers_) {
00245 ROS_ERROR("table has to few inliers");
00246 return;
00247 }
00248
00249
00250 std::vector<pcl::PointIndices> clusters;
00251 cluster_.setInputCloud(cloud_z_ptr);
00252 cluster_.setIndices(table_inliers);
00253 cluster_.extract(clusters);
00254
00255 PointCloudPtr biggest_face(new PointCloud());
00256 if (int(clusters.size()) >= nr_cluster_) {
00257 for (int i = 0; i < nr_cluster_; i++) {
00258 pcl::copyPointCloud(*cloud_z_ptr, clusters[i], *biggest_face);
00259 }
00260 } else {
00261 ROS_ERROR(
00262 "Only %ld clusters found with size > %d points", clusters.size(), object_cluster_min_size_);
00263 return;
00264 }
00265 ROS_INFO(
00266 "Found biggest face with %ld points", biggest_face->points.size());
00267
00268
00269
00270
00271
00272 PointCloudPtr cloud_projected(new PointCloud());
00273 proj_.setInputCloud(biggest_face);
00274 proj_.setModelCoefficients(table_coeff);
00275 proj_.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
00276 proj_.filter(*cloud_projected);
00277
00278
00279
00280
00281 PointCloudPtr cloud_hull(new PointCloud());
00282
00283 chull_.setInputCloud(cloud_projected);
00284 chull_.reconstruct(*cloud_hull);
00285 ROS_INFO(
00286 "Convex hull has: %d data points.", (int)cloud_hull->points.size ());
00287 if ((int) cloud_hull->points.size() == 0) {
00288 ROS_WARN(
00289 "Convex hull has: %d data points. Returning.", (int)cloud_hull->points.size ());
00290 return;
00291 }
00292
00293 cloud_pub_.publish(*cloud_hull);
00294
00295
00296
00297 pcl::PointIndices::Ptr handles_indices(new pcl::PointIndices());
00298 prism_.setHeightLimits(cluster_min_height_, cluster_max_height_);
00299
00300 prism_.setInputCloud(cloud_raw_ptr);
00301 prism_.setInputPlanarHull(cloud_hull);
00302 prism_.segment(*handles_indices);
00303 ROS_INFO(
00304 "[%s] Number of handle candidates: %d.", getName ().c_str (), (int)handles_indices->indices.size ());
00305
00306
00307 PointCloudPtr handles(new PointCloud());
00308 pcl::copyPointCloud(*cloud_raw_ptr, *handles_indices, *handles);
00309
00310
00311
00312
00313 std::vector<pcl::PointIndices> handle_clusters;
00314 handle_cluster_.setInputCloud(handles);
00315 handle_cluster_.extract(handle_clusters);
00316 ROS_INFO(
00317 "[%s] Found handle clusters: %d.", getName ().c_str (), (int)handle_clusters.size ());
00318 if ((int) handle_clusters.size() == 0) {
00319 result_.number_of_handles_detected = 0;
00320 result_.handles.resize(0);
00321 as_.setSucceeded(result_);
00322 return;
00323 }
00324
00325 PointCloudPtr handle_final(new PointCloud());
00326 pcl::ModelCoefficients::Ptr line_coeff(new pcl::ModelCoefficients());
00327 pcl::PointIndices::Ptr line_inliers(new pcl::PointIndices());
00328
00329
00330 int handle_clusters_size;
00331 if ((int)handle_clusters.size() < goal->number_of_handles) {
00332 handle_clusters_size = handle_clusters.size();
00333 } else {
00334 handle_clusters_size = goal->number_of_handles;
00335 }
00336
00337
00338 result_.number_of_handles_detected = handle_clusters_size;
00339 result_.handles.resize(0);
00340
00341
00342 for (int i = 0; i < handle_clusters_size; i++) {
00343 pcl::copyPointCloud(*handles, handle_clusters[i], *handle_final);
00344 seg_line_.setInputCloud(handle_final);
00345 seg_line_.segment(*line_inliers, *line_coeff);
00346 ROS_INFO("line_inliers %ld", line_inliers->indices.size());
00347
00348 PointCloudPtr line_projected(new PointCloud());
00349 proj_.setInputCloud(handle_final);
00350 proj_.setModelCoefficients(line_coeff);
00351 proj_.setModelType(pcl::SACMODEL_LINE);
00352 proj_.filter(*line_projected);
00353
00354 cloud_handle_pub_.publish(*line_projected);
00355
00356
00357 pcl::PointXYZ point_center;
00358 geometry_msgs::PoseStamped handle_pose;
00359 std::string frame(cloud_in->header.frame_id);
00360 getHandlePose(line_projected, table_coeff, frame, handle_pose);
00361 handle_pose_pub_.publish(handle_pose);
00362 ROS_INFO(
00363 "Handle pose published: x %f, y %f, z %f, ox %f, oy \
00364 %f, oz %f, ow %f", handle_pose.pose.position.x, handle_pose.pose.position.y, handle_pose.pose.position.z, handle_pose.pose.orientation.x, handle_pose.pose.orientation.y, handle_pose.pose.orientation.z, handle_pose.pose.orientation.w);
00365
00366
00367 result_.handles.push_back(handle_pose);
00368 }
00369
00370
00371 if (as_.isPreemptRequested() || !ros::ok()) {
00372
00373 as_.setPreempted();
00374 return;
00375 } else {
00376 as_.setSucceeded(result_);
00377 }
00378 }
00379
00380 void getHandlePose(pcl::PointCloud<Point>::Ptr line_projected,
00381 pcl::ModelCoefficients::Ptr table_coeff, std::string & frame,
00382 geometry_msgs::PoseStamped & pose) {
00383
00384 pcl::PointXYZ point_min;
00385 pcl::PointXYZ point_max;
00386
00387 point_min = line_projected->points[line_projected->points.size() - 1];
00388 point_max = line_projected->points[0];
00389 pose.pose.position.x = (point_max.x + point_min.x) / 2;
00390 pose.pose.position.y = (point_max.y + point_min.y) / 2;
00391 pose.pose.position.z = (point_max.z + point_min.z) / 2;
00392
00393 ROS_INFO("min %f %f %f", point_min.x, point_min.y, point_min.z);
00394 ROS_INFO("max %f %f %f", point_max.x, point_max.y, point_max.z);
00395
00396
00397
00398
00399
00400 btVector3 x_axis(table_coeff->values[0], table_coeff->values[1],
00401 table_coeff->values[2]);
00402 btVector3 z_axis(point_max.x - point_min.x, point_max.y - point_min.y,
00403 point_max.z - point_min.z);
00404 x_axis.normalize();
00405 z_axis.normalize();
00406 btVector3 y_axis = (z_axis.cross(x_axis)).normalize();
00407 x_axis = (y_axis.cross(z_axis)).normalize();
00408 z_axis = (x_axis.cross(y_axis)).normalize();
00409 y_axis = (z_axis.cross(x_axis)).normalize();
00410
00411 ROS_DEBUG(
00412 "x_AXIS %f %f %f len %f", x_axis.x(), x_axis.y(), x_axis.z(), x_axis.length());
00413 ROS_DEBUG(
00414 "y_AXIS %f %f %f len %f", y_axis.x(), y_axis.y(), y_axis.z(), y_axis.length());
00415 ROS_DEBUG(
00416 "z_AXIS %f %f %f len %f", z_axis.x(), z_axis.y(), z_axis.z(), z_axis.length());
00417
00418 btMatrix3x3 rot(x_axis.x(), y_axis.x(), z_axis.x(), x_axis.y(),
00419 y_axis.y(), z_axis.y(), x_axis.z(), y_axis.z(), z_axis.z());
00420 btQuaternion rot_quat;
00421 rot.getRotation(rot_quat);
00422 pose.pose.orientation.x = rot_quat.getX();
00423 pose.pose.orientation.y = rot_quat.getY();
00424 pose.pose.orientation.z = rot_quat.getZ();
00425 pose.pose.orientation.w = rot_quat.getW();
00426 pose.header.frame_id = frame;
00427 pose.header.stamp = ros::Time::now();
00428 pose.header.seq = 0;
00429 }
00430
00431 ros::NodeHandle nh_;
00432 double voxel_size_;
00433
00434 std::string point_cloud_topic_, output_handle_topic_, handle_pose_topic_;
00435 double object_cluster_tolerance_, handle_cluster_tolerance_,
00436 cluster_min_height_, cluster_max_height_;int
00437 object_cluster_min_size_, object_cluster_max_size_,
00438 handle_cluster_min_size_, handle_cluster_max_size_;
00439
00440 double sac_distance_, normal_distance_weight_, z_min_limit_, z_max_limit_;
00441 double y_min_limit_, y_max_limit_, x_min_limit_, x_max_limit_;
00442 double eps_angle_, seg_prob_;int k_, max_iter_, min_table_inliers_,
00443 nr_cluster_;
00444
00445 bool publish_largest_handle_pose_;
00446
00447 pcl_ros::Publisher<Point> cloud_pub_;
00448 pcl_ros::Publisher<Point> cloud_handle_pub_;
00449 ros::Publisher handle_pose_pub_;
00450
00451
00452
00453 pcl::VoxelGrid<Point> vgrid_;
00454 pcl::NormalEstimation<Point, pcl::Normal> n3d_;
00455
00456 pcl::PointCloud<pcl::Normal>::ConstPtr cloud_normals_;
00457 pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg_;
00458 pcl::SACSegmentation<Point> seg_line_;
00459 pcl::ProjectInliers<Point> proj_;
00460 pcl::ExtractIndices<Point> extract_;
00461 pcl::ConvexHull<Point> chull_;
00462 pcl::ExtractPolygonalPrismData<Point> prism_;
00463 pcl::PointCloud<Point> cloud_objects_;
00464 pcl::EuclideanClusterExtraction<Point> cluster_, handle_cluster_;
00465 KdTreePtr clusters_tree_, normals_tree_;
00466
00468
00469 actionlib::SimpleActionServer<handle_detection::HandleDetectionAction> as_;
00470 handle_detection::HandleDetectionResult result_;
00471
00473
00474 std::string getName() const {
00475 return ("DrawerHandlesDetector");
00476 }
00477 };
00478
00479 int main(int argc, char** argv) {
00480 ros::init(argc, argv, "drawer_handles_detector");
00481 ros::NodeHandle nh("~");
00482 DrawerHandlesDetector dhd(nh);
00483 ros::spin();
00484 }
00485