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