Cleaned up the DepthImageBodyCollisionFilter in java and in c++, moved it above the RapidHeightMapManager. Deleted the duplicate thread for the height map (#798)

* 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

* Cleaned up the DepthImageBodyCollisionFilter in java and in c++, moved it above the RapidHeightMapManager. Deleted the duplicate thread for the height map

* Don't need to call get here

Author: PotatoPeeler3000

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

@@ -1,34 +1,33 @@
 package us.ihmc.perception;
 
-import com.google.common.util.concurrent.ThreadFactoryBuilder;
+import org.bytedeco.opencv.opencv_core.GpuMat;
 import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel;
-import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor;
+import us.ihmc.commons.thread.RepeatingTaskThread;
+import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.log.LogTools;
+import us.ihmc.perception.camera.CameraIntrinsics;
+import us.ihmc.perception.filters.DepthImageBodyCollisionFilter;
 import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager;
-import us.ihmc.perception.heightMap.TerrainMapData;
 import us.ihmc.robotics.physics.RobotCollisionModel;
 import us.ihmc.robotics.robotSide.RobotSide;
 import us.ihmc.ros2.ROS2Node;
 import us.ihmc.sensorProcessing.heightMap.HeightMapData;
 import us.ihmc.sensorProcessing.heightMap.HeightMapParameters;
 import us.ihmc.sensors.ImageSensor;
 
-import java.util.concurrent.Executors;
-import java.util.concurrent.ScheduledExecutorService;
-import java.util.concurrent.ThreadFactory;
-import java.util.concurrent.TimeUnit;
+import java.time.Instant;
 
-public class RapidHeightMapThread
+public class RapidHeightMapThread extends RepeatingTaskThread
 {
+   private final DepthImageBodyCollisionFilter bodyCollisionFilter;
    private final RapidHeightMapManager heightMapManager;
    private final Object heightMapLock = new Object();
 
    private final ImageSensor imageSensor;
-   private final ReferenceFrame sensorFrame;
+   private final ReferenceFrame cameraFrame;
    private final ReferenceFrame zUpSensorFrame;
    private final int depthImageKey;
-   private ScheduledExecutorService scheduler;
 
    public RapidHeightMapThread(ROS2Node ros2Node,
                                ROS2SyncedRobotModel syncedRobotModel,
@@ -38,17 +37,19 @@ public RapidHeightMapThread(ROS2Node ros2Node,
                                ControllerFootstepQueueMonitor controllerFootstepQueueMonitor,
                                HeightMapParameters heightMapParameters)
    {
+      super(imageSensor.getSensorName() + RapidHeightMapThread.class.getSimpleName());
+
       this.imageSensor = imageSensor;
       this.depthImageKey = depthImageKey;
 
-      sensorFrame = syncedRobotModel.getReferenceFrames().getSteppingCameraFrame();
+      cameraFrame = syncedRobotModel.getReferenceFrames().getSteppingCameraFrame();
       zUpSensorFrame = syncedRobotModel.getReferenceFrames().getSteppingCameraZUpFrame();
 
       ReferenceFrame leftFootFrame = syncedRobotModel.getReferenceFrames().getSoleFrame(RobotSide.LEFT);
       ReferenceFrame rightFootFrame = syncedRobotModel.getReferenceFrames().getSoleFrame(RobotSide.LEFT);
 
+      bodyCollisionFilter = new DepthImageBodyCollisionFilter(robotCollisionModel, syncedRobotModel.getFullRobotModel().getRootBody());
       heightMapManager = new RapidHeightMapManager(ros2Node,
-                                                   robotCollisionModel,
                                                    syncedRobotModel.getFullRobotModel(),
                                                    syncedRobotModel.getRobotModel().getSimpleRobotName(),
                                                    leftFootFrame,
@@ -57,29 +58,35 @@ public RapidHeightMapThread(ROS2Node ros2Node,
                                                    heightMapParameters);
    }
 
-   public void start()
-   {
-      // We create a ThreadFactory here so that when profiling the thread, there is a user-friendly name to identify it with
-      ThreadFactory threadFactory = new ThreadFactoryBuilder().setNameFormat("RemoteHeightMapUpdateThread").build();
-      scheduler = Executors.newScheduledThreadPool(1, threadFactory);
-      scheduler.scheduleWithFixedDelay(this::update, 100, 10, TimeUnit.MILLISECONDS);
-   }
-
-   public void update()
+   @Override
+   protected void runTask()
    {
       try
       {
          imageSensor.waitForGrab();
          RawImage depthImage = imageSensor.getImage(depthImageKey);
 
+         // Get everything we need from the image
+         GpuMat latestDepthImage = depthImage.getGpuImageMat();
+         Instant acquisitionTime = depthImage.getAcquisitionTime();
+         CameraIntrinsics depthIntrinsicsCopy = depthImage.getIntrinsicsCopy();
+
+         // Process body collisions
+         GpuMat depthImageWithoutBodyCollisions = new GpuMat(latestDepthImage.size(), latestDepthImage.type());
+         bodyCollisionFilter.process(latestDepthImage, depthImageWithoutBodyCollisions, depthIntrinsicsCopy, cameraFrame);
+
          // Update height map
          synchronized (heightMapLock)
          {
-            heightMapManager.updateAndPublishHeightMap(depthImage, sensorFrame, zUpSensorFrame);
+            heightMapManager.updateAndPublishHeightMap(depthImageWithoutBodyCollisions, acquisitionTime, depthIntrinsicsCopy, cameraFrame, zUpSensorFrame);
          }
 
+         depthImageWithoutBodyCollisions.close();
          depthImage.release();
       }
+      catch (InterruptedException ignored)
+      {
+      }
       catch (Exception e)
       {
          LogTools.error(e);
@@ -94,9 +101,12 @@ public HeightMapData getLatestHeightMapData()
       }
    }
 
-   public void destroy()
+   @Override
+   public void kill()
    {
-      scheduler.shutdown();
+      super.kill();
+      interrupt();
+
       heightMapManager.destroy();
    }
 }

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

@@ -87,7 +87,7 @@ public StandAloneRealsenseProcess(ROS2Node ros2Node,
                                                       RealSenseImageSensor.DEPTH_IMAGE_KEY,
                                                       controllerFootstepQueueMonitor,
                                                       heightMapParameters);
-      rapidHeightMapThread.start();
+      rapidHeightMapThread.startRepeating();
    }
 
    public HeightMapData getLatestHeightMapData()
@@ -97,7 +97,7 @@ public HeightMapData getLatestHeightMapData()
 
    public void destroy()
    {
-      rapidHeightMapThread.destroy();
+      rapidHeightMapThread.blockingKill();
       realsenseDemandNode.destroy();
       realsensePublishDemandNode.destroy();
       d455Sensor.close();

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

@@ -1,4 +1,4 @@
-package us.ihmc.perception.cuda;
+package us.ihmc.perception.filters;
 
 import org.bytedeco.cuda.cudart.CUstream_st;
 import org.bytedeco.cuda.cudart.dim3;
@@ -7,7 +7,13 @@
 import us.ihmc.euclid.referenceFrame.FrameCapsule3D;
 import us.ihmc.euclid.referenceFrame.FrameSphere3D;
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
+import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
 import us.ihmc.perception.camera.CameraIntrinsics;
+import us.ihmc.perception.cuda.CUDAKernel;
+import us.ihmc.perception.cuda.CUDAProgram;
+import us.ihmc.perception.cuda.CUDAStreamManager;
+import us.ihmc.perception.cuda.CUDATools;
+import us.ihmc.robotics.physics.RobotCollisionModel;
 import us.ihmc.scs2.simulation.collision.Collidable;
 
 import java.net.URL;
@@ -16,8 +22,15 @@
 import static org.bytedeco.cuda.global.cudart.cudaFree;
 import static org.bytedeco.cuda.global.cudart.cudaStreamSynchronize;
 
-public class CUDABodyCollisionFilter
+/**
+ * This class removes the points of a depth image that are on parts of the body.
+ * This class expects a {@link RobotCollisionModel} and a root body in the form of a {@link RigidBodyBasics}.
+ * This class runs a CUDA GPU kernel that checks if a point lies inside any collisions on the {@link RobotCollisionModel}.
+ * The returned value represents a depth image that has zeroed out points for anything that was a collision.
+ */
+public class DepthImageBodyCollisionFilter
 {
+   private static final boolean PRINT_DEBUG_TIMINGS = false;
    private static final int BLOCK_SIZE_XY = 32;
    private final int NUMBER_OF_ATTRIBUTES = 7;
 
@@ -31,33 +44,30 @@ public class CUDABodyCollisionFilter
    private final FloatPointer deviceCollidableGeometryPointer;
    private final FloatPointer collidableGeometryPointer;
 
-   public CUDABodyCollisionFilter(List<Collidable> robotCollidables)
+   public DepthImageBodyCollisionFilter(RobotCollisionModel robotCollisionModel, RigidBodyBasics rootBody)
    {
-      this.robotCollidables = robotCollidables;
-      URL bodyCollisionCheckURL = getClass().getResource("BodyCollisionCheck.cu");
-      URL utilsURL = getClass().getResource("Utils.cu");
-      URL perceptionUtilsURL = getClass().getResource("PerceptionUtils.cu");
-      URL mathUtilsURL = getClass().getResource("MathUtils.cuh");
+      robotCollidables = robotCollisionModel.getRobotCollidables(rootBody);
+
+      URL bodyCollisionCheckURL = getClass().getResource("DepthImageBodyCollisionFilter.cu");
+      URL perceptionUtilsURL = getClass().getClassLoader().getResource("us/ihmc/perception/cuda/PerceptionUtils.cu");
+      URL mathUtilsURL = getClass().getClassLoader().getResource("us/ihmc/perception/cuda/MathUtils.cuh");
 
       try
       {
-         program = new CUDAProgram(bodyCollisionCheckURL, utilsURL, perceptionUtilsURL, mathUtilsURL);
+         program = new CUDAProgram(bodyCollisionCheckURL, perceptionUtilsURL, mathUtilsURL);
          kernel = program.loadKernel("checkBodyCollision");
+         kernel.enableKernelTimings(PRINT_DEBUG_TIMINGS);
       }
       catch (Exception e)
       {
          throw new RuntimeException(e);
       }
 
       stream = CUDAStreamManager.getStream();
-      if (stream == null)
-      {
-         throw new RuntimeException("CUDA stream could not be initialized!");
-      }
 
       // Note: We get the expected number of collidables and allocate that memory amount on the GPU
       // By allocating once we don't have to free the memory in each call, just at the end
-      numberOfCollidables = countSupportedCollidables(robotCollidables);
+      numberOfCollidables = robotCollidables.size();
       dataSize = numberOfCollidables * NUMBER_OF_ATTRIBUTES;
       deviceCollidableGeometryPointer = new FloatPointer();
       CUDATools.mallocAsync(deviceCollidableGeometryPointer, dataSize, stream);
@@ -107,31 +117,15 @@ public void process(GpuMat deviceInputDepthImage, GpuMat deviceOutputImageToPack
       gridSize.close();
    }
 
-   private int countSupportedCollidables(List<Collidable> robotCollidables)
-   {
-      if (robotCollidables == null)
-         return 0;
-
-      int count = 0;
-      for (Collidable collidable : robotCollidables)
-      {
-         if (isCollidableShapeSupported(collidable))
-         {
-            count++;
-         }
-      }
-      return count;
-   }
-
-   public void getCollidablesPointer(List<Collidable> robotCollidables, ReferenceFrame cameraFrame)
+   /**
+    * The supported collidables are a {@link FrameSphere3D} and a {@link FrameCapsule3D}
+    */
+   private void getCollidablesPointer(List<Collidable> robotCollidables, ReferenceFrame cameraFrame)
    {
       int index = 0;
 
       for (Collidable collidable : robotCollidables)
       {
-         if (!isCollidableShapeSupported(collidable))
-            continue;
-
          if (collidable.getShape() instanceof FrameSphere3D sphere)
          {
             FrameSphere3D bodypart = new FrameSphere3D(sphere);
@@ -159,13 +153,6 @@ else if (collidable.getShape() instanceof FrameCapsule3D capsule)
       }
    }
 
-   private boolean isCollidableShapeSupported(Collidable collidable)
-   {
-      return (collidable.getShape() instanceof FrameCapsule3D || collidable.getShape() instanceof FrameSphere3D) && !collidable.getRigidBody()
-                                                                                                                               .getName()
-                                                                                                                               .matches("TORSO_LINK|PELVIS_LINK");
-   }
-
    public void close()
    {
       // Need to free this memory on the GPU to shut down correctly

ihmc-perception/src/main/java/us/ihmc/perception/cuda/CUDABodyCollisionFilter.java renamed to ihmc-perception/src/main/java/us/ihmc/perception/filters/DepthImageBodyCollisionFilter.java

@@ -391,7 +391,8 @@ public void destroy()
       planOffsetKernel.close();
 
       emptyGlobalHeightMapImage.close();
-      planOffsetKernelGridDim.close();
+      if (planOffsetKernelGridDim != null)
+         planOffsetKernelGridDim.close();
 
       // Clean up each resource
       deallocateFloatPointer(groundToSensorTransformHostPointer, groundToSensorTransformDevicePointer);