ThresholdFilter.cpp
Go to the documentation of this file.
1 /*
2  * ThresholdFilter.cpp
3  *
4  * Created on: Mar 18, 2015
5  * Author: Martin Wermelinger, 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  : useLowerThreshold_(false),
21  useUpperThreshold_(false),
22  lowerThreshold_(0.0),
23  upperThreshold_(1.0),
24  setTo_(0.5)
25 {
26 }
27 
28 template<typename T>
30 {
31 }
32 
33 template<typename T>
35 {
36  // Load Parameters
37  if (FilterBase<T>::getParam(std::string("lower_threshold"),
38  lowerThreshold_)) {
39  useLowerThreshold_ = true;
40  ROS_DEBUG("lower threshold = %f", lowerThreshold_);
41  }
42 
43  if (FilterBase<T>::getParam(std::string("upper_threshold"),
44  upperThreshold_)) {
45  useUpperThreshold_ = true;
46  ROS_DEBUG("upper threshold = %f", upperThreshold_);
47  }
48 
50  ROS_ERROR(
51  "ThresholdFilter did not find parameter 'lower_threshold' or 'upper_threshold',");
52  return false;
53  }
54 
56  ROS_ERROR(
57  "Set either 'lower_threshold' or 'upper_threshold'! Only one threshold can be used!");
58  return false;
59  }
60 
61  if (!FilterBase<T>::getParam(std::string("set_to"), setTo_)) {
62  ROS_ERROR("ThresholdFilter did not find parameter 'set_to'.");
63  return false;
64  }
65 
66  if (!FilterBase<T>::getParam(std::string("condition_layer"), conditionLayer_)) {
67  ROS_ERROR("ThresholdFilter did not find parameter 'condition_layer'.");
68  return false;
69  }
70 
71  if (!FilterBase<T>::getParam(std::string("output_layer"), outputLayer_)) {
72  ROS_ERROR("ThresholdFilter did not find parameter 'ouput_layer'.");
73  return false;
74  }
75 
76  return true;
77 }
78 
79 template<typename T>
80 bool ThresholdFilter<T>::update(const T& mapIn, T& mapOut)
81 {
82  mapOut = mapIn;
83 
84  // Check if layer exists.
85  if (!mapOut.exists(conditionLayer_)) {
86  ROS_ERROR("Check your condition_layer! Layer %s does not exist", conditionLayer_.c_str());
87  return false;
88  }
89 
90  if (!mapOut.exists(outputLayer_)) {
91  ROS_ERROR("Check your output_layer! Layer %s does not exist", outputLayer_.c_str());
92  return false;
93  }
94 
95  // For each cell in map.
96  const auto& condition = mapOut[conditionLayer_];
97  auto& data = mapOut[outputLayer_];
98  for (grid_map::GridMapIterator iterator(mapOut); !iterator.isPastEnd(); ++iterator) {
99  const size_t i = iterator.getLinearIndex();
100  const float& conditionValue = condition(i);
101  float& outputValue = data(i);
102  // If the condition_value is nan, the output will also be set to the setTo value (NaN comparisons evaluate to false).
103  if (useLowerThreshold_ && !(conditionValue >= lowerThreshold_)) {
104  outputValue = setTo_;
105  }
106  if (useUpperThreshold_ && !(conditionValue <= upperThreshold_)) {
107  outputValue = setTo_;
108  }
109  }
110 
111  return true;
112 }
113 
114 } /* namespace */
115 
116 // Explicitly define the specialization for GridMap to have the filter implementation available for testing.
118 // Export the filter.
virtual bool update(const T &mapIn, T &mapOut)
double setTo_
If threshold triggered set to this value.
double upperThreshold_
Upper Threshold.
std::string conditionLayer_
Layer the threshold will be evaluated.
double lowerThreshold_
Lower Threshold.
bool useLowerThreshold_
Booleans to decide which threshold should be used.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
#define ROS_ERROR(...)
std::string outputLayer_
Layer the threshold should be applied to.
#define ROS_DEBUG(...)


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