diff --git a/grid_map_core/CMakeLists.txt b/grid_map_core/CMakeLists.txt index cbf8ff5da..749c2500a 100644 --- a/grid_map_core/CMakeLists.txt +++ b/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 ) diff --git a/grid_map_core/include/grid_map_core/GridMap.hpp b/grid_map_core/include/grid_map_core/GridMap.hpp index 25902b5b0..337ec9d76 100644 --- a/grid_map_core/include/grid_map_core/GridMap.hpp +++ b/grid_map_core/include/grid_map_core/GridMap.hpp @@ -164,6 +164,13 @@ class GridMap */ void setBasicLayers(const std::vector & 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 & 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 basicLayers_; + //! Names of layers that are not basic but still require rolling movement + std::vector rollMapLayers_; + //! Side length of the map in x- and y-direction [m]. Length length_; diff --git a/grid_map_core/src/GridMap.cpp b/grid_map_core/src/GridMap.cpp index cbee49dff..a9abc0a6a 100644 --- a/grid_map_core/src/GridMap.cpp +++ b/grid_map_core/src/GridMap.cpp @@ -75,6 +75,11 @@ void GridMap::setBasicLayers(const std::vector & basicLayers) basicLayers_ = basicLayers; } +void GridMap::setRollingMapLayers(const std::vector & rollMapLayers) +{ + rollMapLayers_=rollMapLayers; +} + const std::vector & 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 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 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 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 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_; } diff --git a/grid_map_filters/include/grid_map_filters/NormalVectorsFilter.hpp b/grid_map_filters/include/grid_map_filters/NormalVectorsFilter.hpp index e5b2ab8a7..5accf3d12 100644 --- a/grid_map_filters/include/grid_map_filters/NormalVectorsFilter.hpp +++ b/grid_map_filters/include/grid_map_filters/NormalVectorsFilter.hpp @@ -199,6 +199,9 @@ class NormalVectorsFilter : public filters::FilterBase //! Grid Map Resolution. double gridMapResolution_; + + //! Factor used to update submap radius + double factor_; }; } // namespace grid_map diff --git a/grid_map_filters/src/NormalVectorsFilter.cpp b/grid_map_filters/src/NormalVectorsFilter.cpp index 5889244d8..7a08b8020 100644 --- a/grid_map_filters/src/NormalVectorsFilter.cpp +++ b/grid_map_filters/src/NormalVectorsFilter.cpp @@ -69,6 +69,13 @@ bool NormalVectorsFilter::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,