MeanInRadiusFilter.cpp
Go to the documentation of this file.
1 /*
2  * MeanInRadiusFilter.cpp
3  *
4  * Created on: May 3, 2017
5  * Author: Tanja Baumann, Peter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
10 
13 
14 using namespace filters;
15 
16 namespace grid_map {
17 
18 template<typename T>
20  : radius_(0.0)
21 {
22 }
23 
24 template<typename T>
26 {
27 }
28 
29 template<typename T>
31 {
32  if (!FilterBase < T > ::getParam(std::string("radius"), radius_)) {
33  ROS_ERROR("MeanInRadius filter did not find parameter `radius`.");
34  return false;
35  }
36 
37  if (radius_ < 0.0) {
38  ROS_ERROR("MeanInRadius filter: Radius must be greater than zero.");
39  return false;
40  }
41 
42  ROS_DEBUG("Radius = %f.", radius_);
43 
44  if (!FilterBase < T > ::getParam(std::string("input_layer"), inputLayer_)) {
45  ROS_ERROR("MeanInRadius filter did not find parameter `input_layer`.");
46  return false;
47  }
48 
49  ROS_DEBUG("MeanInRadius input layer is = %s.", inputLayer_.c_str());
50 
51  if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) {
52  ROS_ERROR("MeanInRadius filter did not find parameter `output_layer`.");
53  return false;
54  }
55 
56  ROS_DEBUG("MeanInRadius output_layer = %s.", outputLayer_.c_str());
57  return true;
58 }
59 
60 template<typename T>
61 bool MeanInRadiusFilter<T>::update(const T& mapIn, T& mapOut)
62 {
63  // Add new layers to the elevation map.
64  mapOut = mapIn;
65  mapOut.add(outputLayer_);
66 
67  double value;
68 
69  // First iteration through the elevation map.
70  for (grid_map::GridMapIterator iterator(mapOut); !iterator.isPastEnd(); ++iterator) {
71 
72  double valueSum = 0.0;
73  int counter = 0;
74  // Requested position (center) of circle in map.
75  Eigen::Vector2d center;
76  mapOut.getPosition(*iterator, center);
77 
78  // Find the mean in a circle around the center
79  for (grid_map::CircleIterator submapIterator(mapOut, center, radius_); !submapIterator.isPastEnd();
80  ++submapIterator) {
81  if (!mapOut.isValid(*submapIterator, inputLayer_))
82  continue;
83  value = mapOut.at(inputLayer_, *submapIterator);
84  valueSum += value;
85  counter++;
86  }
87 
88  if (counter != 0)
89  mapOut.at(outputLayer_, *iterator) = valueSum / counter;
90  }
91 
92  return true;
93 }
94 
95 } /* namespace */
96 
double radius_
Radius to take the mean from.
std::string outputLayer_
Output layer name.
virtual bool update(const T &mapIn, T &mapOut)
std::string inputLayer_
Input layer name.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
#define ROS_ERROR(...)
#define ROS_DEBUG(...)


grid_map_filters
Author(s): P├ęter Fankhauser , Martin Wermelinger
autogenerated on Fri Feb 19 2021 03:15:18