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

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

* Close kernel before the rest of the objeccts

* Change things to use the EnvironmentHandler rather then setting the HeightMapData and the TerrainMapData individually

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

* Renaming the methods didn't catch everything

* Fix snapping test

* Remove duplicate variable

* Uncomment bounds check

* Need to increase window height to go down stairs

Author: PotatoPeeler3000

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

@@ -7,6 +7,7 @@
 import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler;
 import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor;
 import us.ihmc.perception.StandAloneRealsenseProcess;
+import us.ihmc.perception.gpuHeightMap.SnappingTerrainExtractor;
 import us.ihmc.robotics.physics.RobotCollisionModel;
 import us.ihmc.ros2.ROS2Node;
 import us.ihmc.ros2.ROS2NodeBuilder;
@@ -26,6 +27,8 @@ public class ContinuousHikingProcess
    private final ContinuousPlannerSchedulingTask continuousPlannerSchedulingTask;
    private final StandAloneRealsenseProcess standAloneRealsenseProcess;
 
+   private final SnappingTerrainExtractor snappingTerrainExtractor;
+
    public ContinuousHikingProcess(DRCRobotModel robotModel, RobotCollisionModel robotCollisionModel)
    {
       ROS2Node ros2Node = new ROS2NodeBuilder().build("nadia_terrain_perception_node");
@@ -41,8 +44,9 @@ public ContinuousHikingProcess(DRCRobotModel robotModel, RobotCollisionModel rob
 
       ControllerFootstepQueueMonitor controllerFootstepQueueMonitor = new ControllerFootstepQueueMonitor(ros2Node, robotModel.getSimpleRobotName());
 
-      environmentHandler = new EnvironmentHandler();
       activeMappingParameterToolBox = new ActiveMappingParameterToolBox(ros2Node, robotModel, "ForContinuousWalking");
+      environmentHandler = new EnvironmentHandler();
+      snappingTerrainExtractor = new SnappingTerrainExtractor(activeMappingParameterToolBox.getHeightMapParameters());
       standAloneRealsenseProcess = new StandAloneRealsenseProcess(ros2Node,
                                                                   ros2Helper,
                                                                   syncedRobot,
@@ -69,17 +73,21 @@ public void update()
       activeMappingParameterToolBox.update();
 
       if (standAloneRealsenseProcess.getLatestHeightMapData() != null)
+      {
          environmentHandler.setHeightMapData(standAloneRealsenseProcess.getLatestHeightMapData());
+         snappingTerrainExtractor.update(environmentHandler.getHeightMapData());
+      }
 
-      if (standAloneRealsenseProcess.getLatestTerrainMapData() != null)
-         environmentHandler.setTerrainMapData(standAloneRealsenseProcess.getLatestTerrainMapData());
+      if (snappingTerrainExtractor.getTerrainMapData() != null)
+         environmentHandler.setTerrainMapData(snappingTerrainExtractor.getTerrainMapData());
 
       continuousPlannerSchedulingTask.setLatestEnvironmentHandler(environmentHandler);
    }
 
    public void destroy()
    {
       continuousPlannerSchedulingTask.destroy();
+      snappingTerrainExtractor.close();
       standAloneRealsenseProcess.destroy();
    }
 }

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

@@ -87,14 +87,6 @@ public HeightMapData getLatestHeightMapData()
       }
    }
 
-   public TerrainMapData getLatestTerrainMapData()
-   {
-      synchronized (heightMapLock)
-      {
-         return heightMapManager.getTerrainMapData();
-      }
-   }
-
    @Override
    public void kill()
    {

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

@@ -94,14 +94,6 @@ public HeightMapData getLatestHeightMapData()
       }
    }
 
