Moved the snapping extractor out of the RapidHeightMapManager and to a higher level closer to where its used #796

PotatoPeeler3000 · 2025-05-02T14:08:18Z

No description provided.

… map thread

…eightMapData and the TerrainMapData individually

# Conflicts: # ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/FootstepPlannerRequest.java

…a higher level closer to where its used

# Conflicts: # ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java # ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java

# Conflicts: # ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java

PotatoPeeler3000 · 2025-05-06T13:36:45Z

...h-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java

      activeMappingParameterToolBox = new ActiveMappingParameterToolBox(ros2Node, robotModel, "ForContinuousWalking");
+      environmentHandler = new EnvironmentHandler();


Both of these are happening at the highest level now

PotatoPeeler3000 · 2025-05-06T13:37:05Z

ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapThread.java

@@ -94,14 +94,6 @@ public HeightMapData getLatestHeightMapData()
      }
   }

-   public TerrainMapData getLatestTerrainMapData()


Not coming from the height map thread anymore

PotatoPeeler3000 · 2025-05-06T13:38:32Z

ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java

@@ -96,11 +95,6 @@ public HeightMapData getLatestHeightMapData()
      return rapidHeightMapThread.getLatestHeightMapData();
   }

-   public TerrainMapData getLatestTerrainMapData()


Not happening in the realsense process anymore

PotatoPeeler3000 · 2025-05-06T13:38:43Z

ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java

@@ -117,8 +114,7 @@ public RapidHeightMapExtractorCUDA(int mode, HeightMapParameters heightMapParame
         globalHeightMapImage = new GpuMat(globalCellsPerAxis, globalCellsPerAxis, opencv_core.CV_16UC1);
         terrainCostImage = new GpuMat(globalCellsPerAxis, globalCellsPerAxis, opencv_core.CV_8UC1);
         contactMapImage = new GpuMat(globalCellsPerAxis, globalCellsPerAxis, opencv_core.CV_8UC1);
-         sensorCroppedHeightMapImage = new GpuMat(cellsPerAxisCropped, cellsPerAxisCropped, opencv_core.CV_16UC1);


This name was confusing

PotatoPeeler3000 · 2025-05-06T13:38:58Z

ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java

@@ -149,9 +145,6 @@ private void recomputeDerivedParameters()
      globalCenterIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getInternalGlobalWidthInMeters(), heightMapParameters.getCellSizeInMeters());
      globalCellsPerAxis = 2 * globalCenterIndex + 1;

-      terrainCenterIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getTerrainWidthInMeters(), heightMapParameters.getCellSizeInMeters());


There is no concept of a terrain map anymore, its just the cropped map

PotatoPeeler3000 · 2025-05-06T13:39:07Z

ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java

@@ -449,13 +438,8 @@ public int getSequenceNumber()
      return sequenceNumber;
   }

-   public GpuMat getTerrainHeightMapImage()


Not happening in here any more

PotatoPeeler3000 · 2025-05-06T13:44:07Z

ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.java

@@ -6,14 +6,16 @@
 import org.bytedeco.opencv.global.opencv_core;
 import org.bytedeco.opencv.opencv_core.GpuMat;
 import org.bytedeco.opencv.opencv_core.Mat;
-import us.ihmc.euclid.tuple3D.Point3D;


We only need the center of the height map which is a 2D point

PotatoPeeler3000 · 2025-05-06T13:46:40Z

...or-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json

@@ -74,13 +74,8 @@
    "lowerBound": 1.0,
    "upperBound": 4.5
  },
-  "Terrain width in meters": {


Got rid of this

PotatoPeeler3000 · 2025-05-06T13:46:54Z

ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractorTest.java

@@ -21,13 +24,27 @@ public void testSnappingTerrainKernelRuns()
      HeightMapParameters heightMapParameters = new HeightMapParameters();
      SnappingTerrainExtractor snappingTerrainExtractor = new SnappingTerrainExtractor(heightMapParameters);

-      GpuMat fakeHeightMap = new GpuMat(500, 500, opencv_core.CV_16UC1);
+      GpuMat fakeHeightMap = new GpuMat(401, 401, opencv_core.CV_16UC1);


A little test to make sure the extractor runs

PotatoPeeler3000 · 2025-05-06T13:47:40Z

ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.cu

@@ -453,32 +451,6 @@ extern "C" __global__ void croppingKernel(unsigned short *inputMap, size_t pitch
        unsigned short *croppedRow = (unsigned short *)((char *)croppedMap + xIndex * pitchCropped);
        croppedRow[yIndex] = 0; // Assign 0 for out-of-bounds cells
    }
-


Only keepin the cropped map now

TomaszTB

So can we still run whatever requires the snapping kernel from the autonomy process?

TomaszTB · 2025-05-06T14:08:53Z

ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java

                                                    (float) heightMapParameters.getCellSizeInMeters(),
                                                    heightMapParameters);
