Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009 #include "grid_map_filters/ThresholdFilter.hpp"
00010
00011 #include <grid_map_core/grid_map_core.hpp>
00012 #include <pluginlib/class_list_macros.h>
00013
00014 using namespace filters;
00015
00016 namespace grid_map {
00017
00018 template<typename T>
00019 ThresholdFilter<T>::ThresholdFilter()
00020 : useLowerThreshold_(false),
00021 useUpperThreshold_(false),
00022 lowerThreshold_(0.0),
00023 upperThreshold_(1.0),
00024 setTo_(0.5)
00025 {
00026 }
00027
00028 template<typename T>
00029 ThresholdFilter<T>::~ThresholdFilter()
00030 {
00031 }
00032
00033 template<typename T>
00034 bool ThresholdFilter<T>::configure()
00035 {
00036
00037 if (FilterBase<T>::getParam(std::string("lower_threshold"),
00038 lowerThreshold_)) {
00039 useLowerThreshold_ = true;
00040 ROS_DEBUG("lower threshold = %f", lowerThreshold_);
00041 }
00042
00043 if (FilterBase<T>::getParam(std::string("upper_threshold"),
00044 upperThreshold_)) {
00045 useUpperThreshold_ = true;
00046 ROS_DEBUG("upper threshold = %f", upperThreshold_);
00047 }
00048
00049 if (!useLowerThreshold_ && !useUpperThreshold_) {
00050 ROS_ERROR(
00051 "ThresholdFilter did not find parameter 'lower_threshold' or 'upper_threshold',");
00052 return false;
00053 }
00054
00055 if (useLowerThreshold_ && useUpperThreshold_) {
00056 ROS_ERROR(
00057 "Set either 'lower_threshold' or 'upper_threshold'! Only one threshold can be used!");
00058 return false;
00059 }
00060
00061 if (!FilterBase<T>::getParam(std::string("set_to"), setTo_)) {
00062 ROS_ERROR("ThresholdFilter did not find parameter 'set_to'.");
00063 return false;
00064 }
00065
00066 if (!FilterBase<T>::getParam(std::string("layer"), layer_)) {
00067 ROS_ERROR("ThresholdFilter did not find parameter 'layer'.");
00068 return false;
00069 }
00070
00071 return true;
00072 }
00073
00074 template<typename T>
00075 bool ThresholdFilter<T>::update(const T& mapIn, T& mapOut)
00076 {
00077 mapOut = mapIn;
00078
00079
00080 if (!mapOut.exists(layer_)) {
00081 ROS_ERROR("Check your threshold types! Type %s does not exist", layer_.c_str());
00082 return false;
00083 }
00084
00085
00086 auto& data = mapOut[layer_];
00087 for (grid_map::GridMapIterator iterator(mapOut); !iterator.isPastEnd(); ++iterator) {
00088 if (!mapOut.isValid(*iterator, layer_)) continue;
00089 const size_t i = iterator.getLinearIndex();
00090 float& value = data(i);
00091 if (useLowerThreshold_) if (value < lowerThreshold_) value = setTo_;
00092 if (useUpperThreshold_) if (value > upperThreshold_) value = setTo_;
00093 }
00094
00095 return true;
00096 }
00097
00098 }
00099
00100 PLUGINLIB_EXPORT_CLASS(grid_map::ThresholdFilter<grid_map::GridMap>, filters::FilterBase<grid_map::GridMap>)