find_container_action.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 *  Copyright (c) 2011, Willow Garage, Inc.
00004 *  All rights reserved.
00005 *
00006 *  Redistribution and use in source and binary forms, with or without
00007 *  modification, are permitted provided that the following conditions
00008 *  are met:
00009 *
00010 *   * Redistributions of source code must retain the above copyright
00011 *     notice, this list of conditions and the following disclaimer.
00012 *   * Redistributions in binary form must reproduce the above
00013 *     copyright notice, this list of conditions and the following
00014 *     disclaimer in the documentation and/or other materials provided
00015 *     with the distribution.
00016 *   * Neither the name of the Willow Garage nor the names of its
00017 *     contributors may be used to endorse or promote products derived
00018 *     from this software without specific prior written permission.
00019 *
00020 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 *  POSSIBILITY OF SUCH DAMAGE.
00032 *********************************************************************/
00033 
00034 // Author(s): Kaijen Hsiao
00035 
00036 #include <ros/ros.h>
00037 #include <actionlib/server/simple_action_server.h>
00038 #include <object_manipulation_msgs/FindContainerAction.h>
00039 #include <pcl/common/common.h>
00040 #include <pcl/io/pcd_io.h>
00041 #include <pcl/point_types.h>
00042 #include <pcl/filters/voxel_grid.h>
00043 #include <pcl/ModelCoefficients.h>
00044 #include <pcl/features/normal_3d.h>
00045 #include <pcl/filters/extract_indices.h>
00046 #include <pcl/filters/passthrough.h>
00047 #include <pcl/sample_consensus/method_types.h>
00048 #include <pcl/sample_consensus/model_types.h>
00049 #include <pcl/segmentation/sac_segmentation.h>
00050 #include <pcl/surface/mls.h>
00051 #include <pcl/segmentation/extract_clusters.h>
00052 #include <pcl/filters/statistical_outlier_removal.h>
00053 #include <visualization_msgs/Marker.h>
00054 #include <visualization_msgs/MarkerArray.h>
00055 #include <tf/transform_listener.h>
00056 #include <pcl_ros/transforms.h>
00057 #include <Eigen/Geometry>
00058 #include <string>
00059 #include <vector>
00060 
00061 typedef pcl::PointXYZRGB PointT;
00062 
00063 //* A node for finding vertical and horizontal surfaces in point clouds
00070 class FindContainerNode
00071 {
00072 protected:
00073   ros::NodeHandle nh_;
00074   ros::NodeHandle pnh_;
00075   actionlib::SimpleActionServer<object_manipulation_msgs::FindContainerAction> as_;
00076   std::string action_name_;
00077   tf::TransformListener tfl_;
00078   ros::Publisher pub_cloud_, pub_contents_, pub_container_;
00079   ros::Publisher pub_marker_, pub_clusters_;
00080 
00081   // the direction taken to be 'vertical' (can really be horizontal in the world)
00082   geometry_msgs::Vector3 opening_dir_;
00083 
00084 public:
00085   explicit FindContainerNode(std::string name):
00086     pnh_("~"),
00087     as_(nh_, name, boost::bind(&FindContainerNode::executeCB, this, _1), false),
00088     action_name_(name)
00089   {
00090     as_.start();
00091     pub_cloud_ = 
00092       pnh_.advertise<sensor_msgs::PointCloud2>("cloud", 1, true);
00093     pub_contents_ = 
00094       pnh_.advertise<sensor_msgs::PointCloud2>("contents", 1, true);
00095     pub_container_ = 
00096       pnh_.advertise<sensor_msgs::PointCloud2>("container", 1, true);
00097     pub_marker_ = 
00098       pnh_.advertise<visualization_msgs::Marker>("box", 1, true);
00099     pub_clusters_ = 
00100       pnh_.advertise<visualization_msgs::MarkerArray>("clusters_array", 
00101                                                       100, true);
00102     ROS_INFO("%s: Server ready.", action_name_.c_str());
00103   }
00104 
00105   ~FindContainerNode(void)
00106   {
00107   }
00108 
00113   void splitCloudRegions(const pcl::PointCloud<PointT>::Ptr &cloud_in,
00114                          const pcl::PointCloud<PointT>::Ptr &horizontal_cloud,
00115                          const pcl::PointCloud<PointT>::Ptr &vertical_cloud)
00116   {
00117     pcl::PointCloud<pcl::PointNormal> mls_points;
00118 
00119     // Estimate point normals
00120     pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
00121     pcl::MovingLeastSquares<PointT, pcl::PointNormal> ne;
00122     ne.setSearchMethod(tree);
00123     ne.setComputeNormals(true);
00124     ne.setInputCloud(cloud_in);
00125     ne.setSearchRadius(0.012);
00126     ne.process(mls_points);
00127 
00128     for (size_t i = 0; i < mls_points.points.size(); i++)
00129     {
00130 
00131       PointT point;
00132       point.x = mls_points.points[i].x;
00133       point.y = mls_points.points[i].y;
00134       point.z = mls_points.points[i].z;
00135       //point.rgba = mls_points.points[i].rgba;
00136 
00137       // check if opening_dir is within 45 deg of horizontal
00138       if (fabs(mls_points[i].normal_x*opening_dir_.x + 
00139                mls_points[i].normal_y*opening_dir_.y + 
00140                mls_points[i].normal_z*opening_dir_.z) > 0.7)
00141       {
00142         horizontal_cloud->points.push_back(point);
00143       }
00144       else
00145       {
00146         vertical_cloud->points.push_back(point);
00147       }
00148     }
00149 
00150     horizontal_cloud->width = horizontal_cloud->points.size();
00151     horizontal_cloud->height = 1;
00152     horizontal_cloud->is_dense = false;
00153 
00154     vertical_cloud->width = vertical_cloud->points.size();
00155     vertical_cloud->height = 1;
00156     vertical_cloud->is_dense = false;
00157 
00158   }
00159 
00163   void removeOutliers(const pcl::PointCloud<PointT>::Ptr &cloud_in,
00164                       const pcl::PointCloud<PointT>::Ptr &cloud_out)
00165   {
00166     pcl::StatisticalOutlierRemoval<PointT> sor;
00167     sor.setInputCloud(cloud_in);
00168     sor.setMeanK(50);
00169     sor.setStddevMulThresh(1.0);
00170     sor.filter(*cloud_out);
00171   }
00172 
00176   void findClusters(const pcl::PointCloud<PointT>::Ptr &cloud, 
00177                     std::vector< pcl::PointCloud<PointT>::Ptr > &clusters)
00178   {
00179     if (cloud->points.size() == 0) return;
00180 
00181     // Create a KdTree for the search method of the extraction
00182     pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
00183     tree->setInputCloud(cloud);
00184 
00185     std::vector<pcl::PointIndices> cluster_indices;
00186     pcl::EuclideanClusterExtraction<PointT> ec;
00187     ec.setClusterTolerance(0.01);
00188     ec.setMinClusterSize(100);
00189     ec.setMaxClusterSize(25000);
00190     ec.setSearchMethod(tree);
00191     ec.setInputCloud(cloud);
00192     ec.extract(cluster_indices);
00193 
00194     int j = 0;
00195     for (std::vector<pcl::PointIndices>::const_iterator it = 
00196            cluster_indices.begin(); it != cluster_indices.end(); ++it)
00197     {
00198       pcl::PointCloud<PointT>::Ptr cloud_cluster(new pcl::PointCloud<PointT>);
00199       pcl::copyPointCloud(*cloud, *it, *cloud_cluster);
00200       clusters.push_back(cloud_cluster);
00201       j++;
00202     }
00203   }
00204 
00208   visualization_msgs::Marker 
00209   makeMarkerFromCloud(const pcl::PointCloud<PointT>::Ptr &cloud_ptr, 
00210                       const std::string &ns, int id = 0, float scale = 0.03)
00211   {
00212     pcl::PointCloud<PointT>& cloud = *cloud_ptr;
00213     ROS_DEBUG("Making a marker with %d points.", (int)cloud.points.size());
00214 
00215     visualization_msgs::Marker marker;
00216     marker.action = marker.ADD;
00217     marker.header = cloud.header;
00218     marker.id = id;
00219     marker.ns = ns;
00220     marker.pose.orientation.w = 1;
00221     marker.color.r = 1.0;
00222     marker.color.g = 1.0;
00223     marker.color.b = 1.0;
00224     marker.color.a = 1.0;
00225     marker.frame_locked = false;
00226 
00227     if (cloud.points.size())
00228     {
00229       marker.scale.x = scale;
00230       marker.scale.y = scale;
00231       marker.scale.z = scale;
00232       marker.type = visualization_msgs::Marker::SPHERE_LIST;
00233 
00234       int num_points = cloud.points.size();
00235       marker.points.resize(num_points);
00236       marker.colors.resize(num_points);
00237 
00238       for (int i = 0; i < num_points; i++)
00239       {
00240         marker.points[i].x = cloud.points[i].x;
00241         marker.points[i].y = cloud.points[i].y;
00242         marker.points[i].z = cloud.points[i].z;
00243         marker.colors[i].r = cloud.points[i].r/255.;
00244         marker.colors[i].g = cloud.points[i].g/255.;
00245         marker.colors[i].b = cloud.points[i].b/255.;
00246         marker.colors[i].a = 1.0;
00247       }
00248     }
00249 
00250     return marker;
00251   }
00252 
00256   void findBoundingBox(const pcl::PointCloud<PointT>::Ptr &cloud,
00257                        geometry_msgs::Vector3 &box_dims,
00258                        geometry_msgs::PoseStamped &box_pose)
00259   {
00260     PointT min_pt, max_pt;
00261     pcl::getMinMax3D(*cloud, min_pt, max_pt);
00262 
00263     box_dims.x = max_pt.x - min_pt.x;
00264     box_dims.y = max_pt.y - min_pt.y;
00265     box_dims.z = max_pt.z - min_pt.z;
00266 
00267     box_pose.pose.orientation.w = 1.0;
00268     box_pose.header = cloud->header;
00269     box_pose.pose.position.x = (max_pt.x + min_pt.x)/2.0;
00270     box_pose.pose.position.y = (max_pt.y + min_pt.y)/2.0;
00271     box_pose.pose.position.z = (max_pt.z + min_pt.z)/2.0;
00272   }
00273 
00278   int boxFilter(const pcl::PointCloud<PointT>::Ptr &cloud, 
00279                 const pcl::PointCloud<PointT>::Ptr &cloud_filtered,
00280                 const geometry_msgs::Vector3 &dims,
00281                 const geometry_msgs::Pose &pose)
00282   {
00283     Eigen::Vector4f center(pose.position.x, pose.position.y, 
00284                            pose.position.z, 0);
00285 
00286     // TODO: maybe allow for box pose that is not aligned with its header frame
00287     Eigen::Vector4f min_pt;
00288     min_pt = center - Eigen::Vector4f(dims.x/2, dims.y/2, dims.z/2, 0);
00289     Eigen::Vector4f max_pt;
00290     max_pt = center + Eigen::Vector4f(dims.x/2, dims.y/2, dims.z/2, 0);
00291 
00292     std::vector<int> indices;
00293     pcl::getPointsInBox(*cloud, min_pt, max_pt, indices);
00294 
00295     pcl::copyPointCloud(*cloud, indices, *cloud_filtered);
00296     return cloud_filtered->points.size();
00297   }
00298 
00302   void drawBox(const geometry_msgs::Vector3 &box_dims,
00303                const geometry_msgs::PoseStamped &box_pose, 
00304                const std::string &ns = "box")
00305   {
00306     visualization_msgs::Marker marker;
00307     marker.header = box_pose.header;
00308     marker.ns = ns;
00309     marker.id = 0;
00310     marker.type = visualization_msgs::Marker::CUBE; 
00311     marker.action = (int32_t)visualization_msgs::Marker::ADD;
00312     marker.pose = box_pose.pose;
00313     marker.scale = box_dims;
00314     marker.color.b = 1.0;
00315     marker.color.a = 0.5;
00316     marker.lifetime = ros::Duration(0);
00317     marker.frame_locked = false;
00318     pub_marker_.publish(marker);
00319   }
00320 
00324   void 
00325   executeCB(const object_manipulation_msgs::FindContainerGoalConstPtr &goal)
00326   {
00327     object_manipulation_msgs::FindContainerResult result;
00328 
00329     ROS_INFO("%s: Processing a goal request!", action_name_.c_str());
00330 
00331     opening_dir_ = goal->opening_dir;
00332 
00333     pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
00334     pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>());
00335     pcl::PointCloud<PointT>::Ptr cloud_ds(new pcl::PointCloud<PointT>());
00336     pcl::PointCloud<PointT>::Ptr contents(new pcl::PointCloud<PointT>());
00337     pcl::PointCloud<PointT>::Ptr container(new pcl::PointCloud<PointT>());
00338 
00339     if (goal->cloud.data.size() == 0)
00340     {
00341       ROS_ERROR("%s: No points in input cloud!", action_name_.c_str());
00342       as_.setAborted();
00343       return;
00344     }
00345 
00346     // Remove NaNs from the cloud
00347     pcl::fromROSMsg(goal->cloud, *cloud);
00348     std::vector<int> non_NaN_indices;
00349     pcl::removeNaNFromPointCloud(*cloud, *cloud, non_NaN_indices);
00350     cloud->header.stamp = ros::Time(0);
00351 
00352     // Transform the cloud into box_pose frame
00353     tf::StampedTransform T;
00354     Eigen::Matrix4f M;
00355     tfl_.waitForTransform(goal->box_pose.header.frame_id, 
00356                           cloud->header.frame_id, ros::Time(0), 
00357                           ros::Duration(2.0));
00358     tfl_.lookupTransform(goal->box_pose.header.frame_id, 
00359                          cloud->header.frame_id, ros::Time(0), T);
00360     pcl_ros::transformAsMatrix(T, M);
00361     pcl::transformPointCloud(*cloud, *cloud, M);
00362     cloud->header.frame_id = goal->box_pose.header.frame_id;
00363 
00364     // Draw the region of interest box in rviz
00365     drawBox(goal->box_dims, goal->box_pose, "input_bounds");
00366 
00367     // Filter out points not in the region of interest
00368     if (!boxFilter(cloud, cloud_filtered, goal->box_dims, goal->box_pose.pose))
00369     {
00370       ROS_ERROR("%s: No cloud points found in input bounding box.", 
00371                 action_name_.c_str());
00372       as_.setAborted();
00373       return;
00374     }
00375 
00376     contents->header = goal->box_pose.header;
00377     container->header = goal->box_pose.header;
00378     cloud_ds->header = goal->box_pose.header;
00379     cloud_filtered->header = goal->box_pose.header;
00380 
00381     // Down-sample the cloud on a grid
00382     pcl::VoxelGrid<PointT> vox;
00383     vox.setInputCloud(cloud_filtered);
00384     vox.setLeafSize(0.002, 0.002, 0.002);
00385     vox.setFilterFieldName("z");
00386     vox.setFilterLimits(-100, 100);
00387     vox.filter(*cloud_ds);
00388 
00389     // Remove outlier points (speckles)
00390     removeOutliers(cloud_ds, cloud_ds);
00391     if (cloud_ds->points.size() == 0)
00392     {
00393       ROS_ERROR("%s: No cloud points remain after outlier removal.", 
00394                 action_name_.c_str());
00395       as_.setSucceeded(result);
00396     }
00397 
00398     // Estimate normals and separate horizontal and vertical surfaces
00399     splitCloudRegions(cloud_ds, contents, container);
00400 
00401     // Find the bounding box for all the points
00402     findBoundingBox(cloud_ds, result.box_dims, result.box_pose);
00403 
00404     // Split the contents into clusters 
00405     std::vector< pcl::PointCloud<PointT>::Ptr > clusters;
00406     findClusters(contents, clusters);
00407     result.clusters.resize(clusters.size());
00408 
00409     // Publish the clouds and box
00410     visualization_msgs::MarkerArray cluster_markers;
00411     for (size_t i = 0; i < clusters.size(); i++)
00412     {
00413       pcl::toROSMsg(*(clusters[i]), result.clusters[i]);
00414       cluster_markers.markers.push_back(makeMarkerFromCloud(clusters[i], 
00415                                                             "clusters", 
00416                                                             i, 0.003));
00417     }
00418     pub_clusters_.publish(cluster_markers);
00419     drawBox(result.box_dims, result.box_pose, "container_box");
00420     pcl::toROSMsg(*contents, result.contents);
00421     pcl::toROSMsg(*container, result.container);
00422     pub_contents_.publish(result.contents);
00423     pub_container_.publish(result.container);
00424 
00425     // Return the result
00426     ROS_INFO("%s: Succeeded", action_name_.c_str());
00427     // set the action state to succeeded
00428     as_.setSucceeded(result);
00429   }
00430 
00431 };
00432 
00433 
00434 int main(int argc, char** argv)
00435 {
00436   ros::init(argc, argv, "find_container_action");
00437 
00438   FindContainerNode find_it(ros::this_node::getName());
00439   ros::spin();
00440 
00441   return 0;
00442 }


segmented_clutter_grasp_planner
Author(s): Kaijen Hsiao
autogenerated on Mon Oct 6 2014 12:51:50