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