30 #ifndef FILTERS_SELF_SEE_H_ 31 #define FILTERS_SELF_SEE_H_ 44 template <
typename Po
intT>
54 double default_padding, default_scale;
55 nh_.
param<
double>(
"self_see_default_padding", default_padding, .01);
56 nh_.
param<
double>(
"self_see_default_scale", default_scale, 1.0);
58 std::vector<robot_self_filter::LinkInfo> links;
59 std::string link_names;
62 ROS_WARN(
"No links specified for self filtering.");
69 ROS_WARN(
"Self see links need to be an array");
72 if(ssl_vals.
size() == 0) {
73 ROS_WARN(
"No values in self see links array");
75 for(
int i = 0; i < ssl_vals.
size(); i++) {
79 ROS_WARN(
"Self see links entry %d is not a structure. Stopping processing of self see links",i);
82 if(!ssl_vals[i].hasMember(
"name")) {
83 ROS_WARN(
"Self see links entry %d has no name. Stopping processing of self see links",i);
86 li.
name = std::string(ssl_vals[i][
"name"]);
87 if(!ssl_vals[i].hasMember(
"padding")) {
88 ROS_DEBUG(
"Self see links entry %d has no padding. Assuming default padding of %g",i,default_padding);
91 li.
padding = ssl_vals[i][
"padding"];
93 if(!ssl_vals[i].hasMember(
"scale")) {
94 ROS_DEBUG(
"Self see links entry %d has no scale. Assuming default scale of %g",i,default_scale);
95 li.
scale = default_scale;
97 li.
scale = ssl_vals[i][
"scale"];
123 ROS_INFO(
"Inverting filter output");
131 return update(data_in, data_out);
138 virtual bool update(
const PointCloud& data_in, PointCloud& data_out)
140 std::vector<int> keep(data_in.points.size());
142 sm_->maskContainment(data_in, keep);
150 bool updateWithSensorFrame(
const PointCloud& data_in, PointCloud& data_out, PointCloud& data_diff,
const std::string& sensor_frame)
153 return update(data_in, data_out, data_diff);
160 virtual bool update(
const PointCloud& data_in, PointCloud& data_out, PointCloud& data_diff)
162 std::vector<int> keep(data_in.points.size());
164 sm_->maskContainment(data_in, keep);
173 void fillDiff(
const PointCloud& data_in,
const std::vector<int> &keep, PointCloud& data_out)
175 const unsigned int np = data_in.points.size();
178 data_out.header = data_in.header;
180 data_out.points.resize(0);
181 data_out.points.reserve(np);
183 for (
unsigned int i = 0 ; i < np ; ++i)
187 data_out.points.push_back(data_in.points[i]);
192 void fillResult(
const PointCloud& data_in,
const std::vector<int> &keep, PointCloud& data_out)
194 const unsigned int np = data_in.points.size();
197 data_out.header = data_in.header;
199 data_out.points.resize(0);
200 data_out.points.reserve(np);
202 nan_point.x = std::numeric_limits<float>::quiet_NaN();
203 nan_point.y = std::numeric_limits<float>::quiet_NaN();
204 nan_point.z = std::numeric_limits<float>::quiet_NaN();
205 for (
unsigned int i = 0 ; i < np ; ++i)
209 data_out.points.push_back(data_in.points[i]);
213 data_out.points.push_back(nan_point);
217 data_out.width = data_in.width;
218 data_out.height = data_in.height;
222 virtual bool updateWithSensorFrame(
const std::vector<PointCloud> & data_in, std::vector<PointCloud>& data_out,
const std::string& sensor_frame)
225 return update(data_in, data_out);
228 virtual bool update(
const std::vector<PointCloud> & data_in, std::vector<PointCloud>& data_out)
231 data_out.resize(data_in.size());
232 for (
unsigned int i = 0 ; i < data_in.size() ; ++i)
233 if (!
update(data_in[i], data_out[i]))
261 #endif //#ifndef FILTERS_SELF_SEE_H_ std::string sensor_frame_
tf::TransformListener tf_
virtual ~SelfFilter(void)
Destructor to clean up.
virtual bool update(const PointCloud &data_in, PointCloud &data_out)
Update the filter and return the data seperately.
virtual bool update(const PointCloud &data_in, PointCloud &data_out, PointCloud &data_diff)
Update the filter and return the data seperately.
void fillDiff(const PointCloud &data_in, const std::vector< int > &keep, PointCloud &data_out)
Type const & getType() const
virtual bool update(const std::vector< PointCloud > &data_in, std::vector< PointCloud > &data_out)
pcl::PointCloud< PointT > PointCloud
virtual bool updateWithSensorFrame(const std::vector< PointCloud > &data_in, std::vector< PointCloud > &data_out, const std::string &sensor_frame)
robot_self_filter::SelfMask< PointT > * sm_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void setSensorFrame(const std::string &frame)
bool updateWithSensorFrame(const PointCloud &data_in, PointCloud &data_out, PointCloud &data_diff, const std::string &sensor_frame)
SelfFilter(ros::NodeHandle nh)
Construct the filter.
robot_self_filter::SelfMask< PointT > * getSelfMask()
Computing a mask for a pointcloud that states which points are inside the robot.
A filter to remove parts of the robot seen in a pointcloud.
bool getParam(const std::string &key, std::string &s) const
void fillResult(const PointCloud &data_in, const std::vector< int > &keep, PointCloud &data_out)
bool hasParam(const std::string &key) const
bool updateWithSensorFrame(const PointCloud &data_in, PointCloud &data_out, const std::string &sensor_frame)
virtual bool configure(void)