diff --git a/.gitignore b/.gitignore
index 7a3633634..618178ac0 100644
--- a/.gitignore
+++ b/.gitignore
@@ -36,4 +36,5 @@
# OS X
.DS_Store
-grid_map_pcl/data/
+grid_map_pcl/data/*
+!grid_map_pcl/data/input_cloud.pcd
diff --git a/README.md b/README.md
index 839c3bd6a..23a5ea791 100644
--- a/README.md
+++ b/README.md
@@ -11,6 +11,7 @@ Features:
* **Based on Eigen:** Grid map data is stored as [Eigen] data types. Users can apply available Eigen algorithms directly to the map data for versatile and efficient data manipulation.
* **Convenience functions:** Several helper methods allow for convenient and memory safe cell data access. For example, iterator functions for rectangular, circular, polygonal regions and lines are implemented.
* **ROS interface:** Grid maps can be directly converted to and from ROS message types such as PointCloud2, OccupancyGrid, GridCells, and our custom GridMap message. Conversion packages provide compatibility with [costmap_2d], [PCL], and [OctoMap] data types.
+ * Note: Currently, PointCloud2 can only be converted one-way; see this [issue](https://github.com/ANYbotics/grid_map/issues/158) for context.
* **OpenCV interface:** Grid maps can be seamlessly converted from and to [OpenCV] image types to make use of the tools provided by [OpenCV].
* **Visualizations:** The *grid_map_rviz_plugin* renders grid maps as 3d surface plots (height maps) in [RViz]. Additionally, the *grid_map_visualization* package helps to visualize grid maps as point clouds, occupancy grids, grid cells etc.
* **Filters:** The *grid_map_filters* provides are range of filters to process grid maps as a sequence of filters. Parsing of mathematical expressions allows to flexibly setup powerful computations such as thresholding, normal vectors, smoothening, variance, inpainting, and matrix kernel convolutions.
@@ -21,8 +22,8 @@ The source code is released under a [BSD 3-Clause license](LICENSE).
**Author: Péter Fankhauser
Affiliation: [ANYbotics](https://www.anybotics.com/)
-Maintainer: Péter Fankhauser, pfankhauser@anybotics.com
**
-With contributions by: Tanja Baumann, Jeff Delmerico, Remo Diethelm, Perry Franklin, Dominic Jud, Ralph Kaestner, Philipp Krüsi, Alex Millane, Daniel Stonier, Elena Stumm, Martin Wermelinger, Christos Zalidis, Edo Jelavic, Ruben Grandia, Simone Arreghini, Magnus Gärtner
+Maintainer: Maximilian Wulf, mwulf@anybotics.com, Magnus Gärtner, mgaertner@anybotics.com
**
+With contributions by: Simone Arreghini, Tanja Baumann, Jeff Delmerico, Remo Diethelm, Perry Franklin, Magnus Gärtner, Ruben Grandia, Edo Jelavic, Dominic Jud, Ralph Kaestner, Philipp Krüsi, Alex Millane, Daniel Stonier, Elena Stumm, Martin Wermelinger, Christos Zalidis
This projected was initially developed at ETH Zurich (Autonomous Systems Lab & Robotic Systems Lab).
@@ -52,6 +53,20 @@ If you use this work in an academic context, please cite the following publicati
url = {http://www.springer.com/de/book/9783319260525}
}
+## Branches
+
+These branches are currently maintained:
+* ROS 1
+ * [master](https://github.com/ANYbotics/grid_map)
+* ROS 2
+ * [humble](https://github.com/ANYbotics/grid_map/tree/humble)
+ * [iron](https://github.com/ANYbotics/grid_map/tree/iron)
+ * [jazzy](https://github.com/ANYbotics/grid_map/tree/jazzy)
+ * [rolling](https://github.com/ANYbotics/grid_map/tree/rolling)
+
+Pull requests for ROS 1 should target `master`.
+Pull requests for ROS 2 should target `rolling` and will be backported if they do not break ABI.
+
## Documentation
An introduction to the grid map library including a tutorial is given in [this book chapter](http://www.researchgate.net/publication/284415855).
@@ -108,6 +123,7 @@ This repository consists of following packages:
* ***grid_map_filters*** builds on the [ROS Filters] package to process grid maps as a sequence of filters.
* ***grid_map_msgs*** holds the [ROS] message and service definitions around the [grid_map_msg/GridMap] message type.
* ***grid_map_rviz_plugin*** is an [RViz] plugin to visualize grid maps as 3d surface plots (height maps).
+* ***grid_map_sdf*** provides an algorithm to convert an elevation map into a 3D signed distance field.
* ***grid_map_visualization*** contains a node written to convert GridMap messages to other [ROS] message types for example for visualization in [RViz].
Additional conversion packages:
@@ -258,11 +274,21 @@ Beware that while iterators are convenient, it is often the cleanest and most ef
There are two different methods to change the position of the map:
* `setPosition(...)`: Changes the position of the map without changing data stored in the map. This changes the corresponce between the data and the map frame.
-* `move(...)`: Relocates the grid map such that the corresponce between data and the map frame does not change. Data in the overlapping region before and after the position change remains stored. Data that falls outside of the map at its new position is discarded. Cells that cover previously unknown regions are emptied (set to nan). The data storage is implemented as two-dimensional circular buffer to minimize computational effort.
-
-`setPosition(...)` | `move(...)`
-:---: | :---:
- | |
+* `move(...)`:
+ Relocates the region captured by grid map w.r.t. to the static grid map frame. Use this to move the grid map boundaries
+ without relocating the grid map data. Takes care of all the data handling, such that the grid map data is stationary in the grid map
+ frame.
+ - Data in the overlapping region before and after the position change remains stored.
+ - Data that falls outside the map at its new position is discarded.
+ - Cells that cover previously unknown regions are emptied (set to nan).
+ The data storage is implemented as two-dimensional circular buffer to minimize computational effort.
+
+ **Note**: Due to the circular buffer structure, neighbouring indices might not fall close in the map frame.
+ This assumption only holds for indices obtained by getUnwrappedIndex().
+
+ `setPosition(...)` | `move(...)`
+ :---: | :---:
+  | |
## Packages
@@ -273,6 +299,11 @@ This [RViz] plugin visualizes a grid map layer as 3d surface plot (height map).
+### grid_map_sdf
+
+This package provides an efficient algorithm to convert an elevation map into a dense 3D signed distance field. Each point in the 3D grid contains the distance to the closest point in the map together with the gradient.
+
+
### grid_map_visualization
@@ -376,12 +407,13 @@ Several basic filters are provided in the *grid_map_filters* package:
* **`gridMapFilters/ThresholdFilter`**
- Set values below/above a threshold to a specified value.
+ Set values in the output layer to a specified value _if_ the condition_layer is exceeding either the upper or lower threshold (only one threshold at a time).
name: lower_threshold
type: gridMapFilters/ThresholdFilter
params:
- layer: layer_name
+ condition_layer: layer_name
+ output_layer: layer_name
lower_threshold: 0.0 # alternative: upper_threshold
set_to: 0.0 # # Other uses: .nan, .inf
@@ -399,6 +431,7 @@ Several basic filters are provided in the *grid_map_filters* package:
Compute for each _NaN_ cell of a layer the median (of finites) inside a patch with radius.
Optionally, apply median calculations for values that are already finite, the patch radius for these points is given by existing_value_radius.
+ Note that the fill computation is only performed if the fill_mask is valid for that point.
name: median
type: gridMapFilters/MedianFillFilter
@@ -408,6 +441,9 @@ Several basic filters are provided in the *grid_map_filters* package:
fill_hole_radius: 0.11 # in m.
filter_existing_values: false # Default is false. If enabled it also does a median computation for existing values.
existing_value_radius: 0.2 # in m. Note that this option only has an effect if filter_existing_values is set true.
+ fill_mask_layer: fill_mask # A layer that is used to compute which areas to fill. If not present in the input it is automatically computed.
+ debug: false # If enabled, the additional debug_infill_mask_layer is published.
+ debug_infill_mask_layer: infill_mask # Layer used to visualize the intermediate, sparse-outlier removed fill mask. Only published if debug is enabled.
* **`gridMapFilters/NormalVectorsFilter`**
@@ -497,29 +533,31 @@ Additionally, the *grid_map_cv* package provides the following filters:
### Devel Job Status
-| | Indigo | Kinetic | Lunar | Melodic |
-| --- | --- | --- | --- | --- |
-| grid_map | [](http://build.ros.org/job/Idev__grid_map__ubuntu_trusty_amd64/) | [](http://build.ros.org/job/Kdev__grid_map__ubuntu_xenial_amd64/) | [](http://build.ros.org/job/Ldev__grid_map__ubuntu_xenial_amd64/) | [](http://build.ros.org/job/Mdev__grid_map__ubuntu_bionic_amd64/) |
-| doc | [](http://build.ros.org/job/Idoc__grid_map__ubuntu_trusty_amd64/) | [](http://build.ros.org/job/Kdoc__grid_map__ubuntu_xenial_amd64/) | [](http://build.ros.org/job/Ldoc__grid_map__ubuntu_xenial_amd64/) | [](http://build.ros.org/job/Mdoc__grid_map__ubuntu_bionic_amd64/) |
+| | Kinetic | Melodic | Noetic |
+| --- | --- | --- | --- |
+| grid_map | [](http://build.ros.org/job/Kdev__grid_map__ubuntu_xenial_amd64/) | [](http://build.ros.org/job/Mdev__grid_map__ubuntu_bionic_amd64/) | [](http://build.ros.org/job/Mdev__grid_map__ubuntu_focal_armhf__binary/) |
+| doc | [](http://build.ros.org/job/Kdoc__grid_map__ubuntu_xenial_amd64/) | [](http://build.ros.org/job/Mdoc__grid_map__ubuntu_bionic_amd64/) | [](http://build.ros.org/job/Mdoc__grid_map__ubuntu_focal_armhf__binary/) |
### Release Job Status
-| | Indigo | Kinetic | Lunar | Melodic |
-| --- | --- | --- | --- | --- |
-| grid_map | [](http://build.ros.org/job/Ibin_uT64__grid_map__ubuntu_trusty_amd64__binary/) | [](http://build.ros.org/job/Kbin_uX64__grid_map__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Lbin_uX64__grid_map__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map__ubuntu_bionic_amd64__binary/) |
-| grid_map_core | [](http://build.ros.org/job/Ibin_uT64__grid_map_core__ubuntu_trusty_amd64__binary/) | [](http://build.ros.org/job/Kbin_uX64__grid_map_core__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Lbin_uX64__grid_map_core__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_core__ubuntu_bionic_amd64__binary/) |
-| grid_map_costmap_2d | [](http://build.ros.org/job/Ibin_uT64__grid_map_costmap_2d__ubuntu_trusty_amd64__binary/) | [](http://build.ros.org/job/Kbin_uX64__grid_map_costmap_2d__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Lbin_uX64__grid_map_costmap_2d__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_costmap_2d__ubuntu_bionic_amd64__binary/) |
-| grid_map_cv | [](http://build.ros.org/job/Ibin_uT64__grid_map_cv__ubuntu_trusty_amd64__binary/) | [](http://build.ros.org/job/Kbin_uX64__grid_map_cv__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Lbin_uX64__grid_map_cv__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_cv__ubuntu_bionic_amd64__binary/) |
-| grid_map_demos | [](http://build.ros.org/job/Ibin_uT64__grid_map_demos__ubuntu_trusty_amd64__binary/) | [](http://build.ros.org/job/Kbin_uX64__grid_map_demos__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Lbin_uX64__grid_map_demos__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_demos__ubuntu_bionic_amd64__binary/) |
-| grid_map_filters | [](http://build.ros.org/job/Ibin_uT64__grid_map_filters__ubuntu_trusty_amd64__binary/) | [](http://build.ros.org/job/Kbin_uX64__grid_map_filters__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Lbin_uX64__grid_map_filters__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_filters__ubuntu_bionic_amd64__binary/) |
-| grid_map_loader | [](http://build.ros.org/job/Ibin_uT64__grid_map_loader__ubuntu_trusty_amd64__binary/) | [](http://build.ros.org/job/Kbin_uX64__grid_map_loader__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Lbin_uX64__grid_map_loader__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_loader__ubuntu_bionic_amd64__binary/) |
-| grid_map_msgs | [](http://build.ros.org/job/Ibin_uT64__grid_map_msgs__ubuntu_trusty_amd64__binary/) | [](http://build.ros.org/job/Kbin_uX64__grid_map_msgs__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Lbin_uX64__grid_map_msgs__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_msgs__ubuntu_bionic_amd64__binary/) |
-| grid_map_octomap | [](http://build.ros.org/job/Ibin_uT64__grid_map_octomap__ubuntu_trusty_amd64__binary/) | [](http://build.ros.org/job/Kbin_uX64__grid_map_octomap__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Lbin_uX64__grid_map_octomap__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_octomap__ubuntu_bionic_amd64__binary/) |
-| grid_map_pcl | [](http://build.ros.org/job/Ibin_uT64__grid_map_pcl__ubuntu_trusty_amd64__binary/) | [](http://build.ros.org/job/Kbin_uX64__grid_map_pcl__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Lbin_uX64__grid_map_pcl__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_pcl__ubuntu_bionic_amd64__binary/) |
-| grid_map_ros | [](http://build.ros.org/job/Ibin_uT64__grid_map_ros__ubuntu_trusty_amd64__binary/) | [](http://build.ros.org/job/Kbin_uX64__grid_map_ros__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Lbin_uX64__grid_map_ros__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_ros__ubuntu_bionic_amd64__binary/) |
-| grid_map_rviz_plugin | [](http://build.ros.org/job/Ibin_uT64__grid_map_rviz_plugin__ubuntu_trusty_amd64__binary/) | [](http://build.ros.org/job/Kbin_uX64__grid_map_rviz_plugin__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Lbin_uX64__grid_map_rviz_plugin__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_rviz_plugin__ubuntu_bionic_amd64__binary/) |
-| grid_map_sdf | [](http://build.ros.org/job/Ibin_uT64__grid_map_sdf__ubuntu_trusty_amd64__binary/) | [](http://build.ros.org/job/Kbin_uX64__grid_map_sdf__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Lbin_uX64__grid_map_sdf__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_sdf__ubuntu_bionic_amd64__binary/) |
-| grid_map_visualization | [](http://build.ros.org/job/Ibin_uT64__grid_map_visualization__ubuntu_trusty_amd64__binary/) | [](http://build.ros.org/job/Kbin_uX64__grid_map_visualization__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Lbin_uX64__grid_map_visualization__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_visualization__ubuntu_bionic_amd64__binary/) |
+| | Kinetic | Melodic | Noetic |
+| --- | --- | --- | --- |
+| grid_map | [](http://build.ros.org/job/Kbin_uX64__grid_map__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map__ubuntu_bionic_amd64__binary/) | [](http://build.ros.org/job/Nbin_ufhf_uFhf__grid_map__ubuntu_focal_armhf__binary/) |
+| grid_map_core | [](http://build.ros.org/job/Kbin_uX64__grid_map_core__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_core__ubuntu_bionic_amd64__binary/) | [](http://build.ros.org/job/Nbin_ufhf_uFhf__grid_map_core__ubuntu_focal_armhf__binary/) |
+| grid_map_costmap_2d | [](http://build.ros.org/job/Kbin_uX64__grid_map_costmap_2d__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_costmap_2d__ubuntu_bionic_amd64__binary/) | [](http://build.ros.org/job/Nbin_ufhf_uFhf__grid_map_costmap_2d__ubuntu_focal_armhf__binary/) |
+| grid_map_cv | [](http://build.ros.org/job/Kbin_uX64__grid_map_cv__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_cv__ubuntu_bionic_amd64__binary/) | [](http://build.ros.org/job/Nbin_ufhf_uFhf__grid_map_cv__ubuntu_focal_armhf__binary/) |
+| grid_map_demos | [](http://build.ros.org/job/Kbin_uX64__grid_map_demos__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_demos__ubuntu_bionic_amd64__binary/) | [](http://build.ros.org/job/Nbin_ufhf_uFhf__grid_map_demos__ubuntu_focal_armhf__binary/) |
+| grid_map_filters | [](http://build.ros.org/job/Kbin_uX64__grid_map_filters__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_filters__ubuntu_bionic_amd64__binary/) | [](http://build.ros.org/job/Nbin_ufhf_uFhf__grid_map_filters__ubuntu_focal_armhf__binary/) |
+| grid_map_loader | [](http://build.ros.org/job/Kbin_uX64__grid_map_loader__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_loader__ubuntu_bionic_amd64__binary/) | [](http://build.ros.org/job/Nbin_ufhf_uFhf__grid_map_loader__ubuntu_focal_armhf__binary/) |
+| grid_map_msgs | [](http://build.ros.org/job/Kbin_uX64__grid_map_msgs__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_msgs__ubuntu_bionic_amd64__binary/) | [](http://build.ros.org/job/Nbin_ufhf_uFhf__grid_map_msgs__ubuntu_focal_armhf__binary/) |
+| grid_map_octomap | [](http://build.ros.org/job/Kbin_uX64__grid_map_octomap__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_octomap__ubuntu_bionic_amd64__binary/) | [](http://build.ros.org/job/Nbin_ufhf_uFhf__grid_map_octomap__ubuntu_focal_armhf__binary/) |
+| grid_map_pcl | [](http://build.ros.org/job/Kbin_uX64__grid_map_pcl__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_pcl__ubuntu_bionic_amd64__binary/) | [](http://build.ros.org/job/Nbin_ufhf_uFhf__grid_map_pcl__ubuntu_focal_armhf__binary/) |
+| grid_map_ros | [](http://build.ros.org/job/Kbin_uX64__grid_map_ros__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_ros__ubuntu_bionic_amd64__binary/) | [](http://build.ros.org/job/Nbin_ufhf_uFhf__grid_map_ros__ubuntu_focal_armhf__binary/) |
+| grid_map_rviz_plugin | [](http://build.ros.org/job/Kbin_uX64__grid_map_rviz_plugin__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_rviz_plugin__ubuntu_bionic_amd64__binary/) | [](http://build.ros.org/job/Nbin_ufhf_uFhf__grid_map_rviz_plugin__ubuntu_focal_armhf__binary/) |
+| grid_map_sdf | [](http://build.ros.org/job/Kbin_uX64__grid_map_sdf__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_sdf__ubuntu_bionic_amd64__binary/) | [](http://build.ros.org/job/Nbin_ufhf_uFhf__grid_map_sdf__ubuntu_focal_armhf__binary/) |
+| grid_map_visualization | [](http://build.ros.org/job/Kbin_uX64__grid_map_visualization__ubuntu_xenial_amd64__binary/) | [](http://build.ros.org/job/Mbin_uB64__grid_map_visualization__ubuntu_bionic_amd64__binary/) | [](http://build.ros.org/job/Nbin_ufhf_uFhf__grid_map_visualization__ubuntu_focal_armhf__binary/) |
+
+
## Bugs & Feature Requests
diff --git a/grid_map/CHANGELOG.rst b/grid_map/CHANGELOG.rst
index bf72135e9..139fe8762 100644
--- a/grid_map/CHANGELOG.rst
+++ b/grid_map/CHANGELOG.rst
@@ -2,6 +2,12 @@
Changelog for package grid_map
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.7.0 (2022-03-17)
+------------------
+
+1.6.4 (2020-12-04)
+------------------
+
1.6.2 (2019-10-14)
------------------
diff --git a/grid_map/package.xml b/grid_map/package.xml
index a150c1d10..2e9e139dd 100644
--- a/grid_map/package.xml
+++ b/grid_map/package.xml
@@ -1,10 +1,10 @@
grid_map
- 1.6.2
+ 1.7.18
Meta-package for the universal grid map library.
- Maximilian Wulf
- Yoshua Nava
+ Magnus Gaertner
+ Mathieu Agostinucci
BSD
http://github.com/anybotics/grid_map
http://github.com/anybotics/grid_map/issues
diff --git a/grid_map_core/CHANGELOG.rst b/grid_map_core/CHANGELOG.rst
index 413502356..7a4acaefd 100644
--- a/grid_map_core/CHANGELOG.rst
+++ b/grid_map_core/CHANGELOG.rst
@@ -2,6 +2,12 @@
Changelog for package grid_map_core
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.7.0 (2022-03-17)
+------------------
+
+1.6.4 (2020-12-04)
+------------------
+
1.6.2 (2019-10-14)
------------------
* Implements a grid map transformation from one map frame to another map frame given the transform between the frames.
diff --git a/grid_map_core/CMakeLists.txt b/grid_map_core/CMakeLists.txt
index e11f8f39e..b290bbd70 100644
--- a/grid_map_core/CMakeLists.txt
+++ b/grid_map_core/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.5.1)
project(grid_map_core)
set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
-
+add_compile_options(-Wall -Wextra -Wpedantic)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
## Find catkin macros and libraries
diff --git a/grid_map_core/include/grid_map_core/BufferRegion.hpp b/grid_map_core/include/grid_map_core/BufferRegion.hpp
index bbe95c776..7821c2d65 100644
--- a/grid_map_core/include/grid_map_core/BufferRegion.hpp
+++ b/grid_map_core/include/grid_map_core/BufferRegion.hpp
@@ -35,8 +35,8 @@ class BufferRegion
constexpr static unsigned int nQuadrants = 4;
BufferRegion();
- BufferRegion(const Index& startIndex, const Size& size, const BufferRegion::Quadrant& quadrant);
- virtual ~BufferRegion();
+ BufferRegion(Index startIndex, Size size, BufferRegion::Quadrant quadrant);
+ virtual ~BufferRegion() = default;
const Index& getStartIndex() const;
void setStartIndex(const Index& startIndex);
@@ -48,7 +48,7 @@ class BufferRegion
private:
//! Start index (typically top-left) of the buffer region.
- Index staretIndex_;
+ Index startIndex_;
//! Size of the buffer region.
Size size_;
diff --git a/grid_map_core/include/grid_map_core/CubicInterpolation.hpp b/grid_map_core/include/grid_map_core/CubicInterpolation.hpp
index 7d6c6e6ec..360e38442 100644
--- a/grid_map_core/include/grid_map_core/CubicInterpolation.hpp
+++ b/grid_map_core/include/grid_map_core/CubicInterpolation.hpp
@@ -35,7 +35,7 @@ class GridMap;
* necessary for interpolation. These are either 16
* function values in the case of bicubic convolution interpolation
* or function values and their derivatives for the case
- * of standard bicubic inteprolation.
+ * of standard bicubic interpolation.
*/
using FunctionValueMatrix = Eigen::Matrix4d;
@@ -46,7 +46,7 @@ using FunctionValueMatrix = Eigen::Matrix4d;
* @param[in] nElem - number of elements in the container
* @return index that is within [0, nElem-1].
*/
-unsigned int bindIndexToRange(int idReq, unsigned int nElem);
+unsigned int bindIndexToRange(unsigned int idReq, unsigned int nElem);
/*!
@@ -74,7 +74,7 @@ static const Eigen::Matrix4d cubicInterpolationConvolutionMatrix {
-1.0, 3.0, -3.0, 1.0).finished() };
/*
- * Index of the middle knot for bicubic inteprolation. This is
+ * Index of the middle knot for bicubic interpolation. This is
* the function value with subscripts (0,0), i.e. f00 in
* https://en.wikipedia.org/wiki/Bicubic_interpolation
* In the grid map it corresponds to the grid map point closest to the
@@ -100,7 +100,7 @@ bool getNormalizedCoordinates(const GridMap &gridMap, const Position &queriedPos
Position *position);
/*
- * Queries the grid map for function values at the coordiantes which are neccesary for
+ * Queries the grid map for function values at the coordinates which are necessary for
* performing the interpolation. The right function values are then assembled
* in a matrix.
* @param[in] gridMap - grid map with the data
@@ -119,7 +119,7 @@ bool assembleFunctionValueMatrix(const GridMap &gridMap, const std::string &laye
* Performs convolution in 1D. the function requires 4 function values
* to compute the convolution. The result is interpolated data in 1D.
* @param[in] t - normalized coordinate (x or y)
- * @param[in] functionValues - vector of 4 function values neccessary to perform
+ * @param[in] functionValues - vector of 4 function values necessary to perform
* interpolation in 1 dimension.
* @return - interpolated value at normalized coordinate t
*/
@@ -177,7 +177,7 @@ struct DataMatrix
/*
* Interpolation is performed on a unit square.
- * Hence we need to compute 4 corners of that unit square,
+ * Hence, we need to compute 4 corners of that unit square,
* and find their indices in the grid map. IndicesMatrix
* is a container that stores those indices. Each index
* contains two numbers (row number, column number) in the
@@ -313,7 +313,7 @@ double mixedSecondOrderDerivativeAt(const Matrix &layerData, const Index &index,
double evaluatePolynomial(const FunctionValueMatrix &functionValues, double tx, double ty);
/*
- * Assemble function value matrix from small submatrices containing function values
+ * Assemble function value matrix from small sub-matrices containing function values
* or derivative values at the corners of the unit square.
* See https://en.wikipedia.org/wiki/Bicubic_interpolation for details.
*
diff --git a/grid_map_core/include/grid_map_core/GridMap.hpp b/grid_map_core/include/grid_map_core/GridMap.hpp
index 9bc1d78f2..74a62b701 100644
--- a/grid_map_core/include/grid_map_core/GridMap.hpp
+++ b/grid_map_core/include/grid_map_core/GridMap.hpp
@@ -8,13 +8,13 @@
#pragma once
-#include "grid_map_core/TypeDefs.hpp"
-#include "grid_map_core/SubmapGeometry.hpp"
#include "grid_map_core/BufferRegion.hpp"
+#include "grid_map_core/SubmapGeometry.hpp"
+#include "grid_map_core/TypeDefs.hpp"
// STL
-#include
#include
+#include
// Eigen
#include
@@ -37,8 +37,7 @@ class SubmapGeometry;
* - "surface_normal_x", "surface_normal_y", "surface_normal_z"
* etc.
*/
-class GridMap
-{
+class GridMap {
public:
// Type traits for use with template methods/classes using GridMap as a template parameter.
typedef grid_map::DataType DataType;
@@ -48,7 +47,7 @@ class GridMap
* Constructor.
* @param layers a vector of strings containing the definition/description of the data layer.
*/
- GridMap(const std::vector& layers);
+ explicit GridMap(const std::vector& layers);
/*!
* Emtpy constructor.
@@ -74,8 +73,7 @@ class GridMap
* @param resolution the cell size in [m/cell].
* @param position the 2d position of the grid map in the grid map frame [m].
*/
- void setGeometry(const Length& length, const double resolution,
- const Position& position = Position::Zero());
+ void setGeometry(const Length& length, const double resolution, const Position& position = Position::Zero());
/*!
* Set the geometry of the grid map from submap geometry information.
@@ -88,7 +86,7 @@ class GridMap
* @param layer the name of the layer.
* @value value the value to initialize the cells with.
*/
- void add(const std::string& layer, const double value = NAN);
+ void add(const std::string& layer, const float value = NAN);
/*!
* Add a new data layer (if the layer already exists, overwrite its data, otherwise add layer and data).
@@ -127,7 +125,7 @@ class GridMap
* @return grid map data as matrix.
* @throw std::out_of_range if no map layer with name `layer` is present.
*/
- const Matrix& operator [](const std::string& layer) const;
+ const Matrix& operator[](const std::string& layer) const;
/*!
* Returns the grid map data for a layer as non-const. Use this method
@@ -136,7 +134,7 @@ class GridMap
* @return grid map data.
* @throw std::out_of_range if no map layer with name `layer` is present.
*/
- Matrix& operator [](const std::string& layer);
+ Matrix& operator[](const std::string& layer);
/*!
* Removes a layer from the grid map.
@@ -283,8 +281,7 @@ class GridMap
* @param vector the vector with the values of the data type.
* @return true if successful, false if no valid data available.
*/
- bool getVector(const std::string& layerPrefix, const Index& index,
- Eigen::Vector3d& vector) const;
+ bool getVector(const std::string& layerPrefix, const Index& index, Eigen::Vector3d& vector) const;
/*!
* Gets a submap from the map. The requested submap is specified with the requested
@@ -309,12 +306,12 @@ class GridMap
* @param[out] isSuccess true if successful, false otherwise.
* @return submap (is empty if success is false).
*/
- GridMap getSubmap(const Position& position, const Length& length, Index& indexInSubmap,
- bool& isSuccess) const;
+ GridMap getSubmap(const Position& position, const Length& length, Index& indexInSubmap, bool& isSuccess) const;
/*!
* Apply isometric transformation (rotation + offset) to grid map and returns the transformed map.
* Note: The returned map may not have the same length since it's geometric description contains
+ * Note: The transformation will only be applied to the height layer of the grid map, other layers will remain untouched.
* the original map.
* @param[in] transform the requested transformation to apply.
* @param[in] heightLayerName the height layer of the map.
@@ -325,26 +322,33 @@ class GridMap
* @return transformed map.
* @throw std::out_of_range if no map layer with name `heightLayerName` is present.
*/
- GridMap getTransformedMap(const Eigen::Isometry3d& transform, const std::string& heightLayerName,
- const std::string& newFrameId,
+ GridMap getTransformedMap(const Eigen::Isometry3d& transform, const std::string& heightLayerName, const std::string& newFrameId,
const double sampleRatio = 0.0) const;
- /*!
- * Set the position of the grid map.
- * Note: This method does not change the data stored in the grid map and
- * is complementary to the `move(...)` method. For a comparison between
- * the `setPosition` and the `move` method, see the `move_demo_node.cpp`
- * file of the `grid_map_demos` package.
- * @param position the 2d position of the grid map in the grid map frame [m].
- */
- void setPosition(const Position& position);
-
/*!
- * Move the grid map w.r.t. to the grid map frame. Use this to move the grid map
- * boundaries without moving the grid map data. Takes care of all the data handling,
- * such that the grid map data is stationary in the grid map frame.
- * Note: For a comparison between the `setPosition` and the `move` method,
- * see the `move_demo_node.cpp` file of the `grid_map_demos` package.
+ * Set the position of the grid map.
+ * Note: This method does not change the data stored in the grid map and
+ * is complementary to the `move(...)` method. For a comparison between
+ * the `setPosition` and the `move` method, see the `move_demo_node.cpp`
+ * file of the `grid_map_demos` package.
+ * @param position the 2d position of the grid map in the grid map frame [m].
+ */
+ void setPosition(const Position& position);
+
+ /*!
+ * Relocates the region captured by grid map w.r.t. to the static grid map frame. Use this to move the grid map boundaries
+ * without relocating the grid map data. Takes care of all the data handling, such that the grid map data is stationary in the grid map
+ * frame.
+ * - Data in the overlapping region before and after the position change remains stored.
+ * - Data that falls outside the map at its new position is discarded.
+ * - Cells that cover previously unknown regions are emptied (set to nan).
+ * The data storage is implemented as two-dimensional circular buffer to minimize computational effort.
+ *
+ * Note: Due to the circular buffer structure, neighbouring indices might not fall close in the map frame.
+ * This assumption only holds for indices obtained by getUnwrappedIndex().
+ *
+ * Note: For a comparison between the `setPosition` and the `move` method, see the `move_demo_node.cpp` file of the `grid_map_demos` package.
+ *
* @param position the new location of the grid map in the map frame.
* @param newRegions the regions of the newly covered / previously uncovered regions of the buffer.
* @return true if map has been moved, false otherwise.
@@ -369,8 +373,7 @@ class GridMap
* @param layers the layers that are copied if not all layers are used.
* @return true if successful.
*/
- bool addDataFrom(const GridMap& other, bool extendMap,
- bool overwriteData, bool copyAllLayers,
+ bool addDataFrom(const GridMap& other, bool extendMap, bool overwriteData, bool copyAllLayers,
std::vector layers = std::vector());
/*!
@@ -515,7 +518,6 @@ class GridMap
*/
bool atPositionLinearInterpolated(const std::string& layer, const Position& position, float& value) const;
-
/*!
* Get cell data at requested position, cubic convolution
* interpolated from 4x4 cells. At the edge of the map,
@@ -527,8 +529,7 @@ class GridMap
* @param[out] value the data of the cell.
* @return true if bicubic convolution interpolation was successful.
*/
- bool atPositionBicubicConvolutionInterpolated(const std::string& layer, const Position& position,
- float& value) const;
+ bool atPositionBicubicConvolutionInterpolated(const std::string& layer, const Position& position, float& value) const;
/*!
* Get cell data at requested position, cubic interpolated
@@ -541,9 +542,7 @@ class GridMap
* @param[out] value the data of the cell.
* @return true if bicubic interpolation was successful.
*/
- bool atPositionBicubicInterpolated(const std::string& layer, const Position& position,
- float& value) const;
-
+ bool atPositionBicubicInterpolated(const std::string& layer, const Position& position, float& value) const;
/*!
* Resize the buffer.
@@ -580,11 +579,11 @@ class GridMap
//! Size of the buffer (rows and cols of the data structure).
Size size_;
- //! Circular buffer start indeces.
+ //! Circular buffer start indices.
Index startIndex_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
-} /* namespace */
+} // namespace grid_map
diff --git a/grid_map_core/include/grid_map_core/GridMapMath.hpp b/grid_map_core/include/grid_map_core/GridMapMath.hpp
index b45f44aaa..fb1a38eb3 100644
--- a/grid_map_core/include/grid_map_core/GridMapMath.hpp
+++ b/grid_map_core/include/grid_map_core/GridMapMath.hpp
@@ -17,6 +17,12 @@
namespace grid_map {
+union Color
+{
+ unsigned long longColor_;
+ float floatColor_;
+};
+
/*!
* Gets the position of a cell specified by its index in the map frame.
* @param[out] position the position of the center of the cell in the map frame.
@@ -77,7 +83,7 @@ void getPositionOfDataStructureOrigin(const Position& position,
Position& positionOfOrigin);
/*!
- * Computes how many cells/indeces the map is moved based on a position shift in
+ * Computes how many cells/indices the map is moved based on a position shift in
* the grid map frame. Use this function if you are moving the grid map
* and want to ensure that the cells match before and after.
* @param[out] indexShift the corresponding shift of the indices.
@@ -94,7 +100,7 @@ bool getIndexShiftFromPositionShift(Index& indexShift,
* if you are moving the grid map and want to ensure that the cells match
* before and after.
* @param[out] positionShift the corresponding shift in position in the grid map frame.
- * @param[in] indexShift the desired shift of the indeces.
+ * @param[in] indexShift the desired shift of the indices.
* @param[in] resolution the resolution of the map.
* @return true if successful.
*/
@@ -114,7 +120,7 @@ bool checkIfIndexInRange(const Index& index, const Size& bufferSize);
* Bounds an index that runs out of the range of the buffer.
* This means that an index that overflows is stopped at the last valid index.
* This is the 2d version of boundIndexToRange(int&, const int&).
- * @param[in/out] index the indeces that will be bounded to the valid region of the buffer.
+ * @param[in/out] index the indices that will be bounded to the valid region of the buffer.
* @param[in] bufferSize the size of the buffer.
*/
void boundIndexToRange(Index& index, const Size& bufferSize);
@@ -131,7 +137,7 @@ void boundIndexToRange(int& index, const int& bufferSize);
* Wraps an index that runs out of the range of the buffer back into allowed the region.
* This means that an index that overflows is reset to zero.
* This is the 2d version of wrapIndexToRange(int&, const int&).
- * @param[in/out] index the indeces that will be wrapped into the valid region of the buffer.
+ * @param[in/out] index the indices that will be wrapped into the valid region of the buffer.
* @param[in] bufferSize the size of the buffer.
*/
void wrapIndexToRange(Index& index, const Size& bufferSize);
@@ -158,7 +164,7 @@ void boundPositionToRange(Position& position, const Length& mapLength, const Pos
* and the map frame (x/y-coordinate).
* @return the alignment transformation.
*/
-const Eigen::Matrix2i getBufferOrderToMapFrameAlignment();
+Eigen::Matrix2i getBufferOrderToMapFrameAlignment();
/*!
* Given a map and a desired submap (defined by position and size), this function computes
@@ -198,7 +204,7 @@ bool getSubmapInformation(Index& submapTopLeftIndex,
* @param bottomRightIndex the bottom right index in the map.
* @return buffer size for the submap.
*/
-Size getSubmapSizeFromCornerIndeces(const Index& topLeftIndex, const Index& bottomRightIndex,
+Size getSubmapSizeFromCornerIndices(const Index& topLeftIndex, const Index& bottomRightIndex,
const Size& bufferSize, const Index& bufferStartIndex);
/*!
@@ -224,7 +230,7 @@ bool getBufferRegionsForSubmap(std::vector& submapBufferRegions,
* @param[in/out] index the index in the map that is incremented (corrected for the circular buffer).
* @param[in] bufferSize the map buffer size.
* @param[in] bufferStartIndex the map buffer start index.
- * @return true if successfully incremented indeces, false if end of iteration limits are reached.
+ * @return true if successfully incremented indices, false if end of iteration limits are reached.
*/
bool incrementIndex(Index& index, const Size& bufferSize,
const Index& bufferStartIndex = Index::Zero());
@@ -243,7 +249,7 @@ bool incrementIndex(Index& index, const Size& bufferSize,
* @param[in] submapBufferSize the submap buffer size.
* @param[in] bufferSize the map buffer size.
* @param[in] bufferStartIndex the map buffer start index.
- * @return true if successfully incremented indeces, false if end of iteration limits are reached.
+ * @return true if successfully incremented indices, false if end of iteration limits are reached.
*/
bool incrementIndexForSubmap(Index& submapIndex, Index& index, const Index& submapTopLeftIndex,
const Size& submapBufferSize, const Size& bufferSize,
@@ -278,7 +284,7 @@ Index getBufferIndexFromIndex(const Index& index, const Size& bufferSize, const
* @param[in] (optional) rowMajor if the linear index is generated for row-major format.
* @return the linear 1d index.
*/
-size_t getLinearIndexFromIndex(const Index& index, const Size& bufferSize, const bool rowMajor = false);
+size_t getLinearIndexFromIndex(const Index& index, const Size& bufferSize, bool rowMajor = false);
/*!
* Returns the regular index (2-dim.) corresponding to the linear index (1-dim.) for a given buffer size.
@@ -287,26 +293,7 @@ size_t getLinearIndexFromIndex(const Index& index, const Size& bufferSize, const
* @param[in] (optional) rowMajor if the linear index is generated for row-major format.
* @return the regular 2d index.
*/
-Index getIndexFromLinearIndex(const size_t linearIndex, const Size& bufferSize, const bool rowMajor = false);
-
-/*!
- * Generates a list of indices for a region in the map.
- * @param regionIndex the region top-left index.
- * @param regionSize the region size.
- * @param indices the list of indices of the region.
- */
-void getIndicesForRegion(const Index& regionIndex, const Size& regionSize,
- std::vector indices);
-
-/*!
- * Generates a list of indices for multiple regions in the map.
- * This method makes sure every index is only once contained in the list.
- * @param regionIndeces the regions' top-left index.
- * @param regionSizes the regions' sizes.
- * @param indices the list of indices of the regions.
- */
-void getIndicesForRegions(const std::vector& regionIndeces, const Size& regionSizes,
- std::vector indices);
+Index getIndexFromLinearIndex(size_t linearIndex, const Size& bufferSize, bool rowMajor = false);
/*!
* Transforms an int color value (concatenated RGB values) to an int color vector (RGB from 0-255).
@@ -354,4 +341,4 @@ void colorVectorToValue(const Eigen::Vector3i& colorVector, float& colorValue);
*/
void colorVectorToValue(const Eigen::Vector3f& colorVector, float& colorValue);
-} // namespace
+} // namespace grid_map
diff --git a/grid_map_core/include/grid_map_core/Polygon.hpp b/grid_map_core/include/grid_map_core/Polygon.hpp
index cf486fbd8..d7720fb55 100644
--- a/grid_map_core/include/grid_map_core/Polygon.hpp
+++ b/grid_map_core/include/grid_map_core/Polygon.hpp
@@ -37,11 +37,6 @@ class Polygon
*/
Polygon(std::vector vertices);
- /*!
- * Destructor.
- */
- virtual ~Polygon();
-
/*!
* Check if point is inside polygon.
* @param point the point to be checked.
@@ -60,7 +55,7 @@ class Polygon
* @param index the index of the requested vertex.
* @return the requested vertex.
*/
- const Position& getVertex(const size_t index) const;
+ const Position& getVertex(size_t index) const;
/*!
* Removes all vertices from the polygon.
@@ -72,7 +67,7 @@ class Polygon
* @param index the index of the requested vertex.
* @return the requested vertex.
*/
- const Position& operator [](const size_t index) const;
+ const Position& operator [](size_t index) const;
/*!
* Returns the vertices of the polygon.
@@ -90,7 +85,7 @@ class Polygon
* Set the timestamp of the polygon.
* @param timestamp the timestamp to set (in nanoseconds).
*/
- void setTimestamp(const uint64_t timestamp);
+ void setTimestamp(uint64_t timestamp);
/*!
* Get the timestamp of the polygon.
@@ -156,15 +151,15 @@ class Polygon
* @param margin the margin to offset the polygon by (in [m]).
* @return true if successful, false otherwise.
*/
- bool offsetInward(const double margin);
+ bool offsetInward(double margin);
/*!
- * If only two verices are given, this methods generates a
+ * If only two vertices are given, this methods generates a
* `thickened` line polygon with four vertices.
* @param thickness the desired thickness of the line.
* @return true if successful, false otherwise.
*/
- bool thickenLine(const double thickness);
+ bool thickenLine(double thickness);
/*!
* Return a triangulated version of the polygon.
@@ -179,8 +174,8 @@ class Polygon
* @param[in] nVertices number of vertices of the approximation polygon. Default = 20.
* @return circle as polygon.
*/
- static Polygon fromCircle(const Position center, const double radius,
- const int nVertices = 20);
+ static Polygon fromCircle(Position center, double radius,
+ int nVertices = 20);
/*!
* Approximates two circles with a convex hull and returns it as polygon.
@@ -190,10 +185,10 @@ class Polygon
* @param[in] nVertices number of vertices of the approximation polygon. Default = 20.
* @return convex hull of the two circles as polygon.
*/
- static Polygon convexHullOfTwoCircles(const Position center1,
- const Position center2,
- const double radius,
- const int nVertices = 20);
+ static Polygon convexHullOfTwoCircles(Position center1,
+ Position center2,
+ double radius,
+ int nVertices = 20);
/*!
* Computes the convex hull of two polygons and returns it as polygon.
@@ -237,7 +232,7 @@ class Polygon
static double vectorsMakeClockwiseTurn(const Eigen::Vector2d& pointO,
const Eigen::Vector2d& pointA,
const Eigen::Vector2d& pointB);
-
+ // NOLINTBEGIN(misc-non-private-member-variables-in-classes)
//! Frame id of the polygon.
std::string frameId_;
@@ -246,7 +241,7 @@ class Polygon
//! Vertices of the polygon.
std::vector vertices_;
-
+ // NOLINTEND(misc-non-private-member-variables-in-classes)
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
diff --git a/grid_map_core/include/grid_map_core/SubmapGeometry.hpp b/grid_map_core/include/grid_map_core/SubmapGeometry.hpp
index 02143b5f0..3e7b0487d 100644
--- a/grid_map_core/include/grid_map_core/SubmapGeometry.hpp
+++ b/grid_map_core/include/grid_map_core/SubmapGeometry.hpp
@@ -35,7 +35,7 @@ class SubmapGeometry
SubmapGeometry(const GridMap& gridMap, const Position& position, const Length& length,
bool& isSuccess);
- virtual ~SubmapGeometry();
+ virtual ~SubmapGeometry() = default;
const GridMap& getGridMap() const;
const Length& getLength() const;
diff --git a/grid_map_core/include/grid_map_core/TypeDefs.hpp b/grid_map_core/include/grid_map_core/TypeDefs.hpp
index 7e19c63fc..04a7a36ad 100644
--- a/grid_map_core/include/grid_map_core/TypeDefs.hpp
+++ b/grid_map_core/include/grid_map_core/TypeDefs.hpp
@@ -7,22 +7,22 @@
*/
// Eigen
-#include
-
#pragma once
+#include
+
namespace grid_map {
- typedef Eigen::MatrixXf Matrix;
- typedef Matrix::Scalar DataType;
- typedef Eigen::Vector2d Position;
- typedef Eigen::Vector2d Vector;
- typedef Eigen::Vector3d Position3;
- typedef Eigen::Vector3d Vector3;
- typedef Eigen::Array2i Index;
- typedef Eigen::Array2i Size;
- typedef Eigen::Array2d Length;
- typedef uint64_t Time;
+ using Matrix = Eigen::MatrixXf;
+ using DataType = Matrix::Scalar;
+ using Position = Eigen::Vector2d;
+ using Vector = Eigen::Vector2d;
+ using Position3 = Eigen::Vector3d;
+ using Vector3 = Eigen::Vector3d;
+ using Index = Eigen::Array2i;
+ using Size = Eigen::Array2i;
+ using Length = Eigen::Array2d;
+ using Time = uint64_t;
/*
* Interpolations are ordered in the order
@@ -43,4 +43,5 @@ namespace grid_map {
INTER_CUBIC // standard bicubic interpolation
};
-} /* namespace */
+} // namespace grid_map
+
diff --git a/grid_map_core/include/grid_map_core/eigen_plugins/DenseBasePlugin.hpp b/grid_map_core/include/grid_map_core/eigen_plugins/DenseBasePlugin.hpp
index 8b3dd6f9b..118a6e368 100644
--- a/grid_map_core/include/grid_map_core/eigen_plugins/DenseBasePlugin.hpp
+++ b/grid_map_core/include/grid_map_core/eigen_plugins/DenseBasePlugin.hpp
@@ -1,12 +1,18 @@
+#pragma once
+
Scalar numberOfFinites() const
{
- if (SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0)) return Scalar(0);
+ if (SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0)) {
+ return Scalar(0);
+ }
return Scalar((derived().array() == derived().array()).count());
}
Scalar sumOfFinites() const
{
- if (SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0)) return Scalar(0);
+ if (SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0)) {
+ return Scalar(0);
+ }
return Scalar(this->redux(Eigen::internal::scalar_sum_of_finites_op()));
}
diff --git a/grid_map_core/include/grid_map_core/eigen_plugins/Functors.hpp b/grid_map_core/include/grid_map_core/eigen_plugins/Functors.hpp
index c4963c0fa..1f9f6658e 100644
--- a/grid_map_core/include/grid_map_core/eigen_plugins/Functors.hpp
+++ b/grid_map_core/include/grid_map_core/eigen_plugins/Functors.hpp
@@ -18,11 +18,11 @@ struct Clamp
max_(max)
{
}
- const Scalar operator()(const Scalar& x) const
+ Scalar operator()(const Scalar& x) const
{
return x < min_ ? min_ : (x > max_ ? max_ : x);
}
Scalar min_, max_;
};
-}
+} // namespace grid_map
diff --git a/grid_map_core/include/grid_map_core/eigen_plugins/FunctorsPlugin.hpp b/grid_map_core/include/grid_map_core/eigen_plugins/FunctorsPlugin.hpp
index 79dcdd665..77aa03902 100644
--- a/grid_map_core/include/grid_map_core/eigen_plugins/FunctorsPlugin.hpp
+++ b/grid_map_core/include/grid_map_core/eigen_plugins/FunctorsPlugin.hpp
@@ -1,4 +1,4 @@
-#include
+#include
template struct scalar_sum_of_finites_op {
EIGEN_EMPTY_STRUCT_CTOR(scalar_sum_of_finites_op)
diff --git a/grid_map_core/include/grid_map_core/gtest_eigen.hpp b/grid_map_core/include/grid_map_core/gtest_eigen.hpp
index c061fb103..8872ab8f2 100644
--- a/grid_map_core/include/grid_map_core/gtest_eigen.hpp
+++ b/grid_map_core/include/grid_map_core/gtest_eigen.hpp
@@ -96,8 +96,9 @@ namespace grid_map {
}
template
- void assertFinite(const M1 & A, std::string const & message = "")
+ void assertFinite(const M1 & A, std::string const & /*message*/ = "")
{
+ std::cout << ("test") << std::endl;
for(int r = 0; r < A.rows(); r++)
{
for(int c = 0; c < A.cols(); c++)
@@ -107,58 +108,58 @@ namespace grid_map {
}
}
- inline bool compareRelative(double a, double b, double percentTolerance, double * percentError = NULL)
- {
- // \todo: does anyone have a better idea?
+ inline bool compareRelative(double a, double b, double percentTolerance, double* percentError = nullptr) {
+ // \todo: does anyone have a better idea?
double fa = fabs(a);
double fb = fabs(b);
- if( (fa < 1e-15 && fb < 1e-15) || // Both zero.
- (fa == 0.0 && fb < 1e-6) || // One exactly zero and the other small
- (fb == 0.0 && fa < 1e-6) ) // ditto
- return true;
+ if ((fa < 1e-15 && fb < 1e-15) || // Both zero.
+ (fa == 0.0 && fb < 1e-6) || // One exactly zero and the other small
+ (fb == 0.0 && fa < 1e-6)) { // ditto
+ return true;
+ }
- double diff = fabs(a - b)/std::max(fa,fb);
- if(diff > percentTolerance * 1e-2)
- {
- if(percentError)
- *percentError = diff * 100.0;
- return false;
- }
+ double diff = fabs(a - b) / std::max(fa, fb);
+ if (diff > percentTolerance * 1e-2) {
+ if (percentError != nullptr) {
+ *percentError = diff * 100.0;
+ }
+ return false;
+ }
return true;
}
#define ASSERT_DOUBLE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG) \
- ASSERT_EQ((size_t)(A).rows(), (size_t)(B).rows()) << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \
- ASSERT_EQ((size_t)(A).cols(), (size_t)(B).cols()) << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \
+ ASSERT_EQ((size_t)(A).rows(), (size_t)(B).rows()) << (MSG) << "\nMatrix " << #A << ":\n" << (A) << "\nand matrix " << #B << "\n" << (B) << "\nare not the same size"; \
+ ASSERT_EQ((size_t)(A).cols(), (size_t)(B).cols()) << (MSG) << "\nMatrix " << #A << ":\n" << (A) << "\nand matrix " << #B << "\n" << (B) << "\nare not the same size"; \
for(int r = 0; r < (A).rows(); r++) \
{ \
for(int c = 0; c < (A).cols(); c++) \
{ \
double percentError = 0.0; \
ASSERT_TRUE(grid_map::compareRelative( (A)(r,c), (B)(r,c), PERCENT_TOLERANCE, &percentError)) \
- << MSG << "\nComparing:\n" \
+ << (MSG) << "\nComparing:\n" \
<< #A << "(" << r << "," << c << ") = " << (A)(r,c) << std::endl \
<< #B << "(" << r << "," << c << ") = " << (B)(r,c) << std::endl \
- << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n" \
- << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B; \
+ << "Error was " << percentError << "% > " << (PERCENT_TOLERANCE) << "%\n" \
+ << "\nMatrix " << #A << ":\n" << (A) << "\nand matrix " << #B << "\n" << (B); \
} \
}
#define ASSERT_DOUBLE_SPARSE_MX_EQ(A, B, PERCENT_TOLERANCE, MSG) \
- ASSERT_EQ((size_t)(A).rows(), (size_t)(B).rows()) << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \
- ASSERT_EQ((size_t)(A).cols(), (size_t)(B).cols()) << MSG << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B << "\nare not the same size"; \
+ ASSERT_EQ((size_t)(A).rows(), (size_t)(B).rows()) << (MSG) << "\nMatrix " << #A << ":\n" << (A) << "\nand matrix " << #B << "\n" << (B) << "\nare not the same size"; \
+ ASSERT_EQ((size_t)(A).cols(), (size_t)(B).cols()) << (MSG) << "\nMatrix " << #A << ":\n" << (A) << "\nand matrix " << #B << "\n" << (B) << "\nare not the same size"; \
for(int r = 0; r < (A).rows(); r++) \
{ \
for(int c = 0; c < (A).cols(); c++) \
{ \
double percentError = 0.0; \
ASSERT_TRUE(grid_map::compareRelative( (A).coeff(r,c), (B).coeff(r,c), PERCENT_TOLERANCE, &percentError)) \
- << MSG << "\nComparing:\n" \
+ << (MSG) << "\nComparing:\n" \
<< #A << "(" << r << "," << c << ") = " << (A).coeff(r,c) << std::endl \
<< #B << "(" << r << "," << c << ") = " << (B).coeff(r,c) << std::endl \
- << "Error was " << percentError << "% > " << PERCENT_TOLERANCE << "%\n" \
- << "\nMatrix " << #A << ":\n" << A << "\nand matrix " << #B << "\n" << B; \
+ << "Error was " << percentError << "% > " << (PERCENT_TOLERANCE) << "%\n" \
+ << "\nMatrix " << #A << ":\n" << (A) << "\nand matrix " << #B << "\n" << (B); \
} \
}
-} // namespace
+} // namespace grid_map
diff --git a/grid_map_core/include/grid_map_core/iterators/CircleIterator.hpp b/grid_map_core/include/grid_map_core/iterators/CircleIterator.hpp
index 054819ba6..14f271344 100644
--- a/grid_map_core/include/grid_map_core/iterators/CircleIterator.hpp
+++ b/grid_map_core/include/grid_map_core/iterators/CircleIterator.hpp
@@ -30,14 +30,7 @@ class CircleIterator
* @param center the position of the circle center.
* @param radius the radius of the circle.
*/
- CircleIterator(const GridMap& gridMap, const Position& center, const double radius);
-
- /*!
- * Assignment operator.
- * @param iterator the iterator to copy data from.
- * @return a reference to *this.
- */
- CircleIterator& operator =(const CircleIterator& other);
+ CircleIterator(const GridMap& gridMap, const Position& center, double radius);
/*!
* Compare to another iterator.
@@ -78,7 +71,7 @@ class CircleIterator
* @param[out] startIndex the start index of the submap.
* @param[out] bufferSize the buffer size of the submap.
*/
- void findSubmapParameters(const Position& center, const double radius,
+ void findSubmapParameters(const Position& center, double radius,
Index& startIndex, Size& bufferSize) const;
//! Position of the circle center;
@@ -104,4 +97,5 @@ class CircleIterator
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
-} /* namespace */
+} // namespace grid_map
+
diff --git a/grid_map_core/include/grid_map_core/iterators/EllipseIterator.hpp b/grid_map_core/include/grid_map_core/iterators/EllipseIterator.hpp
index 2506e4ac9..06be4d480 100644
--- a/grid_map_core/include/grid_map_core/iterators/EllipseIterator.hpp
+++ b/grid_map_core/include/grid_map_core/iterators/EllipseIterator.hpp
@@ -32,14 +32,7 @@ class EllipseIterator
* @param length the length of the main axis.
* @param angle the rotation angle of the ellipse (in [rad]).
*/
- EllipseIterator(const GridMap& gridMap, const Position& center, const Length& length, const double rotation = 0.0);
-
- /*!
- * Assignment operator.
- * @param iterator the iterator to copy data from.
- * @return a reference to *this.
- */
- EllipseIterator& operator =(const EllipseIterator& other);
+ EllipseIterator(const GridMap& gridMap, const Position& center, const Length& length, double rotation = 0.0);
/*!
* Compare to another iterator.
@@ -87,7 +80,7 @@ class EllipseIterator
* @param[out] startIndex the start index of the submap.
* @param[out] bufferSize the buffer size of the submap.
*/
- void findSubmapParameters(const Position& center, const Length& length, const double rotation,
+ void findSubmapParameters(const Position& center, const Length& length, double rotation,
Index& startIndex, Size& bufferSize) const;
//! Position of the circle center;
@@ -113,4 +106,5 @@ class EllipseIterator
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
-} /* namespace */
+} // namespace grid_map
+
diff --git a/grid_map_core/include/grid_map_core/iterators/GridMapIterator.hpp b/grid_map_core/include/grid_map_core/iterators/GridMapIterator.hpp
index 2beeca89a..3a3443b15 100644
--- a/grid_map_core/include/grid_map_core/iterators/GridMapIterator.hpp
+++ b/grid_map_core/include/grid_map_core/iterators/GridMapIterator.hpp
@@ -34,13 +34,6 @@ class GridMapIterator
*/
GridMapIterator(const GridMapIterator* other);
- /*!
- * Assignment operator.
- * @param iterator the iterator to copy data from.
- * @return a reference to *this.
- */
- GridMapIterator& operator =(const GridMapIterator& other);
-
/*!
* Compare to another iterator.
* @return whether the current iterator points to a different address than the other one.
@@ -52,7 +45,7 @@ class GridMapIterator
* to which the iterator is pointing at.
* @return the regular index (2-dim.) of the cell on which the iterator is pointing.
*/
- const Index operator *() const;
+ Index operator *() const;
/*!
* Returns the the linear (1-dim.) index of the cell the iterator is pointing at.
@@ -66,7 +59,7 @@ class GridMapIterator
* Retrieve the index as unwrapped index, i.e., as the corresponding index of a
* grid map with no circular buffer offset.
*/
- const Index getUnwrappedIndex() const;
+ Index getUnwrappedIndex() const;
/*!
* Increase the iterator to the next element.
@@ -86,8 +79,8 @@ class GridMapIterator
*/
bool isPastEnd() const;
-protected:
-
+ protected:
+ // NOLINTBEGIN(misc-non-private-member-variables-in-classes)
//! Size of the buffer.
Size size_;
@@ -102,9 +95,11 @@ class GridMapIterator
//! Is iterator out of scope.
bool isPastEnd_;
+ // NOLINTEND(misc-non-private-member-variables-in-classes)
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
-} /* namespace */
+} // namespace grid_map
+
diff --git a/grid_map_core/include/grid_map_core/iterators/LineIterator.hpp b/grid_map_core/include/grid_map_core/iterators/LineIterator.hpp
index d3f23f188..ffbdc9ce1 100644
--- a/grid_map_core/include/grid_map_core/iterators/LineIterator.hpp
+++ b/grid_map_core/include/grid_map_core/iterators/LineIterator.hpp
@@ -40,13 +40,6 @@ class LineIterator
*/
LineIterator(const grid_map::GridMap& gridMap, const Index& start, const Index& end);
- /*!
- * Assignment operator.
- * @param iterator the iterator to copy data from.
- * @return a reference to *this.
- */
- LineIterator& operator =(const LineIterator& other);
-
/*!
* Compare to another iterator.
* @return whether the current iterator points to a different address than the other one.
@@ -96,7 +89,7 @@ class LineIterator
* @param[out] index the index of the moved start position.
* @return true if successful, false otherwise.
*/
- bool getIndexLimitedToMapRange(const grid_map::GridMap& gridMap, const Position& start,
+ static bool getIndexLimitedToMapRange(const grid_map::GridMap& gridMap, const Position& start,
const Position& end, Index& index);
//! Current index.
@@ -116,12 +109,12 @@ class LineIterator
//! Helper variables for Bresenham Line Drawing algorithm.
Size increment1_, increment2_;
- int denominator_, numerator_, numeratorAdd_;
+ int denominator_{0}, numerator_{0}, numeratorAdd_{0};
//! Map information needed to get position from iterator.
Length mapLength_;
Position mapPosition_;
- double resolution_;
+ double resolution_{NAN};
Size bufferSize_;
Index bufferStartIndex_;
@@ -129,4 +122,5 @@ class LineIterator
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
-} /* namespace */
+} // namespace grid_map
+
diff --git a/grid_map_core/include/grid_map_core/iterators/PolygonIterator.hpp b/grid_map_core/include/grid_map_core/iterators/PolygonIterator.hpp
index da68fd65c..6027670ab 100644
--- a/grid_map_core/include/grid_map_core/iterators/PolygonIterator.hpp
+++ b/grid_map_core/include/grid_map_core/iterators/PolygonIterator.hpp
@@ -30,13 +30,6 @@ class PolygonIterator
*/
PolygonIterator(const grid_map::GridMap& gridMap, const grid_map::Polygon& polygon);
- /*!
- * Assignment operator.
- * @param iterator the iterator to copy data from.
- * @return a reference to *this.
- */
- PolygonIterator& operator =(const PolygonIterator& other);
-
/*!
* Compare to another iterator.
* @return whether the current iterator points to a different address than the other one.
@@ -94,4 +87,5 @@ class PolygonIterator
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
-} /* namespace */
+} // namespace grid_map
+
diff --git a/grid_map_core/include/grid_map_core/iterators/SlidingWindowIterator.hpp b/grid_map_core/include/grid_map_core/iterators/SlidingWindowIterator.hpp
index c581f8fbf..42b2a95a2 100644
--- a/grid_map_core/include/grid_map_core/iterators/SlidingWindowIterator.hpp
+++ b/grid_map_core/include/grid_map_core/iterators/SlidingWindowIterator.hpp
@@ -41,32 +41,32 @@ class SlidingWindowIterator : public GridMapIterator
*/
SlidingWindowIterator(const GridMap& gridMap, const std::string& layer,
const EdgeHandling& edgeHandling = EdgeHandling::CROP,
- const size_t windowSize = 3);
+ size_t windowSize = 3);
/*!
* Copy constructor.
* @param other the object to copy.
*/
- SlidingWindowIterator(const SlidingWindowIterator* other);
+ explicit SlidingWindowIterator(const SlidingWindowIterator* other);
/*!
* Set the side length of the moving window (in m).
* @param gridMap the grid map to iterate on.
* @param windowLength the side length of the window (in m).
*/
- void setWindowLength(const GridMap& gridMap, const double windowLength);
+ void setWindowLength(const GridMap& gridMap, double windowLength);
/*!
* Increase the iterator to the next element.
* @return a reference to the updated iterator.
*/
- SlidingWindowIterator& operator ++();
+ SlidingWindowIterator& operator ++() override;
/*!
* Return the data of the sliding window.
* @return the data of the sliding window.
*/
- const Matrix getData() const;
+ Matrix getData() const;
private:
//! Setup members.
@@ -85,10 +85,11 @@ class SlidingWindowIterator : public GridMapIterator
size_t windowSize_;
//! Size of the border of the window around the center cell.
- size_t windowMargin_;
+ size_t windowMargin_{0};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
-} /* namespace */
+} // namespace grid_map
+
diff --git a/grid_map_core/include/grid_map_core/iterators/SpiralIterator.hpp b/grid_map_core/include/grid_map_core/iterators/SpiralIterator.hpp
index b3715cb3d..f1c3966a8 100644
--- a/grid_map_core/include/grid_map_core/iterators/SpiralIterator.hpp
+++ b/grid_map_core/include/grid_map_core/iterators/SpiralIterator.hpp
@@ -29,14 +29,7 @@ class SpiralIterator
* @param center the position of the circle center.
* @param radius the radius of the circle.
*/
- SpiralIterator(const grid_map::GridMap& gridMap, const Eigen::Vector2d& center, const double radius);
-
- /*!
- * Assignment operator.
- * @param iterator the iterator to copy data from.
- * @return a reference to *this.
- */
- SpiralIterator& operator =(const SpiralIterator& other);
+ SpiralIterator(const grid_map::GridMap& gridMap, Eigen::Vector2d center, double radius);
/*!
* Compare to another iterator.
@@ -74,7 +67,7 @@ class SpiralIterator
* Check if index is inside the circle.
* @return true if inside, false otherwise.
*/
- bool isInside(const Index index) const;
+ bool isInside(const Index& index) const;
/*!
* Uses the current distance to get the points of a ring
@@ -82,8 +75,8 @@ class SpiralIterator
*/
void generateRing();
- int signum(const int val) {
- return (0 < val) - (val < 0);
+ static int signum(const int val) {
+ return static_cast(0 < val) - static_cast(val < 0);
}
//! Position of the circle center;
@@ -112,4 +105,5 @@ class SpiralIterator
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
-} /* namespace */
+} // namespace grid_map
+
diff --git a/grid_map_core/include/grid_map_core/iterators/SubmapIterator.hpp b/grid_map_core/include/grid_map_core/iterators/SubmapIterator.hpp
index 9ca7706bd..3cf1a1a48 100644
--- a/grid_map_core/include/grid_map_core/iterators/SubmapIterator.hpp
+++ b/grid_map_core/include/grid_map_core/iterators/SubmapIterator.hpp
@@ -52,13 +52,6 @@ class SubmapIterator
*/
SubmapIterator(const SubmapIterator* other);
- /*!
- * Assignment operator.
- * @param iterator the iterator to copy data from.
- * @return a reference to *this.
- */
- SubmapIterator& operator =(const SubmapIterator& other);
-
/*!
* Compare to another iterator.
* @return whether the current iterator points to a different address than the other one.
@@ -122,4 +115,5 @@ class SubmapIterator
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
-} /* namespace */
+} // namespace grid_map
+
diff --git a/grid_map_core/include/grid_map_core/utils/testing.hpp b/grid_map_core/include/grid_map_core/utils/testing.hpp
new file mode 100644
index 000000000..e997c2f86
--- /dev/null
+++ b/grid_map_core/include/grid_map_core/utils/testing.hpp
@@ -0,0 +1,37 @@
+/**
+ * Copied from ANYbotics/eigen_utils, to avoid testing dependency.
+ */
+
+#pragma once
+
+#include
+#include
+
+#define ASSERT_MATRICES_EQ_WITH_NAN(first, second) assertMatrixesEqualWithNan((first), #first, (second), #second, __LINE__)
+static void assertMatrixesEqualWithNan(Eigen::Ref first, std::string firstName,
+ Eigen::Ref second, std::string secondName, int line) {
+ ASSERT_EQ(first.rows(), second.rows());
+ ASSERT_EQ(first.cols(), second.cols());
+
+ bool matricesAreEqual = true;
+ for (Eigen::Index row = 0; row < first.rows() && matricesAreEqual; ++row) {
+ for (Eigen::Index col = 0; col < first.cols() && matricesAreEqual; ++col) {
+ bool ifRealThenValid = first.block<1, 1>(row, col).isApprox(second.block<1, 1>(row, col));
+ bool bothNaN = std::isnan(first(row, col)) && std::isnan(second(row, col));
+ if (ifRealThenValid || bothNaN) {
+ continue;
+ } else {
+ matricesAreEqual = false;
+ }
+ }
+ }
+
+ Eigen::IOFormat compactFormat(2, 0, ",", "\n", "[", "]");
+ ASSERT_TRUE(matricesAreEqual) // NO LINT
+ << "L. " << std::to_string(line) << ": Matrices are not equal" // NO LINT
+ << "\n" // NO LINT
+ << firstName << "\n" // NO LINT
+ << first.format(compactFormat) << "\n" // NO LINT
+ << secondName << "\n" // NO LINT
+ << second.format(compactFormat) << "\n"; // NO LINT
+}
diff --git a/grid_map_core/package.xml b/grid_map_core/package.xml
index cfeacb11f..445ef048e 100644
--- a/grid_map_core/package.xml
+++ b/grid_map_core/package.xml
@@ -1,10 +1,10 @@
grid_map_core
- 1.6.2
+ 1.7.18
Universal grid map library to manage two-dimensional grid maps with multiple data layers.
- Maximilian Wulf
- Yoshua Nava
+ Magnus Gaertner
+ Mathieu Agostinucci
BSD
http://github.com/anybotics/grid_map
http://github.com/anybotics/grid_map/issues
diff --git a/grid_map_core/src/BufferRegion.cpp b/grid_map_core/src/BufferRegion.cpp
index bc4782b80..4c1e8e616 100644
--- a/grid_map_core/src/BufferRegion.cpp
+++ b/grid_map_core/src/BufferRegion.cpp
@@ -9,32 +9,26 @@
namespace grid_map {
-BufferRegion::BufferRegion() :
- staretIndex_(Index::Zero()),
+BufferRegion::BufferRegion() : startIndex_(Index::Zero()),
size_(Size::Zero()),
quadrant_(BufferRegion::Quadrant::Undefined)
{
}
-BufferRegion::BufferRegion(const Index& index, const Size& size, const BufferRegion::Quadrant& quadrant) :
- staretIndex_(index),
- size_(size),
- quadrant_(quadrant)
-{
-}
-
-BufferRegion::~BufferRegion()
+BufferRegion::BufferRegion(Index index, Size size, BufferRegion::Quadrant quadrant) : startIndex_(std::move(index)),
+ size_(std::move(size)),
+ quadrant_(std::move(quadrant))
{
}
const Index& BufferRegion::getStartIndex() const
{
- return staretIndex_;
+ return startIndex_;
}
-void BufferRegion::setStartIndex(const Index& staretIndex)
+void BufferRegion::setStartIndex(const Index& startIndex)
{
- staretIndex_ = staretIndex;
+ startIndex_ = startIndex;
}
const Size& BufferRegion::getSize() const
diff --git a/grid_map_core/src/CubicInterpolation.cpp b/grid_map_core/src/CubicInterpolation.cpp
index 89ce6b32f..d9eb2fc54 100644
--- a/grid_map_core/src/CubicInterpolation.cpp
+++ b/grid_map_core/src/CubicInterpolation.cpp
@@ -9,25 +9,21 @@
#include "grid_map_core/CubicInterpolation.hpp"
#include "grid_map_core/GridMap.hpp"
-#include "grid_map_core/GridMapMath.hpp"
namespace grid_map {
-unsigned int bindIndexToRange(int idReq, unsigned int nElem)
+unsigned int bindIndexToRange(unsigned int idReq, unsigned int nElem)
{
- if (idReq < 0) {
- return 0;
- }
if (idReq >= nElem) {
- return static_cast(nElem - 1);
+ return (nElem - 1);
}
- return static_cast(idReq);
+ return idReq;
}
-double getLayerValue(const Matrix &layerMat, int rowReq, int colReq)
+double getLayerValue(const Matrix &layerMat, unsigned int rowReq, unsigned int colReq)
{
- const auto numCol = layerMat.cols();
- const auto numRow = layerMat.rows();
+ const auto numCol = static_cast(layerMat.cols());
+ const auto numRow = static_cast(layerMat.rows());
const unsigned int iBoundToRange = bindIndexToRange(rowReq, numRow);
const unsigned int jBoundToRange = bindIndexToRange(colReq, numCol);
return layerMat(iBoundToRange, jBoundToRange);
@@ -35,7 +31,7 @@ double getLayerValue(const Matrix &layerMat, int rowReq, int colReq)
/**
- * BICUBIC CONVLUTION INTERPOLATION ALGORITHM
+ * BICUBIC CONVOLUTION INTERPOLATION ALGORITHM
* also known as piecewise bicubic interpolation,
* it does not ensure continuity of the first derivatives.
* see:
@@ -92,7 +88,7 @@ bool assembleFunctionValueMatrix(const GridMap &gridMap, const std::string &laye
}
const Matrix &layerMatrix = gridMap.get(layer);
- auto f = [&layerMatrix](int rowReq, int colReq) {
+ auto f = [&layerMatrix](unsigned int rowReq, unsigned int colReq) {
double retVal = getLayerValue(layerMatrix, rowReq, colReq);
return retVal;
};
@@ -135,11 +131,7 @@ bool getNormalizedCoordinates(const GridMap &gridMap, const Position &queriedPos
bool getIndicesOfMiddleKnot(const GridMap &gridMap, const Position &queriedPosition, Index *index)
{
-
- if (!gridMap.getIndex(queriedPosition, *index)) {
- return false;
- }
- return true;
+ return gridMap.getIndex(queriedPosition, *index);
}
} /* namespace bicubic_conv */
@@ -195,7 +187,7 @@ bool evaluateBicubicInterpolation(const GridMap &gridMap, const std::string &lay
FunctionValueMatrix functionValues;
assembleFunctionValueMatrix(f, dfx, dfy, ddfxy, &functionValues);
- // get normalized coordiantes
+ // get normalized coordinates
Position normalizedCoordinates;
if (!computeNormalizedCoordinates(gridMap, unitSquareCornerIndices.bottomLeft_, queriedPosition,
&normalizedCoordinates)) {
@@ -264,11 +256,7 @@ bool getUnitSquareCornerIndices(const GridMap &gridMap, const Position &queriedP
bool getClosestPointIndices(const GridMap &gridMap, const Position &queriedPosition, Index *index)
{
-
- if (!gridMap.getIndex(queriedPosition, *index)) {
- return false;
- }
- return true;
+ return gridMap.getIndex(queriedPosition, *index);
}
bool computeNormalizedCoordinates(const GridMap &gridMap, const Index &originIndex,
@@ -298,8 +286,8 @@ bool getFunctionValues(const Matrix &layerData, const IndicesMatrix &indices, Da
void bindIndicesToRange(const GridMap &gridMap, IndicesMatrix *indices)
{
- const int numCol = gridMap.getSize().y();
- const int numRow = gridMap.getSize().x();
+ const unsigned int numCol = gridMap.getSize().y();
+ const unsigned int numRow = gridMap.getSize().x();
//top left
{
@@ -346,10 +334,11 @@ bool getFirstOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indi
double firstOrderDerivativeAt(const Matrix &layerData, const Index &index, Dim2D dim,
double resolution)
{
- const int numCol = layerData.cols();
- const int numRow = layerData.rows();
+ const auto numCol{static_cast(layerData.cols())};
+ const auto numRow{static_cast(layerData.rows())};
- double left, right;
+ double left;
+ double right;
switch (dim) {
case Dim2D::X: {
left = layerData(bindIndexToRange(index.x() + 1, numRow), index.y());
@@ -376,13 +365,12 @@ double firstOrderDerivativeAt(const Matrix &layerData, const Index &index, Dim2D
double mixedSecondOrderDerivativeAt(const Matrix &layerData, const Index &index, double resolution)
{
/*
- * no need for dimensions since the we have to differentiate w.r.t. x and y
+ * no need for dimensions since we have to differentiate w.r.t. x and y
* the order doesn't matter. Derivative values are the same.
* Taken from https://www.mathematik.uni-dortmund.de/~kuzmin/cfdintro/lecture4.pdf
*/
-
- const int numCol = layerData.cols();
- const int numRow = layerData.rows();
+ const auto numCol{static_cast(layerData.cols())};
+ const auto numRow{static_cast(layerData.rows())};
const double f11 = layerData(bindIndexToRange(index.x() - 1, numRow),
bindIndexToRange(index.y() - 1, numCol));
diff --git a/grid_map_core/src/GridMap.cpp b/grid_map_core/src/GridMap.cpp
index 71695a4aa..b72ea4c2a 100644
--- a/grid_map_core/src/GridMap.cpp
+++ b/grid_map_core/src/GridMap.cpp
@@ -13,16 +13,15 @@
#include "grid_map_core/SubmapGeometry.hpp"
#include "grid_map_core/iterators/GridMapIterator.hpp"
-#include
-
-#include
+#include
#include
#include
#include
#include
-using namespace std;
-using namespace grid_map;
+using std::cout;
+using std::endl;
+using std::isfinite;
namespace grid_map {
@@ -57,8 +56,6 @@ void GridMap::setGeometry(const Length& length, const double resolution, const P
length_ = (size_.cast() * resolution_).matrix();
position_ = position;
startIndex_.setZero();
-
- return;
}
void GridMap::setGeometry(const SubmapGeometry& geometry) {
@@ -74,19 +71,15 @@ const std::vector& GridMap::getBasicLayers() const {
}
bool GridMap::hasBasicLayers() const {
- return basicLayers_.size() > 0;
+ return !basicLayers_.empty();
}
bool GridMap::hasSameLayers(const GridMap& other) const {
- for (const auto& layer : layers_) {
- if (!other.exists(layer)) {
- return false;
- }
- }
- return true;
+ return std::all_of(layers_.begin(), layers_.end(),
+ [&](const std::string& layer){return other.exists(layer);});
}
-void GridMap::add(const std::string& layer, const double value) {
+void GridMap::add(const std::string& layer, const float value) {
add(layer, Matrix::Constant(size_(0), size_(1), value));
}
@@ -177,6 +170,7 @@ float GridMap::atPosition(const std::string& layer, const Position& position, In
interpolationMethod = InterpolationMethods::INTER_LINEAR;
skipNextSwitchCase = true;
}
+ [[fallthrough]];
}
case InterpolationMethods::INTER_CUBIC: {
if (!skipNextSwitchCase) {
@@ -187,20 +181,25 @@ float GridMap::atPosition(const std::string& layer, const Position& position, In
interpolationMethod = InterpolationMethods::INTER_LINEAR;
}
}
+ [[fallthrough]];
}
case InterpolationMethods::INTER_LINEAR: {
float value;
- if (atPositionLinearInterpolated(layer, position, value))
+ if (atPositionLinearInterpolated(layer, position, value)){
return value;
- else
+ }
+ else {
interpolationMethod = InterpolationMethods::INTER_NEAREST;
+ }
+ [[fallthrough]];
}
case InterpolationMethods::INTER_NEAREST: {
Index index;
if (getIndex(position, index)) {
return at(layer, index);
- } else
+ } else {
throw std::out_of_range("GridMap::atPosition(...) : Position is out of range.");
+ }
break;
}
default:
@@ -254,12 +253,8 @@ bool GridMap::isValid(const Index& index, const std::vector& layers
if (layers.empty()) {
return false;
}
- for (const auto& layer : layers) {
- if (!isValid(index, layer)) {
- return false;
- }
- }
- return true;
+ return std::all_of(layers.begin(), layers.end(),
+ [&](const std::string& layer){return isValid(index, layer);});
}
bool GridMap::getPosition3(const std::string& layer, const Index& index, Position3& position) const {
@@ -276,7 +271,7 @@ bool GridMap::getPosition3(const std::string& layer, const Index& index, Positio
bool GridMap::getVector(const std::string& layerPrefix, const Index& index, Eigen::Vector3d& vector) const {
Eigen::Vector3d temp{at(layerPrefix + "x", index), at(layerPrefix + "y", index), at(layerPrefix + "z", index)};
- if (!isValid(temp[0]) || !isValid(temp[1]) || !isValid(temp[2])) {
+ if (!isValid(temp[0]) || !isValid(temp[1]) || !isValid(temp[2])) { // NOLINT (implicit-float-conversion)
return false;
} else {
vector = temp;
@@ -289,8 +284,8 @@ GridMap GridMap::getSubmap(const Position& position, const Length& length, bool&
return getSubmap(position, length, index, isSuccess);
}
-GridMap GridMap::getSubmap(const Position& position, const Length& length, Index& indexInSubmap, bool& isSuccess) const {
- // Submap the generate.
+GridMap GridMap::getSubmap(const Position& position, const Length& length, Index& /*indexInSubmap*/, bool& isSuccess) const {
+ // Submap to generate.
GridMap submap(layers_);
submap.setBasicLayers(basicLayers_);
submap.setTimestamp(timestamp_);
@@ -299,7 +294,7 @@ GridMap GridMap::getSubmap(const Position& position, const Length& length, Index
// Get submap geometric information.
SubmapGeometry submapInformation(*this, position, length, isSuccess);
if (!isSuccess) {
- return GridMap(layers_);
+ return GridMap{layers_};
}
submap.setGeometry(submapInformation);
submap.startIndex_.setZero(); // Because of the way we copy the data below.
@@ -310,7 +305,7 @@ GridMap GridMap::getSubmap(const Position& position, const Length& length, Index
if (!getBufferRegionsForSubmap(bufferRegions, submapInformation.getStartIndex(), submap.getSize(), size_, startIndex_)) {
cout << "Cannot access submap of this size." << endl;
isSuccess = false;
- return GridMap(layers_);
+ return GridMap{layers_};
}
for (const auto& data : data_) {
@@ -399,10 +394,10 @@ GridMap GridMap::getTransformedMap(const Eigen::Isometry3d& transform, const std
if (sampleRatio > 0.0) {
positionSamples.reserve(5);
positionSamples.push_back(center);
- positionSamples.push_back(Position3(center.x() - sampleLength, center.y(), center.z()));
- positionSamples.push_back(Position3(center.x() + sampleLength, center.y(), center.z()));
- positionSamples.push_back(Position3(center.x(), center.y() - sampleLength, center.z()));
- positionSamples.push_back(Position3(center.x(), center.y() + sampleLength, center.z()));
+ positionSamples.emplace_back(center.x() - sampleLength, center.y(), center.z());
+ positionSamples.emplace_back(center.x() + sampleLength, center.y(), center.z());
+ positionSamples.emplace_back(center.x(), center.y() - sampleLength, center.z());
+ positionSamples.emplace_back(center.x(), center.y() + sampleLength, center.z());
} else {
positionSamples.push_back(center);
}
@@ -428,7 +423,7 @@ GridMap GridMap::getTransformedMap(const Eigen::Isometry3d& transform, const std
const auto currentValueInOldGrid = at(layer, *iterator);
auto& newValue = newMap.at(layer, newIndex);
if (layer == heightLayerName) {
- newValue = transformedPosition.z();
+ newValue = static_cast(transformedPosition.z());
} // adjust height
else {
newValue = currentValueInOldGrid;
@@ -457,7 +452,7 @@ bool GridMap::move(const Position& position, std::vector& newRegio
if (abs(indexShift(i)) >= getSize()(i)) {
// Entire map is dropped.
clearAll();
- newRegions.push_back(BufferRegion(Index(0, 0), getSize(), BufferRegion::Quadrant::Undefined));
+ newRegions.emplace_back(Index(0, 0), getSize(), BufferRegion::Quadrant::Undefined);
} else {
// Drop cells out of map.
int sign = (indexShift(i) > 0 ? 1 : -1);
@@ -471,10 +466,10 @@ bool GridMap::move(const Position& position, std::vector& newRegio
// One region to drop.
if (i == 0) {
clearRows(index, nCells);
- newRegions.push_back(BufferRegion(Index(index, 0), Size(nCells, getSize()(1)), BufferRegion::Quadrant::Undefined));
+ newRegions.emplace_back(Index(index, 0), Size(nCells, getSize()(1)), BufferRegion::Quadrant::Undefined);
} else if (i == 1) {
clearCols(index, nCells);
- newRegions.push_back(BufferRegion(Index(0, index), Size(getSize()(0), nCells), BufferRegion::Quadrant::Undefined));
+ newRegions.emplace_back(Index(0, index), Size(getSize()(0), nCells), BufferRegion::Quadrant::Undefined);
}
} else {
// Two regions to drop.
@@ -482,20 +477,20 @@ bool GridMap::move(const Position& position, std::vector& newRegio
int firstNCells = getSize()(i) - firstIndex;
if (i == 0) {
clearRows(firstIndex, firstNCells);
- newRegions.push_back(BufferRegion(Index(firstIndex, 0), Size(firstNCells, getSize()(1)), BufferRegion::Quadrant::Undefined));
+ newRegions.emplace_back(Index(firstIndex, 0), Size(firstNCells, getSize()(1)), BufferRegion::Quadrant::Undefined);
} else if (i == 1) {
clearCols(firstIndex, firstNCells);
- newRegions.push_back(BufferRegion(Index(0, firstIndex), Size(getSize()(0), firstNCells), BufferRegion::Quadrant::Undefined));
+ newRegions.emplace_back(Index(0, firstIndex), Size(getSize()(0), firstNCells), BufferRegion::Quadrant::Undefined);
}
int secondIndex = 0;
int secondNCells = nCells - firstNCells;
if (i == 0) {
clearRows(secondIndex, secondNCells);
- newRegions.push_back(BufferRegion(Index(secondIndex, 0), Size(secondNCells, getSize()(1)), BufferRegion::Quadrant::Undefined));
+ newRegions.emplace_back(Index(secondIndex, 0), Size(secondNCells, getSize()(1)), BufferRegion::Quadrant::Undefined);
} else if (i == 1) {
clearCols(secondIndex, secondNCells);
- newRegions.push_back(BufferRegion(Index(0, secondIndex), Size(getSize()(0), secondNCells), BufferRegion::Quadrant::Undefined));
+ newRegions.emplace_back(Index(0, secondIndex), Size(getSize()(0), secondNCells), BufferRegion::Quadrant::Undefined);
}
}
}
@@ -508,7 +503,7 @@ bool GridMap::move(const Position& position, std::vector& newRegio
position_ += alignedPositionShift;
// Check if map has been moved at all.
- return (indexShift.any() != 0);
+ return indexShift.any();
}
bool GridMap::move(const Position& position) {
@@ -518,10 +513,14 @@ bool GridMap::move(const Position& position) {
bool GridMap::addDataFrom(const GridMap& other, bool extendMap, bool overwriteData, bool copyAllLayers, std::vector layers) {
// Set the layers to copy.
- if (copyAllLayers) layers = other.getLayers();
+ if (copyAllLayers) {
+ layers = other.getLayers();
+ }
// Resize map.
- if (extendMap) extendToInclude(other);
+ if (extendMap) {
+ extendToInclude(other);
+ }
// Check if all layers to copy exist and add missing layers.
for (const auto& layer : layers) {
@@ -531,14 +530,20 @@ bool GridMap::addDataFrom(const GridMap& other, bool extendMap, bool overwriteDa
}
// Copy data.
for (GridMapIterator iterator(*this); !iterator.isPastEnd(); ++iterator) {
- if (isValid(*iterator) && !overwriteData) continue;
+ if (isValid(*iterator) && !overwriteData) {
+ continue;
+ }
Position position;
getPosition(*iterator, position);
Index index;
- if (!other.isInside(position)) continue;
+ if (!other.isInside(position)) {
+ continue;
+ }
other.getIndex(position, index);
for (const auto& layer : layers) {
- if (!other.isValid(index, layer)) continue;
+ if (!other.isValid(index, layer)) {
+ continue;
+ }
at(layer, *iterator) = other.at(layer, index);
}
}
@@ -603,11 +608,15 @@ bool GridMap::extendToInclude(const GridMap& other) {
}
// Copy data.
for (GridMapIterator iterator(*this); !iterator.isPastEnd(); ++iterator) {
- if (isValid(*iterator)) continue;
+ if (isValid(*iterator)) {
+ continue;
+ }
Position position;
getPosition(*iterator, position);
Index index;
- if (!mapCopy.isInside(position)) continue;
+ if (!mapCopy.isInside(position)) {
+ continue;
+ }
mapCopy.getIndex(position, index);
for (const auto& layer : layers_) {
at(layer, *iterator) = mapCopy.at(layer, index);
@@ -666,7 +675,9 @@ bool GridMap::isDefaultStartIndex() const {
}
void GridMap::convertToDefaultStartIndex() {
- if (isDefaultStartIndex()) return;
+ if (isDefaultStartIndex()) {
+ return;
+ }
std::vector bufferRegions;
if (!getBufferRegionsForSubmap(bufferRegions, startIndex_, size_, size_, startIndex_)) {
@@ -696,7 +707,7 @@ void GridMap::convertToDefaultStartIndex() {
}
Position GridMap::getClosestPositionInMap(const Position& position) const {
- if (getSize().x() < 1u || getSize().y() < 1u) {
+ if (getSize().x() < 1 || getSize().y() < 1) {
return position_;
}
@@ -720,12 +731,13 @@ Position GridMap::getClosestPositionInMap(const Position& position) const {
const double maxY = bottomLeftCorner.y();
const double minY = bottomRightCorner.y();
- // Clip to box constraints.
- positionInMap.x() = std::fmin(positionInMap.x(), maxX);
- positionInMap.y() = std::fmin(positionInMap.y(), maxY);
+ // Clip to box constraints and correct for indexing precision.
+ // Points on the border can lead to invalid indices because the cells represent half open intervals, i.e. [...).
+ positionInMap.x() = std::fmin(positionInMap.x(), maxX - std::numeric_limits::epsilon());
+ positionInMap.y() = std::fmin(positionInMap.y(), maxY - std::numeric_limits::epsilon());
- positionInMap.x() = std::fmax(positionInMap.x(), minX);
- positionInMap.y() = std::fmax(positionInMap.y(), minY);
+ positionInMap.x() = std::fmax(positionInMap.x(), minX + std::numeric_limits::epsilon());
+ positionInMap.y() = std::fmax(positionInMap.y(), minY + std::numeric_limits::epsilon());
return positionInMap;
}
@@ -751,23 +763,13 @@ void GridMap::clearAll() {
}
void GridMap::clearRows(unsigned int index, unsigned int nRows) {
- std::vector layersToClear;
- if (basicLayers_.size() > 0)
- layersToClear = basicLayers_;
- else
- layersToClear = layers_;
- for (auto& layer : layersToClear) {
+ for (auto& layer : layers_) {
data_.at(layer).block(index, 0, nRows, getSize()(1)).setConstant(NAN);
}
}
void GridMap::clearCols(unsigned int index, unsigned int nCols) {
- std::vector layersToClear;
- if (basicLayers_.size() > 0)
- layersToClear = basicLayers_;
- else
- layersToClear = layers_;
- for (auto& layer : layersToClear) {
+ for (auto& layer : layers_) {
data_.at(layer).block(0, index, getSize()(0), nCols).setConstant(NAN);
}
}
@@ -828,7 +830,9 @@ bool GridMap::atPositionLinearInterpolated(const std::string& layer, const Posit
for (size_t i = 0; i < 4; ++i) {
const size_t indexLin = getLinearIndexFromIndex(indices[idxShift[i]], mapSize);
- if ((indexLin < startIndexLin) || (indexLin > endIndexLin)) return false;
+ if ((indexLin < startIndexLin) || (indexLin > endIndexLin)) {
+ return false;
+ }
f[i] = layerMat(indexLin);
}
@@ -836,8 +840,9 @@ bool GridMap::atPositionLinearInterpolated(const std::string& layer, const Posit
const Position positionRed = (position - point) / resolution_;
const Position positionRedFlip = Position(1., 1.) - positionRed;
- value = f[0] * positionRedFlip.x() * positionRedFlip.y() + f[1] * positionRed.x() * positionRedFlip.y() +
- f[2] * positionRedFlip.x() * positionRed.y() + f[3] * positionRed.x() * positionRed.y();
+
+ value = f[0] * positionRedFlip.x() * positionRedFlip.y() + f[1] * positionRed.x() * positionRedFlip.y() + // NOLINT (implicit-float-conversion)
+ f[2] * positionRedFlip.x() * positionRed.y() + f[3] * positionRed.x() * positionRed.y(); // NOLINT (implicit-float-conversion)
return true;
}
@@ -860,7 +865,7 @@ bool GridMap::atPositionBicubicConvolutionInterpolated(const std::string& layer,
if (!std::isfinite(interpolatedValue)) {
return false;
}
- value = interpolatedValue;
+ value = static_cast(interpolatedValue);
return true;
}
@@ -876,7 +881,7 @@ bool GridMap::atPositionBicubicInterpolated(const std::string& layer, const Posi
if (!std::isfinite(interpolatedValue)) {
return false;
}
- value = interpolatedValue;
+ value = static_cast(interpolatedValue);
return true;
diff --git a/grid_map_core/src/GridMapMath.cpp b/grid_map_core/src/GridMapMath.cpp
index 1e9ec2c39..b56d618fd 100644
--- a/grid_map_core/src/GridMapMath.cpp
+++ b/grid_map_core/src/GridMapMath.cpp
@@ -14,7 +14,7 @@
// Limits
#include
-using namespace std;
+using std::numeric_limits;
namespace grid_map {
@@ -98,18 +98,34 @@ inline Index getIndexFromIndexVector(
return getBufferIndexFromIndex(index, bufferSize, bufferStartIndex);
}
-inline BufferRegion::Quadrant getQuadrant(const Index& index, const Index& bufferStartIndex)
-{
- if (index[0] >= bufferStartIndex[0] && index[1] >= bufferStartIndex[1]) return BufferRegion::Quadrant::TopLeft;
- if (index[0] >= bufferStartIndex[0] && index[1] < bufferStartIndex[1]) return BufferRegion::Quadrant::TopRight;
- if (index[0] < bufferStartIndex[0] && index[1] >= bufferStartIndex[1]) return BufferRegion::Quadrant::BottomLeft;
- if (index[0] < bufferStartIndex[0] && index[1] < bufferStartIndex[1]) return BufferRegion::Quadrant::BottomRight;
+inline BufferRegion::Quadrant getQuadrant(const Index& index, const Index& bufferStartIndex) {
+ if (index[0] >= bufferStartIndex[0] && index[1] >= bufferStartIndex[1]) {
+ return BufferRegion::Quadrant::TopLeft;
+ }
+ if (index[0] >= bufferStartIndex[0] && index[1] < bufferStartIndex[1]) {
+ return BufferRegion::Quadrant::TopRight;
+ }
+ if (index[0] < bufferStartIndex[0] && index[1] >= bufferStartIndex[1]) {
+ return BufferRegion::Quadrant::BottomLeft;
+ }
+ if (index[0] < bufferStartIndex[0] && index[1] < bufferStartIndex[1]) {
+ return BufferRegion::Quadrant::BottomRight;
+ }
return BufferRegion::Quadrant::Undefined;
}
-} // namespace
+} // namespace internal
-using namespace internal;
+using internal::checkIfStartIndexAtDefaultPosition;
+using internal::getBufferOrderToMapFrameTransformation;
+using internal::getIndexFromIndexVector;
+using internal::getIndexVectorFromIndex;
+using internal::getMapFrameToBufferOrderTransformation;
+using internal::getQuadrant;
+using internal::getVectorToFirstCell;
+using internal::getVectorToOrigin;
+using internal::transformBufferOrderToMapFrame;
+using internal::transformMapFrameToBufferOrder;
bool getPositionFromIndex(Position& position,
const Index& index,
@@ -119,7 +135,9 @@ bool getPositionFromIndex(Position& position,
const Size& bufferSize,
const Index& bufferStartIndex)
{
- if (!checkIfIndexInRange(index, bufferSize)) return false;
+ if (!checkIfIndexInRange(index, bufferSize)) {
+ return false;
+ }
Vector offset;
getVectorToFirstCell(offset, mapLength, resolution);
position = mapPosition + offset + resolution * getIndexVectorFromIndex(index, bufferSize, bufferStartIndex);
@@ -149,11 +167,8 @@ bool checkIfPositionWithinMap(const Position& position,
getVectorToOrigin(offset, mapLength);
Position positionTransformed = getMapFrameToBufferOrderTransformation().cast() * (position - mapPosition - offset);
- if (positionTransformed.x() >= 0.0 && positionTransformed.y() >= 0.0
- && positionTransformed.x() < mapLength(0) && positionTransformed.y() < mapLength(1)) {
- return true;
- }
- return false;
+ return positionTransformed.x() >= 0.0 && positionTransformed.y() >= 0.0
+ && positionTransformed.x() < mapLength(0) && positionTransformed.y() < mapLength(1);
}
void getPositionOfDataStructureOrigin(const Position& position,
@@ -190,11 +205,7 @@ bool getPositionShiftFromIndexShift(Vector& positionShift,
bool checkIfIndexInRange(const Index& index, const Size& bufferSize)
{
- if (index[0] >= 0 && index[1] >= 0 && index[0] < bufferSize[0] && index[1] < bufferSize[1])
- {
- return true;
- }
- return false;
+ return index[0] >= 0 && index[1] >= 0 && index[0] < bufferSize[0] && index[1] < bufferSize[1];
}
void boundIndexToRange(Index& index, const Size& bufferSize)
@@ -206,8 +217,11 @@ void boundIndexToRange(Index& index, const Size& bufferSize)
void boundIndexToRange(int& index, const int& bufferSize)
{
- if (index < 0) index = 0;
- else if (index >= bufferSize) index = bufferSize - 1;
+ if (index < 0) {
+ index = 0;
+ } else if (index >= bufferSize) {
+ index = bufferSize - 1;
+ }
}
void wrapIndexToRange(Index& index, const Size& bufferSize)
@@ -248,7 +262,9 @@ void boundPositionToRange(Position& position, const Length& mapLength, const Pos
for (int i = 0; i < positionShifted.size(); i++) {
double epsilon = 10.0 * numeric_limits::epsilon(); // TODO Why is the factor 10 necessary.
- if (std::fabs(position(i)) > 1.0) epsilon *= std::fabs(position(i));
+ if (std::fabs(position(i)) > 1.0) {
+ epsilon *= std::fabs(position(i));
+ }
if (positionShifted(i) <= 0) {
positionShifted(i) = epsilon;
@@ -263,7 +279,7 @@ void boundPositionToRange(Position& position, const Length& mapLength, const Pos
position = positionShifted + mapPosition - vectorToOrigin;
}
-const Eigen::Matrix2i getBufferOrderToMapFrameAlignment()
+Eigen::Matrix2i getBufferOrderToMapFrameAlignment()
{
return getBufferOrderToMapFrameTransformation().array().abs().matrix();
}
@@ -287,19 +303,25 @@ bool getSubmapInformation(Index& submapTopLeftIndex,
// Corners of submap.
Position topLeftPosition = requestedSubmapPosition - halfTransform * requestedSubmapLength.matrix();
boundPositionToRange(topLeftPosition, mapLength, mapPosition);
- if(!getIndexFromPosition(submapTopLeftIndex, topLeftPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) return false;
+ if (!getIndexFromPosition(submapTopLeftIndex, topLeftPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) {
+ return false;
+ }
Index topLeftIndex;
topLeftIndex = getIndexFromBufferIndex(submapTopLeftIndex, bufferSize, bufferStartIndex);
Position bottomRightPosition = requestedSubmapPosition + halfTransform * requestedSubmapLength.matrix();
boundPositionToRange(bottomRightPosition, mapLength, mapPosition);
Index bottomRightIndex;
- if(!getIndexFromPosition(bottomRightIndex, bottomRightPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) return false;
+ if (!getIndexFromPosition(bottomRightIndex, bottomRightPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) {
+ return false;
+ }
bottomRightIndex = getIndexFromBufferIndex(bottomRightIndex, bufferSize, bufferStartIndex);
// Get the position of the top left corner of the generated submap.
Position topLeftCorner;
- if(!getPositionFromIndex(topLeftCorner, submapTopLeftIndex, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) return false;
+ if (!getPositionFromIndex(topLeftCorner, submapTopLeftIndex, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex)) {
+ return false;
+ }
topLeftCorner -= halfTransform * Position::Constant(resolution);
// Size of submap.
@@ -318,7 +340,7 @@ bool getSubmapInformation(Index& submapTopLeftIndex,
return getIndexFromPosition(requestedIndexInSubmap, requestedSubmapPosition, submapLength, submapPosition, resolution, submapBufferSize);
}
-Size getSubmapSizeFromCornerIndeces(const Index& topLeftIndex, const Index& bottomRightIndex,
+Size getSubmapSizeFromCornerIndices(const Index& topLeftIndex, const Index& bottomRightIndex,
const Size& bufferSize, const Index& bufferStartIndex)
{
const Index unwrappedTopLeftIndex = getIndexFromBufferIndex(topLeftIndex, bufferSize, bufferStartIndex);
@@ -332,7 +354,9 @@ bool getBufferRegionsForSubmap(std::vector& submapBufferRegions,
const Size& bufferSize,
const Index& bufferStartIndex)
{
- if ((getIndexFromBufferIndex(submapIndex, bufferSize, bufferStartIndex) + submapBufferSize > bufferSize).any()) return false;
+ if ((getIndexFromBufferIndex(submapIndex, bufferSize, bufferStartIndex) + submapBufferSize > bufferSize).any()) {
+ return false;
+ }
submapBufferRegions.clear();
@@ -345,87 +369,87 @@ bool getBufferRegionsForSubmap(std::vector& submapBufferRegions,
if (quadrantOfTopLeft == BufferRegion::Quadrant::TopLeft) {
if (quadrantOfBottomRight == BufferRegion::Quadrant::TopLeft) {
- submapBufferRegions.push_back(BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::TopLeft));
+ submapBufferRegions.emplace_back(submapIndex, submapBufferSize, BufferRegion::Quadrant::TopLeft);
return true;
}
if (quadrantOfBottomRight == BufferRegion::Quadrant::TopRight) {
Size topLeftSize(submapBufferSize(0), bufferSize(1) - submapIndex(1));
- submapBufferRegions.push_back(BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft));
+ submapBufferRegions.emplace_back(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft);
Index topRightIndex(submapIndex(0), 0);
Size topRightSize(submapBufferSize(0), submapBufferSize(1) - topLeftSize(1));
- submapBufferRegions.push_back(BufferRegion(topRightIndex, topRightSize, BufferRegion::Quadrant::TopRight));
+ submapBufferRegions.emplace_back(topRightIndex, topRightSize, BufferRegion::Quadrant::TopRight);
return true;
}
if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomLeft) {
Size topLeftSize(bufferSize(0) - submapIndex(0), submapBufferSize(1));
- submapBufferRegions.push_back(BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft));
+ submapBufferRegions.emplace_back(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft);
Index bottomLeftIndex(0, submapIndex(1));
Size bottomLeftSize(submapBufferSize(0) - topLeftSize(0), submapBufferSize(1));
- submapBufferRegions.push_back(BufferRegion(bottomLeftIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft));
+ submapBufferRegions.emplace_back(bottomLeftIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft);
return true;
}
if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
Size topLeftSize(bufferSize(0) - submapIndex(0), bufferSize(1) - submapIndex(1));
- submapBufferRegions.push_back(BufferRegion(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft));
+ submapBufferRegions.emplace_back(submapIndex, topLeftSize, BufferRegion::Quadrant::TopLeft);
Index topRightIndex(submapIndex(0), 0);
Size topRightSize(bufferSize(0) - submapIndex(0), submapBufferSize(1) - topLeftSize(1));
- submapBufferRegions.push_back(BufferRegion(topRightIndex, topRightSize, BufferRegion::Quadrant::TopRight));
+ submapBufferRegions.emplace_back(topRightIndex, topRightSize, BufferRegion::Quadrant::TopRight);
Index bottomLeftIndex(0, submapIndex(1));
Size bottomLeftSize(submapBufferSize(0) - topLeftSize(0), bufferSize(1) - submapIndex(1));
- submapBufferRegions.push_back(BufferRegion(bottomLeftIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft));
+ submapBufferRegions.emplace_back(bottomLeftIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft);
Index bottomRightIndex = Index::Zero();
Size bottomRightSize(bottomLeftSize(0), topRightSize(1));
- submapBufferRegions.push_back(BufferRegion(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight));
+ submapBufferRegions.emplace_back(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight);
return true;
}
} else if (quadrantOfTopLeft == BufferRegion::Quadrant::TopRight) {
if (quadrantOfBottomRight == BufferRegion::Quadrant::TopRight) {
- submapBufferRegions.push_back(BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::TopRight));
+ submapBufferRegions.emplace_back(submapIndex, submapBufferSize, BufferRegion::Quadrant::TopRight);
return true;
}
if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
Size topRightSize(bufferSize(0) - submapIndex(0), submapBufferSize(1));
- submapBufferRegions.push_back(BufferRegion(submapIndex, topRightSize, BufferRegion::Quadrant::TopRight));
+ submapBufferRegions.emplace_back(submapIndex, topRightSize, BufferRegion::Quadrant::TopRight);
Index bottomRightIndex(0, submapIndex(1));
Size bottomRightSize(submapBufferSize(0) - topRightSize(0), submapBufferSize(1));
- submapBufferRegions.push_back(BufferRegion(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight));
+ submapBufferRegions.emplace_back(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight);
return true;
}
} else if (quadrantOfTopLeft == BufferRegion::Quadrant::BottomLeft) {
if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomLeft) {
- submapBufferRegions.push_back(BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::BottomLeft));
+ submapBufferRegions.emplace_back(submapIndex, submapBufferSize, BufferRegion::Quadrant::BottomLeft);
return true;
}
if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
Size bottomLeftSize(submapBufferSize(0), bufferSize(1) - submapIndex(1));
- submapBufferRegions.push_back(BufferRegion(submapIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft));
+ submapBufferRegions.emplace_back(submapIndex, bottomLeftSize, BufferRegion::Quadrant::BottomLeft);
Index bottomRightIndex(submapIndex(0), 0);
Size bottomRightSize(submapBufferSize(0), submapBufferSize(1) - bottomLeftSize(1));
- submapBufferRegions.push_back(BufferRegion(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight));
+ submapBufferRegions.emplace_back(bottomRightIndex, bottomRightSize, BufferRegion::Quadrant::BottomRight);
return true;
}
} else if (quadrantOfTopLeft == BufferRegion::Quadrant::BottomRight) {
if (quadrantOfBottomRight == BufferRegion::Quadrant::BottomRight) {
- submapBufferRegions.push_back(BufferRegion(submapIndex, submapBufferSize, BufferRegion::Quadrant::BottomRight));
+ submapBufferRegions.emplace_back(submapIndex, submapBufferSize, BufferRegion::Quadrant::BottomRight);
return true;
}
@@ -449,7 +473,9 @@ bool incrementIndex(Index& index, const Size& bufferSize, const Index& bufferSta
}
// End of iterations reached.
- if (!checkIfIndexInRange(unwrappedIndex, bufferSize)) return false;
+ if (!checkIfIndexInRange(unwrappedIndex, bufferSize)) {
+ return false;
+ }
// Return true iterated index.
index = getBufferIndexFromIndex(unwrappedIndex, bufferSize, bufferStartIndex);
@@ -475,7 +501,9 @@ bool incrementIndexForSubmap(Index& submapIndex, Index& index, const Index& subm
}
// End of iterations reached.
- if (!checkIfIndexInRange(tempSubmapIndex, submapBufferSize)) return false;
+ if (!checkIfIndexInRange(tempSubmapIndex, submapBufferSize)) {
+ return false;
+ }
// Get corresponding index in map.
Index unwrappedSubmapTopLeftIndex = getIndexFromBufferIndex(submapTopLeftIndex, bufferSize, bufferStartIndex);
@@ -489,7 +517,9 @@ bool incrementIndexForSubmap(Index& submapIndex, Index& index, const Index& subm
Index getIndexFromBufferIndex(const Index& bufferIndex, const Size& bufferSize, const Index& bufferStartIndex)
{
- if (checkIfStartIndexAtDefaultPosition(bufferStartIndex)) return bufferIndex;
+ if (checkIfStartIndexAtDefaultPosition(bufferStartIndex)) {
+ return bufferIndex;
+ }
Index index = bufferIndex - bufferStartIndex;
wrapIndexToRange(index, bufferSize);
@@ -498,7 +528,9 @@ Index getIndexFromBufferIndex(const Index& bufferIndex, const Size& bufferSize,
Index getBufferIndexFromIndex(const Index& index, const Size& bufferSize, const Index& bufferStartIndex)
{
- if (checkIfStartIndexAtDefaultPosition(bufferStartIndex)) return index;
+ if (checkIfStartIndexAtDefaultPosition(bufferStartIndex)) {
+ return index;
+ }
Index bufferIndex = index + bufferStartIndex;
wrapIndexToRange(bufferIndex, bufferSize);
@@ -507,31 +539,20 @@ Index getBufferIndexFromIndex(const Index& index, const Size& bufferSize, const
size_t getLinearIndexFromIndex(const Index& index, const Size& bufferSize, const bool rowMajor)
{
- if (!rowMajor) return index(1) * bufferSize(0) + index(0);
+ if (!rowMajor) {
+ return index(1) * bufferSize(0) + index(0);
+ }
return index(0) * bufferSize(1) + index(1);
}
Index getIndexFromLinearIndex(const size_t linearIndex, const Size& bufferSize, const bool rowMajor)
{
- if (!rowMajor) return Index((int)linearIndex % bufferSize(0), (int)linearIndex / bufferSize(0));
+ if (!rowMajor) {
+ return Index((int)linearIndex % bufferSize(0), (int)linearIndex / bufferSize(0));
+ }
return Index((int)linearIndex / bufferSize(1), (int)linearIndex % bufferSize(1));
}
-void getIndicesForRegion(const Index& regionIndex, const Size& regionSize,
- std::vector indices)
-{
-// for (int i = line.index_; col < line.endIndex(); col++) {
-// for (int i = 0; i < getSize()(0); i++) {
-//
-// }
-// }
-}
-
-void getIndicesForRegions(const std::vector& regionIndeces, const Size& regionSizes,
- std::vector indices)
-{
-}
-
bool colorValueToVector(const unsigned long& colorValue, Eigen::Vector3i& colorVector)
{
colorVector(0) = (colorValue >> 16) & 0x0000ff;
@@ -564,9 +585,9 @@ bool colorVectorToValue(const Eigen::Vector3i& colorVector, unsigned long& color
void colorVectorToValue(const Eigen::Vector3i& colorVector, float& colorValue)
{
- unsigned long color = (colorVector(0) << 16) + (colorVector(1) << 8) + colorVector(2);
- // cppcheck-suppress invalidPointerCast
- colorValue = *reinterpret_cast(&color);
+ Color colors;
+ colors.longColor_ = (colorVector(0) << 16) + (colorVector(1) << 8) + colorVector(2);
+ colorValue = colors.floatColor_;
}
void colorVectorToValue(const Eigen::Vector3f& colorVector, float& colorValue)
@@ -575,5 +596,5 @@ void colorVectorToValue(const Eigen::Vector3f& colorVector, float& colorValue)
colorVectorToValue(tempColorVector, colorValue);
}
-} // namespace
+} // namespace grid_map
diff --git a/grid_map_core/src/Polygon.cpp b/grid_map_core/src/Polygon.cpp
index 715aec4c6..3ab234c1a 100644
--- a/grid_map_core/src/Polygon.cpp
+++ b/grid_map_core/src/Polygon.cpp
@@ -27,12 +27,10 @@ Polygon::Polygon(std::vector vertices)
vertices_ = vertices;
}
-Polygon::~Polygon() {}
-
bool Polygon::isInside(const Position& point) const
{
int cross = 0;
- for (int i = 0, j = vertices_.size() - 1; i < vertices_.size(); j = i++) {
+ for (size_t i = 0, j = vertices_.size() - 1; i < vertices_.size(); j = i++) {
if ( ((vertices_[i].y() > point.y()) != (vertices_[j].y() > point.y()))
&& (point.x() < (vertices_[j].x() - vertices_[i].x()) * (point.y() - vertices_[i].y()) /
(vertices_[j].y() - vertices_[i].y()) + vertices_[i].x()) )
@@ -102,7 +100,7 @@ double Polygon::getArea() const
{
double area = 0.0;
int j = vertices_.size() - 1;
- for (int i = 0; i < vertices_.size(); i++) {
+ for (size_t i = 0; i < vertices_.size(); i++) {
area += (vertices_.at(j).x() + vertices_.at(i).x())
* (vertices_.at(j).y() - vertices_.at(i).y());
j = i;
@@ -116,7 +114,7 @@ Position Polygon::getCentroid() const
std::vector vertices = getVertices();
vertices.push_back(vertices.at(0));
double area = 0.0;
- for (int i = 0; i < vertices.size() - 1; i++) {
+ for (size_t i = 0; i < vertices.size() - 1; i++) {
const double a = vertices[i].x() * vertices[i+1].y() - vertices[i+1].x() * vertices[i].y();
area += a;
centroid.x() += a * (vertices[i].x() + vertices[i+1].x());
@@ -220,7 +218,7 @@ bool Polygon::offsetInward(const double margin)
return true;
}
-std::vector Polygon::triangulate(const TriangulationMethods& method) const
+std::vector Polygon::triangulate(const TriangulationMethods& /*method*/) const
{
// TODO Add more triangulation methods.
// https://en.wikipedia.org/wiki/Polygon_triangulation
@@ -311,7 +309,7 @@ Polygon Polygon::monotoneChainConvexHullOfPoints(const std::vector& po
int k = 0;
// Build lower hull
- for (int i = 0; i < sortedPoints.size(); ++i) {
+ for (size_t i = 0; i < sortedPoints.size(); ++i) {
while (k >= 2 && vectorsMakeClockwiseTurn(pointsConvexHull.at(k - 2), pointsConvexHull.at(k - 1), sortedPoints.at(i))) {
k--;
}
diff --git a/grid_map_core/src/SubmapGeometry.cpp b/grid_map_core/src/SubmapGeometry.cpp
index df2a41eee..065adac29 100644
--- a/grid_map_core/src/SubmapGeometry.cpp
+++ b/grid_map_core/src/SubmapGeometry.cpp
@@ -21,10 +21,6 @@ SubmapGeometry::SubmapGeometry(const GridMap& gridMap, const Position& position,
gridMap_.getSize(), gridMap_.getStartIndex());
}
-SubmapGeometry::~SubmapGeometry()
-{
-}
-
const GridMap& SubmapGeometry::getGridMap() const
{
return gridMap_;
diff --git a/grid_map_core/src/iterators/CircleIterator.cpp b/grid_map_core/src/iterators/CircleIterator.cpp
index ecdc5fe15..b681b33ac 100644
--- a/grid_map_core/src/iterators/CircleIterator.cpp
+++ b/grid_map_core/src/iterators/CircleIterator.cpp
@@ -1,5 +1,5 @@
/*
- * Circleterator.hpp
+ * CircleIterator.hpp
*
* Created on: Nov 13, 2014
* Author: Péter Fankhauser
@@ -7,9 +7,9 @@
*/
#include "grid_map_core/iterators/CircleIterator.hpp"
-#include "grid_map_core/GridMapMath.hpp"
-using namespace std;
+#include
+#include "grid_map_core/GridMapMath.hpp"
namespace grid_map {
@@ -26,22 +26,10 @@ CircleIterator::CircleIterator(const GridMap& gridMap, const Position& center, c
Index submapStartIndex;
Index submapBufferSize;
findSubmapParameters(center, radius, submapStartIndex, submapBufferSize);
- internalIterator_ = std::shared_ptr(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize));
- if(!isInside()) ++(*this);
-}
-
-CircleIterator& CircleIterator::operator =(const CircleIterator& other)
-{
- center_ = other.center_;
- radius_ = other.radius_;
- radiusSquare_ = other.radiusSquare_;
- internalIterator_ = other.internalIterator_;
- mapLength_ = other.mapLength_;
- mapPosition_ = other.mapPosition_;
- resolution_ = other.resolution_;
- bufferSize_ = other.bufferSize_;
- bufferStartIndex_ = other.bufferStartIndex_;
- return *this;
+ internalIterator_ = std::make_shared(gridMap, submapStartIndex, submapBufferSize);
+ if(!isInside()) {
+ ++(*this);
+ }
}
bool CircleIterator::operator !=(const CircleIterator& other) const
@@ -57,10 +45,14 @@ const Index& CircleIterator::operator *() const
CircleIterator& CircleIterator::operator ++()
{
++(*internalIterator_);
- if (internalIterator_->isPastEnd()) return *this;
+ if (internalIterator_->isPastEnd()) {
+ return *this;
+ }
for ( ; !internalIterator_->isPastEnd(); ++(*internalIterator_)) {
- if (isInside()) break;
+ if (isInside()) {
+ break;
+ }
}
return *this;
@@ -89,7 +81,7 @@ void CircleIterator::findSubmapParameters(const Position& center, const double r
getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
Index endIndex;
getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
- bufferSize = getSubmapSizeFromCornerIndeces(startIndex, endIndex, bufferSize_, bufferStartIndex_);
+ bufferSize = getSubmapSizeFromCornerIndices(startIndex, endIndex, bufferSize_, bufferStartIndex_);
}
} /* namespace grid_map */
diff --git a/grid_map_core/src/iterators/EllipseIterator.cpp b/grid_map_core/src/iterators/EllipseIterator.cpp
index 6463c509c..7fe7632bd 100644
--- a/grid_map_core/src/iterators/EllipseIterator.cpp
+++ b/grid_map_core/src/iterators/EllipseIterator.cpp
@@ -9,19 +9,17 @@
#include "grid_map_core/iterators/EllipseIterator.hpp"
#include "grid_map_core/GridMapMath.hpp"
-#include
+#include
#include
-using namespace std;
-
namespace grid_map {
EllipseIterator::EllipseIterator(const GridMap& gridMap, const Position& center, const Length& length, const double rotation)
: center_(center)
{
semiAxisSquare_ = (0.5 * length).square();
- double sinRotation = sin(rotation);
- double cosRotation = cos(rotation);
+ double sinRotation = std::sin(rotation);
+ double cosRotation = std::cos(rotation);
transformMatrix_ << cosRotation, sinRotation, sinRotation, -cosRotation;
mapLength_ = gridMap.getLength();
mapPosition_ = gridMap.getPosition();
@@ -31,22 +29,10 @@ EllipseIterator::EllipseIterator(const GridMap& gridMap, const Position& center,
Index submapStartIndex;
Index submapBufferSize;
findSubmapParameters(center, length, rotation, submapStartIndex, submapBufferSize);
- internalIterator_ = std::shared_ptr(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize));
- if(!isInside()) ++(*this);
-}
-
-EllipseIterator& EllipseIterator::operator =(const EllipseIterator& other)
-{
- center_ = other.center_;
- semiAxisSquare_ = other.semiAxisSquare_;
- transformMatrix_ = other.transformMatrix_;
- internalIterator_ = other.internalIterator_;
- mapLength_ = other.mapLength_;
- mapPosition_ = other.mapPosition_;
- resolution_ = other.resolution_;
- bufferSize_ = other.bufferSize_;
- bufferStartIndex_ = other.bufferStartIndex_;
- return *this;
+ internalIterator_ = std::make_shared(gridMap, submapStartIndex, submapBufferSize);
+ if (!isInside()) {
+ ++(*this);
+ }
}
bool EllipseIterator::operator !=(const EllipseIterator& other) const
@@ -62,10 +48,14 @@ const Eigen::Array2i& EllipseIterator::operator *() const
EllipseIterator& EllipseIterator::operator ++()
{
++(*internalIterator_);
- if (internalIterator_->isPastEnd()) return *this;
+ if (internalIterator_->isPastEnd()) {
+ return *this;
+ }
for ( ; !internalIterator_->isPastEnd(); ++(*internalIterator_)) {
- if (isInside()) break;
+ if (isInside()) {
+ break;
+ }
}
return *this;
@@ -103,7 +93,7 @@ void EllipseIterator::findSubmapParameters(const Position& center, const Length&
getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
Index endIndex;
getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
- bufferSize = getSubmapSizeFromCornerIndeces(startIndex, endIndex, bufferSize_, bufferStartIndex_);
+ bufferSize = getSubmapSizeFromCornerIndices(startIndex, endIndex, bufferSize_, bufferStartIndex_);
}
} /* namespace grid_map */
diff --git a/grid_map_core/src/iterators/GridMapIterator.cpp b/grid_map_core/src/iterators/GridMapIterator.cpp
index 68ae57b73..756c103e0 100644
--- a/grid_map_core/src/iterators/GridMapIterator.cpp
+++ b/grid_map_core/src/iterators/GridMapIterator.cpp
@@ -29,22 +29,12 @@ GridMapIterator::GridMapIterator(const GridMapIterator* other)
isPastEnd_ = other->isPastEnd_;
}
-GridMapIterator& GridMapIterator::operator =(const GridMapIterator& other)
-{
- size_ = other.size_;
- startIndex_ = other.startIndex_;
- linearSize_ = other.linearSize_;
- linearIndex_ = other.linearIndex_;
- isPastEnd_ = other.isPastEnd_;
- return *this;
-}
-
bool GridMapIterator::operator !=(const GridMapIterator& other) const
{
return linearIndex_ != other.linearIndex_;
}
-const Index GridMapIterator::operator *() const
+Index GridMapIterator::operator *() const
{
return getIndexFromLinearIndex(linearIndex_, size_);
}
@@ -54,7 +44,7 @@ const size_t& GridMapIterator::getLinearIndex() const
return linearIndex_;
}
-const Index GridMapIterator::getUnwrappedIndex() const
+Index GridMapIterator::getUnwrappedIndex() const
{
return getIndexFromBufferIndex(*(*this), size_, startIndex_);
}
diff --git a/grid_map_core/src/iterators/LineIterator.cpp b/grid_map_core/src/iterators/LineIterator.cpp
index 0afc4eb97..2c6337d28 100644
--- a/grid_map_core/src/iterators/LineIterator.cpp
+++ b/grid_map_core/src/iterators/LineIterator.cpp
@@ -9,14 +9,13 @@
#include "grid_map_core/iterators/LineIterator.hpp"
#include "grid_map_core/GridMapMath.hpp"
-using namespace std;
-
namespace grid_map {
LineIterator::LineIterator(const grid_map::GridMap& gridMap, const Position& start,
const Position& end)
{
- Index startIndex, endIndex;
+ Index startIndex;
+ Index endIndex;
if (getIndexLimitedToMapRange(gridMap, start, end, startIndex)
&& getIndexLimitedToMapRange(gridMap, end, start, endIndex)) {
initialize(gridMap, startIndex, endIndex);
@@ -31,26 +30,6 @@ LineIterator::LineIterator(const grid_map::GridMap& gridMap, const Index& start,
initialize(gridMap, start, end);
}
-LineIterator& LineIterator::operator =(const LineIterator& other)
-{
- index_ = other.index_;
- start_ = other.start_;
- end_ = other.end_;
- iCell_ = other.iCell_;
- nCells_ = other.nCells_;
- increment1_ = other.increment1_;
- increment2_ = other.increment2_;
- denominator_ = other.denominator_;
- numerator_ = other.numerator_;
- numeratorAdd_ = other.numeratorAdd_;
- mapLength_ = other.mapLength_;
- mapPosition_ = other.mapPosition_;
- resolution_ = other.resolution_;
- bufferSize_ = other.bufferSize_;
- bufferStartIndex_ = other.bufferStartIndex_;
- return *this;
-}
-
bool LineIterator::operator !=(const LineIterator& other) const
{
return (index_ != other.index_).any();
@@ -101,8 +80,9 @@ bool LineIterator::getIndexLimitedToMapRange(const grid_map::GridMap& gridMap,
Vector direction = (end - start).normalized();
while (!gridMap.getIndex(newStart, index)) {
newStart += (gridMap.getResolution() - std::numeric_limits::epsilon()) * direction;
- if ((end - newStart).norm() < gridMap.getResolution() - std::numeric_limits::epsilon())
+ if ((end - newStart).norm() < gridMap.getResolution() - std::numeric_limits::epsilon()) {
return false;
+ }
}
return true;
}
diff --git a/grid_map_core/src/iterators/PolygonIterator.cpp b/grid_map_core/src/iterators/PolygonIterator.cpp
index e20849ff9..733a8421e 100644
--- a/grid_map_core/src/iterators/PolygonIterator.cpp
+++ b/grid_map_core/src/iterators/PolygonIterator.cpp
@@ -6,11 +6,11 @@
* Institute: ETH Zurich, ANYbotics
*/
+#include
+
#include "grid_map_core/iterators/PolygonIterator.hpp"
#include "grid_map_core/GridMapMath.hpp"
-using namespace std;
-
namespace grid_map {
PolygonIterator::PolygonIterator(const grid_map::GridMap& gridMap, const grid_map::Polygon& polygon)
@@ -24,20 +24,10 @@ PolygonIterator::PolygonIterator(const grid_map::GridMap& gridMap, const grid_ma
Index submapStartIndex;
Size submapBufferSize;
findSubmapParameters(polygon, submapStartIndex, submapBufferSize);
- internalIterator_ = std::shared_ptr(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize));
- if(!isInside()) ++(*this);
-}
-
-PolygonIterator& PolygonIterator::operator =(const PolygonIterator& other)
-{
- polygon_ = other.polygon_;
- internalIterator_ = other.internalIterator_;
- mapLength_ = other.mapLength_;
- mapPosition_ = other.mapPosition_;
- resolution_ = other.resolution_;
- bufferSize_ = other.bufferSize_;
- bufferStartIndex_ = other.bufferStartIndex_;
- return *this;
+ internalIterator_ = std::make_shared(gridMap, submapStartIndex, submapBufferSize);
+ if (!isInside()) {
+ ++(*this);
+ }
}
bool PolygonIterator::operator !=(const PolygonIterator& other) const
@@ -53,10 +43,14 @@ const Index& PolygonIterator::operator *() const
PolygonIterator& PolygonIterator::operator ++()
{
++(*internalIterator_);
- if (internalIterator_->isPastEnd()) return *this;
+ if (internalIterator_->isPastEnd()) {
+ return *this;
+ }
- for ( ; !internalIterator_->isPastEnd(); ++(*internalIterator_)) {
- if (isInside()) break;
+ for (; !internalIterator_->isPastEnd(); ++(*internalIterator_)) {
+ if (isInside()) {
+ break;
+ }
}
return *this;
@@ -74,7 +68,7 @@ bool PolygonIterator::isInside() const
return polygon_.isInside(position);
}
-void PolygonIterator::findSubmapParameters(const grid_map::Polygon& polygon, Index& startIndex, Size& bufferSize) const
+void PolygonIterator::findSubmapParameters(const grid_map::Polygon& /*polygon*/, Index& startIndex, Size& bufferSize) const
{
Position topLeft = polygon_.getVertices()[0];
Position bottomRight = topLeft;
@@ -87,7 +81,7 @@ void PolygonIterator::findSubmapParameters(const grid_map::Polygon& polygon, Ind
getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
Index endIndex;
getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
- bufferSize = getSubmapSizeFromCornerIndeces(startIndex, endIndex, bufferSize_, bufferStartIndex_);
+ bufferSize = getSubmapSizeFromCornerIndices(startIndex, endIndex, bufferSize_, bufferStartIndex_);
}
} /* namespace grid_map */
diff --git a/grid_map_core/src/iterators/SlidingWindowIterator.cpp b/grid_map_core/src/iterators/SlidingWindowIterator.cpp
index 6ade5a4b3..c1ba2a43a 100644
--- a/grid_map_core/src/iterators/SlidingWindowIterator.cpp
+++ b/grid_map_core/src/iterators/SlidingWindowIterator.cpp
@@ -34,8 +34,10 @@ SlidingWindowIterator::SlidingWindowIterator(const SlidingWindowIterator* other)
void SlidingWindowIterator::setWindowLength(const GridMap& gridMap, const double windowLength)
{
- windowSize_ = std::round(windowLength / gridMap.getResolution());
- if (windowSize_ % 2 != 1) ++windowSize_;
+ windowSize_ = static_cast(std::round(windowLength / gridMap.getResolution()));
+ if (windowSize_ % 2 != 1) {
+ ++windowSize_;
+ }
setup(gridMap);
}
@@ -44,7 +46,9 @@ SlidingWindowIterator& SlidingWindowIterator::operator ++()
if (edgeHandling_ == EdgeHandling::INSIDE) {
while (!isPastEnd()) {
GridMapIterator::operator++();
- if (dataInsideMap()) break;
+ if (dataInsideMap()) {
+ break;
+ }
}
} else {
GridMapIterator::operator++();
@@ -52,10 +56,10 @@ SlidingWindowIterator& SlidingWindowIterator::operator ++()
return *this;
}
-const Matrix SlidingWindowIterator::getData() const
+Matrix SlidingWindowIterator::getData() const
{
const Index centerIndex(*(*this));
- const Index windowMargin(Index::Constant(windowMargin_));
+ const Index windowMargin(Index::Constant(static_cast(windowMargin_)));
const Index originalTopLeftIndex(centerIndex - windowMargin);
Index topLeftIndex(originalTopLeftIndex);
boundIndexToRange(topLeftIndex, size_);
@@ -71,8 +75,11 @@ const Matrix SlidingWindowIterator::getData() const
case EdgeHandling::MEAN:
const Matrix data = data_.block(topLeftIndex(0), topLeftIndex(1), adjustedWindowSize(0), adjustedWindowSize(1));
Matrix returnData(windowSize_, windowSize_);
- if (edgeHandling_ == EdgeHandling::EMPTY) returnData.setConstant(NAN);
- else if (edgeHandling_ == EdgeHandling::MEAN) returnData.setConstant(data.meanOfFinites());
+ if (edgeHandling_ == EdgeHandling::EMPTY) {
+ returnData.setConstant(NAN);
+ } else if (edgeHandling_ == EdgeHandling::MEAN) {
+ returnData.setConstant(data.meanOfFinites());
+ }
const Index topLeftIndexShift(topLeftIndex - originalTopLeftIndex);
returnData.block(topLeftIndexShift(0), topLeftIndexShift(1), adjustedWindowSize(0), adjustedWindowSize(1)) =
data_.block(topLeftIndex(0), topLeftIndex(1), adjustedWindowSize(0), adjustedWindowSize(1));
@@ -83,10 +90,12 @@ const Matrix SlidingWindowIterator::getData() const
void SlidingWindowIterator::setup(const GridMap& gridMap)
{
- if (!gridMap.isDefaultStartIndex()) throw std::runtime_error(
- "SlidingWindowIterator cannot be used with grid maps that don't have a default buffer start index.");
- if (windowSize_ % 2 == 0) throw std::runtime_error(
- "SlidingWindowIterator has a wrong window size!");
+ if (!gridMap.isDefaultStartIndex()) {
+ throw std::runtime_error("SlidingWindowIterator cannot be used with grid maps that don't have a default buffer start index.");
+ }
+ if (windowSize_ % 2 == 0) {
+ throw std::runtime_error("SlidingWindowIterator has a wrong window size!");
+ }
windowMargin_ = (windowSize_ - 1) / 2;
if (edgeHandling_ == EdgeHandling::INSIDE) {
@@ -99,7 +108,7 @@ void SlidingWindowIterator::setup(const GridMap& gridMap)
bool SlidingWindowIterator::dataInsideMap() const
{
const Index centerIndex(*(*this));
- const Index windowMargin(Index::Constant(windowMargin_));
+ const Index windowMargin(Index::Constant(static_cast(windowMargin_)));
const Index topLeftIndex(centerIndex - windowMargin);
const Index bottomRightIndex(centerIndex + windowMargin);
return checkIfIndexInRange(topLeftIndex, size_) && checkIfIndexInRange(bottomRightIndex, size_);
diff --git a/grid_map_core/src/iterators/SpiralIterator.cpp b/grid_map_core/src/iterators/SpiralIterator.cpp
index 2f8899e6e..9ee5b3cd9 100644
--- a/grid_map_core/src/iterators/SpiralIterator.cpp
+++ b/grid_map_core/src/iterators/SpiralIterator.cpp
@@ -10,14 +10,14 @@
#include "grid_map_core/GridMapMath.hpp"
#include
+#include
-using namespace std;
namespace grid_map {
-SpiralIterator::SpiralIterator(const grid_map::GridMap& gridMap, const Eigen::Vector2d& center,
+SpiralIterator::SpiralIterator(const grid_map::GridMap& gridMap, Eigen::Vector2d center,
const double radius)
- : center_(center),
+ : center_(std::move(center)),
radius_(radius),
distance_(0)
{
@@ -27,31 +27,17 @@ SpiralIterator::SpiralIterator(const grid_map::GridMap& gridMap, const Eigen::Ve
resolution_ = gridMap.getResolution();
bufferSize_ = gridMap.getSize();
gridMap.getIndex(center_, indexCenter_);
- nRings_ = std::ceil(radius_ / resolution_);
- if (checkIfIndexInRange(indexCenter_, bufferSize_))
+ nRings_ = static_cast(std::ceil(radius_ / resolution_));
+ if (checkIfIndexInRange(indexCenter_, bufferSize_)) {
pointsRing_.push_back(indexCenter_);
- else
- while(pointsRing_.empty() && !isPastEnd())
+ } else {
+ while (pointsRing_.empty() && !isPastEnd()) {
generateRing();
+ }
+ }
}
-SpiralIterator& SpiralIterator::operator =(const SpiralIterator& other)
-{
- center_ = other.center_;
- indexCenter_ = other.indexCenter_;
- radius_ = other.radius_;
- radiusSquare_ = other.radiusSquare_;
- nRings_ = other.nRings_;
- distance_ = other.distance_;
- pointsRing_ = other.pointsRing_;
- mapLength_ = other.mapLength_;
- mapPosition_ = other.mapPosition_;
- resolution_ = other.resolution_;
- bufferSize_ = other.bufferSize_;
- return *this;
-}
-
-bool SpiralIterator::operator !=(const SpiralIterator& other) const
+bool SpiralIterator::operator !=(const SpiralIterator& /*other*/) const
{
return (pointsRing_.back() != pointsRing_.back()).any();
}
@@ -64,7 +50,9 @@ const Eigen::Array2i& SpiralIterator::operator *() const
SpiralIterator& SpiralIterator::operator ++()
{
pointsRing_.pop_back();
- if (pointsRing_.empty() && !isPastEnd()) generateRing();
+ if (pointsRing_.empty() && !isPastEnd()) {
+ generateRing();
+ }
return *this;
}
@@ -73,7 +61,7 @@ bool SpiralIterator::isPastEnd() const
return (distance_ == nRings_ && pointsRing_.empty());
}
-bool SpiralIterator::isInside(const Index index) const
+bool SpiralIterator::isInside(const Index& index) const
{
Eigen::Vector2d position;
getPositionFromIndex(position, index, mapLength_, mapPosition_, resolution_, bufferSize_);
@@ -92,25 +80,24 @@ void SpiralIterator::generateRing()
pointInMap.y() = point.y() + indexCenter_.y();
if (checkIfIndexInRange(pointInMap, bufferSize_)) {
if (distance_ == nRings_ || distance_ == nRings_ - 1) {
- if (isInside(pointInMap))
+ if (isInside(pointInMap)) {
pointsRing_.push_back(pointInMap);
+ }
} else {
pointsRing_.push_back(pointInMap);
}
}
normal.x() = -signum(point.y());
normal.y() = signum(point.x());
- if (normal.x() != 0
- && (int) Vector(point.x() + normal.x(), point.y()).norm() == distance_)
+ if (normal.x() != 0 && static_cast(Vector(point.x() + normal.x(), point.y()).norm()) == distance_) {
point.x() += normal.x();
- else if (normal.y() != 0
- && (int) Vector(point.x(), point.y() + normal.y()).norm() == distance_)
+ } else if (normal.y() != 0 && static_cast(Vector(point.x(), point.y() + normal.y()).norm()) == distance_) {
point.y() += normal.y();
- else {
+ } else {
point.x() += normal.x();
point.y() += normal.y();
}
- } while (point.x() != distance_ || point.y() != 0);
+ } while (static_cast(point.x()) != distance_ || point.y() != 0);
}
double SpiralIterator::getCurrentRadius() const
diff --git a/grid_map_core/src/iterators/SubmapIterator.cpp b/grid_map_core/src/iterators/SubmapIterator.cpp
index bf1232547..12fb120b3 100644
--- a/grid_map_core/src/iterators/SubmapIterator.cpp
+++ b/grid_map_core/src/iterators/SubmapIterator.cpp
@@ -9,7 +9,7 @@
#include "grid_map_core/iterators/SubmapIterator.hpp"
#include "grid_map_core/GridMapMath.hpp"
-using namespace std;
+
namespace grid_map {
@@ -48,18 +48,6 @@ SubmapIterator::SubmapIterator(const SubmapIterator* other)
isPastEnd_ = other->isPastEnd_;
}
-SubmapIterator& SubmapIterator::operator =(const SubmapIterator& other)
-{
- size_ = other.size_;
- startIndex_ = other.startIndex_;
- submapSize_ = other.submapSize_;
- submapStartIndex_ = other.submapStartIndex_;
- index_ = other.index_;
- submapIndex_ = other.submapIndex_;
- isPastEnd_ = other.isPastEnd_;
- return *this;
-}
-
bool SubmapIterator::operator !=(const SubmapIterator& other) const
{
return (index_ != other.index_).any();
diff --git a/grid_map_core/test/EigenPluginsTest.cpp b/grid_map_core/test/EigenPluginsTest.cpp
index 0ec5cf652..cdbd1e810 100644
--- a/grid_map_core/test/EigenPluginsTest.cpp
+++ b/grid_map_core/test/EigenPluginsTest.cpp
@@ -14,8 +14,7 @@
// Eigen
#include
-using namespace std;
-using namespace Eigen;
+using Eigen::Matrix;
TEST(EigenMatrixBaseAddons, numberOfFinites)
{
@@ -70,7 +69,8 @@ TEST(EigenMatrixBaseAddons, minCoeffOfFinites)
double min = matrix.minCoeff();
EXPECT_NEAR(min, matrix.minCoeffOfFinites(), 1e-10);
- int i, j;
+ int i;
+ int j;
matrix.maxCoeff(&i, &j);
matrix(i, j) = NAN;
EXPECT_NEAR(min, matrix.minCoeffOfFinites(), 1e-10);
@@ -88,7 +88,8 @@ TEST(EigenMatrixBaseAddons, maxCoeffOfFinites)
double max = matrix.maxCoeff();
EXPECT_NEAR(max, matrix.maxCoeffOfFinites(), 1e-10);
- int i, j;
+ int i;
+ int j;
matrix.minCoeff(&i, &j);
matrix(i, j) = NAN;
EXPECT_NEAR(max, matrix.maxCoeffOfFinites(), 1e-10);
diff --git a/grid_map_core/test/EllipseIteratorTest.cpp b/grid_map_core/test/EllipseIteratorTest.cpp
index 801cd9552..f77fee01e 100644
--- a/grid_map_core/test/EllipseIteratorTest.cpp
+++ b/grid_map_core/test/EllipseIteratorTest.cpp
@@ -9,21 +9,16 @@
#include "grid_map_core/iterators/EllipseIterator.hpp"
#include "grid_map_core/GridMap.hpp"
-// Eigen
-#include
-
// gtest
#include
-// Limits
-#include
-
// Vector
#include
-using namespace std;
-using namespace Eigen;
-using namespace grid_map;
+using grid_map::GridMap;
+using grid_map::Length;
+using grid_map::Position;
+using grid_map::EllipseIterator;
TEST(EllipseIterator, OneCellWideEllipse)
{
diff --git a/grid_map_core/test/GridMapIteratorTest.cpp b/grid_map_core/test/GridMapIteratorTest.cpp
index c04e472d5..33c1ac4f7 100644
--- a/grid_map_core/test/GridMapIteratorTest.cpp
+++ b/grid_map_core/test/GridMapIteratorTest.cpp
@@ -9,20 +9,16 @@
#include "grid_map_core/iterators/GridMapIterator.hpp"
#include "grid_map_core/GridMap.hpp"
-// Eigen
-#include
-
// gtest
#include
-// Limits
-#include
-
// Vector
#include
-using namespace std;
-using namespace grid_map;
+using grid_map::GridMap;
+using grid_map::Length;
+using grid_map::Position;
+using grid_map::GridMapIterator;
TEST(GridMapIterator, Simple)
{
@@ -52,7 +48,7 @@ TEST(GridMapIterator, LinearIndex)
auto& data = map["layer"];
unsigned int i = 0;
for (; !iterator.isPastEnd(); ++iterator, ++i) {
- data(iterator.getLinearIndex()) = 1.0;
+ data(static_cast(iterator.getLinearIndex())) = 1.0;
EXPECT_EQ(i, iterator.getLinearIndex());
EXPECT_FALSE(iterator.isPastEnd());
}
diff --git a/grid_map_core/test/GridMapMathTest.cpp b/grid_map_core/test/GridMapMathTest.cpp
index a8c188606..1382f7651 100644
--- a/grid_map_core/test/GridMapMathTest.cpp
+++ b/grid_map_core/test/GridMapMathTest.cpp
@@ -8,9 +8,6 @@
#include "grid_map_core/GridMapMath.hpp"
-// Eigen
-#include
-
// gtest
#include
@@ -20,8 +17,9 @@
// Vector
#include
-using namespace std;
-using namespace grid_map;
+using std::numeric_limits;
+
+namespace grid_map{
TEST(PositionFromIndex, Simple)
{
@@ -303,10 +301,9 @@ TEST(checkIfIndexInRange, All)
TEST(boundIndexToRange, All)
{
- int index;
int bufferSize = 10;
- index = 0;
+ int index = 0;
boundIndexToRange(index, bufferSize);
EXPECT_EQ(0, index);
@@ -337,10 +334,9 @@ TEST(boundIndexToRange, All)
TEST(wrapIndexToRange, All)
{
- int index;
int bufferSize = 10;
- index = 0;
+ int index = 0;
wrapIndexToRange(index, bufferSize);
EXPECT_EQ(0, index);
@@ -501,7 +497,7 @@ TEST(getSubmapInformation, Simple)
Position requestedSubmapPosition;
Position requestedSubmapLength;
- // The returned submap indeces
+ // The returned submap indices
Index submapTopLeftIndex;
Index submapSize;
Position submapPosition;
@@ -536,7 +532,7 @@ TEST(getSubmapInformation, Zero)
Position requestedSubmapPosition;
Length requestedSubmapLength;
- // The returned submap indeces
+ // The returned submap indices
Index submapTopLeftIndex;
Index submapSize;
Position submapPosition;
@@ -572,7 +568,7 @@ TEST(getSubmapInformation, ExceedingBoundaries)
Position requestedSubmapPosition;
Length requestedSubmapLength;
- // The returned submap indeces
+ // The returned submap indices
Index submapTopLeftIndex;
Size submapSize;
Position submapPosition;
@@ -626,7 +622,7 @@ TEST(getSubmapInformation, CircularBuffer)
Position requestedSubmapPosition;
Length requestedSubmapLength;
- // The returned submap indeces
+ // The returned submap indices
Index submapTopLeftIndex;
Size submapSize;
Position submapPosition;
@@ -696,7 +692,7 @@ TEST(getSubmapInformation, Debug1)
Position requestedSubmapPosition(-7.44, -3.42);
Length requestedSubmapLength(0.12, 0.12);
- // The returned submap indeces
+ // The returned submap indices
Index submapTopLeftIndex;
Size submapSize;
Position submapPosition;
@@ -725,7 +721,7 @@ TEST(getSubmapInformation, Debug2)
Position requestedSubmapPosition(0.24, -26.82);
Length requestedSubmapLength(0.624614, 0.462276);
- // The returned submap indeces
+ // The returned submap indices
Index submapTopLeftIndex;
Size submapSize;
Position submapPosition;
@@ -1039,3 +1035,5 @@ TEST(getIndexFromLinearIndex, Simple)
EXPECT_TRUE((Index(0, 1) == getIndexFromLinearIndex(8, Size(8, 5), false)).all());
EXPECT_TRUE((Index(7, 4) == getIndexFromLinearIndex(39, Size(8, 5), false)).all());
}
+
+} // namespace grid_map
\ No newline at end of file
diff --git a/grid_map_core/test/GridMapTest.cpp b/grid_map_core/test/GridMapTest.cpp
index 0acf5e08b..64b15f88b 100644
--- a/grid_map_core/test/GridMapTest.cpp
+++ b/grid_map_core/test/GridMapTest.cpp
@@ -11,14 +11,9 @@
// gtest
#include
-// Math
-#include
+namespace grid_map {
-using namespace std;
-using namespace grid_map;
-
-TEST(GridMap, CopyConstructor)
-{
+TEST(GridMap, CopyConstructor) {
GridMap map({"layer_a", "layer_b"});
map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2));
map["layer_a"].setConstant(1.0);
@@ -66,12 +61,21 @@ TEST(GridMap, Move)
EXPECT_EQ(3, startIndex(0));
EXPECT_EQ(2, startIndex(1));
-
- EXPECT_FALSE(map.isValid(Index(0, 0))); // TODO Check entire map.
- EXPECT_TRUE(map.isValid(Index(3, 2)));
- EXPECT_FALSE(map.isValid(Index(2, 2)));
- EXPECT_FALSE(map.isValid(Index(3, 1)));
- EXPECT_TRUE(map.isValid(Index(7, 4)));
+
+ Eigen::Matrix isValidExpected;
+ isValidExpected << false, false, false, false, false, // clang-format off
+ false, false, false, false, false,
+ false, false, false, false, false,
+ false, false, true, true, true,
+ false, false, true, true, true,
+ false, false, true, true, true,
+ false, false, true, true, true,
+ false, false, true, true, true; // clang-format on
+ for(int row{0}; row < 8; row++){
+ for(int col{0}; col < 5; col++){
+ EXPECT_EQ(map.isValid(Index(row, col)), isValidExpected(row, col)) << "Value of map.isValid at ["< stringVector;
- stringVector.push_back("nan");
+ stringVector.emplace_back("nan");
map1.addDataFrom(map2, true, false, false, stringVector);
Index index;
map1.getIndex(Position(-2, -2), index);
@@ -208,7 +418,8 @@ TEST(AddDataFrom, ExtendMapNotAligned)
TEST(AddDataFrom, CopyData)
{
- GridMap map1, map2;
+ GridMap map1;
+ GridMap map2;
map1.setGeometry(Length(5.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(5, 5)
map1.add("zero", 0.0);
map1.add("one");
@@ -249,9 +460,7 @@ TEST(ValueAtPosition, NearestNeighbor)
map.at("types", Index(2,1)) = 2.0;
map.at("types", Index(2,2)) = 2.0;
- double value;
-
- value = map.atPosition("types", Position(1.35,-0.4));
+ double value = map.atPosition("types", Position(1.35,-0.4));
EXPECT_DOUBLE_EQ((float)3.8, value);
value = map.atPosition("types", Position(-0.3,0.0));
@@ -273,10 +482,8 @@ TEST(ValueAtPosition, LinearInterpolated)
map.at("types", Index(2,1)) = 2.0;
map.at("types", Index(2,2)) = 2.0;
- double value;
-
// Close to the border -> reverting to INTER_NEAREST.
- value = map.atPosition("types", Position(-0.5,-1.2), InterpolationMethods::INTER_LINEAR);
+ double value = map.atPosition("types", Position(-0.5,-1.2), InterpolationMethods::INTER_LINEAR);
EXPECT_DOUBLE_EQ(2.0, value);
// In between 1.0 and 2.0 field.
value = map.atPosition("types", Position(-0.5,0.0), InterpolationMethods::INTER_LINEAR);
@@ -285,3 +492,5 @@ TEST(ValueAtPosition, LinearInterpolated)
value = map.atPosition("types", Position(0.69,0.38), InterpolationMethods::INTER_LINEAR);
EXPECT_NEAR(2.1963200, value, 0.0000001);
}
+
+} // namespace grid_map
\ No newline at end of file
diff --git a/grid_map_core/test/LineIteratorTest.cpp b/grid_map_core/test/LineIteratorTest.cpp
index 664267bef..2086fc141 100644
--- a/grid_map_core/test/LineIteratorTest.cpp
+++ b/grid_map_core/test/LineIteratorTest.cpp
@@ -12,13 +12,9 @@
// gtest
#include
-// Limits
-#include
+namespace grid_map {
-using namespace grid_map;
-
-TEST(LineIterator, StartOutsideMap)
-{
+TEST(LineIterator, StartOutsideMap) {
GridMap map( { "types" });
map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0));
@@ -178,3 +174,5 @@ TEST(LineIterator, StartAndEndOutsideMovedMap)
++iterator;
EXPECT_TRUE(iterator.isPastEnd());
}
+
+} // namespace grid_map
\ No newline at end of file
diff --git a/grid_map_core/test/PolygonIteratorTest.cpp b/grid_map_core/test/PolygonIteratorTest.cpp
index 4bbc5d6c2..ef73ae6e2 100644
--- a/grid_map_core/test/PolygonIteratorTest.cpp
+++ b/grid_map_core/test/PolygonIteratorTest.cpp
@@ -10,26 +10,21 @@
#include "grid_map_core/GridMap.hpp"
#include "grid_map_core/Polygon.hpp"
-// Eigen
-#include
-
// gtest
#include
-// Limits
-#include
-
// Vector
#include
-using namespace std;
-using namespace Eigen;
-using namespace grid_map;
+using grid_map::GridMap;
+using grid_map::Length;
+using grid_map::Polygon;
+using grid_map::PolygonIterator;
+using grid_map::Position;
-TEST(PolygonIterator, FullCover)
-{
- vector types;
- types.push_back("type");
+TEST(PolygonIterator, FullCover) {
+ std::vector types;
+ types.emplace_back("type");
GridMap map(types);
map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
@@ -55,7 +50,9 @@ TEST(PolygonIterator, FullCover)
EXPECT_EQ(0, (*iterator)(0));
EXPECT_EQ(2, (*iterator)(1));
- for (int i = 0; i < 37; ++i) ++iterator;
+ for (int i = 0; i < 37; ++i) {
+ ++iterator;
+ }
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(7, (*iterator)(0));
@@ -173,7 +170,9 @@ TEST(PolygonIterator, MoveMap)
EXPECT_EQ(6, (*iterator)(0));
EXPECT_EQ(2, (*iterator)(1));
- for (int i = 0; i < 4; ++i) ++iterator;
+ for (int i = 0; i < 4; ++i) {
+ ++iterator;
+ }
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(7, (*iterator)(0));
@@ -184,7 +183,9 @@ TEST(PolygonIterator, MoveMap)
EXPECT_EQ(0, (*iterator)(0));
EXPECT_EQ(1, (*iterator)(1));
- for (int i = 0; i < 8; ++i) ++iterator;
+ for (int i = 0; i < 8; ++i) {
+ ++iterator;
+ }
EXPECT_FALSE(iterator.isPastEnd());
EXPECT_EQ(2, (*iterator)(0));
diff --git a/grid_map_core/test/PolygonTest.cpp b/grid_map_core/test/PolygonTest.cpp
index 5540377ca..0974a0514 100644
--- a/grid_map_core/test/PolygonTest.cpp
+++ b/grid_map_core/test/PolygonTest.cpp
@@ -13,11 +13,14 @@
// Eigen
#include
-#include
-using namespace std;
-using namespace Eigen;
-using namespace grid_map;
+using Eigen::MatrixXd;
+using Eigen::Vector2d;
+using Eigen::VectorXd;
+
+using grid_map::Length;
+using grid_map::Polygon;
+using grid_map::Position;
TEST(Polygon, getCentroidTriangle)
{
@@ -71,10 +74,10 @@ TEST(Polygon, convexHullPoints)
{
// Test that points which already create a convex shape (square) can be used to create a convex polygon.
std::vector points1;
- points1.push_back(Vector2d(0.0, 0.0));
- points1.push_back(Vector2d(1.0, 0.0));
- points1.push_back(Vector2d(1.0, 1.0));
- points1.push_back(Vector2d(0.0, 1.0));
+ points1.emplace_back(0.0, 0.0);
+ points1.emplace_back(1.0, 0.0);
+ points1.emplace_back(1.0, 1.0);
+ points1.emplace_back(0.0, 1.0);
Polygon polygon1 = Polygon::monotoneChainConvexHullOfPoints(points1);
EXPECT_EQ(4, polygon1.nVertices());
EXPECT_TRUE(polygon1.isInside(Vector2d(0.5, 0.5)));
@@ -82,14 +85,14 @@ TEST(Polygon, convexHullPoints)
// Test that a random set of points can be used to create a convex polygon.
std::vector points2;
- points2.push_back(Vector2d(0.0, 0.0));
- points2.push_back(Vector2d(1.0, 0.0));
- points2.push_back(Vector2d(2.0, 1.0));
- points2.push_back(Vector2d(1.0, 2.0));
- points2.push_back(Vector2d(-1.0, 2.0));
- points2.push_back(Vector2d(-1.0, -2.0));
- points2.push_back(Vector2d(0.0, 1.0));
- points2.push_back(Vector2d(1.0, 1.0));
+ points2.emplace_back(0.0, 0.0);
+ points2.emplace_back(1.0, 0.0);
+ points2.emplace_back(2.0, 1.0);
+ points2.emplace_back(1.0, 2.0);
+ points2.emplace_back(-1.0, 2.0);
+ points2.emplace_back(-1.0, -2.0);
+ points2.emplace_back(0.0, 1.0);
+ points2.emplace_back(1.0, 1.0);
Polygon polygon2 = Polygon::monotoneChainConvexHullOfPoints(points2);
EXPECT_EQ(4, polygon2.nVertices());
EXPECT_TRUE(polygon2.isInside(Vector2d(0.5, 0.5)));
diff --git a/grid_map_core/test/SlidingWindowIteratorTest.cpp b/grid_map_core/test/SlidingWindowIteratorTest.cpp
index b1ff23b01..902144a69 100644
--- a/grid_map_core/test/SlidingWindowIteratorTest.cpp
+++ b/grid_map_core/test/SlidingWindowIteratorTest.cpp
@@ -9,13 +9,14 @@
#include "grid_map_core/iterators/SlidingWindowIterator.hpp"
#include "grid_map_core/GridMap.hpp"
-#include
-#include
#include
#include
-using namespace std;
-using namespace grid_map;
+using grid_map::GridMap;
+using grid_map::Index;
+using grid_map::Length;
+using grid_map::Position;
+using grid_map::SlidingWindowIterator;
TEST(SlidingWindowIterator, WindowSize3Cutoff)
{
@@ -41,7 +42,9 @@ TEST(SlidingWindowIterator, WindowSize3Cutoff)
for (; !iterator.isPastEnd(); ++iterator) {
EXPECT_FALSE(iterator.isPastEnd());
- if ((*iterator == Index(3, 2)).all()) break;
+ if ((*iterator == Index(3, 2)).all()) {
+ break;
+ }
}
EXPECT_EQ(iterator.getData().rows(), 3);
@@ -50,7 +53,9 @@ TEST(SlidingWindowIterator, WindowSize3Cutoff)
for (; !iterator.isPastEnd(); ++iterator) {
EXPECT_FALSE(iterator.isPastEnd());
- if ((*iterator == Index(7, 4)).all()) break;
+ if ((*iterator == Index(7, 4)).all()) {
+ break;
+ }
}
EXPECT_EQ(iterator.getData().rows(), 2);
@@ -85,7 +90,9 @@ TEST(SlidingWindowIterator, WindowSize5)
for (; !iterator.isPastEnd(); ++iterator) {
EXPECT_FALSE(iterator.isPastEnd());
- if ((*iterator == Index(3, 2)).all()) break;
+ if ((*iterator == Index(3, 2)).all()) {
+ break;
+ }
}
EXPECT_EQ(iterator.getData().rows(), 5);
@@ -94,7 +101,9 @@ TEST(SlidingWindowIterator, WindowSize5)
for (; !iterator.isPastEnd(); ++iterator) {
EXPECT_FALSE(iterator.isPastEnd());
- if ((*iterator == Index(7, 4)).all()) break;
+ if ((*iterator == Index(7, 4)).all()) {
+ break;
+ }
}
EXPECT_EQ(iterator.getData().rows(), 3);
@@ -119,7 +128,9 @@ TEST(SlidingWindowIterator, WindowSize3Inside)
for (; !iterator.isPastEnd(); ++iterator) {
EXPECT_FALSE(iterator.isPastEnd());
- if ((*iterator == Index(3, 2)).all()) break;
+ if ((*iterator == Index(3, 2)).all()) {
+ break;
+ }
}
EXPECT_EQ(iterator.getData().rows(), 3);
@@ -128,7 +139,9 @@ TEST(SlidingWindowIterator, WindowSize3Inside)
for (; !iterator.isPastEnd(); ++iterator) {
EXPECT_FALSE(iterator.isPastEnd());
- if ((*iterator == Index(6, 3)).all()) break;
+ if ((*iterator == Index(6, 3)).all()) {
+ break;
+ }
}
EXPECT_EQ(iterator.getData().rows(), 3);
diff --git a/grid_map_core/test/SpiralIteratorTest.cpp b/grid_map_core/test/SpiralIteratorTest.cpp
index a43dd2a8b..f206dfc91 100644
--- a/grid_map_core/test/SpiralIteratorTest.cpp
+++ b/grid_map_core/test/SpiralIteratorTest.cpp
@@ -9,21 +9,16 @@
#include "grid_map_core/iterators/SpiralIterator.hpp"
#include "grid_map_core/GridMap.hpp"
-// Eigen
-#include
-
// gtest
#include
-// Limits
-#include
-
// Vector
#include
-using namespace std;
-using namespace Eigen;
-using namespace grid_map;
+using grid_map::GridMap;
+using grid_map::Length;
+using grid_map::Position;
+using grid_map::SpiralIterator;
TEST(SpiralIterator, CenterOutOfMap)
{
diff --git a/grid_map_core/test/SubmapIteratorTest.cpp b/grid_map_core/test/SubmapIteratorTest.cpp
index 9134b5651..3c838ea62 100644
--- a/grid_map_core/test/SubmapIteratorTest.cpp
+++ b/grid_map_core/test/SubmapIteratorTest.cpp
@@ -15,15 +15,13 @@
// gtest
#include
-// Limits
-#include
-
// Vector
#include
-using namespace std;
-using namespace Eigen;
-using namespace grid_map;
+using std::vector;
+using std::string;
+
+namespace grid_map{
TEST(SubmapIterator, Simple) {
Eigen::Array2i submapTopLeftIndex(3, 1);
@@ -32,9 +30,9 @@ TEST(SubmapIterator, Simple) {
Eigen::Array2i submapIndex;
vector types;
- types.push_back("type");
+ types.emplace_back("type");
GridMap map(types);
- map.setGeometry(Array2d(8.1, 5.1), 1.0, Vector2d(0.0, 0.0)); // bufferSize(8, 5)
+ map.setGeometry(Eigen::Array2d(8.1, 5.1), 1.0, Eigen::Vector2d(0.0, 0.0)); // bufferSize(8, 5)
SubmapIterator iterator(map, submapTopLeftIndex, submapBufferSize);
@@ -94,7 +92,7 @@ TEST(SubmapIterator, CircularBuffer) {
Eigen::Array2i submapIndex;
vector types;
- types.push_back("type");
+ types.emplace_back("type");
GridMap map(types);
map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5)
map.move(Position(-3.0, -2.0)); // bufferStartIndex(3, 2)
@@ -203,8 +201,8 @@ TEST(SubmapIterator, InterleavedExecutionWithMove) {
auto& layer = map.get("layer");
// Initialize the layer as sketched.
- for (size_t colIndex = 0; colIndex < layer.cols(); colIndex++) {
- layer.col(colIndex).setConstant(colIndex);
+ for (long colIndex = 0; colIndex < layer.cols(); colIndex++) {
+ layer.col(colIndex).setConstant(static_cast(colIndex));
}
std::cout << "(4,7) contains " << map.at("layer", {4, 7}) << std::endl;
@@ -213,7 +211,8 @@ TEST(SubmapIterator, InterleavedExecutionWithMove) {
// check that the submap iterator returns {1,1,2,2}
auto checkCorrectValues = [](std::array given) {
- int countOnes = 0, countTwos = 0;
+ int countOnes = 0;
+ int countTwos = 0;
for (auto& value : given) {
if (std::abs(value - 1.0) < 1e-6) {
countOnes++;
@@ -227,7 +226,7 @@ TEST(SubmapIterator, InterleavedExecutionWithMove) {
EXPECT_EQ(countTwos, 2);
};
- std::array returnedSequence;
+ std::array returnedSequence{};
returnedSequence.fill(0);
for (size_t submapIndex = 0; submapIndex < 4; submapIndex++) {
@@ -263,4 +262,6 @@ TEST(SubmapIterator, InterleavedExecutionWithMove) {
// returnedSequence.at(submapIndex) = map.at("layer", *iterator);
// ++iterator;
// });
-}
\ No newline at end of file
+}
+
+} // namespace grid_map
diff --git a/grid_map_core/test/test_grid_map_core.cpp b/grid_map_core/test/test_grid_map_core.cpp
index 862017d3b..78b7680c9 100644
--- a/grid_map_core/test/test_grid_map_core.cpp
+++ b/grid_map_core/test/test_grid_map_core.cpp
@@ -13,6 +13,6 @@
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- srand((int)time(0));
+ srand((int)time(nullptr));
return RUN_ALL_TESTS();
}
diff --git a/grid_map_core/test/test_helpers.cpp b/grid_map_core/test/test_helpers.cpp
index d00dcb49c..96d78faef 100644
--- a/grid_map_core/test/test_helpers.cpp
+++ b/grid_map_core/test/test_helpers.cpp
@@ -23,7 +23,7 @@ AnalyticalFunctions createFlatWorld(grid_map::GridMap *map)
AnalyticalFunctions func;
- func.f_ = [](double x, double y) {
+ func.f_ = [](double /*x*/, double /*y*/) {
return 0.0;
};
@@ -111,7 +111,7 @@ AnalyticalFunctions createTanhWorld(grid_map::GridMap *map)
std::uniform_real_distribution scaling(0.1, 2.0);
const double s = scaling(rndGenerator);
- func.f_ = [s](double x,double y) {
+ func.f_ = [s](double x,double /*y*/) {
const double expZ = std::exp(2 *s* x);
return (expZ - 1) / (expZ + 1);
};
@@ -149,12 +149,12 @@ AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map)
func.f_ = [g](double x,double y) {
double value = 0.0;
- for (int i = 0; i < g.size(); ++i) {
- const double x0 = g.at(i).x0;
- const double y0 = g.at(i).y0;
- const double varX = g.at(i).varX;
- const double varY = g.at(i).varY;
- const double s = g.at(i).s;
+ for (const auto & i : g) {
+ const double x0 = i.x0;
+ const double y0 = i.y0;
+ const double varX = i.varX;
+ const double varY = i.varY;
+ const double s = i.s;
value += s * std::exp(-(x-x0)*(x-x0) / (2.0*varX) - (y-y0)*(y-y0) / (2.0 * varY));
}
@@ -168,12 +168,18 @@ AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map)
void fillGridMap(grid_map::GridMap *map, const AnalyticalFunctions &functions)
{
- grid_map::Matrix& data = (*map)[testLayer];
- for (grid_map::GridMapIterator iterator(*map); !iterator.isPastEnd(); ++iterator) {
- const grid_map::Index index(*iterator);
- grid_map::Position pos;
+ using grid_map::DataType;
+ using grid_map::GridMapIterator;
+ using grid_map::Index;
+ using grid_map::Matrix;
+ using grid_map::Position;
+
+ Matrix& data = (*map)[testLayer];
+ for (GridMapIterator iterator(*map); !iterator.isPastEnd(); ++iterator) {
+ const Index index(*iterator);
+ Position pos;
map->getPosition(index, pos);
- data(index(0), index(1)) = functions.f_(pos.x(), pos.y());
+ data(index(0), index(1)) = static_cast(functions.f_(pos.x(), pos.y()));
}
}
diff --git a/grid_map_core/test/test_helpers.hpp b/grid_map_core/test/test_helpers.hpp
index d6cf51ad6..075963c7b 100644
--- a/grid_map_core/test/test_helpers.hpp
+++ b/grid_map_core/test/test_helpers.hpp
@@ -93,4 +93,4 @@ void verifyValuesAtQueryPointsAreClose(const grid_map::GridMap &map, const Analy
const std::vector &queryPoints,
grid_map::InterpolationMethods interpolationMethod);
-} /*grid_map_test*/
+} // namespace grid_map_test
diff --git a/grid_map_costmap_2d/CHANGELOG.rst b/grid_map_costmap_2d/CHANGELOG.rst
index e6192cc91..474859e03 100644
--- a/grid_map_costmap_2d/CHANGELOG.rst
+++ b/grid_map_costmap_2d/CHANGELOG.rst
@@ -2,6 +2,12 @@
Changelog for package grid_map_costmap_2d
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.7.0 (2022-03-17)
+------------------
+
+1.6.4 (2020-12-04)
+------------------
+
1.6.2 (2019-10-14)
------------------
diff --git a/grid_map_costmap_2d/CMakeLists.txt b/grid_map_costmap_2d/CMakeLists.txt
index d59d4121f..33737e8f5 100644
--- a/grid_map_costmap_2d/CMakeLists.txt
+++ b/grid_map_costmap_2d/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.5.1)
project(grid_map_costmap_2d)
set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
-
+add_compile_options(-Wall -Wextra -Wpedantic)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
## Find catkin macros and libraries
@@ -71,7 +71,8 @@ if(CATKIN_ENABLE_TESTING)
test/test_grid_map_costmap_2d.cpp
test/Costmap2DConverterTest.cpp
)
- add_subdirectory(rostest)
+ # TODO(mw): Disabled rostest because pipeline was failing. Locally not reproducible.
+ # add_subdirectory(rostest)
target_include_directories(${PROJECT_NAME}-test PRIVATE
include
)
@@ -103,4 +104,4 @@ if(cmake_clang_tools_FOUND)
add_default_clang_tooling(
DISABLE_CLANG_FORMAT
)
-endif(cmake_clang_tools_FOUND)
\ No newline at end of file
+endif(cmake_clang_tools_FOUND)
diff --git a/grid_map_costmap_2d/package.xml b/grid_map_costmap_2d/package.xml
index 634db3281..18b4d1c6a 100644
--- a/grid_map_costmap_2d/package.xml
+++ b/grid_map_costmap_2d/package.xml
@@ -1,10 +1,10 @@
grid_map_costmap_2d
- 1.6.2
+ 1.7.18
Interface for grid maps to the costmap_2d format.
- Maximilian Wulf
- Yoshua Nava
+ Magnus Gaertner
+ Francisco Eli Viña Barrientos
BSD
http://github.com/anybotics/grid_map
http://github.com/anybotics/grid_map/issues
diff --git a/grid_map_costmap_2d/rostest/README.md b/grid_map_costmap_2d/rostest/README.md
index 9a1f1c28e..4ca99ae30 100644
--- a/grid_map_costmap_2d/rostest/README.md
+++ b/grid_map_costmap_2d/rostest/README.md
@@ -7,4 +7,3 @@ I can never remember how to run them...
rostest --text mytest.test
```
-
diff --git a/grid_map_cv/CHANGELOG.rst b/grid_map_cv/CHANGELOG.rst
index 4ac85c6c8..b770b0af0 100644
--- a/grid_map_cv/CHANGELOG.rst
+++ b/grid_map_cv/CHANGELOG.rst
@@ -1,6 +1,13 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package grid_map_cv
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.7.0 (2022-03-17)
+------------------
+
+1.6.4 (2020-12-04)
+------------------
+
1.6.3 (2020-09-30)
------------------
* Added getTransformedMap.
diff --git a/grid_map_cv/CMakeLists.txt b/grid_map_cv/CMakeLists.txt
index d23798f4c..d67dbc209 100644
--- a/grid_map_cv/CMakeLists.txt
+++ b/grid_map_cv/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.5.1)
project(grid_map_cv)
set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
-
+add_compile_options(-Wall -Wextra -Wpedantic)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
## Find catkin macros and libraries
diff --git a/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp b/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp
index 540f77eca..bc1d412ce 100644
--- a/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp
+++ b/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp
@@ -116,7 +116,7 @@ class GridMapCvConverter
}
return true;
- };
+ }
/*!
* Adds a color layer with data from an image.
@@ -177,7 +177,7 @@ class GridMapCvConverter
const float minValue = gridMap.get(layer).minCoeffOfFinites();
const float maxValue = gridMap.get(layer).maxCoeffOfFinites();
return toImage(gridMap, layer, encoding, minValue, maxValue, image);
- };
+ }
/*!
* Creates a cv mat from a grid map layer.
@@ -244,7 +244,7 @@ class GridMapCvConverter
}
return true;
- };
+ }
};
diff --git a/grid_map_cv/include/grid_map_cv/InpaintFilter.hpp b/grid_map_cv/include/grid_map_cv/InpaintFilter.hpp
index dcf023c11..f57c21ed4 100644
--- a/grid_map_cv/include/grid_map_cv/InpaintFilter.hpp
+++ b/grid_map_cv/include/grid_map_cv/InpaintFilter.hpp
@@ -8,7 +8,7 @@
#pragma once
-#include
+#include
//OpenCV
#include "grid_map_cv/grid_map_cv.hpp"
diff --git a/grid_map_cv/package.xml b/grid_map_cv/package.xml
index 2904c4e66..377ceaa27 100644
--- a/grid_map_cv/package.xml
+++ b/grid_map_cv/package.xml
@@ -1,10 +1,10 @@
grid_map_cv
- 1.6.3
+ 1.7.18
Conversions between grid maps and OpenCV images.
- Maximilian Wulf
- Yoshua Nava
+ Magnus Gaertner
+ Mathieu Agostinucci
BSD
http://github.com/anybotics/grid_map
http://github.com/anybotics/grid_map/issues
@@ -15,6 +15,7 @@
grid_map_core
cv_bridge
filters
+ libopencv-dev
gtest
diff --git a/grid_map_demos/CHANGELOG.rst b/grid_map_demos/CHANGELOG.rst
index 0c0156b03..500764b84 100644
--- a/grid_map_demos/CHANGELOG.rst
+++ b/grid_map_demos/CHANGELOG.rst
@@ -2,6 +2,12 @@
Changelog for package grid_map_demos
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.7.0 (2022-03-17)
+------------------
+
+1.6.4 (2020-12-04)
+------------------
+
1.6.3 (2020-10-19)
------------------
* Added GridMapToImageDemo. Saves the elevation layer of a grid_map to a file.
diff --git a/grid_map_demos/CMakeLists.txt b/grid_map_demos/CMakeLists.txt
index 2ac7f1335..375fea9f0 100644
--- a/grid_map_demos/CMakeLists.txt
+++ b/grid_map_demos/CMakeLists.txt
@@ -1,21 +1,22 @@
cmake_minimum_required(VERSION 3.5.1)
project(grid_map_demos)
-set(CMAKE_CXX_STANDARD 11)
-
+set(CMAKE_CXX_STANDARD 17)
+add_compile_options(-Wall -Wextra -Wpedantic)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
roscpp
grid_map_core
- grid_map_ros
grid_map_cv
grid_map_filters
grid_map_loader
grid_map_msgs
grid_map_octomap
+ grid_map_ros
grid_map_rviz_plugin
+ grid_map_sdf
grid_map_visualization
geometry_msgs
sensor_msgs
@@ -120,6 +121,11 @@ add_executable(interpolation_demo
src/InterpolationDemo.cpp
)
+add_executable(sdf_demo
+ src/sdf_demo_node.cpp
+ src/SdfDemo.cpp
+)
+
## Specify libraries to link a library or executable target against
target_link_libraries(
simple_demo
@@ -188,6 +194,11 @@ target_link_libraries(
${catkin_LIBRARIES}
)
+target_link_libraries(
+ sdf_demo
+ ${catkin_LIBRARIES}
+)
+
#############
## Install ##
#############
@@ -213,6 +224,7 @@ install(
resolution_change_demo
simple_demo
tutorial_demo
+ sdf_demo
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
diff --git a/grid_map_demos/config/filters_demo_filter_chain.yaml b/grid_map_demos/config/filters_demo_filter_chain.yaml
index 7c3214602..5de01d98a 100644
--- a/grid_map_demos/config/filters_demo_filter_chain.yaml
+++ b/grid_map_demos/config/filters_demo_filter_chain.yaml
@@ -3,6 +3,7 @@ grid_map_filters:
type: gridMapFilters/MockFilter
params:
processing_time: 100
+ print_name: true
- name: buffer_normalizer
type: gridMapFilters/BufferNormalizerFilter
@@ -110,7 +111,8 @@ grid_map_filters:
- name: traversability_lower_threshold
type: gridMapFilters/ThresholdFilter
params:
- layer: traversability
+ condition_layer: traversability
+ output_layer: traversability
lower_threshold: 0.0
set_to: 0.0
@@ -118,6 +120,7 @@ grid_map_filters:
- name: traversability_upper_threshold
type: gridMapFilters/ThresholdFilter
params:
- layer: traversability
+ condition_layer: traversability
+ output_layer: traversability
upper_threshold: 1.0
set_to: 1.0 # Other uses: .nan, .inf
diff --git a/grid_map_demos/config/normal_filter_comparison.yaml b/grid_map_demos/config/normal_filter_comparison.yaml
index c25b7aa86..f669149e4 100644
--- a/grid_map_demos/config/normal_filter_comparison.yaml
+++ b/grid_map_demos/config/normal_filter_comparison.yaml
@@ -1,23 +1,26 @@
grid_map_filters:
+ filter_chain:
# Compute surface normals.
- - name: surface_normals_area
- type: gridMapFilters/NormalVectorsFilter
- params:
- input_layer: elevation_filtered
- algorithm: area
- output_layers_prefix: normal_area_
- radius: 0.05
- normal_vector_positive_axis: z
- parallelization_enabled: true
- thread_number: 4
- # thread_number: -1 doesn't limit the number of threads to be used
+ -
+ name: surface_normals_area
+ type: gridMapFilters/NormalVectorsFilter
+ params:
+ input_layer: elevation_filtered
+ algorithm: area
+ output_layers_prefix: normal_area_
+ radius: 0.05
+ normal_vector_positive_axis: z
+ parallelization_enabled: true
+ thread_number: 4
+ # thread_number: -1 doesn't limit the number of threads to be used
- - name: surface_normals_raster
- type: gridMapFilters/NormalVectorsFilter
- params:
- input_layer: elevation_filtered
- algorithm: raster
- output_layers_prefix: normal_raster_
- normal_vector_positive_axis: z
- parallelization_enabled: true
- thread_number: 4
\ No newline at end of file
+ -
+ name: surface_normals_raster
+ type: gridMapFilters/NormalVectorsFilter
+ params:
+ input_layer: elevation_filtered
+ algorithm: raster
+ output_layers_prefix: normal_raster_
+ normal_vector_positive_axis: z
+ parallelization_enabled: true
+ thread_number: 4
\ No newline at end of file
diff --git a/grid_map_demos/config/sdf_demo.yaml b/grid_map_demos/config/sdf_demo.yaml
new file mode 100644
index 000000000..1b3299b0b
--- /dev/null
+++ b/grid_map_demos/config/sdf_demo.yaml
@@ -0,0 +1,16 @@
+grid_map_topic: /image_to_gridmap_demo/grid_map
+pointcloud_topic: /grid_map_sdf_demo/signed_distance_field
+elevation_layer: "elevation"
+image_to_gridmap_demo:
+ image_topic: "/image_publisher/image"
+ resolution: 0.03
+ map_frame_id: "map"
+ min_height: -0.3
+ max_height: 0.4
+grid_map_visualization:
+ grid_map_topic: /image_to_gridmap_demo/grid_map
+ grid_map_visualizations:
+ - name: elevation_points
+ type: point_cloud
+ params:
+ layer: elevation
\ No newline at end of file
diff --git a/grid_map_demos/include/grid_map_demos/FiltersDemo.hpp b/grid_map_demos/include/grid_map_demos/FiltersDemo.hpp
index 2a81c25a9..55bef936a 100644
--- a/grid_map_demos/include/grid_map_demos/FiltersDemo.hpp
+++ b/grid_map_demos/include/grid_map_demos/FiltersDemo.hpp
@@ -11,7 +11,11 @@
#include
-#include
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpedantic"
+#pragma GCC diagnostic ignored "-Wformat"
+#include
+#pragma GCC diagnostic pop
#include
#include
diff --git a/grid_map_demos/include/grid_map_demos/SdfDemo.hpp b/grid_map_demos/include/grid_map_demos/SdfDemo.hpp
new file mode 100644
index 000000000..a6f367573
--- /dev/null
+++ b/grid_map_demos/include/grid_map_demos/SdfDemo.hpp
@@ -0,0 +1,56 @@
+/*
+ * SdfDemo.hpp
+ *
+ * Created on: May 3, 2022
+ * Author: Ruben Grandia
+ * Institute: ETH Zurich
+ */
+
+#pragma once
+
+#include
+
+// ROS
+#include
+
+// gridmap
+#include
+#include
+
+namespace grid_map_demos {
+
+/**
+ * Subscribes to a gridmap, converts it to a signed distance field, and publishes the signed distance field as a pointcloud.
+ */
+class SdfDemo {
+ public:
+
+ /**
+ * Constructor.
+ * @param nodeHandle : the ROS node handle.
+ * @param mapTopic : grid map topic to subscribe to
+ * @param elevationLayer : name of the elevation layer
+ * @param pointcloudTopic : point cloud topic to publish the result to
+ */
+ SdfDemo(ros::NodeHandle& nodeHandle, const std::string& mapTopic, std::string elevationLayer, const std::string& pointcloudTopic);
+
+ private:
+ void callback(const grid_map_msgs::GridMap& message);
+
+ //! Grid map subscriber.
+ ros::Subscriber gridmapSubscriber_;
+
+ //! Pointcloud publisher.
+ ros::Publisher pointcloudPublisher_;
+
+ //! Free space publisher.
+ ros::Publisher freespacePublisher_;
+
+ //! Occupied space publisher.
+ ros::Publisher occupiedPublisher_;
+
+ //! Elevation layer in the received grid map
+ std::string elevationLayer_;
+};
+
+} // namespace grid_map_demos
\ No newline at end of file
diff --git a/grid_map_demos/launch/sdf_demo.launch b/grid_map_demos/launch/sdf_demo.launch
new file mode 100644
index 000000000..3bb155db6
--- /dev/null
+++ b/grid_map_demos/launch/sdf_demo.launch
@@ -0,0 +1,31 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/grid_map_demos/package.xml b/grid_map_demos/package.xml
index 9b31a038a..98b4650f9 100644
--- a/grid_map_demos/package.xml
+++ b/grid_map_demos/package.xml
@@ -1,10 +1,10 @@
grid_map_demos
- 1.6.3
+ 1.7.18
Demo nodes to demonstrate the usage of the grid map library.
- Maximilian Wulf
- Yoshua Nava
+ Magnus Gaertner
+ Mathieu Agostinucci
BSD
http://github.com/anybotics/grid_map
http://github.com/anybotics/grid_map/issues
@@ -13,13 +13,14 @@
roscpp
grid_map_core
- grid_map_ros
grid_map_cv
grid_map_filters
grid_map_loader
grid_map_msgs
grid_map_octomap
+ grid_map_ros
grid_map_rviz_plugin
+ grid_map_sdf
grid_map_visualization
geometry_msgs
sensor_msgs
diff --git a/grid_map_demos/rviz/sdf_demo.rviz b/grid_map_demos/rviz/sdf_demo.rviz
new file mode 100644
index 000000000..aeae58829
--- /dev/null
+++ b/grid_map_demos/rviz/sdf_demo.rviz
@@ -0,0 +1,236 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 0
+ Name: Displays
+ Property Tree Widget:
+ Expanded: ~
+ Splitter Ratio: 0.6114649772644043
+ Tree Height: 375
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: OccupiedSpace
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Class: grid_map_rviz_plugin/GridMap
+ Color: 200; 200; 200
+ Color Layer: elevation
+ Color Transformer: GridMapLayer
+ ColorMap: default
+ Enabled: false
+ Grid Cell Decimation: 1
+ Grid Line Thickness: 0.10000000149011612
+ Height Layer: elevation
+ Height Transformer: GridMapLayer
+ History Length: 1
+ Invert ColorMap: false
+ Max Color: 255; 255; 255
+ Max Intensity: 10
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: GridMap
+ Show Grid Lines: true
+ Topic: /image_to_gridmap_demo/grid_map
+ Unreliable: false
+ Use ColorMap: true
+ Value: false
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 0.08491750061511993
+ Min Value: -0.16515299677848816
+ Value: false
+ Axis: Z
+ Channel Name: z
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: Intensity
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 85; 127
+ Name: Elevation
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: false
+ Size (Pixels): 3
+ Size (m): 0.029999999329447746
+ Style: Boxes
+ Topic: /grid_map_visualization/elevation_points
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: false
+ Value: true
+ - Alpha: 0.30000001192092896
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: Intensity
+ Decay Time: 0
+ Enabled: false
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: SignedDistanceField
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Flat Squares
+ Topic: /grid_map_sdf_demo/signed_distance_field/full_sdf
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: false
+ - Alpha: 0.30000001192092896
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: Intensity
+ Decay Time: 0
+ Enabled: false
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: FreeSpace
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Flat Squares
+ Topic: /grid_map_sdf_demo/signed_distance_field/free_space
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: false
+ - Alpha: 0.30000001192092896
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: Intensity
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: OccupiedSpace
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Flat Squares
+ Topic: /grid_map_sdf_demo/signed_distance_field/occupied_space
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 255; 255; 255
+ Default Light: true
+ Fixed Frame: map
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Theta std deviation: 0.2617993950843811
+ Topic: /initialpose
+ X std deviation: 0.5
+ Y std deviation: 0.5
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 8.372665405273438
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Field of View: 0.7853981852531433
+ Focal Point:
+ X: 0.5306656956672668
+ Y: -0.5836256742477417
+ Z: -1.7025400400161743
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.4247995615005493
+ Target Frame:
+ Yaw: 2.58538818359375
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 594
+ Hide Left Dock: false
+ Hide Right Dock: true
+ QMainWindow State: 000000ff00000000fd00000004000000000000016a000001b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001b4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000040efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000040e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000003efc0100000002fb0000000800540069006d0065010000000000000500000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000390000001b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: true
+ Width: 1280
+ X: 2386
+ Y: 252
diff --git a/grid_map_demos/src/InterpolationDemo.cpp b/grid_map_demos/src/InterpolationDemo.cpp
index 5f4d3678b..35f5ad392 100644
--- a/grid_map_demos/src/InterpolationDemo.cpp
+++ b/grid_map_demos/src/InterpolationDemo.cpp
@@ -138,7 +138,7 @@ AnalyticalFunctions createGaussianWorld(grid_map::GridMap *map)
func.f_ = [g](double x,double y) {
double value = 0.0;
- for (int i = 0; i < g.size(); ++i) {
+ for (size_t i{0}; i < g.size(); ++i) {
const double x0 = g.at(i).x0;
const double y0 = g.at(i).y0;
const double varX = g.at(i).varX;
@@ -206,10 +206,6 @@ void interpolateInputMap(const grid_map::GridMap &dataMap,
Error computeInterpolationError(const AnalyticalFunctions &groundTruth,
const grid_map::GridMap &map)
{
- const double r = map.getResolution();
- const double dimX = map.getLength().x() / 2.0 - 3.0 * r;
- const double dimY = map.getLength().y() / 2.0 - 3.0 * r;
-
unsigned int count = 0;
Error error;
const int nRow = map.getSize().x();
@@ -295,7 +291,7 @@ InterpolationDemo::Statistics InterpolationDemo::computeStatistics() const
stats.insert( { world->first, methodsStats });
}
- return std::move(stats);
+ return stats;
}
InterpolationDemo::ErrorAndDuration InterpolationDemo::interpolateAndComputeError(
diff --git a/grid_map_demos/src/SdfDemo.cpp b/grid_map_demos/src/SdfDemo.cpp
new file mode 100644
index 000000000..0764ed6b8
--- /dev/null
+++ b/grid_map_demos/src/SdfDemo.cpp
@@ -0,0 +1,58 @@
+/*
+* SdfDemo.cpp
+*
+* Created on: May 3, 2022
+* Author: Ruben Grandia
+* Institute: ETH Zurich
+ */
+
+#include "grid_map_demos/SdfDemo.hpp"
+
+#include
+
+#include
+#include
+
+namespace grid_map_demos {
+
+SdfDemo::SdfDemo(ros::NodeHandle& nodeHandle, const std::string& mapTopic, std::string elevationLayer, const std::string& pointcloudTopic)
+ : elevationLayer_(std::move(elevationLayer)) {
+ pointcloudPublisher_ = nodeHandle.advertise(pointcloudTopic + "/full_sdf", 1);
+ freespacePublisher_ = nodeHandle.advertise(pointcloudTopic + "/free_space", 1);
+ occupiedPublisher_ = nodeHandle.advertise(pointcloudTopic + "/occupied_space", 1);
+ gridmapSubscriber_ = nodeHandle.subscribe(mapTopic, 1, &SdfDemo::callback, this);
+}
+
+void SdfDemo::callback(const grid_map_msgs::GridMap& message) {
+ // Convert message to map.
+ grid_map::GridMap map;
+ std::vector layers{elevationLayer_};
+ grid_map::GridMapRosConverter::fromMessage(message, map, layers, false, false);
+ auto& elevationData = map.get(elevationLayer_);
+
+ // Inpaint if needed.
+ if (elevationData.hasNaN()) {
+ const float inpaint{elevationData.minCoeffOfFinites()};
+ ROS_WARN("[SdfDemo] Map contains NaN values. Will apply inpainting with min value.");
+ elevationData = elevationData.unaryExpr([=](float v) { return std::isfinite(v)? v : inpaint; });
+ }
+
+ // Generate SDF.
+ const float heightMargin{0.1};
+ const float minValue{elevationData.minCoeffOfFinites() - heightMargin};
+ const float maxValue{elevationData.maxCoeffOfFinites() + heightMargin};
+ grid_map::SignedDistanceField sdf(map, elevationLayer_, minValue, maxValue);
+
+ // Extract as point clouds.
+ sensor_msgs::PointCloud2 pointCloud2Msg;
+ grid_map::GridMapRosConverter::toPointCloud(sdf, pointCloud2Msg);
+ pointcloudPublisher_.publish(pointCloud2Msg);
+
+ grid_map::GridMapRosConverter::toPointCloud(sdf, pointCloud2Msg, 1, [](float sdfValue) { return sdfValue > 0.0; });
+ freespacePublisher_.publish(pointCloud2Msg);
+
+ grid_map::GridMapRosConverter::toPointCloud(sdf, pointCloud2Msg, 1, [](float sdfValue) { return sdfValue <= 0.0; });
+ occupiedPublisher_.publish(pointCloud2Msg);
+}
+
+} // namespace grid_map_demos
\ No newline at end of file
diff --git a/grid_map_demos/src/iterator_benchmark.cpp b/grid_map_demos/src/iterator_benchmark.cpp
index ec09714e4..7ead4fb69 100644
--- a/grid_map_demos/src/iterator_benchmark.cpp
+++ b/grid_map_demos/src/iterator_benchmark.cpp
@@ -74,8 +74,8 @@ void runCustomIndexIteration(GridMap& map, const string& layer_from, const strin
{
const auto& data_from = map[layer_from];
auto& data_to = map[layer_to];
- for (size_t j = 0; j < data_to.cols(); ++j) {
- for (size_t i = 0; i < data_to.rows(); ++i) {
+ for (Eigen::Index j{0}; j < data_to.cols(); ++j) {
+ for (Eigen::Index i{0}; i < data_to.rows(); ++i) {
const float value_from = data_from(i, j);
float& value_to = data_to(i, j);
value_to = value_to > value_from ? value_to : value_from;
@@ -90,12 +90,12 @@ void runCustomLinearIndexIteration(GridMap& map, const string& layer_from, const
{
const auto& data_from = map[layer_from];
auto& data_to = map[layer_to];
- for (size_t i = 0; i < data_to.size(); ++i) {
+ for (Eigen::Index i = 0; i < data_to.size(); ++i) {
data_to(i) = data_to(i) > data_from(i) ? data_to(i) : data_from(i);
}
}
-int main(int argc, char* argv[])
+int main()
{
GridMap map;
map.setGeometry(Length(20.0, 20.0), 0.004, Position(0.0, 0.0));
diff --git a/grid_map_demos/src/normal_filter_comparison_node.cpp b/grid_map_demos/src/normal_filter_comparison_node.cpp
index bb78e1ca8..01a314382 100644
--- a/grid_map_demos/src/normal_filter_comparison_node.cpp
+++ b/grid_map_demos/src/normal_filter_comparison_node.cpp
@@ -4,7 +4,11 @@
* @brief Node for comparing different normal filters performances.
*/
-#include
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpedantic"
+#pragma GCC diagnostic ignored "-Wformat"
+#include
+#pragma GCC diagnostic pop
#include
#include
#include
diff --git a/grid_map_demos/src/sdf_demo_node.cpp b/grid_map_demos/src/sdf_demo_node.cpp
new file mode 100644
index 000000000..3c0c4f1d1
--- /dev/null
+++ b/grid_map_demos/src/sdf_demo_node.cpp
@@ -0,0 +1,33 @@
+/*
+* sdf_demo_node.cpp
+*
+* Created on: May 3, 2022
+* Author: Ruben Grandia
+* Institute: ETH Zurich
+*/
+
+#include
+
+#include
+
+#include "grid_map_demos/SdfDemo.hpp"
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "grid_map_sdf_demo");
+ ros::NodeHandle nodeHandle("");
+
+ std::string elevationLayer;
+ nodeHandle.getParam("elevation_layer", elevationLayer);
+
+ std::string mapTopic;
+ nodeHandle.getParam("grid_map_topic", mapTopic);
+
+ std::string pointcloudTopic;
+ nodeHandle.getParam("pointcloud_topic", pointcloudTopic);
+
+ grid_map_demos::SdfDemo sdfDemo(nodeHandle, mapTopic, elevationLayer, pointcloudTopic);
+
+ ros::spin();
+ return 0;
+}
\ No newline at end of file
diff --git a/grid_map_demos/src/tutorial_demo_node.cpp b/grid_map_demos/src/tutorial_demo_node.cpp
index 343dea3d5..76e6341cd 100644
--- a/grid_map_demos/src/tutorial_demo_node.cpp
+++ b/grid_map_demos/src/tutorial_demo_node.cpp
@@ -88,9 +88,6 @@ int main(int argc, char** argv)
// Show absolute difference and compute mean squared error.
map.add("error", (map.get("elevation_filtered") - map.get("elevation")).cwiseAbs());
- unsigned int nCells = map.getSize().prod();
- // cppcheck-suppress unreadVariable
- double rootMeanSquaredError = sqrt((map["error"].array().pow(2).sum()) / nCells);
// Publish grid map.
map.setTimestamp(time.toNSec());
diff --git a/grid_map_filters/CHANGELOG.rst b/grid_map_filters/CHANGELOG.rst
index 32e70199e..e80862cf6 100644
--- a/grid_map_filters/CHANGELOG.rst
+++ b/grid_map_filters/CHANGELOG.rst
@@ -2,6 +2,22 @@
Changelog for package grid_map_filters
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.7.0 (2022-03-17)
+------------------
+
+1.6.8 (2021-12-15)
+------------------
+* Improved the template definition structure:
+* - grid_map_filters do not need to be template classes anymore.
+* Contributors: Magnus Gärtner
+
+1.6.4 (2020-12-04)
+------------------
+* Extended the ThresholdFilter:
+* - The layer parameter was replaced by condition_layer and output_layer.
+* - The previous behavior can be restored by setting both new parameters to the same value.
+* Contributors: Magnus Gärtner
+
1.6.3 (2020-09-08)
------------------
* Implemented a new filter:
diff --git a/grid_map_filters/CMakeLists.txt b/grid_map_filters/CMakeLists.txt
index b18882ec3..37194096b 100644
--- a/grid_map_filters/CMakeLists.txt
+++ b/grid_map_filters/CMakeLists.txt
@@ -1,41 +1,40 @@
cmake_minimum_required(VERSION 3.5.1)
project(grid_map_filters)
-# Better with parallelized algorithms.
-#set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS} -ffast-math")
-
# Better with serial algorithms.
-set(CMAKE_CXX_STANDARD 11)
+set(CMAKE_CXX_STANDARD 17)
# We want performance (fast-math) but also need a representation for NaN values to represent missing values.
# Therefore, we disable the finite-math-only flag that was set by fast-math.
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ffast-math -fno-finite-math-only")
-
+add_compile_options(-Wall -Wextra -Wpedantic)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
# Other possible options.
#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native -mtune=native -ffast-math")
#set(TARGET_ARCHITECTURE "kaby-lake")
-## Find catkin macros and libraries
-find_package(catkin REQUIRED COMPONENTS
+## Create a list of catkin package dependencies used in header files of this package.
+set(CATKIN_PACKAGE_HEADER_DEPENDENCIES
grid_map_core
grid_map_ros
grid_map_msgs
filters
)
-## System dependencies are found with CMake's conventions
-find_package(PkgConfig REQUIRED)
+## Create a list of catkin package dependencies, now for both header and source files.
+set(CATKIN_PACKAGE_DEPENDENCIES
+ ${CATKIN_PACKAGE_HEADER_DEPENDENCIES}
+)
-pkg_check_modules(TBB "tbb")
-if (TBB_FOUND)
- add_definitions(
- ${TBB_CFLAGS}
- ${TBB_CFLAGS_OTHER}
- )
-else()
- message([FATAL_ERROR] "tbb module not found")
-endif ()
+## Find catkin dependencies for building this package.
+find_package(catkin REQUIRED
+ COMPONENTS
+ ${CATKIN_PACKAGE_DEPENDENCIES}
+)
+
+find_package(TBB 2020.1 EXACT REQUIRED)
+
+find_package(OpenCV REQUIRED)
###################################
## catkin specific configuration ##
@@ -49,14 +48,14 @@ endif ()
catkin_package(
INCLUDE_DIRS
include
- ${TBB_INCLUDE_DIRS}
+ ${OpenCV_INCLUDE_DIRS} # TODO Remove this include directory, currently it exported by the medianFillFilter header.
LIBRARIES
${PROJECT_NAME}
+ ${PROJECT_NAME}_plugins
CATKIN_DEPENDS
- grid_map_ros
- grid_map_core
- grid_map_msgs
- filters
+ ${CATKIN_PACKAGE_HEADER_DEPENDENCIES}
+ DEPENDS
+ OpenCV
)
###########
@@ -68,7 +67,7 @@ include_directories(
include
SYSTEM
${catkin_INCLUDE_DIRS}
- ${TBB_INCLUDE_DIRS}
+ ${OpenCV_INCLUDE_DIRS}
)
## Declare a cpp library
@@ -93,9 +92,45 @@ add_library(${PROJECT_NAME}
src/BufferNormalizerFilter.cpp
)
+target_include_directories(${PROJECT_NAME}
+ PRIVATE
+ ${TBB_INCLUDE_DIRS}
+)
+
+target_include_directories(${PROJECT_NAME}
+ SYSTEM PUBLIC
+ ${catkin_INCLUDE_DIRS}
+)
+
target_link_libraries(${PROJECT_NAME}
${catkin_LIBRARIES}
${TBB_LIBRARIES}
+ ${OpenCV_LIBRARIES}
+)
+
+add_dependencies(${PROJECT_NAME}
+ ${catkin_EXPORTED_TARGETS}
+)
+
+# Instantiate plugin exports for every filter in this package.
+add_library(${PROJECT_NAME}_plugins
+ src/plugins.cpp
+)
+
+target_include_directories(${PROJECT_NAME}_plugins
+ SYSTEM PUBLIC
+ ${catkin_INCLUDE_DIRS}
+)
+
+target_link_libraries(${PROJECT_NAME}_plugins
+ ${PROJECT_NAME}
+ ${TBB_LIBRARIES} # is this necessary?
+ ${OpenCV_LIBRARIES} # is this necessary?
+)
+
+add_dependencies(${PROJECT_NAME}_plugins
+ ${catkin_EXPORTED_TARGETS}
+ ${PROJECT_NAME}
)
#############
@@ -104,23 +139,32 @@ target_link_libraries(${PROJECT_NAME}
# Mark executables and/or libraries for installation
install(
- TARGETS ${PROJECT_NAME}
- ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ TARGETS
+ ${PROJECT_NAME}
+ ${PROJECT_NAME}_plugins
+ ARCHIVE DESTINATION
+ ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION
+ ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION
+ ${CATKIN_PACKAGE_BIN_DESTINATION}
)
# Mark cpp header files for installation
install(
- DIRECTORY include/${PROJECT_NAME}/
- DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+ DIRECTORY
+ include/${PROJECT_NAME}/
+ DESTINATION
+ ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.hpp"
)
# Mark other files for installation
install(
- FILES filter_plugins.xml
- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ FILES
+ filter_plugins.xml
+ DESTINATION
+ ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
#############
@@ -131,6 +175,8 @@ if (CATKIN_ENABLE_TESTING)
catkin_add_gtest(${PROJECT_NAME}-test
test/test_grid_map_filters.cpp
test/median_fill_filter_test.cpp
+ test/mock_filter_test.cpp
+ test/threshold_filter_test.cpp
)
target_include_directories(${PROJECT_NAME}-test PRIVATE
include
@@ -164,4 +210,4 @@ if(cmake_clang_tools_FOUND)
add_default_clang_tooling(
DISABLE_CLANG_FORMAT
)
-endif(cmake_clang_tools_FOUND)
\ No newline at end of file
+endif(cmake_clang_tools_FOUND)
diff --git a/grid_map_filters/filter_plugins.xml b/grid_map_filters/filter_plugins.xml
index 669d80a1a..d8e96ac60 100644
--- a/grid_map_filters/filter_plugins.xml
+++ b/grid_map_filters/filter_plugins.xml
@@ -1,91 +1,91 @@
-
-
+
+
Set values below/above a threshold to a specified value.
-
+
Compute for each cell of a layer the minimal value inside a radius.
-
+
Compute for each cell of a layer the mean value inside a radius.
-
+
Fill holes (NaNs) in a layer using the median value inside a radius. Optionally, apply median calculations for values that are already finite.
-
+
Does Identity, but allows to set a delay and debug prints. Used for testing purposes.
-
+
Compute the normal vectors of a layer in a map.
-
+
Compute the curvature (second derivative) of a layer in the map.
-
+
Compute a new color layer based on normal vectors layers.
-
+
Compute the diffuse lighting of a surface as new black and white color layer.
-
+
Parse and evaluate a mathematical matrix expression with layers of a grid map.
-
+
Parse and evaluate a mathematical matrix expression within a sliding window on a layer of a grid map.
-
+
Duplicate a layer of a grid map.
-
+
Delete layers from a grid map.
-
+
Creates a new color layer.
-
+
Creates a new color layer with the color mapped between min. and max. value.
-
+
Blend two color layers.
-
+
Set specified layers of a grid map as basic layers.
-
+
Normalizes the buffer of a map such that it has default (zero) start index.
diff --git a/grid_map_filters/include/grid_map_filters/BufferNormalizerFilter.hpp b/grid_map_filters/include/grid_map_filters/BufferNormalizerFilter.hpp
index 999191ad6..a22a8f3dd 100644
--- a/grid_map_filters/include/grid_map_filters/BufferNormalizerFilter.hpp
+++ b/grid_map_filters/include/grid_map_filters/BufferNormalizerFilter.hpp
@@ -8,19 +8,17 @@
#pragma once
-#include
-
#include
+#include
+#include
+
namespace grid_map {
/*!
* Normalizes the buffer of a map such that it has default (zero) start index.
*/
-template
-class BufferNormalizerFilter : public filters::FilterBase
-{
-
+class BufferNormalizerFilter : public filters::FilterBase {
public:
/*!
* Constructor
@@ -30,19 +28,19 @@ class BufferNormalizerFilter : public filters::FilterBase
/*!
* Destructor.
*/
- virtual ~BufferNormalizerFilter();
+ ~BufferNormalizerFilter() override;
/*!
* Configures the filter from parameters on the parameter server.
*/
- virtual bool configure();
+ bool configure() override;
/*!
* Normalizes the buffer of a map.
* @param mapIn the input map before normalization.
* @param mapOut the normalized map.
*/
- virtual bool update(const T& mapIn, T& mapOut);
+ bool update(const GridMap& mapIn, GridMap& mapOut) override;
};
-} /* namespace */
+} // namespace grid_map
diff --git a/grid_map_filters/include/grid_map_filters/ColorBlendingFilter.hpp b/grid_map_filters/include/grid_map_filters/ColorBlendingFilter.hpp
index c9b51d50c..3f4c56645 100644
--- a/grid_map_filters/include/grid_map_filters/ColorBlendingFilter.hpp
+++ b/grid_map_filters/include/grid_map_filters/ColorBlendingFilter.hpp
@@ -8,19 +8,18 @@
#pragma once
-#include
-
#include
#include
+#include
+#include
+
namespace grid_map {
/*!
* Blend two color layers.
*/
-template
-class ColorBlendingFilter : public filters::FilterBase
-{
+class ColorBlendingFilter : public filters::FilterBase {
public:
/*!
* Constructor
@@ -30,39 +29,34 @@ class ColorBlendingFilter : public filters::FilterBase
/*!
* Destructor.
*/
- virtual ~ColorBlendingFilter();
+ ~ColorBlendingFilter() override;
/*!
* Configures the filter.
*/
- virtual bool configure();
+ bool configure() override;
/*!
* Compute a new color layer based on blending two color layers.
* @param mapIn grid map containing the two color layers.
* @param mapOut grid map containing mapIn and the blended color layer.
*/
- virtual bool update(const T& mapIn, T& mapOut);
+ bool update(const GridMap& mapIn, GridMap& mapOut) override;
private:
-
- enum class BlendModes {
- Normal,
- HardLight,
- SoftLight
- };
+ enum class BlendModes { Normal, HardLight, SoftLight };
//! Input layers.
std::string backgroundLayer_, foregroundLayer_;
- //! Blend mode.
- BlendModes blendMode_;
-
//! Opacity of foreground layer.
double opacity_;
+ //! Blend mode.
+ BlendModes blendMode_;
+
//! Output layer name.
std::string outputLayer_;
};
-} /* namespace */
+} // namespace grid_map
diff --git a/grid_map_filters/include/grid_map_filters/ColorFillFilter.hpp b/grid_map_filters/include/grid_map_filters/ColorFillFilter.hpp
index 21b6bcb15..a6ccfac72 100644
--- a/grid_map_filters/include/grid_map_filters/ColorFillFilter.hpp
+++ b/grid_map_filters/include/grid_map_filters/ColorFillFilter.hpp
@@ -8,20 +8,17 @@
#pragma once
-#include
-
#include
-#include
+
+#include
+#include
namespace grid_map {
/*!
* Creates a new color layer.
*/
-template
-class ColorFillFilter : public filters::FilterBase
-{
-
+class ColorFillFilter : public filters::FilterBase {
public:
/*!
* Constructor
@@ -31,19 +28,19 @@ class ColorFillFilter : public filters::FilterBase
/*!
* Destructor.
*/
- virtual ~ColorFillFilter();
+ ~ColorFillFilter() override;
/*!
* Configures the filter.
*/
- virtual bool configure();
+ bool configure() override;
/*!
* Adds a new color layer.
* @param mapIn grid map to add the new layer.
* @param mapOut grid map the grid map with the new color layer.
*/
- virtual bool update(const T& mapIn, T& mapOut);
+ bool update(const GridMap& mapIn, GridMap& mapOut) override;
private:
//! Color.
@@ -56,4 +53,4 @@ class ColorFillFilter : public filters::FilterBase
std::string outputLayer_;
};
-} /* namespace */
+} // namespace grid_map
diff --git a/grid_map_filters/include/grid_map_filters/ColorMapFilter.hpp b/grid_map_filters/include/grid_map_filters/ColorMapFilter.hpp
index 87b1dda38..d19e2d1ce 100644
--- a/grid_map_filters/include/grid_map_filters/ColorMapFilter.hpp
+++ b/grid_map_filters/include/grid_map_filters/ColorMapFilter.hpp
@@ -8,21 +8,19 @@
#pragma once
-#include
-#include
-
#include
#include
+#include
+#include
+#include
+
namespace grid_map {
/*!
* Creates a new color layer with the color mapped between min. and max. value.
*/
-template
-class ColorMapFilter : public filters::FilterBase
-{
-
+class ColorMapFilter : public filters::FilterBase {
public:
/*!
* Constructor
@@ -32,19 +30,19 @@ class ColorMapFilter : public filters::FilterBase
/*!
* Destructor.
*/
- virtual ~ColorMapFilter();
+ ~ColorMapFilter() override;
/*!
* Configures the filter.
*/
- virtual bool configure();
+ bool configure() override;
/*!
* Adds a new color layer.
* @param mapIn grid map to add the new layer.
* @param mapOut grid map the grid map with the new color layer.
*/
- virtual bool update(const T& mapIn, T& mapOut);
+ bool update(const GridMap& mapIn, GridMap& mapOut) override;
private:
//! Min./max. colors.
@@ -60,4 +58,4 @@ class ColorMapFilter : public filters::FilterBase
std::string outputLayer_;
};
-} /* namespace */
+} // namespace grid_map
diff --git a/grid_map_filters/include/grid_map_filters/CurvatureFilter.hpp b/grid_map_filters/include/grid_map_filters/CurvatureFilter.hpp
index 18858bd43..0f3762a32 100644
--- a/grid_map_filters/include/grid_map_filters/CurvatureFilter.hpp
+++ b/grid_map_filters/include/grid_map_filters/CurvatureFilter.hpp
@@ -8,20 +8,18 @@
#pragma once
-#include
-
#include
#include
+#include
+#include
+
namespace grid_map {
/*!
* Compute the curvature (second derivative) of a layer in the map.
*/
-template
-class CurvatureFilter : public filters::FilterBase
-{
-
+class CurvatureFilter : public filters::FilterBase {
public:
/*!
* Constructor
@@ -31,12 +29,12 @@ class CurvatureFilter : public filters::FilterBase
/*!
* Destructor.
*/
- virtual ~CurvatureFilter();
+ ~CurvatureFilter() override;
/*!
* Configures the filter from parameters on the Parameter Server
*/
- virtual bool configure();
+ bool configure() override;
/*!
* Compute the curvature of a layer in a map and
@@ -44,7 +42,7 @@ class CurvatureFilter : public filters::FilterBase
* @param mapIn grid map containing the layer for which the curvature is computed for.
* @param mapOut grid map containing mapIn and the new layer for the curvature.
*/
- virtual bool update(const T& mapIn, T& mapOut);
+ bool update(const GridMap& mapIn, GridMap& mapOut) override;
private:
//! Input layer name.
@@ -54,4 +52,4 @@ class CurvatureFilter : public filters::FilterBase
std::string outputLayer_;
};
-} /* namespace */
+} // namespace grid_map
diff --git a/grid_map_filters/include/grid_map_filters/DeletionFilter.hpp b/grid_map_filters/include/grid_map_filters/DeletionFilter.hpp
index 048da3219..658b02720 100644
--- a/grid_map_filters/include/grid_map_filters/DeletionFilter.hpp
+++ b/grid_map_filters/include/grid_map_filters/DeletionFilter.hpp
@@ -8,20 +8,18 @@
#pragma once
-#include
-
-#include
#include