00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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 PointT>
00045 class SelfFilter: public FilterBase <pcl::PointCloud<PointT> >
00046 {
00047
00048 public:
00049 typedef pcl::PointCloud<PointT> PointCloud;
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 nh_.param<bool>("keep_organized", keep_organized_, false);
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<PointT>(tf_, links);
00105 if (!sensor_frame_.empty())
00106 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_);
00107 }
00108
00111 virtual ~SelfFilter(void)
00112 {
00113 delete sm_;
00114 }
00115
00116 virtual bool configure(void)
00117 {
00118
00119
00120 nh_.param("invert", invert_, false);
00121
00122 if (invert_)
00123 ROS_INFO("Inverting filter output");
00124
00125 return true;
00126 }
00127
00128 bool updateWithSensorFrame(const PointCloud& data_in, PointCloud& data_out, const std::string& sensor_frame)
00129 {
00130 sensor_frame_ = sensor_frame;
00131 return update(data_in, data_out);
00132 }
00133
00138 virtual bool update(const PointCloud& data_in, PointCloud& data_out)
00139 {
00140 std::vector<int> keep(data_in.points.size());
00141 if(sensor_frame_.empty()) {
00142 sm_->maskContainment(data_in, keep);
00143 } else {
00144 sm_->maskIntersection(data_in, sensor_frame_, min_sensor_dist_, keep);
00145 }
00146 fillResult(data_in, keep, data_out);
00147 return true;
00148 }
00149
00150 bool updateWithSensorFrame(const PointCloud& data_in, PointCloud& data_out, PointCloud& data_diff, const std::string& sensor_frame)
00151 {
00152 sensor_frame_ = sensor_frame;
00153 return update(data_in, data_out, data_diff);
00154 }
00155
00160 virtual bool update(const PointCloud& data_in, PointCloud& data_out, PointCloud& data_diff)
00161 {
00162 std::vector<int> keep(data_in.points.size());
00163 if(sensor_frame_.empty()) {
00164 sm_->maskContainment(data_in, keep);
00165 } else {
00166 sm_->maskIntersection(data_in, sensor_frame_, min_sensor_dist_, keep);
00167 }
00168 fillResult(data_in, keep, data_out);
00169 fillDiff(data_in,keep,data_diff);
00170 return true;
00171 }
00172
00173 void fillDiff(const PointCloud& data_in, const std::vector<int> &keep, PointCloud& data_out)
00174 {
00175 const unsigned int np = data_in.points.size();
00176
00177
00178 data_out.header = data_in.header;
00179
00180 data_out.points.resize(0);
00181 data_out.points.reserve(np);
00182
00183 for (unsigned int i = 0 ; i < np ; ++i)
00184 {
00185 if ((keep[i] && invert_) || (!keep[i] && !invert_))
00186 {
00187 data_out.points.push_back(data_in.points[i]);
00188 }
00189 }
00190 }
00191
00192 void fillResult(const PointCloud& data_in, const std::vector<int> &keep, PointCloud& data_out)
00193 {
00194 const unsigned int np = data_in.points.size();
00195
00196
00197 data_out.header = data_in.header;
00198
00199 data_out.points.resize(0);
00200 data_out.points.reserve(np);
00201 PointT nan_point;
00202 nan_point.x = std::numeric_limits<float>::quiet_NaN();
00203 nan_point.y = std::numeric_limits<float>::quiet_NaN();
00204 nan_point.z = std::numeric_limits<float>::quiet_NaN();
00205 for (unsigned int i = 0 ; i < np ; ++i)
00206 {
00207 if (keep[i] == robot_self_filter::OUTSIDE)
00208 {
00209 data_out.points.push_back(data_in.points[i]);
00210 }
00211 if (keep_organized_ && keep[i] != robot_self_filter::OUTSIDE)
00212 {
00213 data_out.points.push_back(nan_point);
00214 }
00215 }
00216 if (keep_organized_) {
00217 data_out.width = data_in.width;
00218 data_out.height = data_in.height;
00219 }
00220 }
00221
00222 virtual bool updateWithSensorFrame(const std::vector<PointCloud> & data_in, std::vector<PointCloud>& data_out, const std::string& sensor_frame)
00223 {
00224 sensor_frame_ = sensor_frame;
00225 return update(data_in, data_out);
00226 }
00227
00228 virtual bool update(const std::vector<PointCloud> & data_in, std::vector<PointCloud>& data_out)
00229 {
00230 bool result = true;
00231 data_out.resize(data_in.size());
00232 for (unsigned int i = 0 ; i < data_in.size() ; ++i)
00233 if (!update(data_in[i], data_out[i]))
00234 result = false;
00235 return true;
00236 }
00237
00238 robot_self_filter::SelfMask<PointT>* getSelfMask() {
00239 return sm_;
00240 }
00241
00242 void setSensorFrame(const std::string& frame) {
00243 sensor_frame_ = frame;
00244 }
00245
00246 protected:
00247
00248 tf::TransformListener tf_;
00249 robot_self_filter::SelfMask<PointT>* sm_;
00250
00251 ros::NodeHandle nh_;
00252 bool invert_;
00253 std::string sensor_frame_;
00254 double min_sensor_dist_;
00255 bool keep_organized_;
00256
00257 };
00258
00259 }
00260
00261 #endif //#ifndef FILTERS_SELF_SEE_H_