$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 #ifndef FILTERS_SELF_SEE_H_ 00031 #define FILTERS_SELF_SEE_H_ 00032 00033 #include <filters/filter_base.h> 00034 #include <robot_self_filter_color/self_mask_color.h> 00035 #include <ros/console.h> 00036 00037 #include <pcl/point_types.h> 00038 #include <pcl/point_cloud.h> 00039 00040 namespace filters 00041 { 00043 template <typename T> 00044 class SelfFilter: public FilterBase <T> 00045 { 00046 public: 00048 SelfFilter (ros::NodeHandle nh) : nh_(nh) 00049 { 00050 nh_.param<double> ("min_sensor_dist", min_sensor_dist_, 0.01); 00051 double default_padding, default_scale; 00052 nh_.param<double> ("self_see_default_padding", default_padding, .01); 00053 nh_.param<double> ("self_see_default_scale", default_scale, 1.0); 00054 00055 std::vector<robot_self_filter_color::LinkInfo> links; 00056 std::string link_names; 00057 00058 if (!nh_.hasParam ("self_see_links")) 00059 { 00060 ROS_WARN ("No links specified for self filtering."); 00061 } 00062 else 00063 { 00064 XmlRpc::XmlRpcValue ssl_vals;; 00065 00066 nh_.getParam ("self_see_links", ssl_vals); 00067 if (ssl_vals.getType () != XmlRpc::XmlRpcValue::TypeArray) 00068 { 00069 ROS_WARN ("Self see links need to be an array"); 00070 } 00071 else 00072 { 00073 if (ssl_vals.size () == 0) 00074 { 00075 ROS_WARN ("No values in self see links array"); 00076 } 00077 else 00078 { 00079 for (int i = 0; i < ssl_vals.size (); ++i) 00080 { 00081 robot_self_filter_color::LinkInfo li; 00082 00083 if (ssl_vals[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) 00084 { 00085 ROS_WARN ("Self see links entry %d is not a structure. Stopping processing of self see links", i); 00086 break; 00087 } 00088 if (!ssl_vals[i].hasMember ("name")) 00089 { 00090 ROS_WARN ("Self see links entry %d has no name. Stopping processing of self see links", i); 00091 break; 00092 } 00093 li.name = std::string (ssl_vals[i]["name"]); 00094 if (!ssl_vals[i].hasMember ("padding")) 00095 { 00096 ROS_DEBUG ("Self see links entry %d has no padding. Assuming default padding of %g", i, default_padding); 00097 li.padding = default_padding; 00098 } 00099 else 00100 { 00101 li.padding = ssl_vals[i]["padding"]; 00102 } 00103 if (!ssl_vals[i].hasMember ("scale")) 00104 { 00105 ROS_DEBUG ("Self see links entry %d has no scale. Assuming default scale of %g", i, default_scale); 00106 li.scale = default_scale; 00107 } 00108 else 00109 { 00110 li.scale = ssl_vals[i]["scale"]; 00111 } 00112 links.push_back (li); 00113 } 00114 } 00115 } 00116 } 00117 sm_ = new robot_self_filter_color::SelfMask (tf_, links); 00118 // nh_.param<std::string> ("annotate", annotate_, std::string ()); 00119 // if (!annotate_.empty ()) 00120 // ROS_INFO ("Self filter is adding annotation channel '%s'", annotate_.c_str ()); 00121 if (!sensor_frame_.empty ()) 00122 ROS_INFO("Self filter is removing shadow points for sensor in frame '%s'. Minimum distance to sensor is %f.", sensor_frame_.c_str (), min_sensor_dist_); 00123 } 00124 00127 virtual ~SelfFilter (void) 00128 { 00129 delete sm_; 00130 } 00131 00132 virtual bool 00133 configure (void) 00134 { 00135 // keep only the points that are outside of the robot 00136 // for testing purposes this may be changed to true 00137 nh_.param ("invert", invert_, false); 00138 00139 if (invert_) 00140 ROS_INFO ("Inverting filter output"); 00141 00142 return (true); 00143 } 00144 00145 bool updateWithSensorFrame 00146 (const pcl::PointCloud<pcl::PointXYZRGB>& data_in, pcl::PointCloud<pcl::PointXYZRGB>& data_out, const std::string& sensor_frame) 00147 { 00148 sensor_frame_ = sensor_frame; 00149 return (update (data_in, data_out)); 00150 } 00151 00156 virtual bool 00157 update (const pcl::PointCloud<pcl::PointXYZRGB>& data_in, pcl::PointCloud<pcl::PointXYZRGB>& data_out) 00158 { 00159 std::vector<int> keep (data_in.points.size ()); 00160 if (sensor_frame_.empty ()) 00161 { 00162 sm_->maskContainment (data_in, keep); 00163 } 00164 else 00165 { 00166 sm_->maskIntersection (data_in, sensor_frame_, min_sensor_dist_, keep); 00167 } 00168 fillResult (data_in, keep, data_out); 00169 return (true); 00170 } 00171 00172 bool updateWithSensorFrame (const pcl::PointCloud<pcl::PointXYZRGB>& data_in, pcl::PointCloud<pcl::PointXYZRGB>& data_out, pcl::PointCloud<pcl::PointXYZRGB>& data_diff, const std::string& sensor_frame) 00173 { 00174 sensor_frame_ = sensor_frame; 00175 return (update (data_in, data_out, data_diff)); 00176 } 00177 00182 virtual bool update (const pcl::PointCloud<pcl::PointXYZRGB>& data_in, pcl::PointCloud<pcl::PointXYZRGB>& data_out, pcl::PointCloud<pcl::PointXYZRGB>& data_diff) 00183 { 00184 std::vector<int> keep (data_in.points.size ()); 00185 if (sensor_frame_.empty ()) 00186 { 00187 sm_->maskContainment (data_in, keep); 00188 } 00189 else 00190 { 00191 sm_->maskIntersection (data_in, sensor_frame_, min_sensor_dist_, keep); 00192 } 00193 fillResult (data_in, keep, data_out); 00194 fillDiff (data_in,keep,data_diff); 00195 return (true); 00196 } 00197 00198 void fillDiff (const pcl::PointCloud<pcl::PointXYZRGB>& data_in, const std::vector<int> &keep, pcl::PointCloud<pcl::PointXYZRGB>& data_out) 00199 { 00200 const unsigned int np = data_in.points.size (); 00201 00202 // fill in output data 00203 data_out.header = data_in.header; 00204 00205 data_out.points.resize (0); 00206 data_out.points.reserve (np); 00207 00208 /* data_out.channels.resize(data_in.channels.size()); 00209 for (unsigned int i = 0 ; i < data_out.channels.size () ; ++i) 00210 { 00211 ROS_ASSERT(data_in.channels[i].values.size() == data_in.points.size()); 00212 data_out.channels[i].name = data_in.channels[i].name; 00213 data_out.channels[i].values.reserve(data_in.channels[i].values.size()); 00214 } 00215 */ 00216 for (unsigned int i = 0 ; i < np ; ++i) 00217 { 00218 if ((keep[i] && invert_) || (!keep[i] && !invert_)) 00219 { 00220 data_out.points.push_back(data_in.points[i]); 00221 /* for (unsigned int j = 0 ; j < data_out.channels.size() ; ++j) 00222 data_out.channels[j].values.push_back(data_in.channels[j].values[i]);*/ 00223 } 00224 } 00225 } 00226 00227 void fillResult (const pcl::PointCloud<pcl::PointXYZRGB>& data_in, const std::vector<int> &keep, pcl::PointCloud<pcl::PointXYZRGB>& data_out) 00228 { 00229 const unsigned int np = data_in.points.size (); 00230 00231 // fill in output data with points that are NOT on the robot 00232 data_out.header = data_in.header; 00233 00234 data_out.points.resize (0); 00235 data_out.points.reserve (np); 00236 00237 /* data_out.channels.resize(data_in.channels.size()); 00238 for (unsigned int i = 0 ; i < data_out.channels.size() ; ++i) 00239 { 00240 ROS_ASSERT(data_in.channels[i].values.size() == data_in.points.size()); 00241 data_out.channels[i].name = data_in.channels[i].name; 00242 data_out.channels[i].values.reserve(data_in.channels[i].values.size()); 00243 }*/ 00244 00245 /* int c = -1; 00246 if (!annotate_.empty()) 00247 { 00248 // add annotation for points 00249 for (unsigned int i = 0 ; i < data_out.channels.size() ; ++i) 00250 if (data_out.channels[i].name == annotate_) 00251 { 00252 c = i; 00253 break; 00254 } 00255 if (c < 0) 00256 { 00257 c = data_out.channels.size(); 00258 data_out.channels.resize(c + 1); 00259 data_out.channels[c].name = annotate_; 00260 } 00261 data_out.channels[c].values.reserve(np); 00262 }*/ 00263 00264 for (unsigned int i = 0 ; i < np ; ++i) 00265 //if (annotate_.empty()) 00266 { 00267 if (keep[i] == robot_self_filter_color::OUTSIDE) 00268 { 00269 data_out.points.push_back (data_in.points[i]); 00270 // for (unsigned int j = 0 ; j < data_out.channels.size () ; ++j) 00271 // data_out.channels[j].values.push_back (data_in.channels[j].values[i]); 00272 } 00273 } 00274 /* else 00275 { 00276 data_out.points.push_back(data_in.points[i]); 00277 for (unsigned int j = 0 ; j < data_out.channels.size() ; ++j) 00278 { 00279 if ((int)j == c) 00280 { 00281 float flag = 0.0; 00282 if (keep[i] != robot_self_filter_color::SHADOW) 00283 flag = keep[i] == robot_self_filter_color::OUTSIDE ? 1.0f : -1.0f; 00284 data_out.channels[c].values.push_back(flag); 00285 } 00286 else 00287 data_out.channels[j].values.push_back(data_in.channels[j].values[i]); 00288 } 00289 }*/ 00290 } 00291 00292 /* virtual bool updateWithSensorFrame(const std::vector<sensor_msgs::PointCloud> & data_in, std::vector<sensor_msgs::PointCloud>& data_out, const std::string& sensor_frame) 00293 { 00294 sensor_frame_ = sensor_frame; 00295 return update(data_in, data_out); 00296 } 00297 00298 virtual bool update(const std::vector<sensor_msgs::PointCloud> & data_in, std::vector<sensor_msgs::PointCloud>& data_out) 00299 { 00300 bool result = true; 00301 data_out.resize(data_in.size()); 00302 for (unsigned int i = 0 ; i < data_in.size() ; ++i) 00303 if (!update(data_in[i], data_out[i])) 00304 result = false; 00305 return true; 00306 }*/ 00307 00308 robot_self_filter_color::SelfMask* getSelfMask () 00309 { 00310 return (sm_); 00311 } 00312 00313 void setSensorFrame (const std::string& frame) 00314 { 00315 sensor_frame_ = frame; 00316 } 00317 00318 protected: 00319 00320 tf::TransformListener tf_; 00321 robot_self_filter_color::SelfMask* sm_; 00322 00323 ros::NodeHandle nh_; 00324 bool invert_; 00325 std::string sensor_frame_; 00326 std::string annotate_; 00327 double min_sensor_dist_; 00328 }; 00329 } 00330 00331 #endif //#ifndef FILTERS_SELF_SEE_H_