Bumblebee/testing

grid_map_core/include/grid_map_core/Polygon.hpp

@@ -210,6 +210,15 @@ class Polygon
    */
   static Polygon monotoneChainConvexHullOfPoints(
       const std::vector<Position, Eigen::aligned_allocator<Position>>& points);
+
+  /*!
+   * Computes the convex hull of given points, using Andrew's monotone chain
+   * convex hull algorithm, and returns it as polygon. Overload for standard allocator.
+   * @param[in] points points to use to compute the convex hull used to create
+   * the polygon.
+   * @return convex hull as polygon.
+   */
+  static Polygon monotoneChainConvexHullOfPoints(const std::vector<Position>& points);
 
  protected:
   /*!

grid_map_core/src/GridMapMath.cpp

@@ -111,13 +111,26 @@ void getIndexFromPositionUnsafe(Index& index, const Position& position, const Le
 {
   Vector offset;
   getVectorToOrigin(offset, mapLength);
+
+  // Prevent division by zero or invalid resolution
+  if (resolution <= 0.0 || !std::isfinite(resolution)) {
+    index = Index::Zero();
+    return;
+  }
+
   Vector indexVector = ((position - offset - mapPosition).array() / resolution).matrix();
   index = getIndexFromIndexVector(indexVector, bufferSize, bufferStartIndex);
 }
 
 bool getIndexFromPosition(Index& index, const Position& position, const Length& mapLength, const Position& mapPosition,
                           const double& resolution, const Size& bufferSize, const Index& bufferStartIndex)
 {
+  // Prevent division by zero or invalid resolution
+  if (resolution <= 0.0 || !std::isfinite(resolution)) {
+    index = Index::Zero();
+    return false;
+  }
+
   Vector offset;
   getVectorToOrigin(offset, mapLength);
   Vector indexVector = ((position - offset - mapPosition).array() / resolution).matrix();
@@ -149,6 +162,12 @@ void getPositionOfDataStructureOrigin(const Position& position, const Length& ma
 
 bool getIndexShiftFromPositionShift(Index& indexShift, const Vector& positionShift, const double& resolution)
 {
+  // Prevent division by zero or invalid resolution
+  if (resolution <= 0.0 || !std::isfinite(resolution)) {
+    indexShift = Index::Zero();
+    return false;
+  }
+
   Vector indexShiftVectorTemp = (positionShift.array() / resolution).matrix();
   Eigen::Vector2i indexShiftVector;
 
@@ -202,6 +221,12 @@ void wrapIndexToRange(Index& index, const Size& bufferSize)
 
 void wrapIndexToRange(int& index, int bufferSize)
 {
+  // Safety check: prevent modulo by zero
+  if (bufferSize <= 0) {
+    index = 0;
+    return;
+  }
+
   // Try shortcuts before resorting to the expensive modulo operation.
   if (index < bufferSize)
   {

grid_map_core/src/Polygon.cpp

@@ -267,6 +267,13 @@ Polygon Polygon::convexHull(Polygon& polygon1, Polygon& polygon2)
   return monotoneChainConvexHullOfPoints(vertices);
 }
 
+Polygon Polygon::monotoneChainConvexHullOfPoints(const std::vector<Position>& points)
+{
+  // Convert to aligned allocator version and delegate
+  std::vector<Position, Eigen::aligned_allocator<Position>> alignedPoints(points.begin(), points.end());
+  return monotoneChainConvexHullOfPoints(alignedPoints);
+}
+
 Polygon Polygon::monotoneChainConvexHullOfPoints(
     const std::vector<Position, Eigen::aligned_allocator<Position>>& points)
 {

grid_map_core/src/iterators/CircleIterator.cpp

@@ -78,14 +78,33 @@ bool CircleIterator::isInside() const
 void CircleIterator::findSubmapParameters(const Position& center, const double radius, Index& startIndex,
                                           Size& bufferSize) const
 {
+  // Safety checks
+  if (resolution_ <= 0.0 || !std::isfinite(resolution_) || (bufferSize_.array() <= 0).any()) {
+    startIndex = bufferStartIndex_;
+    bufferSize = Size::Zero();
+    return;
+  }
+
   Position topLeft = center.array() + radius;
   Position bottomRight = center.array() - radius;
   boundPositionToRange(topLeft, mapLength_, mapPosition_);
   boundPositionToRange(bottomRight, mapLength_, mapPosition_);
-  getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
+
   Index endIndex;
-  getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
+  if (!getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_) ||
+      !getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_)) {
+    startIndex = bufferStartIndex_;
+    bufferSize = Size::Zero();
+    return;
+  }
+
   bufferSize = getSubmapSizeFromCornerIndeces(startIndex, endIndex, bufferSize_, bufferStartIndex_);
+
+  // Validate positive size
+  if ((bufferSize.array() <= 0).any()) {
+    bufferSize = Size::Zero();
+    startIndex = bufferStartIndex_;
+  }
 }
 
 } /* namespace grid_map */

grid_map_core/src/iterators/EllipseIterator.cpp

