Go to the documentation of this file.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
00031 #include "laser_filters/median_filter.h"
00032
00033 namespace laser_filters
00034 {
00035 LaserMedianFilter::LaserMedianFilter() :
00036 num_ranges_(1), xmlrpc_value_(), range_filter_(NULL), intensity_filter_(NULL)
00037 {
00038 ROS_WARN("LaserMedianFilter has been deprecated. Please use LaserArrayFilter instead.\n");
00039 };
00040
00041 bool LaserMedianFilter::configure()
00042 {
00043
00044 if (!getParam("internal_filter", xmlrpc_value_))
00045 {
00046 ROS_ERROR("Cannot Configure LaserMedianFilter: Didn't find \"internal_filter\" tag within LaserMedianFilter params. Filter definitions needed inside for processing range and intensity");
00047 return false;
00048 }
00049
00050 if (range_filter_) delete range_filter_;
00051 range_filter_ = new filters::MultiChannelFilterChain<float>("float");
00052 if (!range_filter_->configure(num_ranges_, xmlrpc_value_)) return false;
00053
00054 if (intensity_filter_) delete intensity_filter_;
00055 intensity_filter_ = new filters::MultiChannelFilterChain<float>("float");
00056 if (!intensity_filter_->configure(num_ranges_, xmlrpc_value_)) return false;
00057 return true;
00058 };
00059
00060 LaserMedianFilter::~LaserMedianFilter()
00061 {
00062 delete range_filter_;
00063 delete intensity_filter_;
00064 };
00065
00066 bool LaserMedianFilter::update(const sensor_msgs::LaserScan& scan_in, sensor_msgs::LaserScan& scan_out)
00067 {
00068 if (!this->configured_)
00069 {
00070 ROS_ERROR("LaserMedianFilter not configured");
00071 return false;
00072 }
00073 boost::mutex::scoped_lock lock(data_lock);
00074 scan_out = scan_in;
00075
00076
00077 if (scan_in.ranges.size() != num_ranges_)
00078 {
00079 ROS_INFO("Laser filter clearning and reallocating due to larger scan size");
00080 delete range_filter_;
00081 delete intensity_filter_;
00082
00083
00084 num_ranges_ = scan_in.ranges.size();
00085
00086 range_filter_ = new filters::MultiChannelFilterChain<float>("float");
00087 if (!range_filter_->configure(num_ranges_, xmlrpc_value_)) return false;
00088
00089 intensity_filter_ = new filters::MultiChannelFilterChain<float>("float");
00090 if (!intensity_filter_->configure(num_ranges_, xmlrpc_value_)) return false;
00091
00092 }
00093
00095 range_filter_->update(scan_in.ranges, scan_out.ranges);
00096 intensity_filter_->update(scan_in.intensities, scan_out.intensities);
00097
00098
00099 return true;
00100 }
00101 }