35 #ifndef LASER_SCAN_RANGE_FILTER_H 36 #define LASER_SCAN_RANGE_FILTER_H 43 #include <dynamic_reconfigure/server.h> 44 #include <laser_filters/RangeFilterConfig.h> 46 #include "sensor_msgs/LaserScan.h" 53 std::shared_ptr<dynamic_reconfigure::Server<RangeFilterConfig>>
dyn_server_;
56 RangeFilterConfig
config_ = RangeFilterConfig::__getDefault__();
62 dyn_server_.reset(
new dynamic_reconfigure::Server<RangeFilterConfig>(own_mutex_, private_nh));
63 dynamic_reconfigure::Server<RangeFilterConfig>::CallbackType
f;
65 dyn_server_->setCallback(f);
67 getParam(
"lower_threshold", config_.lower_threshold);
68 getParam(
"upper_threshold", config_.upper_threshold);
69 getParam(
"use_message_range_limits", config_.use_message_range_limits);
70 getParam(
"lower_replacement_value", config_.lower_replacement_value);
71 getParam(
"upper_replacement_value", config_.upper_replacement_value);
73 dyn_server_->updateConfig(config_);
82 bool update(
const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan)
84 double lower_threshold = config_.lower_threshold;
85 double upper_threshold = config_.upper_threshold;
87 if (config_.use_message_range_limits)
89 lower_threshold = input_scan.range_min;
90 upper_threshold = input_scan.range_max;
92 filtered_scan = input_scan;
93 for (
unsigned int i=0;
94 i < input_scan.ranges.size();
98 if (filtered_scan.ranges[i] <= lower_threshold)
100 filtered_scan.ranges[i] = config_.lower_replacement_value;
103 else if (filtered_scan.ranges[i] >= upper_threshold)
105 filtered_scan.ranges[i] = config_.upper_replacement_value;
120 #endif // LASER_SCAN_RANGE_FILTER_H boost::recursive_mutex own_mutex_
LaserScanMaskFilter removes points on directions defined in a mask from a laser scan.
const std::string & getName() const
std::shared_ptr< dynamic_reconfigure::Server< RangeFilterConfig > > dyn_server_
RangeFilterConfig config_
void reconfigureCB(RangeFilterConfig &config, uint32_t level)
virtual ~LaserScanRangeFilter()
bool getParam(const std::string &name, std::string &value) const
bool update(const sensor_msgs::LaserScan &input_scan, sensor_msgs::LaserScan &filtered_scan)