@@ -88,18 +88,38 @@ bool EllipseIterator::isInside() const
 void EllipseIterator::findSubmapParameters(const Position& center, const Length& length, const double rotation,
                                            Index& startIndex, Size& bufferSize) const
 {
+  // Safety checks
+  if (resolution_ <= 0.0 || !std::isfinite(resolution_) || (bufferSize_.array() <= 0).any()) {
+    startIndex = bufferStartIndex_;
+    bufferSize = Size::Zero();
+    return;
+  }
+
   const Eigen::Rotation2Dd rotationMatrix(rotation);
   Eigen::Vector2d u = rotationMatrix * Eigen::Vector2d(length(0), 0.0);
   Eigen::Vector2d v = rotationMatrix * Eigen::Vector2d(0.0, length(1));
   const Length boundingBoxHalfLength = (u.cwiseAbs2() + v.cwiseAbs2()).array().sqrt();
+
   Position topLeft = center.array() + boundingBoxHalfLength;
   Position bottomRight = center.array() - boundingBoxHalfLength;
   boundPositionToRange(topLeft, mapLength_, mapPosition_);
   boundPositionToRange(bottomRight, mapLength_, mapPosition_);
-  getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
+
   Index endIndex;
-  getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
+  if (!getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_) ||
+      !getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_)) {
+    startIndex = bufferStartIndex_;
+    bufferSize = Size::Zero();
+    return;
+  }
+
   bufferSize = getSubmapSizeFromCornerIndeces(startIndex, endIndex, bufferSize_, bufferStartIndex_);
+
+  // Validate positive size
+  if ((bufferSize.array() <= 0).any()) {
+    bufferSize = Size::Zero();
+    startIndex = bufferStartIndex_;
+  }
 }
 
 } /* namespace grid_map */

grid_map_core/src/iterators/LineIterator.cpp

@@ -26,11 +26,13 @@ LineIterator::LineIterator(const grid_map::GridMap& gridMap, const Position& sta
   }
   else
   {
+#ifdef DEBUG
     std::cerr << "LineIterator: Could not initialize line iterator, start and end points are out of map range."
               << std::endl;
     std::cerr << "Start: " << start.transpose() << ", End: " << end.transpose()
               << ", Map length: " << gridMap.getLength().transpose()
               << ", Map position: " << gridMap.getPosition().transpose() << std::endl;
+#endif
     throw std::invalid_argument("Failed to construct LineIterator.");
   }
 }
@@ -66,6 +68,15 @@ const Index& LineIterator::operator*() const { return index_; }
 
 LineIterator& LineIterator::operator++()
 {
+  if (isPastEnd()) {
+    return *this;
+  }
+
+  if (denominator_ <= 0) {
+    iCell_ = nCells_;
+    return *this;
+  }
+
   numerator_ += numeratorAdd_;  // Increase the numerator by the top of the fraction.
   if (numerator_ >= denominator_)
   {
@@ -97,12 +108,45 @@ bool LineIterator::initialize(const grid_map::GridMap& gridMap, const Index& sta
 bool LineIterator::getIndexLimitedToMapRange(const grid_map::GridMap& gridMap, const Position& start,
                                              const Position& end, Index& index)
 {
+  const double maxDistance = (end - start).norm();
+
+  // Check for degenerate case: start and end are the same
+  if (maxDistance < std::numeric_limits<double>::epsilon()) {
+    return gridMap.getIndex(start, index);
+  }
+
   Position newStart = start;
   Vector direction = (end - start).normalized();
+
+  // Verify direction is valid (no NaN or inf)
+  if (!direction.allFinite()) {
+    return false;
+  }
+
+  // Use smaller step size for better accuracy near boundaries
+  const double stepSize = gridMap.getResolution() * 0.5;
+  double traveledDistance = 0.0;
+
+  // Special case: if start is already valid, use it
+  if (gridMap.getIndex(newStart, index)) {
+    return true;
+  }
+
+  // Step along the line until we find a valid point or exceed max distance
   while (!gridMap.getIndex(newStart, index))
   {
-    newStart += (gridMap.getResolution() - std::numeric_limits<double>::epsilon()) * direction;
-    if ((end - newStart).norm() < gridMap.getResolution() - std::numeric_limits<double>::epsilon()) return false;
+    newStart += stepSize * direction;
+    traveledDistance += stepSize;
+
+    // Give up if we've traveled past the end point
+    if (traveledDistance > maxDistance) {
+      return false;
+    }
+
+    // Safety check: limit iterations to prevent infinite loop
+    if (traveledDistance > maxDistance + gridMap.getResolution()) {
+      return false;
+    }
   }
   return true;
 }
@@ -162,6 +206,11 @@ void LineIterator::initializeIterationParameters()
     numeratorAdd_ = delta.x();
     nCells_ = delta.y() + 1;  // There are more y-values than x-values.
   }
+
+  // Safety check: if denominator is 0, we have a degenerate line (single point)
+  if (denominator_ == 0) {
+    nCells_ = 1;
+  }
 }
 
 } /* namespace grid_map */