hole_detector.cpp
Go to the documentation of this file.
00001 /*                                                                                                                       
00002  * Copyright (c) 2010, 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 #include "ros/ros.h"
00030 #include "pluginlib/class_list_macros.h"
00031 #include "nodelet/nodelet.h"
00032 
00033 #include "pcl/point_cloud.h"
00034 #include "pcl_ros/point_cloud.h"
00035 #include "pcl/point_types.h"
00036 #include "pcl/ros/conversions.h"
00037 
00038 #include <boost/foreach.hpp>
00039 
00040 
00041 
00042 namespace pcl_to_scan {
00043 typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00044 
00045 class HoleDetector: public nodelet::Nodelet {
00046 public:
00047         //Constructor
00048         HoleDetector() :
00049                 threshold_(0.0), h_(0.4) {
00050         }
00051 
00052 private:
00053         virtual void onInit() {
00054                 ros::NodeHandle& nh = getNodeHandle();
00055                 ros::NodeHandle& private_nh = getPrivateNodeHandle();
00056 
00057 
00058                 private_nh.getParam("threshold", threshold_);
00059 
00060                 pub_ = nh.advertise<PointCloud> ("output", 10);
00061                 sub_ = nh.subscribe<PointCloud> ("input", 10, &HoleDetector::callback, this);
00062         }
00063 
00064         void callback(const PointCloud::ConstPtr& msg) {
00065                 PointCloud::Ptr output (new PointCloud);
00066 
00067                 output->header = msg->header;
00068                 output->height = msg->height;
00069                 output->width = msg->width;
00070 
00071                 BOOST_FOREACH (pcl::PointXYZ pt, msg->points) {
00072                         float vx, vy, vz, sc;
00073                         if (pt.z > threshold_) {
00074 
00075                         } else {
00076                                 sc = h_ / (h_ - pt.z);
00077                                 pt.x = pt.x * sc;
00078                                 pt.y = pt.y * sc;
00079                                 pt.z = 0.1;
00080                         }
00081 
00082                         output->points.push_back (pt);
00083                 }
00084 
00085                 pub_.publish(output);
00086         }
00087 
00088         double threshold_;
00089         double h_;
00090 
00091         ros::Publisher pub_;
00092         ros::Subscriber sub_;
00093 
00094 };
00095 
00096 PLUGINLIB_DECLARE_CLASS(pcl_to_scan, HoleDetector, pcl_to_scan::HoleDetector, nodelet::Nodelet);
00097 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


pcl_to_scan
Author(s): Maciej Stefanczyk
autogenerated on Thu Nov 14 2013 11:56:51