ColorFillFilter.cpp
Go to the documentation of this file.
1 /*
2  * ColorFillFilter.cpp
3  *
4  * Created on: Sep 14, 2017
5  * Author: Peter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
10 
13 
14 #include <Eigen/Dense>
15 
16 using namespace filters;
17 
18 namespace grid_map {
19 
20 template<typename T>
22  : r_(0.0),
23  g_(0.0),
24  b_(0.0)
25 {
26 }
27 
28 template<typename T>
30 {
31 }
32 
33 template<typename T>
35 {
36  if (!FilterBase < T > ::getParam(std::string("red"), r_)) {
37  ROS_ERROR("Color fill filter did not find parameter `red`.");
38  return false;
39  }
40  ROS_DEBUG("Color fill filter red is = %f.", r_);
41 
42  if (!FilterBase < T > ::getParam(std::string("green"), g_)) {
43  ROS_ERROR("Color fill filter did not find parameter `green`.");
44  return false;
45  }
46  ROS_DEBUG("Color fill filter green is = %f.", g_);
47 
48  if (!FilterBase < T > ::getParam(std::string("blue"), b_)) {
49  ROS_ERROR("Color fill filter did not find parameter `blue`.");
50  return false;
51  }
52  ROS_DEBUG("Color fill filter blue is = %f.", b_);
53 
54  if (!FilterBase < T > ::getParam(std::string("mask_layer"), maskLayer_));
55  ROS_DEBUG("Color fill filter mask_layer = %s.", maskLayer_.c_str());
56 
57  if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) {
58  ROS_ERROR("Color fill filter did not find parameter `output_layer`.");
59  return false;
60  }
61  ROS_DEBUG("Color fill filter output_layer = %s.", outputLayer_.c_str());
62  return true;
63 }
64 
65 template<typename T>
66 bool ColorFillFilter<T>::update(const T& mapIn, T& mapOut)
67 {
68  mapOut = mapIn;
69  const Eigen::Vector3f colorVector(r_, g_, b_);
70  float colorValue;
71  colorVectorToValue(colorVector, colorValue);
72 
73  if (maskLayer_.empty()) {
74  mapOut.add(outputLayer_, colorValue);
75 
76  } else {
77  mapOut.add(outputLayer_);
78  auto& output = mapOut[outputLayer_];
79  auto& mask = mapOut[maskLayer_];
80 
81  // For each cell in map.
82  for (size_t i = 0; i < output.size(); ++i) {
83  output(i) = std::isfinite(mask(i)) ? colorValue : NAN;
84  }
85  }
86  return true;
87 }
88 
89 } /* namespace */
90 
virtual bool update(const T &mapIn, T &mapOut)
std::string outputLayer_
Output layer name.
bool colorVectorToValue(const Eigen::Vector3i &colorVector, unsigned long &colorValue)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
#define ROS_ERROR(...)
std::string maskLayer_
Mask layer name.
#define ROS_DEBUG(...)


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