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 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
00122
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
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
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
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_