-   public TerrainMapData getLatestTerrainMapData()
-   {
-      synchronized (heightMapLock)
-      {
-         return heightMapManager.getTerrainMapData();
-      }
-   }
-
    public void destroy()
    {
       scheduler.shutdown();

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

@@ -8,7 +8,6 @@
 import us.ihmc.communication.ros2.ROS2Helper;
 import us.ihmc.communication.ros2.ROS2TunedRigidBodyTransform;
 import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor;
-import us.ihmc.perception.heightMap.TerrainMapData;
 import us.ihmc.robotics.physics.RobotCollisionModel;
 import us.ihmc.ros2.ROS2Node;
 import us.ihmc.sensorProcessing.heightMap.HeightMapData;
@@ -96,11 +95,6 @@ public HeightMapData getLatestHeightMapData()
       return rapidHeightMapThread.getLatestHeightMapData();
    }
 
-   public TerrainMapData getLatestTerrainMapData()
-   {
-      return rapidHeightMapThread.getLatestTerrainMapData();
-   }
-
    public void destroy()
    {
       rapidHeightMapThread.destroy();

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

@@ -35,8 +35,7 @@ public class RapidHeightMapExtractorCUDA
    private final GpuMat globalHeightMapImage;
    private final GpuMat terrainCostImage;
    private final GpuMat contactMapImage;
-   private final GpuMat sensorCroppedHeightMapImage;
-   private final GpuMat terrainHeightMapImage;
+   private final GpuMat croppedHeightMapImage;
    private final GpuMat emptyGlobalHeightMapImage;
    private final CUDAProgram heightMapCUDAProgram;
 
@@ -65,11 +64,9 @@ public class RapidHeightMapExtractorCUDA
    private float gridOffsetX;
    private int centerIndex;
    private int cellsPerAxisLocal;
-   private int cellsPerAxisTerrain;
    private int globalCenterIndex;
    private int croppedCenterIndex;
    private int cellsPerAxisCropped;
-   private int terrainCenterIndex;
    private int globalCellsPerAxis;
    private dim3 blockSize;
    private dim3 updateKernelGridDim;
@@ -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);
-         terrainHeightMapImage = new GpuMat(cellsPerAxisTerrain, cellsPerAxisTerrain, opencv_core.CV_16UC1);
+         croppedHeightMapImage = new GpuMat(cellsPerAxisCropped, cellsPerAxisCropped, opencv_core.CV_16UC1);
 
          emptyGlobalHeightMapImage = new GpuMat(globalCellsPerAxis, globalCellsPerAxis, opencv_core.CV_16UC1);
 
@@ -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());
-      cellsPerAxisTerrain = 2 * terrainCenterIndex + 1;
-
       croppedCenterIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getCroppedWidthInMeters(), heightMapParameters.getCellSizeInMeters());
       cellsPerAxisCropped = 2 * croppedCenterIndex + 1;
 
@@ -269,11 +262,9 @@ public void update(GpuMat latestDepthImageGPU,
 
       // Run the cropping kernel
       croppingKernel.withPointer(globalHeightMapImage.data()).withLong(globalHeightMapImage.step());
-      croppingKernel.withPointer(sensorCroppedHeightMapImage.data()).withLong(sensorCroppedHeightMapImage.step());
-      croppingKernel.withPointer(terrainHeightMapImage.data()).withLong(terrainHeightMapImage.step());
+      croppingKernel.withPointer(croppedHeightMapImage.data()).withLong(croppedHeightMapImage.step());
       croppingKernel.withPointer(parametersDevicePointer);
       croppingKernel.withInt(cellsPerAxisCropped);
-      croppingKernel.withInt(terrainHeightMapImage.rows());
       error = cudaStreamSynchronize(stream);
       CUDATools.checkCUDAError(error);
 
@@ -377,7 +368,6 @@ public float[] populateParameterArray(HeightMapParameters parameters,
                           (float) parameters.getSearchWindowHeight(),
                           (float) parameters.getSearchWindowWidth(),
                           (float) croppedCenterIndex,
-                          (float) terrainCenterIndex,
                           (float) parameters.getMinClampHeight(),
                           (float) parameters.getMaxClampHeight(),
                           (float) parameters.getHeightOffset(),
@@ -418,8 +408,7 @@ public void destroy()
       globalHeightMapImage.close();
       terrainCostImage.close();
       contactMapImage.close();
-      sensorCroppedHeightMapImage.close();
-      terrainHeightMapImage.close();
+      croppedHeightMapImage.close();
 
       filteredRapidHeightMapExtractor.destroy();
       verticalSurfacesExtractor.destroy();
@@ -449,13 +438,8 @@ public int getSequenceNumber()
       return sequenceNumber;
    }
 
-   public GpuMat getTerrainHeightMapImage()
-   {
-      return terrainHeightMapImage;
-   }
-
-   public GpuMat getVisualizedHeightMap()
+   public GpuMat getCroppedHeightMap()
    {
-      return sensorCroppedHeightMapImage.clone();
+      return croppedHeightMapImage.clone();
    }
 }

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

