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 #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
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
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
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
00136
00137
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
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
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
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
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
00365 drawBox(goal->box_dims, goal->box_pose, "input_bounds");
00366
00367
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
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
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
00399 splitCloudRegions(cloud_ds, contents, container);
00400
00401
00402 findBoundingBox(cloud_ds, result.box_dims, result.box_pose);
00403
00404
00405 std::vector< pcl::PointCloud<PointT>::Ptr > clusters;
00406 findClusters(contents, clusters);
00407 result.clusters.resize(clusters.size());
00408
00409
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
00426 ROS_INFO("%s: Succeeded", action_name_.c_str());
00427
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 }