Merge branch 'feature/elevation_mapping_postprocessing_unit_test' int…

…o 'master'

[elevation_mapping] Postprocessing unit test

Closes #UNKNOWN

GitOrigin-RevId: 5876ae92b2d64766999d6692660cbb5759b5a01b

grid_map_demos/config/filters_demo_filter_chain.yaml

@@ -1,5 +1,8 @@
 grid_map_filters:
-
+  - name: first
+    type: gridMapFilters/MockFilter
+    params:
+      processing_time: 100
   - name: buffer_normalizer
     type: gridMapFilters/BufferNormalizerFilter
 

grid_map_filters/CMakeLists.txt

@@ -80,6 +80,7 @@ add_library(${PROJECT_NAME}
   src/MinInRadiusFilter.cpp
   src/MeanInRadiusFilter.cpp
   src/MedianFillFilter.cpp
+  src/MockFilter.cpp
   src/NormalVectorsFilter.cpp
   src/CurvatureFilter.cpp
   src/NormalColorMapFilter.cpp

grid_map_filters/filter_plugins.xml

@@ -20,6 +20,11 @@
         Fill holes (NaNs) in a layer using the median value inside a radius. Optionally, apply median calculations for values that are already finite.
       </description>
     </class>
+    <class name="gridMapFilters/MockFilter" type="grid_map::MockFilter<grid_map::GridMap>" base_class_type="filters::FilterBase<grid_map::GridMap>" >
+      <description>
+        Does Identity, but allows to set a delay and debug prints. Used for testing purposes.
+      </description>
+    </class>
     <class name="gridMapFilters/NormalVectorsFilter" type="grid_map::NormalVectorsFilter<grid_map::GridMap>" base_class_type="filters::FilterBase<grid_map::GridMap>" >
       <description>
         Compute the normal vectors of a layer in a map.

grid_map_filters/include/grid_map_filters/MockFilter.hpp

@@ -0,0 +1,56 @@
+/*
+ * MockFilter.hpp
+ *
+ *  Created on: Sep 24, 2020
+ *      Author: Magnus Gärtner
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#pragma once
+
+#include <filters/filter_base.h>
+
+#include <string>
+
+namespace grid_map {
+
+/*!
+ * Duplication filter class duplicates a layer of a grid map.
+ */
+template<typename T>
+class MockFilter : public filters::FilterBase<T>
+{
+
+ public:
+  /*!
+   * Constructor
+   */
+  MockFilter();
+
+  /*!
+   * Destructor.
+   */
+  virtual ~MockFilter();
+
+  /*!
+   * Configures the filter from parameters on the parameter server.
+   */
+  virtual bool configure();
+
+  /*!
+   * Copies the input to the output. The time for the update is specified by processingTime_. Optionally the update is logged.
+   * @param mapIn Input.
+   * @param mapOut Output.
+   */
+  virtual bool update(const T& mapIn, T& mapOut);
+
+ private:
+
+  //! Flag indicating wheter to also log on update.
+  bool printName_;
+
+  //! The time [ms] that the update function takes.
+  uint processingTime_;
+};
+
+} /* namespace */

grid_map_filters/src/MockFilter.cpp

@@ -0,0 +1,59 @@
+/*
+ * MockFilter.cpp
+ *
+ *  Created on: Sep 24, 2020
+ *      Author: Magnus Gärtner
+ *   Institute: ETH Zurich, ANYbotics
+ */
+
+#include <grid_map_core/GridMap.hpp>
+#include <pluginlib/class_list_macros.h>
+#include <thread>
+#include <chrono>
+
+#include "grid_map_filters/MockFilter.hpp"
+
+using namespace filters;
+
+namespace grid_map {
+
+template<typename T>
+MockFilter<T>::MockFilter()
+{
+}
+
+template<typename T>
+MockFilter<T>::~MockFilter()
+{
+}
+
+template<typename T>
+bool MockFilter<T>::configure()
+{
+  if (!FilterBase<T>::getParam(std::string("processing_time"), processingTime_)) {
+    ROS_ERROR("MockFilter did not find parameter 'processing_time'.");
+    return false;
+  }
+
+  if (!FilterBase<T>::getParam(std::string("print_name"), printName_)) {
+    ROS_ERROR("MockFilter did not find parameter 'print_name'.");
+    return false;
+  }
+
+  return true;
+}
+
+template<typename T>
+bool MockFilter<T>::update(const T& mapIn, T& mapOut)
+{
+  mapOut = mapIn;
+  if(printName_){
+    ROS_INFO_STREAM(this->getName()<<": update()");
+  }
+  std::this_thread::sleep_for(std::chrono::milliseconds(processingTime_));
+  return true;
+}
+
+} /* namespace */
+
+PLUGINLIB_EXPORT_CLASS(grid_map::MockFilter<grid_map::GridMap>, filters::FilterBase<grid_map::GridMap>)