35 #ifndef LASER_FILTERS_SCAN_MASK_FILTER_H 36 #define LASER_FILTERS_SCAN_MASK_FILTER_H 44 #include "sensor_msgs/LaserScan.h" 59 std::map<std::string, std::vector<size_t> >
masks_;
66 ROS_ERROR(
"LaserScanMaskFilter: masks is not defined in the config.");
71 ROS_ERROR(
"LaserScanMaskFilter: masks must be an array of frame_ids with direction list.");
75 it != config.
end(); ++it)
79 std::string frame_id = (std::string)(it->first);
80 masks_[frame_id] = std::vector<size_t>();
83 for (
size_t i = 0; i < it->second.size(); ++i)
85 size_t id =
static_cast<int>(it->second[i]);
86 masks_[frame_id].push_back(
id);
88 ROS_INFO(
"LaserScanMaskFilter: %s: %d directions will be masked.",
89 frame_id.c_str(), (int)masks_[frame_id].size());
105 bool update(
const sensor_msgs::LaserScan& data_in, sensor_msgs::LaserScan& data_out)
108 if (masks_.find(data_out.header.frame_id) == masks_.end())
110 ROS_WARN(
"LaserScanMaskFilter: frame_id %s is not registered.",
111 data_out.header.frame_id.c_str());
115 const std::vector<size_t> &mask = masks_[data_out.header.frame_id];
116 const size_t len = data_out.ranges.size();
117 for (std::vector<size_t>::const_iterator it = mask.begin();
118 it != mask.end(); ++it)
120 if (*it > len)
continue;
121 data_out.ranges[*it] = std::numeric_limits<float>::quiet_NaN();
130 #endif // LASER_FILTERS_SCAN_MASK_FILTER_H
const std::string & getMessage() const
std::map< std::string, std::vector< size_t > > masks_
ValueStruct::iterator iterator
LaserScanMaskFilter removes points on directions defined in a mask from a laser scan.
Type const & getType() const
bool getParam(const std::string &name, std::string &value)
bool update(const sensor_msgs::LaserScan &data_in, sensor_msgs::LaserScan &data_out)
virtual ~LaserScanMaskFilter()