Feature/allow map growth

grid_map_core/CMakeLists.txt

@@ -1,9 +1,11 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.10)
 project(grid_map_core)
 
+add_compile_options(-fopenmp)
 ## Find ament_cmake macros and libraries
 find_package(ament_cmake REQUIRED)
 find_package(grid_map_cmake_helpers REQUIRED)
+find_package(OpenMP REQUIRED)
 
 ## Define Eigen addons.
 include(cmake/${PROJECT_NAME}-extras.cmake)
@@ -35,6 +37,13 @@ add_library(${PROJECT_NAME}
   src/iterators/SlidingWindowIterator.cpp
 )
 
+if(OpenMP_FOUND)
+  target_link_libraries(grid_map_core
+    PUBLIC
+      OpenMP::OpenMP_CXX
+  )
+endif()
+
 ## Specify additional locations of header files
 target_include_directories(${PROJECT_NAME}
   PUBLIC
@@ -126,7 +135,7 @@ if(TARGET ${PROJECT_NAME}-test)
 endif()
 
 ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
-ament_export_dependencies(Eigen3)
+ament_export_dependencies(Eigen3 OpenMP)
 ament_package(CONFIG_EXTRAS
   cmake/${PROJECT_NAME}-extras.cmake
 )

grid_map_core/include/grid_map_core/GridMap.hpp

@@ -164,6 +164,13 @@ class GridMap
    */
   void setBasicLayers(const std::vector<std::string> & basicLayers);
 
+  /*!
+   * Set the layers that are not considered basic but still require rolling movement.
+   * By default the list of rolling layers is empty.
+   * @param rollMapLayers the list of rolling layers.
+   */
+  void setRollingMapLayers(const std::vector<std::string> & rollMapLayers);
+
   /*!
    * Gets the names of the basic layers.
    * @return the names of the basic layers.
@@ -567,6 +574,9 @@ class GridMap
   //! Also, the basic layers are set to NAN when clearing the map with `clear()`.
   std::vector<std::string> basicLayers_;
 
+  //! Names of layers that are not basic but still require rolling movement
+  std::vector<std::string> rollMapLayers_;
+
   //! Side length of the map in x- and y-direction [m].
   Length length_;
 

grid_map_core/src/GridMap.cpp

@@ -75,6 +75,11 @@ void GridMap::setBasicLayers(const std::vector<std::string> & basicLayers)
   basicLayers_ = basicLayers;
 }
 
+void GridMap::setRollingMapLayers(const std::vector<std::string> & rollMapLayers)
+{
+  rollMapLayers_=rollMapLayers;
+}
+
 const std::vector<std::string> & GridMap::getBasicLayers() const
 {
   return basicLayers_;
@@ -605,16 +610,22 @@ bool GridMap::addDataFrom(
     }
   }
   // Copy data.
-  for (GridMapIterator iterator(*this); !iterator.isPastEnd(); ++iterator) {
-    if (isValid(*iterator) && !overwriteData) {continue;}
+  std::vector<grid_map::Index> all_indices;
+  for (grid_map::GridMapIterator iterator(*this); !iterator.isPastEnd(); ++iterator) {
+    all_indices.push_back(*iterator);
+  }
+  #pragma omp parallel for
+  for (size_t i = 0; i < all_indices.size(); ++i) {
+    const grid_map::Index& index_i = all_indices[i];
+    if (isValid(index_i) && !overwriteData) {continue;}
     Position position;
-    getPosition(*iterator, position);
+    getPosition(index_i, position);
     Index index;
     if (!other.isInside(position)) {continue;}
     other.getIndex(position, index);
     for (const auto & layer : layers) {
       if (!other.isValid(index, layer)) {continue;}
-      at(layer, *iterator) = other.at(layer, index);
+      at(layer, index_i) = other.at(layer, index);
     }
   }
 
@@ -658,36 +669,22 @@ bool GridMap::extendToInclude(const GridMap & other)
   if (resizeMap) {
     GridMap mapCopy = *this;
     setGeometry(extendedMapLength, resolution_, extendedMapPosition);
-    // Align new map with old one.
-    Vector shift = position_ - mapCopy.getPosition();
-    shift.x() = std::fmod(shift.x(), resolution_);
-    shift.y() = std::fmod(shift.y(), resolution_);
-    if (std::abs(shift.x()) < resolution_ / 2.0) {
-      position_.x() -= shift.x();
-    } else {
-      position_.x() += resolution_ - shift.x();
-    }
-    if (size_.x() % 2 != mapCopy.getSize().x() % 2) {
-      position_.x() += -std::copysign(resolution_ / 2.0, shift.x());
-    }
-    if (std::abs(shift.y()) < resolution_ / 2.0) {
-      position_.y() -= shift.y();
-    } else {
-      position_.y() += resolution_ - shift.y();
-    }
-    if (size_.y() % 2 != mapCopy.getSize().y() % 2) {
-      position_.y() += -std::copysign(resolution_ / 2.0, shift.y());
-    }
     // Copy data.
-    for (GridMapIterator iterator(*this); !iterator.isPastEnd(); ++iterator) {
-      if (isValid(*iterator)) {continue;}
+    std::vector<grid_map::Index> all_indices;
+    for (grid_map::GridMapIterator iterator(*this); !iterator.isPastEnd(); ++iterator) {
+      all_indices.push_back(*iterator);
+    }
+    #pragma omp parallel for
+    for (size_t i = 0; i < all_indices.size(); ++i) {
+      const grid_map::Index& index_i = all_indices[i];
+      if (isValid(index_i)) {continue;}
       Position position;
-      getPosition(*iterator, position);
+      getPosition(index_i, position);
       Index index;
       if (!mapCopy.isInside(position)) {continue;}
       mapCopy.getIndex(position, index);
       for (const auto & layer : layers_) {
-        at(layer, *iterator) = mapCopy.at(layer, index);
+        at(layer, index_i) = mapCopy.at(layer, index);
       }
     }
   }
@@ -856,7 +853,9 @@ void GridMap::clearRows(unsigned int index, unsigned int nRows)
 {
   std::vector<std::string> layersToClear;
   if (basicLayers_.size() > 0) {
-    layersToClear = basicLayers_;
+    layersToClear.reserve(basicLayers_.size() + rollMapLayers_.size());
+    layersToClear.insert(layersToClear.end(), basicLayers_.begin(), basicLayers_.end());
+    layersToClear.insert(layersToClear.end(), rollMapLayers_.begin(), rollMapLayers_.end());
   } else {
     layersToClear = layers_;
   }
@@ -869,7 +868,9 @@ void GridMap::clearCols(unsigned int index, unsigned int nCols)
 {
   std::vector<std::string> layersToClear;
   if (basicLayers_.size() > 0) {
-    layersToClear = basicLayers_;
+    layersToClear.reserve(basicLayers_.size() + rollMapLayers_.size());
+    layersToClear.insert(layersToClear.end(), basicLayers_.begin(), basicLayers_.end());
+    layersToClear.insert(layersToClear.end(), rollMapLayers_.begin(), rollMapLayers_.end());
   } else {
     layersToClear = layers_;
   }

grid_map_filters/include/grid_map_filters/NormalVectorsFilter.hpp

@@ -199,6 +199,9 @@ class NormalVectorsFilter : public filters::FilterBase<T>
 
   //! Grid Map Resolution.
   double gridMapResolution_;
+
+  //! Factor used to update submap radius
+  double factor_;
 };
 
 }  // namespace grid_map

grid_map_filters/src/NormalVectorsFilter.cpp

@@ -69,6 +69,13 @@ bool NormalVectorsFilter<T>::configure()
         "Parameter `radius` is not positive. Switching to raster method.");
       algorithm = "raster";
     }
+    // Define factor parameter
+    if (!param_reader.get(std::string("factor"), factor_)) {
+      RCLCPP_WARN(
+        this->logging_interface_->get_logger(),
+        "Could not find the parameter: `factor`. Switching to raster method.");
+      algorithm = "raster";
+    }
   }
 
   // Read parallelization_enabled to decide whether parallelization has to be used,