$search
00001 /* 00002 * Copyright (c) 2008, 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 */ 00030 00031 #include <cart_state_estimator/polygon_point_cloud_filter.h> 00032 #include "pluginlib/class_list_macros.h" 00033 #include <geometry_msgs/PolygonStamped.h> 00034 #include <cart_state_estimator/PolygonPointCloudFilterConfig.h> 00035 00036 PLUGINLIB_DECLARE_CLASS(cart_state_estimator, PolygonPointCloudFilter, 00037 cart_state_estimator::PolygonPointCloudFilter, filters::FilterBase<sensor_msgs::PointCloud>) 00038 00039 namespace cart_state_estimator 00040 { 00041 00042 using sensor_msgs::PointCloud; 00043 using geometry_msgs::Point32; 00044 using XmlRpc::XmlRpcValue; 00045 using geometry_msgs::PolygonStamped; 00046 00047 bool PolygonPointCloudFilter::configure () 00048 { 00049 filter_polygon_pub_ = nh_.advertise<PolygonStamped>("filter_polygon", 10); 00050 XmlRpcValue polygon; 00051 if (!getParam("polygon", polygon)) { 00052 ROS_ERROR ("PolygonPointCloudFilter needs polygon to be set"); 00053 return false; 00054 } 00055 else if ((polygon.getType() == XmlRpcValue::TypeArray) && 00056 (polygon.size() >= 3)) { 00057 poly_.resize(polygon.size()); 00058 for (int i=0; i< polygon.size(); i++) { 00059 XmlRpcValue pt = polygon[i]; 00060 ROS_ASSERT_MSG (pt.getType() == XmlRpcValue::TypeArray && pt.size() == 2, 00061 "Invalid xml rpc value for polygon parameter"); 00062 poly_[i].x = (double) pt[0]; 00063 poly_[i].y = (double) pt[1]; 00064 ROS_INFO_STREAM ("Adding point " << poly_[i]); 00065 } 00066 } 00067 else { 00068 ROS_ERROR ("Invalid xml rpc value for polygon"); 00069 return false; 00070 } 00071 if (!getParam("frame", frame_)) { 00072 ROS_ERROR ("PolygonPointCloudFilter needs frame to be set"); 00073 } 00074 00075 ros::NodeHandle dyn_nh(nh_, ros::this_node::getName() + "/" + getName()); 00076 dyn_srv_ = new dynamic_reconfigure::Server<cart_state_estimator::PolygonPointCloudFilterConfig>(dyn_nh); 00077 dyn_srv_->setCallback(boost::bind(&PolygonPointCloudFilter::configCB, this, _1, _2)); 00078 00079 return true; 00080 } 00081 00082 void PolygonPointCloudFilter::configCB(cart_state_estimator::PolygonPointCloudFilterConfig &config, uint32_t level) 00083 { 00084 passthrough_ = config.passthrough; 00085 00086 frame_ = config.frame_id; 00087 } 00088 00089 bool PolygonPointCloudFilter::update (const PointCloud& input_scan, PointCloud& filtered_scan) 00090 { 00091 if (&input_scan == &filtered_scan) { 00092 ROS_ERROR ("This filter does not currently support in place copying"); 00093 return false; 00094 } 00095 00096 if (passthrough_) { 00097 filtered_scan = input_scan; 00098 return true; 00099 } 00100 00101 PointCloud laser_cloud; 00102 00103 try { 00104 tf_.waitForTransform(input_scan.header.frame_id, frame_, input_scan.header.stamp, ros::Duration(0.2)); 00105 tf_.transformPointCloud(frame_, input_scan, laser_cloud); 00106 } 00107 catch (tf::TransformException& ex) { 00108 ROS_ERROR ("Transform unavailable %s", ex.what()); 00109 return false; 00110 } 00111 00112 const unsigned num_channels = input_scan.channels.size(); 00113 00114 filtered_scan.header = input_scan.header; 00115 filtered_scan.points.resize (input_scan.points.size()); 00116 filtered_scan.channels.resize(num_channels); 00117 for (unsigned d=0; d<num_channels; d++) { 00118 filtered_scan.channels[d].values.resize (input_scan.points.size()); 00119 filtered_scan.channels[d].name = input_scan.channels[d].name; 00120 } 00121 00122 int num_pts = 0; 00123 for (unsigned i=0; i<laser_cloud.points.size(); i++) { 00124 ROS_DEBUG_STREAM_NAMED ("polygon_filter_internal", "Looking at point " << laser_cloud.points[i]); 00125 if (!inPolygon(laser_cloud.points[i])) { 00126 ROS_DEBUG_STREAM_NAMED ("polygon_filter_internal", "Not in polygon"); 00127 filtered_scan.points[num_pts] = input_scan.points[i]; 00128 for (unsigned d=0; d<num_channels; d++) 00129 filtered_scan.channels[d].values[num_pts] = input_scan.channels[d].values[i]; 00130 num_pts++; 00131 } 00132 else 00133 ROS_DEBUG_STREAM_NAMED ("polygon_filter_internal", "In polygon"); 00134 } 00135 ROS_DEBUG_NAMED ("polygon_filter", "In update, received a cloud of size %zu, of which we're keeping %d", 00136 laser_cloud.points.size(), num_pts); 00137 00138 // Resize output vectors 00139 filtered_scan.points.resize(num_pts); 00140 for (unsigned d=0; d<num_channels; d++) 00141 filtered_scan.channels[d].values.resize(num_pts); 00142 00143 // For debugging, visualize polygon 00144 { 00145 PolygonStamped poly; 00146 poly.polygon.points = poly_; 00147 poly.header.frame_id = frame_; 00148 poly.header.stamp = ros::Time::now(); 00149 filter_polygon_pub_.publish(poly); 00150 } 00151 00152 return true; 00153 } 00154 00155 00156 bool PolygonPointCloudFilter::inPolygon (const Point32& p) const 00157 { 00158 bool c=false; 00159 const int num_vertices = poly_.size(); 00160 for (int i=0; i<num_vertices; i++) { 00161 const int j = i ? i-1 : num_vertices-1; 00162 if ( ((poly_[i].y>p.y) != (poly_[j].y>p.y)) && 00163 (p.x < (poly_[j].x-poly_[i].x) * (p.y-poly_[i].y) / (poly_[j].y-poly_[i].y) + poly_[i].x)) 00164 c = !c; 00165 } 00166 return c; 00167 } 00168 00169 00170 00171 } // namespace cart_state_estimator 00172