MinInRadiusFilter.cpp
Go to the documentation of this file.
1 /*
2  * MinInRadiusFilter.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("MinInRadius filter did not find parameter `radius`.");
34  return false;
35  }
36 
37  if (radius_ < 0.0) {
38  ROS_ERROR("MinInRadius filter: Radius must be greater than zero.");
39  return false;
40  }
41  ROS_DEBUG("Radius = %f.", radius_);
42 
43  if (!FilterBase < T > ::getParam(std::string("input_layer"), inputLayer_)) {
44  ROS_ERROR("MinInRadius filter did not find parameter `input_layer`.");
45  return false;
46  }
47 
48  ROS_DEBUG("MinInRadius input layer is = %s.", inputLayer_.c_str());
49 
50  if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) {
51  ROS_ERROR("Step filter did not find parameter `output_layer`.");
52  return false;
53  }
54 
55  ROS_DEBUG("MinInRadius output_layer = %s.", outputLayer_.c_str());
56  return true;
57 }
58 
59 template<typename T>
60 bool MinInRadiusFilter<T>::update(const T& mapIn, T& mapOut)
61 {
62  // Add new layer to the elevation map.
63  mapOut = mapIn;
64  mapOut.add(outputLayer_);
65 
66  double value;
67 
68  // First iteration through the elevation map.
69  for (grid_map::GridMapIterator iterator(mapOut); !iterator.isPastEnd(); ++iterator) {
70  if (!mapOut.isValid(*iterator, inputLayer_))
71  continue;
72  value = mapOut.at(inputLayer_, *iterator);
73  double valueMin = 0.0;
74 
75  // Requested position (center) of circle in map.
76  Eigen::Vector2d center;
77  mapOut.getPosition(*iterator, center);
78 
79  // Get minimal value in the circular window.
80  bool init = false;
81  for (grid_map::CircleIterator submapIterator(mapOut, center, radius_); !submapIterator.isPastEnd();
82  ++submapIterator) {
83  if (!mapOut.isValid(*submapIterator, inputLayer_))
84  continue;
85  value = mapOut.at(inputLayer_, *submapIterator);
86 
87  if (!init) {
88  valueMin = value;
89  init = true;
90  continue;
91  }
92  if (value < valueMin)
93  valueMin = value;
94  }
95 
96  if (init)
97  mapOut.at(outputLayer_, *iterator) = valueMin;
98  }
99 
100  return true;
101 }
102 
103 } /* namespace */
104 
std::string inputLayer_
Input layer name.
virtual bool update(const T &mapIn, T &mapOut)
std::string outputLayer_
Output layer name.
double radius_
Radius to take the minimum in.
#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