block_detection_action_server.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  * 
00029  * Author: Michael Ferguson, Helen Oleynikova
00030  */
00031 
00032 #include <ros/ros.h>
00033 #include <sensor_msgs/PointCloud2.h>
00034 #include <actionlib/server/simple_action_server.h>
00035 #include <turtlebot_arm_block_manipulation/BlockDetectionAction.h>
00036 
00037 #include <tf/transform_listener.h>
00038 
00039 #include <pcl/ros/conversions.h>
00040 #include <pcl/point_cloud.h>
00041 #include <pcl/point_types.h>
00042 #include <pcl/filters/passthrough.h>
00043 #include <pcl/segmentation/extract_clusters.h>
00044 #include <pcl/filters/extract_indices.h>
00045 #include <pcl/kdtree/kdtree_flann.h>
00046 #include <pcl/sample_consensus/method_types.h>
00047 #include <pcl/sample_consensus/model_types.h>
00048 #include <pcl/segmentation/sac_segmentation.h>
00049 #include <pcl/segmentation/extract_clusters.h>
00050 
00051 #include <pcl_ros/point_cloud.h>
00052 #include <pcl_ros/transforms.h>
00053 
00054 #include <cmath>
00055 #include <algorithm>
00056 
00057 namespace turtlebot_arm_block_manipulation
00058 {
00059 
00060 class BlockDetectionServer
00061 {
00062 private:
00063     
00064   ros::NodeHandle nh_;
00065   actionlib::SimpleActionServer<turtlebot_arm_block_manipulation::BlockDetectionAction> as_;
00066   std::string action_name_;
00067   turtlebot_arm_block_manipulation::BlockDetectionFeedback feedback_;
00068   turtlebot_arm_block_manipulation::BlockDetectionResult result_;
00069   turtlebot_arm_block_manipulation::BlockDetectionGoalConstPtr goal_;
00070   ros::Subscriber sub_;
00071   ros::Publisher pub_;
00072   
00073   tf::TransformListener tf_listener_;
00074   
00075   // Parameters from goal
00076   std::string arm_link;
00077   double block_size;
00078   double table_height;
00079   
00080   ros::Publisher block_pub_;
00081   
00082   // Parameters from node
00083   
00084 public:
00085   BlockDetectionServer(const std::string name) : 
00086     nh_("~"), as_(name, false), action_name_(name)
00087   {
00088     // Load parameters from the server.
00089     
00090     
00091     
00092     // Register the goal and feeback callbacks.
00093     as_.registerGoalCallback(boost::bind(&BlockDetectionServer::goalCB, this));
00094     as_.registerPreemptCallback(boost::bind(&BlockDetectionServer::preemptCB, this));
00095     
00096     as_.start();
00097     
00098     // Subscribe to point cloud
00099     sub_ = nh_.subscribe("/camera/depth_registered/points", 1, &BlockDetectionServer::cloudCb, this);
00100     pub_ = nh_.advertise< pcl::PointCloud<pcl::PointXYZRGB> >("block_output", 1);
00101     
00102     block_pub_ = nh_.advertise< geometry_msgs::PoseArray >("/turtlebot_blocks", 1, true);
00103   }
00104 
00105   void goalCB()
00106   {
00107     ROS_INFO("[block detection] Received goal!");
00108     // accept the new goal
00109     result_.blocks.poses.clear();
00110     
00111     goal_ = as_.acceptNewGoal();
00112     
00113     block_size = goal_->block_size;
00114     table_height = goal_->table_height;
00115     arm_link = goal_->frame;
00116 
00117     result_.blocks.header.frame_id = arm_link;
00118   }
00119 
00120   void preemptCB()
00121   {
00122     ROS_INFO("%s: Preempted", action_name_.c_str());
00123     // set the action state to preempted
00124     as_.setPreempted();
00125   }
00126 
00127   void cloudCb ( const sensor_msgs::PointCloud2ConstPtr& msg )
00128   {
00129     // Only do this if we're actually actively working on a goal.
00130     if (!as_.isActive()) return;
00131     
00132     result_.blocks.header.stamp = msg->header.stamp;
00133     
00134     // convert to PCL
00135     pcl::PointCloud<pcl::PointXYZRGB> cloud;
00136     pcl::fromROSMsg (*msg, cloud);
00137     
00138     // transform to whatever frame we're working in, probably the arm frame.
00139     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_transformed (new pcl::PointCloud<pcl::PointXYZRGB>);
00140     
00141     tf_listener_.waitForTransform(std::string(arm_link), cloud.header.frame_id, ros::Time(cloud.header.stamp), ros::Duration(1.0));
00142     if (!pcl_ros::transformPointCloud (std::string(arm_link), cloud, *cloud_transformed, tf_listener_))
00143     {
00144       ROS_ERROR ("Error converting to desired frame");
00145       return;
00146     }
00147 
00148     // Create the segmentation object for the planar model and set all the parameters
00149     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
00150     pcl::SACSegmentation<pcl::PointXYZRGB> seg;
00151     pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00152     pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00153     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZRGB> ());
00154     seg.setOptimizeCoefficients (true);
00155     seg.setModelType (pcl::SACMODEL_PLANE);
00156     seg.setMethodType (pcl::SAC_RANSAC);
00157     seg.setMaxIterations (200);
00158     seg.setDistanceThreshold (0.005);
00159     
00160     // Limit to things we think are roughly at the table height.
00161     pcl::PassThrough<pcl::PointXYZRGB> pass;
00162     pass.setInputCloud(cloud_transformed); 
00163     pass.setFilterFieldName("z");
00164     
00165     pass.setFilterLimits(table_height - 0.05, table_height + block_size + 0.05);
00166     pass.filter(*cloud_filtered);
00167     if( cloud_filtered->points.size() == 0 ){
00168       ROS_ERROR("0 points left");
00169       return;
00170     }else
00171       ROS_INFO("Filtered, %d points left", (int) cloud_filtered->points.size());
00172 
00173     int nr_points = cloud_filtered->points.size ();
00174     while (cloud_filtered->points.size () > 0.3 * nr_points)
00175     {
00176       // Segment the largest planar component from the remaining cloud
00177       seg.setInputCloud(cloud_filtered);
00178       seg.segment (*inliers, *coefficients);
00179       if (inliers->indices.size () == 0)
00180       {
00181         std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
00182         return;
00183       }
00184       
00185       std::cout << "Inliers: " << (inliers->indices.size ()) << std::endl;
00186 
00187       // Extract the planar inliers from the input cloud
00188       pcl::ExtractIndices<pcl::PointXYZRGB> extract;
00189       extract.setInputCloud (cloud_filtered);
00190       extract.setIndices (inliers);
00191       extract.setNegative (false);
00192 
00193       // Write the planar inliers to disk
00194       extract.filter (*cloud_plane);
00195       std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
00196 
00197       // Remove the planar inliers, extract the rest
00198       extract.setNegative (true);
00199       extract.filter (*cloud_filtered);
00200     }
00201 
00202     // Creating the KdTree object for the search method of the extraction
00203     pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
00204     tree->setInputCloud (cloud_filtered);
00205 
00206     std::vector<pcl::PointIndices> cluster_indices;
00207     pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
00208     ec.setClusterTolerance (0.005);
00209     ec.setMinClusterSize (100);
00210     ec.setMaxClusterSize (25000);
00211     ec.setSearchMethod (tree);
00212     ec.setInputCloud( cloud_filtered);
00213     ec.extract (cluster_indices);
00214 
00215     pub_.publish(cloud_filtered);
00216 
00217     // for each cluster, see if it is a block
00218     for (size_t c = 0; c < cluster_indices.size (); ++c)
00219     {  
00220       // find the outer dimensions of the cluster
00221       float xmin = 0; float xmax = 0; float ymin = 0; float ymax = 0;
00222       float zmin = 0; float zmax = 0;
00223       for (size_t i = 0; i < cluster_indices[c].indices.size(); i++)
00224       {
00225           int j = cluster_indices[c].indices[i];
00226           float x = cloud_filtered->points[j].x;
00227           float y = cloud_filtered->points[j].y;
00228           float z = cloud_filtered->points[j].z;
00229           if (i == 0)
00230           {
00231             xmin = xmax = x;
00232             ymin = ymax = y;
00233             zmin = zmax = z;
00234           }
00235           else
00236           {
00237             xmin = std::min(xmin, x);
00238             xmax = std::max(xmax, x);
00239             ymin = std::min(ymin, y);
00240             ymax = std::max(ymax, y);
00241             zmin = std::min(zmin, z);
00242             zmax = std::max(zmax, z);
00243           }    
00244       }
00245       
00246       // Check if these dimensions make sense for the block size specified
00247       float xside = xmax-xmin;
00248       float yside = ymax-ymin;
00249       float zside = zmax-zmin;
00250       
00251       const float tol = 0.01; // 1 cm error tolerance
00252       // In order to be part of the block, xside and yside must be between
00253       // blocksize and blocksize*sqrt(2)
00254       // z must be equal to or smaller than blocksize
00255       if (xside > block_size-tol && xside < block_size*sqrt(2)+tol &&
00256           yside > block_size-tol && yside < block_size*sqrt(2)+tol &&
00257           zside > tol && zside < block_size+tol)
00258       {
00259         // If so, then figure out the position and the orientation of the block
00260         float angle = atan(block_size/((xside+yside)/2));
00261         
00262         if (yside < block_size)
00263           angle = 0.0;
00264         
00265         ROS_INFO_STREAM("xside: " << xside << " yside: " << yside << " zside " << zside << " angle: " << angle);
00266         // Then add it to our set
00267         ROS_INFO("Adding a new block!");
00268         addBlock( xmin+(xside)/2.0, ymin+(yside)/2.0, zmax - block_size/2.0, angle);
00269       }
00270     }
00271     
00272     if (result_.blocks.poses.size() > 0)
00273     {
00274       as_.setSucceeded(result_);
00275       block_pub_.publish(result_.blocks);
00276       ROS_INFO("[block detection] Set as succeeded!");
00277     }
00278     else
00279       ROS_INFO("[block detection] Couldn't find any blocks this iteration!");
00280     //  as_.setAborted(result_);
00281   }
00282     
00283   void addBlock(float x, float y, float z, float angle)
00284   {
00285     geometry_msgs::Pose block_pose;
00286     block_pose.position.x = x;
00287     block_pose.position.y = y;
00288     block_pose.position.z = z;
00289     
00290     Eigen::Quaternionf quat(Eigen::AngleAxis<float>(angle, Eigen::Vector3f(0,0,1)));
00291     
00292     block_pose.orientation.x = quat.x();
00293     block_pose.orientation.y = quat.y();
00294     block_pose.orientation.z = quat.z();
00295     block_pose.orientation.w = quat.w();
00296     
00297     result_.blocks.poses.push_back(block_pose);
00298   }
00299 
00300 };
00301 
00302 };
00303 
00304 int main(int argc, char** argv)
00305 {
00306   ros::init(argc, argv, "block_detection_action_server");
00307 
00308   turtlebot_arm_block_manipulation::BlockDetectionServer server("block_detection");
00309   ros::spin();
00310 
00311   return 0;
00312 }
00313 


turtlebot_arm_block_manipulation
Author(s): Michael Ferguson, Helen Oleynikova
autogenerated on Thu Jun 6 2019 20:54:14