+      heightMap.close();


PotatoPeeler3000 · 2025-05-06T19:16:39Z

So can we still run whatever requires the snapping kernel from the autonomy process?

The only thing that requires the snapping kernel is the footstep planner. But in the way the autonomy process use's the planner its probably not using the snapping kernel.
However, the snapping kernel is still used the same was as before which is for Continuous Hiking, I think that's the only place that uses it.

PotatoPeeler3000 added 13 commits May 1, 2025 11:41

Split the autonomy height map thread and the continuous hiking height…

c5dd8ed

… map thread

Close kernel before the rest of the objeccts

22fa69d

Change things to use the EnvironmentHandler rather then setting the H…

3d0de81

…eightMapData and the TerrainMapData individually

Merge branch 'develop' into cleanup/height-map-thread

cddcd85

Merge branch 'cleanup/height-map-thread' into cleanup/environment

576cc91

# Conflicts: # ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/FootstepPlannerRequest.java

Moved the snapping extractor out of the RapidHeightMapManager and to …

863c4e7

…a higher level closer to where its used

Renaming the methods didn't catch everything

288ced8

Merge branch 'cleanup/environment' into cleanup/move-snapping-extractor

44ecb64

Fix snapping test

ac5da78

Merge branch 'develop' into cleanup/environment

09efd4f

Merge branch 'develop' into cleanup/environment

efb10d7

Merge branch 'cleanup/environment' into cleanup/move-snapping-extractor

4eb717e

# Conflicts: # ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java # ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java

Remove duplicate variable

ae8c44b

Base automatically changed from cleanup/environment to develop May 5, 2025 21:47

PotatoPeeler3000 added 3 commits May 5, 2025 16:51

Merge branch 'develop' into cleanup/move-snapping-extractor

4131ba8

# Conflicts: # ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java

Uncomment bounds check

3fab427

Need to increase window height to go down stairs

128263b

PotatoPeeler3000 commented May 6, 2025

View reviewed changes

TomaszTB reviewed May 6, 2025

View reviewed changes

TomaszTB approved these changes May 7, 2025

View reviewed changes

PotatoPeeler3000 merged commit 69a3853 into develop May 7, 2025
64 of 68 checks passed

PotatoPeeler3000 deleted the cleanup/move-snapping-extractor branch May 7, 2025 16:14

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Moved the snapping extractor out of the RapidHeightMapManager and to a higher level closer to where its used #796

Moved the snapping extractor out of the RapidHeightMapManager and to a higher level closer to where its used #796

PotatoPeeler3000 commented May 2, 2025

PotatoPeeler3000 May 6, 2025

PotatoPeeler3000 May 6, 2025

PotatoPeeler3000 May 6, 2025

PotatoPeeler3000 May 6, 2025

PotatoPeeler3000 May 6, 2025

PotatoPeeler3000 May 6, 2025

PotatoPeeler3000 May 6, 2025

PotatoPeeler3000 May 6, 2025

PotatoPeeler3000 May 6, 2025

PotatoPeeler3000 May 6, 2025

TomaszTB left a comment

TomaszTB May 6, 2025

PotatoPeeler3000 commented May 6, 2025

		activeMappingParameterToolBox = new ActiveMappingParameterToolBox(ros2Node, robotModel, "ForContinuousWalking");
		environmentHandler = new EnvironmentHandler();

Moved the snapping extractor out of the RapidHeightMapManager and to a higher level closer to where its used #796

Moved the snapping extractor out of the RapidHeightMapManager and to a higher level closer to where its used #796

Conversation

PotatoPeeler3000 commented May 2, 2025

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

TomaszTB left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

PotatoPeeler3000 commented May 6, 2025