From fb457255b7899c43cec04d7717558153008544e2 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Wed, 9 Sep 2026 10:42:46 -0700
Subject: [PATCH] Fixing occupancy grids not 3d spam when subscribing octomap
 and grids are 2d (#1763)

---
 corelib/src/global_map/GridMap.cpp | 20 ++++++++++++++++++--
 corelib/src/global_map/OctoMap.cpp | 20 ++++++++++++++++++--
 2 files changed, 36 insertions(+), 4 deletions(-)

diff --git a/corelib/src/global_map/GridMap.cpp b/corelib/src/global_map/GridMap.cpp
index e36a3c4c..97c0e418 100644
--- a/corelib/src/global_map/GridMap.cpp
+++ b/corelib/src/global_map/GridMap.cpp
@@ -237,6 +237,11 @@ void GridMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 	if(!cache().empty())
 	{
 		UDEBUG("Updating from cache");
+		int not3DCount = 0;
+		int not3DFirstId = 0;
+		int not3DGroundType = 0;
+		int not3DObstaclesType = 0;
+		int not3DEmptyType = 0;
 		for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
 		{
 			if(uContains(cache(), iter->first))
@@ -245,8 +250,13 @@ void GridMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 
 				if(!localGrid.is3D())
 				{
-					UWARN("It seems the local occupancy grids are not 3d, cannot update GridMap! (ground type=%d, obstacles type=%d, empty type=%d)",
-							localGrid.groundCells.type(), localGrid.obstacleCells.type(), localGrid.emptyCells.type());
+					if(++not3DCount == 1)
+					{
+						not3DFirstId = iter->first;
+						not3DGroundType = localGrid.groundCells.type();
+						not3DObstaclesType = localGrid.obstacleCells.type();
+						not3DEmptyType = localGrid.emptyCells.type();
+					}
 					continue;
 				}
 
@@ -339,6 +349,12 @@ void GridMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 				uInsert(occupiedLocalMaps, std::make_pair(iter->first, occupied));
 			}
 		}
+		if(not3DCount)
+		{
+			UWARN("It seems the local occupancy grids are not 3d, cannot update GridMap! "
+					"(%d local grid(s) ignored, first one (id=%d) had ground type=%d, obstacles type=%d, empty type=%d)",
+					not3DCount, not3DFirstId, not3DGroundType, not3DObstaclesType, not3DEmptyType);
+		}
 	}
 
 	if(minX != maxX && minY != maxY)
diff --git a/corelib/src/global_map/OctoMap.cpp b/corelib/src/global_map/OctoMap.cpp
index 8b7240bc..2f18ce93 100644
--- a/corelib/src/global_map/OctoMap.cpp
+++ b/corelib/src/global_map/OctoMap.cpp
@@ -479,6 +479,11 @@ void OctoMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 	{
 		float rangeMaxSqrd = rangeMax_*rangeMax_;
 		float cellSize = octree_->getResolution();
+		int not3DCount = 0;
+		int not3DFirstId = 0;
+		int not3DGroundType = 0;
+		int not3DObstaclesType = 0;
+		int not3DEmptyType = 0;
 		for(std::list<std::pair<int, Transform> >::const_iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
 		{
 			std::map<int, LocalGrid>::const_iterator localGridIter;
@@ -491,8 +496,13 @@ void OctoMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 
 				if(!localGridIter->second.is3D())
 				{
-					UWARN("It seems the local occupancy grids are not 3d, cannot update OctoMap! (ground type=%d, obstacles type=%d, empty type=%d)",
-											ground.type(), obstacles.type(), emptyCells.type());
+					if(++not3DCount == 1)
+					{
+						not3DFirstId = iter->first;
+						not3DGroundType = ground.type();
+						not3DObstaclesType = obstacles.type();
+						not3DEmptyType = emptyCells.type();
+					}
 					continue;
 				}
 
@@ -762,6 +772,12 @@ void OctoMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 				UDEBUG("Did not find %d in cache", iter->first);
 			}
 		}
+		if(not3DCount)
+		{
+			UWARN("It seems the local occupancy grids are not 3d, cannot update OctoMap! "
+					"(%d local grid(s) ignored, first one (id=%d) had ground type=%d, obstacles type=%d, empty type=%d)",
+					not3DCount, not3DFirstId, not3DGroundType, not3DObstaclesType, not3DEmptyType);
+		}
 	}
 
     if(emptyFloodFillDepth_>0)