wall_extractor_pcl.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  * author: Wim Meeussen
00035  *
00036  */
00037 
00038 
00039 
00040 
00041 #include <ros/ros.h>
00042 #include <sensor_msgs/PointCloud2.h>
00043 #include <stereo_wall_detection/DetectWall.h>
00044 
00045 #include <pcl_msgs/ModelCoefficients.h>
00046 #include <pcl_ros/io/pcd_io.h>
00047 #include <pcl/common/centroid.h>
00048 #include <pcl_ros/segmentation/sac_segmentation.h>
00049 
00050 #include <boost/thread/mutex.hpp>
00051 #include <boost/thread/condition.hpp>
00052 #include <boost/bind.hpp>
00053 #include <pcl_conversions/pcl_conversions.h>
00054 
00055 #include <visualization_msgs/Marker.h>
00056 
00057 class PlanarFit
00058 {
00059 public:
00060   PlanarFit () : nh_ ("~")
00061   {
00062     serv_ = nh_.advertiseService ("detect_wall", &PlanarFit::detect_wall, this);
00063 
00064     // subscribe to pointcloud messages from stereo camera
00065     //cloud_sub_ = nh_.subscribe("points2", 10, boost::bind(&PlanarFit::cloudCb, this, _1));  // what's wrong with this line???
00066     cloud_sub_ = nh_.subscribe("points2", 10, &PlanarFit::cloudCb, this);
00067 
00068     // advertise selected points from wall detection algorithm
00069     plane_points_ = nh_.advertise<sensor_msgs::PointCloud2>("wall_points", 10);
00070     plane_marker_ = nh_.advertise<visualization_msgs::Marker>("wall_marker", 10);
00071   }
00072   
00073   virtual ~PlanarFit () { }
00074   
00075 
00076 
00077   //void cloudCb(const sensor_msgs::PointCloud2ConstPtr& cloud)
00078   void cloudCb(const boost::shared_ptr<const sensor_msgs::PointCloud2>& cloud)
00079   {
00080     boost::mutex::scoped_lock lock(cloud_mutex_);
00081     cloud_msg_ = cloud;
00082     cloud_condition_.notify_all();
00083   }
00084 
00085 
00086 
00087   bool detect_wall (stereo_wall_detection::DetectWall::Request &req, stereo_wall_detection::DetectWall::Response &resp)
00088   {
00089     ros::Time start_time = ros::Time::now();
00090 
00091     // wait for new pointcloud
00092     boost::mutex::scoped_lock lock(cloud_mutex_);
00093     cloud_condition_.wait(lock);
00094     bool cloud_found = false;
00095     while (ros::ok() && !cloud_found){
00096       // check for timeout
00097       if (ros::Time::now() > start_time + ros::Duration(10.0)){
00098         ROS_ERROR("Did not receive a pointcloud in 10 seconds");
00099         return false;
00100       }
00101 
00102       // check if this is a good pointcloud
00103       if (cloud_msg_->header.stamp > start_time){
00104         unsigned count = 0;
00105         for (unsigned i=0; i<cloud_msg_->height * cloud_msg_->width; i++){
00106           if (!std::isnan(cloud_msg_->data[i])){
00107             count++;
00108           }
00109         }
00110         if (count > 30000)
00111           cloud_found = true;
00112         else
00113           ROS_INFO("Received a cloud, but it only had %d points", count);
00114       }
00115       else
00116         ROS_INFO("Received a cloud, but the cloud is still %f seconds before start time", (start_time - cloud_msg_->header.stamp).toSec());
00117 
00118       // wait for another cloud
00119       if (!cloud_found)     
00120         cloud_condition_.wait(lock);
00121     }
00122     ROS_INFO("point cloud received");
00123 
00124 
00125     double threshold;
00126     nh_.param<double>("distance_threshold", threshold, 0.01);
00127 
00128     int tmp_points;
00129     // minimum number of inliers; a good detection appears to have over 
00130     // 100,000 inliers
00131     nh_.param<int>("minimum_points", tmp_points, 20000);
00132     size_t min_points = tmp_points;
00133 
00134     // find plane in pointcloud
00135     pcl::PointCloud<pcl::PointXYZ> cloud;
00136     pcl::fromROSMsg(*cloud_msg_, cloud);
00137     pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00138     pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00139     pcl::SACSegmentation<pcl::PointXYZ> seg;
00140     seg.setOptimizeCoefficients (true);
00141     seg.setModelType (pcl::SACMODEL_PLANE);
00142     seg.setMethodType (pcl::SAC_RANSAC);
00143     seg.setDistanceThreshold (threshold);
00144     seg.setInputCloud (cloud.makeShared ());
00145     seg.segment (*inliers, *coefficients);
00146     if (inliers->indices.size () == 0) {
00147       ROS_ERROR ("Could not estimate a planar model for the given dataset.");
00148       return false;
00149     }
00150 
00151     // check for minimum number of inliers in plane; if we have too few, our
00152     // detection is bad and we should return an error
00153     if( inliers->indices.size() < min_points ) {
00154        ROS_ERROR("Too few inliers in deteted plane: %zd", inliers->indices.size());
00155        return false;
00156     }
00157 
00158     pcl::copyPointCloud(cloud, *inliers, cloud); // add publisher here to see resulting point cloud.
00159     Eigen::Vector4f centroid;
00160     pcl::compute3DCentroid(cloud, centroid);
00161 
00162     // publish cloud with detected points for debugging
00163     if( plane_points_.getNumSubscribers() > 0 ) {
00164       sensor_msgs::PointCloud2 plane_msg;
00165       pcl::toROSMsg(cloud, plane_msg); 
00166       plane_points_.publish(plane_msg);
00167     }
00168 
00169     geometry_msgs::Vector3 normal;
00170     normal.x = coefficients->values[0];
00171     normal.y = coefficients->values[1];
00172     normal.z = coefficients->values[2];
00173 
00174     if( normal.z < 0 ) {
00175       // Normal points towards camera; reverse it
00176       ROS_WARN("Wall Normal points towards camera frame: %s", 
00177         cloud_msg_->header.frame_id.c_str());
00178       normal.x = -normal.x;
00179       normal.y = -normal.y;
00180       normal.z = -normal.z;
00181     } else {
00182       ROS_WARN("Wall normal points away from camera frame: %s", 
00183         cloud_msg_->header.frame_id.c_str());
00184     }
00185 
00186     resp.wall_norm.header = cloud_msg_->header;
00187     resp.wall_norm.vector = normal;
00188 
00189     resp.wall_point.header = cloud_msg_->header;
00190     resp.wall_point.point.x = centroid[0];
00191     resp.wall_point.point.y = centroid[1];
00192     resp.wall_point.point.z = centroid[2];
00193 
00194 
00195     // publish Marker vector of wall normal for debugging
00196     if( plane_marker_.getNumSubscribers() > 0 ) {
00197       visualization_msgs::Marker marker;
00198 
00199       // set up marker
00200       marker.header = cloud_msg_->header;
00201       marker.id = 0;
00202       marker.type =   visualization_msgs::Marker::ARROW;
00203       marker.action = visualization_msgs::Marker::ADD; // add/modify
00204 
00205       // visible, red marker
00206       marker.color.a = 1.0;
00207       marker.color.r = 1.0;
00208       marker.color.g = 0.0;
00209       marker.color.b = 0.0;
00210 
00211       geometry_msgs::Point end;   // vector end
00212       end.x = resp.wall_point.point.x + resp.wall_norm.vector.x;
00213       end.y = resp.wall_point.point.y + resp.wall_norm.vector.y;
00214       end.z = resp.wall_point.point.z + resp.wall_norm.vector.z;
00215 
00216       marker.points.push_back(resp.wall_point.point);
00217       marker.points.push_back(end);
00218 
00219       marker.scale.x = 0.05;
00220       marker.scale.y = 0.1;
00221       //marker.scale.z = 0.1;
00222 
00223       plane_marker_.publish(marker);
00224     }
00225 
00226     ROS_INFO("Found wall at %f %f %f with normal: %f %f %f in frame %s",
00227              resp.wall_point.point.x, resp.wall_point.point.y, resp.wall_point.point.z,
00228              resp.wall_norm.vector.x, resp.wall_norm.vector.y, resp.wall_norm.vector.z, 
00229              cloud_msg_->header.frame_id.c_str());
00230 
00231     return true;
00232   }
00233 
00234 private:
00235   ros::NodeHandle nh_;
00236   ros::ServiceServer serv_;
00237   ros::Publisher plane_points_;
00238   ros::Publisher plane_marker_;
00239   ros::Subscriber cloud_sub_;
00240 
00241   sensor_msgs::PointCloud2ConstPtr cloud_msg_;
00242   boost::condition cloud_condition_;
00243   boost::mutex cloud_mutex_;
00244 };
00245 
00246 
00247 
00248 int main (int argc, char** argv)
00249 {
00250   ros::init (argc, argv, "wall_extractor");
00251 
00252   PlanarFit p;
00253   ros::MultiThreadedSpinner spinner(2);  // extra thread so we can receive cloud messages while in the service call
00254   spinner.spin();
00255 
00256   return (0);
00257 }
00258 


stereo_wall_detection
Author(s): Radu Bogdan Rusu
autogenerated on Thu Aug 27 2015 14:29:42