@@ -18,7 +18,6 @@
 import us.ihmc.perception.camera.CameraIntrinsics;
 import us.ihmc.perception.cuda.CUDABodyCollisionFilter;
 import us.ihmc.perception.filters.CUDAFlyingPointsFilter;
-import us.ihmc.perception.heightMap.TerrainMapData;
 import us.ihmc.perception.opencv.OpenCVTools;
 import us.ihmc.perception.tools.PerceptionMessageTools;
 import us.ihmc.robotModels.FullHumanoidRobotModel;
@@ -28,6 +27,7 @@
 import us.ihmc.scs2.simulation.collision.Collidable;
 import us.ihmc.sensorProcessing.heightMap.HeightMapData;
 import us.ihmc.sensorProcessing.heightMap.HeightMapParameters;
+import us.ihmc.sensorProcessing.heightMap.HeightMapTools;
 
 import java.time.Instant;
 import java.util.ArrayList;
@@ -40,7 +40,6 @@ public class RapidHeightMapManager
 {
    private final HeightMapParameters heightMapParameters;
    private final RapidHeightMapExtractorCUDA rapidHeightMapExtractor;
-   private final SnappingTerrainExtractor snappedFootstepsExtractor;
 
    private final Point3D sensorOrigin = new Point3D();
    private final FramePose3D cameraPose = new FramePose3D();
@@ -56,6 +55,7 @@ public class RapidHeightMapManager
    private final ROS2Publisher<ImageMessage> heightMapPublisher;
    private final ImageMessage croppedHeightMapImageMessage = new ImageMessage();
    private final BytePointer compressedCroppedHeightMapPointer = new BytePointer();
+   private final GpuMat deviceCroppedHeightMap;
 
    public RapidHeightMapManager(ROS2Node ros2Node,
                                 RobotCollisionModel robotCollisionModel,
@@ -76,8 +76,10 @@ public RapidHeightMapManager(ROS2Node ros2Node,
       rapidHeightMapDriftOffset = new RapidHeightMapDriftOffset(controllerFootstepQueueMonitor);
       flyingPointsFilter = new CUDAFlyingPointsFilter();
 
+      int croppedCenterIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getCroppedWidthInMeters(), heightMapParameters.getCellSizeInMeters());
+      int cellsPerAxisCropped = 2 * croppedCenterIndex + 1;
+      deviceCroppedHeightMap = new GpuMat(cellsPerAxisCropped, cellsPerAxisCropped, opencv_core.CV_16UC1);
       rapidHeightMapExtractor = new RapidHeightMapExtractorCUDA(1, heightMapParameters);
-      snappedFootstepsExtractor = new SnappingTerrainExtractor(heightMapParameters);
 
       // We use a notification to only call resetting the height map in one place
       heightMapPublisher = ros2Node.createPublisher(PerceptionAPI.HEIGHT_MAP_CROPPED);
@@ -96,15 +98,14 @@ public RapidHeightMapManager(ROS2Node ros2Node,
       }
    }
 
-   public void updateAndPublishHeightMap(RawImage depthImage, ReferenceFrame cameraFrame, ReferenceFrame cameraZUpFrame) throws Exception
+   public void updateAndPublishHeightMap(RawImage depthImage, ReferenceFrame cameraFrame, ReferenceFrame cameraZUpFrame)
    {
       RawImage depthImageCopy = depthImage.get();
       Mat latestDepthImage = depthImageCopy.getCpuImageMat();
       Instant acquisitionTime = depthImageCopy.getAcquisitionTime();
       CameraIntrinsics depthIntrinsicsCopy = depthImageCopy.getIntrinsicsCopy();
 
       // -------- Update the Height Map with the latest depth image from the sensor --------------
-      GpuMat deviceCroppedHeightMap = new GpuMat();
       updateInternal(latestDepthImage, deviceCroppedHeightMap, depthIntrinsicsCopy, cameraFrame, cameraZUpFrame);
 
       // Publish the height map to anyone who is subscribing
@@ -121,16 +122,18 @@ public void updateAndPublishHeightMap(RawImage depthImage, ReferenceFrame camera
                                                          hostCroppedHeightMap.cols(),
                                                          (float) heightMapParameters.getHeightScaleFactor());
 
-      deviceCroppedHeightMap.close();
       hostCroppedHeightMap.close();
       depthImageCopy.release();
    }
 
+   /**
+    * Update the Height Map with the latest depth image from the sensor
+    */
    private void updateInternal(Mat latestDepthImage,
                                GpuMat deviceHeightMapToPack,
                                CameraIntrinsics depthIntrinsicsCopy,
                                ReferenceFrame cameraFrame,
-                               ReferenceFrame cameraZUpFrame) throws Exception
+                               ReferenceFrame cameraZUpFrame)
    {
       // Option that gets triggered from a message sent from the user
       if (lowerHeightMapBackdropRequested.poll())
@@ -194,7 +197,7 @@ private void updateInternal(Mat latestDepthImage,
                                      sensorOrigin,
                                      computeFootHeight());
 
-      GpuMat deviceCroppedHeightMap = rapidHeightMapExtractor.getVisualizedHeightMap();
+      GpuMat deviceCroppedHeightMap = rapidHeightMapExtractor.getCroppedHeightMap();
       // We have used the depth image without the robot, close this to avoid creating a memory leak
       depthImageWithoutRobot.close();
 
@@ -209,30 +212,24 @@ private void updateInternal(Mat latestDepthImage,
 
       // Don't close this mat as its being used in the extractor till that finish's
       deviceCroppedHeightMap.convertTo(deviceHeightMapToPack, deviceCroppedHeightMap.type());
-
-      // Now extract the maps to be used in the footstep planning algorithm
-      snappedFootstepsExtractor.update(rapidHeightMapExtractor.getTerrainHeightMapImage(), sensorOrigin);
-   }
-
-   public TerrainMapData getTerrainMapData()
-   {
-      return snappedFootstepsExtractor.getTerrainMapData();
    }
 
    public HeightMapData getLatestHeightMapData()
    {
       HeightMapData latestHeightMapData = new HeightMapData((float) heightMapParameters.getCellSizeInMeters(),
-                                                            (float) heightMapParameters.getTerrainWidthInMeters(),
+                                                            (float) heightMapParameters.getCroppedWidthInMeters(),
                                                             sensorOrigin.getX(),
                                                             sensorOrigin.getY());
 
-      Mat heightMapMat = getTerrainMapData().getHeightMap();
-      PerceptionMessageTools.convertToHeightMapData(heightMapMat,
+      Mat heightMap = new Mat();
+      deviceCroppedHeightMap.download(heightMap);
+      PerceptionMessageTools.convertToHeightMapData(heightMap,
                                                     latestHeightMapData,
                                                     sensorOrigin,
-                                                    (float) heightMapParameters.getTerrainWidthInMeters(),
+                                                    (float) heightMapParameters.getCroppedWidthInMeters(),
                                                     (float) heightMapParameters.getCellSizeInMeters(),
                                                     heightMapParameters);
+      heightMap.close();
 
       return latestHeightMapData;
    }
@@ -257,8 +254,8 @@ private double computeFootHeight()
    public void destroy()
    {
       rapidHeightMapExtractor.destroy();
-      snappedFootstepsExtractor.close();
       flyingPointsFilter.destroy();
+      deviceCroppedHeightMap.close();
       compressedCroppedHeightMapPointer.close();
    }
 }