drawer_handles_detector.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  */
00035 
00042 // ROS core
00043 #include <ros/ros.h>
00044 // Messages
00045 #include <visualization_msgs/Marker.h>
00046 #include <sensor_msgs/PointCloud2.h>
00047 // PCL stuff
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 // includes for actionlib interface
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 // Waits for a point cloud with pan-tilt close to (0,0) and deduces the position of ptu_base
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                 // start the actionlib server
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                 //    cluster_.setMaxClusterSize (object_cluster_max_size_);
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 //    seg_line_.setNormalDistanceWeight (0.0);
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                 //cloud_extracted_pub_.advertise (nh_, "cloud_extracted", 1);
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                 // if weird number of handles is requested do not do anything
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                 // get the point cloud in
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                 // Downsample + filter the input dataser
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                 //Downsample
00209                 vgrid_.setInputCloud(cloud_raw_ptr);
00210                 vgrid_.setLeafSize(voxel_size_, voxel_size_, voxel_size_);
00211                 //Filter x
00212                 vgrid_.setFilterFieldName("x");
00213                 vgrid_.setFilterLimits(x_min_limit_, x_max_limit_);
00214                 vgrid_.filter(*cloud_x_ptr);
00215                 //Filter y
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                 //Filter z
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                 //For Debug
00226                 //cloud_pub_.publish(cloud);
00227                 //return;
00228 
00229                 //Estimate Point Normals
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                 //Segment the biggest furniture_face plane
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                 //Extract the biggest cluster correponding to above inliers
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                 //For Debug
00268                 //cloud_pub_.publish(*biggest_face);
00269                 //return;
00270 
00271                 //Project Points into the Perfect plane
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                 //For Debug
00278                 //cloud_pub_.publish(*cloud_projected);
00279                 //return;
00280 
00281                 PointCloudPtr cloud_hull(new PointCloud());
00282                 // Create a Convex Hull representation of the projected inliers
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                 //For Debug
00293                 cloud_pub_.publish(*cloud_hull);
00294                 //return;
00295 
00296                 // Extract the handle clusters using a polygonal prism
00297                 pcl::PointIndices::Ptr handles_indices(new pcl::PointIndices());
00298                 prism_.setHeightLimits(cluster_min_height_, cluster_max_height_);
00299                 //prism_.setInputCloud (cloud_z_ptr);
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                 //Cluster handles
00307                 PointCloudPtr handles(new PointCloud());
00308                 pcl::copyPointCloud(*cloud_raw_ptr, *handles_indices, *handles);
00309                 //For Debug
00310                 //cloud_pub_.publish(*handles);
00311                 //return;
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                 // determine the number of handles that will be returned
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                 // init the result message accordingly
00338                 result_.number_of_handles_detected = handle_clusters_size;
00339                 result_.handles.resize(0);
00340 
00341                 //fit lines, project points into perfect lines
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                         //Project Points into the Perfect plane
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                         //For Debug
00354                         cloud_handle_pub_.publish(*line_projected);
00355                         //TODO: get centroid, get orientation and publish pose
00356                         //stamped, change topic name
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                         // write the pose into the result message
00367                         result_.handles.push_back(handle_pose);
00368                 }
00369 
00370                 // check that preempt has not been requested by the client
00371                 if (as_.isPreemptRequested() || !ros::ok()) {
00372                         // set the action state to preempted
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                 //Calculate the centroid of the line
00384                 pcl::PointXYZ point_min;
00385                 pcl::PointXYZ point_max;
00386                 //pcl::getMinMax3D (*line_projected, point_min, point_max);
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                 //Calculate orientation of the line
00397                 //x = orientation of the normal of the plane
00398                 //y = cross product of x and y
00399                 //z = orientation of the handle line
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         //whether to publish the pose of the largest handle found or all of them
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         // PCL objects
00452         //pcl::PassThrough<Point> vgrid_;                   // Filtering + downsampling object
00453         pcl::VoxelGrid<Point> vgrid_; // Filtering + downsampling object
00454         pcl::NormalEstimation<Point, pcl::Normal> n3d_; //Normal estimation
00455         // The resultant estimated point cloud normals for \a cloud_filtered_
00456         pcl::PointCloud<pcl::Normal>::ConstPtr cloud_normals_;
00457         pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg_; // Planar segmentation object
00458         pcl::SACSegmentation<Point> seg_line_; // Planar segmentation object
00459         pcl::ProjectInliers<Point> proj_; // Inlier projection object
00460         pcl::ExtractIndices<Point> extract_; // Extract (too) big tables
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         // needed for action lib interface
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 /* ]--- */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


handle_detection
Author(s): Nico Blodow
autogenerated on Sun Oct 6 2013 12:01:47