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 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 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 robot_self_filter::PointCloud& data_in, robot_self_filter::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 robot_self_filter::PointCloud& data_in, robot_self_filter::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 robot_self_filter::PointCloud& data_in, robot_self_filter::PointCloud& data_out, robot_self_filter::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 robot_self_filter::PointCloud& data_in, robot_self_filter::PointCloud& data_out, robot_self_filter::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 robot_self_filter::PointCloud& data_in, const std::vector<int> &keep, robot_self_filter::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 robot_self_filter::PointCloud& data_in, const std::vector<int> &keep, robot_self_filter::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
00202 for (unsigned int i = 0 ; i < np ; ++i)
00203 {
00204 data_out.points.push_back(data_in.points[i]);
00205 }
00206 }
00207
00208 virtual bool updateWithSensorFrame(const std::vector<robot_self_filter::PointCloud> & data_in, std::vector<robot_self_filter::PointCloud>& data_out, const std::string& sensor_frame)
00209 {
00210 sensor_frame_ = sensor_frame;
00211 return update(data_in, data_out);
00212 }
00213
00214 virtual bool update(const std::vector<robot_self_filter::PointCloud> & data_in, std::vector<robot_self_filter::PointCloud>& data_out)
00215 {
00216 bool result = true;
00217 data_out.resize(data_in.size());
00218 for (unsigned int i = 0 ; i < data_in.size() ; ++i)
00219 if (!update(data_in[i], data_out[i]))
00220 result = false;
00221 return true;
00222 }
00223
00224 robot_self_filter::SelfMask* getSelfMask() {
00225 return sm_;
00226 }
00227
00228 void setSensorFrame(const std::string& frame) {
00229 sensor_frame_ = frame;
00230 }
00231
00232 protected:
00233
00234 tf::TransformListener tf_;
00235 robot_self_filter::SelfMask* sm_;
00236
00237 ros::NodeHandle nh_;
00238 bool invert_;
00239 std::string sensor_frame_;
00240 double min_sensor_dist_;
00241
00242
00243 };
00244
00245 }
00246
00247 #endif //#ifndef FILTERS_SELF_SEE_H_