src
median_filter.cpp
Go to the documentation of this file.
1
/*
2
* Copyright (c) 2008 Radu Bogdan Rusu <rusu@cs.tum.edu>
3
*
4
* All rights reserved.
5
*
6
* Redistribution and use in source and binary forms, with or without
7
* modification, are permitted provided that the following conditions are met:
8
*
9
* * Redistributions of source code must retain the above copyright
10
* notice, this list of conditions and the following disclaimer.
11
* * Redistributions in binary form must reproduce the above copyright
12
* notice, this list of conditions and the following disclaimer in the
13
* documentation and/or other materials provided with the distribution.
14
*
15
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25
* POSSIBILITY OF SUCH DAMAGE.
26
*
27
* $Id: $
28
*
29
*/
30
31
#include "
laser_filters/median_filter.h
"
32
33
namespace
laser_filters
34
{
35
LaserMedianFilter::LaserMedianFilter
() :
36
num_ranges_(1), xmlrpc_value_(), range_filter_(NULL), intensity_filter_(NULL)
37
{
38
ROS_WARN
(
"LaserMedianFilter has been deprecated. Please use LaserArrayFilter instead.\n"
);
39
};
40
41
bool
LaserMedianFilter::configure
()
42
{
43
44
if
(!
getParam
(
"internal_filter"
,
xmlrpc_value_
))
45
{
46
ROS_ERROR
(
"Cannot Configure LaserMedianFilter: Didn't find \"internal_filter\" tag within LaserMedianFilter params. Filter definitions needed inside for processing range and intensity"
);
47
return
false
;
48
}
49
50
if
(
range_filter_
)
delete
range_filter_
;
51
range_filter_
=
new
filters::MultiChannelFilterChain<float>
(
"float"
);
52
if
(!
range_filter_
->
configure
(
num_ranges_
,
xmlrpc_value_
))
return
false
;
53
54
if
(
intensity_filter_
)
delete
intensity_filter_
;
55
intensity_filter_
=
new
filters::MultiChannelFilterChain<float>
(
"float"
);
56
if
(!
intensity_filter_
->
configure
(
num_ranges_
,
xmlrpc_value_
))
return
false
;
57
return
true
;
58
};
59
60
LaserMedianFilter::~LaserMedianFilter
()
61
{
62
delete
range_filter_
;
63
delete
intensity_filter_
;
64
};
65
66
bool
LaserMedianFilter::update
(
const
sensor_msgs::LaserScan& scan_in, sensor_msgs::LaserScan& scan_out)
67
{
68
if
(!this->
configured_
)
69
{
70
ROS_ERROR
(
"LaserMedianFilter not configured"
);
71
return
false
;
72
}
73
boost::mutex::scoped_lock lock(
data_lock
);
74
scan_out = scan_in;
75
76
77
if
(scan_in.ranges.size() !=
num_ranges_
)
//Reallocating
78
{
79
ROS_INFO
(
"Laser filter clearning and reallocating due to larger scan size"
);
80
delete
range_filter_
;
81
delete
intensity_filter_
;
82
83
84
num_ranges_
= scan_in.ranges.size();
85
86
range_filter_
=
new
filters::MultiChannelFilterChain<float>
(
"float"
);
87
if
(!
range_filter_
->
configure
(
num_ranges_
,
xmlrpc_value_
))
return
false
;
88
89
intensity_filter_
=
new
filters::MultiChannelFilterChain<float>
(
"float"
);
90
if
(!
intensity_filter_
->
configure
(
num_ranges_
,
xmlrpc_value_
))
return
false
;
91
92
}
93
95
range_filter_
->
update
(scan_in.ranges, scan_out.ranges);
96
intensity_filter_
->
update
(scan_in.intensities, scan_out.intensities);
97
98
99
return
true
;
100
}
101
}
filters::MultiChannelFilterChain::update
bool update(const std::vector< T > &data_in, std::vector< T > &data_out)
filters::MultiChannelFilterChain< float >
filters::MultiChannelFilterChain::configure
bool configure(unsigned int size, std::string param_name, ros::NodeHandle node=ros::NodeHandle())
laser_filters::LaserMedianFilter::intensity_filter_
filters::MultiChannelFilterChain< float > * intensity_filter_
Definition:
median_filter.h:77
laser_filters::LaserMedianFilter::data_lock
boost::mutex data_lock
How many data point are in each row.
Definition:
median_filter.h:71
laser_filters::LaserMedianFilter::configure
bool configure()
Definition:
median_filter.cpp:41
laser_filters::LaserMedianFilter::range_filter_
filters::MultiChannelFilterChain< float > * range_filter_
Definition:
median_filter.h:76
laser_filters::LaserMedianFilter::LaserMedianFilter
LaserMedianFilter()
Constructor.
Definition:
median_filter.cpp:35
laser_filters::LaserMedianFilter::~LaserMedianFilter
~LaserMedianFilter()
Definition:
median_filter.cpp:60
laser_filters::LaserMedianFilter::num_ranges_
unsigned int num_ranges_
How many scans to average over.
Definition:
median_filter.h:69
filters::FilterBase< sensor_msgs::LaserScan >::getParam
bool getParam(const std::string &name, bool &value) const
ROS_WARN
#define ROS_WARN(...)
median_filter.h
filters::FilterBase< sensor_msgs::LaserScan >::configured_
bool configured_
laser_filters::LaserMedianFilter::update
bool update(const sensor_msgs::LaserScan &scan_in, sensor_msgs::LaserScan &scan_out)
Update the filter and get the response.
Definition:
median_filter.cpp:66
laser_filters::LaserMedianFilter::xmlrpc_value_
XmlRpc::XmlRpcValue xmlrpc_value_
Definition:
median_filter.h:74
ROS_ERROR
#define ROS_ERROR(...)
ROS_INFO
#define ROS_INFO(...)
laser_filters
LaserScanMaskFilter removes points on directions defined in a mask from a laser scan.
Definition:
angular_bounds_filter.h:43
laser_filters
Author(s): Tully Foote
autogenerated on Mon Apr 3 2023 02:51:57