[ROS 2][grid_map_core] Update ROS 2 branch with the latest changes from master

grid_map_core/CHANGELOG.rst

@@ -2,6 +2,10 @@
 Changelog for package grid_map_core
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.4.0 (2024-11-26)
+------------------
+* Update changes from master
+
 2.3.0 (2024-07-29)
 ------------------
 
@@ -27,6 +31,12 @@ Changelog for package grid_map_core
 * Initial ROS2 port
 * Contributors: Maximilian Wulf, Steve Macenski
 
+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.

grid_map_core/include/grid_map_core/GridMap.hpp

@@ -6,8 +6,7 @@
  *	 Institute: ETH Zurich, ANYbotics
  */
 
-#ifndef GRID_MAP_CORE__GRIDMAP_HPP_
-#define GRID_MAP_CORE__GRIDMAP_HPP_
+#pragma once
 
 // Eigen
 #include <Eigen/Core>
@@ -308,6 +307,7 @@ class GridMap
   /*!
    * 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.
@@ -334,11 +334,19 @@ class GridMap
   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.
+   * 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.
@@ -579,12 +587,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 grid_map
-#endif  // GRID_MAP_CORE__GRIDMAP_HPP_

grid_map_core/include/grid_map_core/GridMapMath.hpp

@@ -6,8 +6,7 @@
  *	 Institute: ETH Zurich, ANYbotics
  */
 
-#ifndef GRID_MAP_CORE__GRIDMAPMATH_HPP_
-#define GRID_MAP_CORE__GRIDMAPMATH_HPP_
+#pragma once
 
 #include <Eigen/Core>
 #include <vector>
@@ -18,6 +17,10 @@
 
 namespace grid_map
 {
+union Color{
+  uint64_t longColor_;
+  float floatColor_;
+};
 
 /*!
  * Gets the position of a cell specified by its index in the map frame.
@@ -83,7 +86,7 @@ void getPositionOfDataStructureOrigin(
   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.
@@ -101,7 +104,7 @@ bool getIndexShiftFromPositionShift(
  * 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.
  */
@@ -122,7 +125,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);
@@ -139,7 +142,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);
@@ -237,7 +240,7 @@ bool getBufferRegionsForSubmap(
  * @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,
@@ -257,7 +260,7 @@ bool incrementIndex(
  * @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,
@@ -311,26 +314,6 @@ 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<Index> 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<Index> & regionIndeces, const Size & regionSizes,
-  std::vector<Index> indices);
 
 /*!
  * Transforms an int color value (concatenated RGB values) to an int color vector (RGB from 0-255).
@@ -379,4 +362,3 @@ void colorVectorToValue(const Eigen::Vector3i & colorVector, float & colorValue)
 void colorVectorToValue(const Eigen::Vector3f & colorVector, float & colorValue);
 
 }  // namespace grid_map
-#endif  // GRID_MAP_CORE__GRIDMAPMATH_HPP_

grid_map_core/include/grid_map_core/gtest_eigen.hpp

@@ -5,9 +5,7 @@
  * @brief  Code to simplify Eigen integration into GTest. Pretty basic but the error messages are good.
  */
 
-
-#ifndef GRID_MAP_CORE__GTEST_EIGEN_HPP_
-#define GRID_MAP_CORE__GTEST_EIGEN_HPP_
+#pragma once
 
 #include <gtest/gtest.h>
 #include <Eigen/Core>
@@ -49,7 +47,9 @@ void assertEqual(const M1 & A, const M2 & B, std::string const & message = "")
   for (int r = 0; r < A.rows(); r++) {
     for (int c = 0; c < A.cols(); c++) {
       if (std::isnan(A(r, c))) {
-        ASSERT_TRUE(std::isnan(B(r, c)));
+        ASSERT_TRUE(std::isnan(B(r, c))) << message << "\nNaN check failed at (" << r
+                                         << "," << c << ")\n"
+                                         << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
       } else {
         ASSERT_EQ(
           A(r, c),
@@ -78,7 +78,9 @@ void assertNear(const M1 & A, const M2 & B, T tolerance, std::string const & mes
   for (int r = 0; r < A.rows(); r++) {
     for (int c = 0; c < A.cols(); c++) {
       if (std::isnan(A(r, c))) {
-        ASSERT_TRUE(std::isnan(B(r, c)));
+        ASSERT_TRUE(std::isnan(B(r, c))) << message << "\nNaN check failed at (" << r
+                                         << "," << c << ")\n"
+                                         << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
       } else {
         ASSERT_NEAR(
           A(r, c), B(r, c),
@@ -105,7 +107,9 @@ void expectNear(const M1 & A, const M2 & B, T tolerance, std::string const & mes
   for (int r = 0; r < A.rows(); r++) {
     for (int c = 0; c < A.cols(); c++) {
       if (std::isnan(A(r, c))) {
-        EXPECT_TRUE(std::isnan(B(r, c)));
+        EXPECT_TRUE(std::isnan(B(r, c))) << message << "\nNaN check failed at (" << r
+                                         << "," << c << ")\n"
+                                         << "\nMatrix A:\n" << A << "\nand matrix B\n" << B;
       } else {
         EXPECT_NEAR(
           A(r, c), B(r, c),
@@ -209,4 +213,3 @@ inline bool compareRelative(
   }
 
 }  // namespace grid_map
-#endif  // GRID_MAP_CORE__GTEST_EIGEN_HPP_

grid_map_core/include/grid_map_core/utils/testing.hpp

@@ -0,0 +1,44 @@
+/**
+ * Copied from ANYbotics/eigen_utils, to avoid testing dependency.
+ */
+
+#pragma once
+
+#include <gtest/gtest.h>
+#include <Eigen/Core>
+#include <string>
+
+#define ASSERT_MATRICES_EQ_WITH_NAN(first, second) \
+  assertMatrixesEqualWithNan((first), #first, (second), #second, __LINE__)
+static void assertMatrixesEqualWithNan(
+  Eigen::Ref<const Eigen::MatrixXf> first,
+  std::string firstName,
+  Eigen::Ref<const Eigen::MatrixXf> 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
+}

grid_map_core/src/CubicInterpolation.cpp

@@ -17,9 +17,6 @@ namespace grid_map
 
 unsigned int bindIndexToRange(int idReq, unsigned int nElem)
 {
-  if (idReq < 0) {
-    return 0;
-  }
   if ((unsigned)idReq >= nElem) {
     return static_cast<unsigned int>(nElem - 1);
   }
@@ -37,7 +34,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:
@@ -142,10 +139,7 @@ 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
@@ -202,7 +196,7 @@ bool evaluateBicubicInterpolation(
   FunctionValueMatrix functionValues;
   assembleFunctionValueMatrix(f, dfx, dfy, ddfxy, &functionValues);
 
-  // get normalized coordiantes
+  // get normalized coordinates
   Position normalizedCoordinates;
   if (!computeNormalizedCoordinates(
       gridMap, unitSquareCornerIndices.bottomLeft_, queriedPosition,
@@ -275,10 +269,7 @@ 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(
@@ -390,7 +381,7 @@ double mixedSecondOrderDerivativeAt(
   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
    */