Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009 #include "grid_map_filters/MeanInRadiusFilter.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 MeanInRadiusFilter<T>::MeanInRadiusFilter()
00020 : radius_(0.0)
00021 {
00022 }
00023
00024 template<typename T>
00025 MeanInRadiusFilter<T>::~MeanInRadiusFilter()
00026 {
00027 }
00028
00029 template<typename T>
00030 bool MeanInRadiusFilter<T>::configure()
00031 {
00032 if (!FilterBase < T > ::getParam(std::string("radius"), radius_)) {
00033 ROS_ERROR("MeanInRadius filter did not find parameter `radius`.");
00034 return false;
00035 }
00036
00037 if (radius_ < 0.0) {
00038 ROS_ERROR("MeanInRadius filter: Radius must be greater than zero.");
00039 return false;
00040 }
00041
00042 ROS_DEBUG("Radius = %f.", radius_);
00043
00044 if (!FilterBase < T > ::getParam(std::string("input_layer"), inputLayer_)) {
00045 ROS_ERROR("MeanInRadius filter did not find parameter `input_layer`.");
00046 return false;
00047 }
00048
00049 ROS_DEBUG("MeanInRadius input layer is = %s.", inputLayer_.c_str());
00050
00051 if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) {
00052 ROS_ERROR("MeanInRadius filter did not find parameter `output_layer`.");
00053 return false;
00054 }
00055
00056 ROS_DEBUG("MeanInRadius output_layer = %s.", outputLayer_.c_str());
00057 return true;
00058 }
00059
00060 template<typename T>
00061 bool MeanInRadiusFilter<T>::update(const T& mapIn, T& mapOut)
00062 {
00063
00064 mapOut = mapIn;
00065 mapOut.add(outputLayer_);
00066
00067 double value;
00068
00069
00070 for (grid_map::GridMapIterator iterator(mapOut); !iterator.isPastEnd(); ++iterator) {
00071
00072 double valueSum = 0.0;
00073 int counter = 0;
00074
00075 Eigen::Vector2d center;
00076 mapOut.getPosition(*iterator, center);
00077
00078
00079 for (grid_map::CircleIterator submapIterator(mapOut, center, radius_); !submapIterator.isPastEnd();
00080 ++submapIterator) {
00081 if (!mapOut.isValid(*submapIterator, inputLayer_))
00082 continue;
00083 value = mapOut.at(inputLayer_, *submapIterator);
00084 valueSum += value;
00085 counter++;
00086 }
00087
00088 if (counter != 0)
00089 mapOut.at(outputLayer_, *iterator) = valueSum / counter;
00090 }
00091
00092 return true;
00093 }
00094
00095 }
00096
00097 PLUGINLIB_EXPORT_CLASS(grid_map::MeanInRadiusFilter<grid_map::GridMap>, filters::FilterBase<grid_map::GridMap>)