Fix rebase, add comments

ihmcrobotics · Feb 28, 2025 · e0b7e00 · e0b7e00
1 parent 9d9b583
commit e0b7e00
Show file tree

Hide file tree

Showing 3 changed files with 31 additions and 7 deletions.
diff --git a/...h-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXDoorDetectionManagerDemo.java b/...h-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXDoorDetectionManagerDemo.java
@@ -2,6 +2,7 @@
 
 import us.ihmc.communication.ros2.ROS2Helper;
 import us.ihmc.perception.RawImage;
+import us.ihmc.perception.cuda.CUDATools;
 import us.ihmc.perception.detections.doors.DoorDetectionManager;
 import us.ihmc.perception.detections.yolo.YOLOv8DetectionThread;
 import us.ihmc.perception.opencl.OpenCLManager;
@@ -23,14 +24,16 @@
 
 import java.util.concurrent.atomic.AtomicBoolean;
 
+import static us.ihmc.zed.global.zed.SL_DEPTH_MODE_NEURAL;
+import static us.ihmc.zed.global.zed.SL_DEPTH_MODE_PERFORMANCE;
+
 public class RDXDoorDetectionManagerDemo
 {
    private static final String SVO_FILE = IHMCCommonPaths.PERCEPTION_LOGS_DIRECTORY.resolve("20240715_103234_ZEDRecording_NewONRCourseWalk.svo2")
                                                                                    .toAbsolutePath()
                                                                                    .toString();
 
    private final ROS2Node ros2Node;
-   private final ROS2Helper ros2Helper;
    private final OpenCLManager openCLManager;
 
    private final DoorDetectionManager doorDetectionManager;
@@ -54,12 +57,13 @@ private RDXDoorDetectionManagerDemo()
       Runtime.getRuntime().addShutdownHook(new Thread(this::destroy));
 
       ros2Node = new ROS2NodeBuilder().build(getClass().getSimpleName());
-      ros2Helper = new ROS2Helper(ros2Node);
+      ROS2Helper ros2Helper = new ROS2Helper(ros2Node);
       openCLManager = new OpenCLManager();
 
       doorDetectionManager = new DoorDetectionManager(ros2Node);
 
-      zed = new ZEDSVOPlaybackSensor(ros2Helper, 0, ZEDModelData.ZED_2, SVO_FILE);
+      boolean enableNeuralMode = CUDATools.hasCUDADeviceOfAtLeast(CUDATools.getDeviceName(0), "RTX 3080");
+      zed = new ZEDSVOPlaybackSensor(ros2Helper, 0, ZEDModelData.ZED_2, enableNeuralMode ? SL_DEPTH_MODE_NEURAL : SL_DEPTH_MODE_PERFORMANCE, SVO_FILE);
       zed.useTrackedPose(true);
 
       yoloThread = new YOLOv8DetectionThread(() -> true);

diff --git a/...vel-behaviors/src/libgdx/java/us/ihmc/rdx/perception/doors/RDXROS2DoorDetectionPanel.java b/...vel-behaviors/src/libgdx/java/us/ihmc/rdx/perception/doors/RDXROS2DoorDetectionPanel.java
@@ -73,14 +73,15 @@ public RDXROS2DoorDetectionPanel(ROS2Node ros2Node)
          {
             UUID detectionUUID = MessageTools.toUUID(detectedDoorMessage.getDetectionUuid());
             RDXDetectedDoor rdxDetectedDoor = rdxDetectedDoors.get(detectionUUID);
-            if (rdxDetectedDoor == null)
+            if (rdxDetectedDoor == null) // Add new RDXDetectedDoors if new detections show up
             {
                rdxDetectedDoor = new RDXDetectedDoor();
                rdxDetectedDoors.put(detectionUUID, rdxDetectedDoor);
                detectionsToRender.put(detectionUUID, new ImBoolean(false));
             }
-            rdxDetectedDoor.update(detectedDoorMessage);
+            rdxDetectedDoor.update(detectedDoorMessage); // Update the RDXDetectedDoor
 
+            // Ensure each door is in the correct stable/unstable set
             if (rdxDetectedDoor.getDetection().isDetectionStable())
             {
                if (!stableDetections.contains(rdxDetectedDoor))

diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/detections/doors/DoorDetectionManager.java b/ihmc-perception/src/main/java/us/ihmc/perception/detections/doors/DoorDetectionManager.java
@@ -48,12 +48,25 @@ public DoorDetectionManager(ROS2Node ros2Node)
       publishThrottler = new Throttler().setFrequency(PUBLISH_FREQUENCY);
    }
 
+   /**
+    * Get all currently detected doors
+    *
+    * @return A list of all currently detected doors (might be empty).
+    */
    public List<DetectedDoor> getDetectedDoors()
    {
       return new LinkedList<>(detectedDoors);
    }
 
-   public List<DetectedDoor> getDetectedDoorsInRange(Point3DReadOnly pointInWorld, double radius)
+   /**
+    * Get detected doors within some distance of a point.
+    * Distance is calculated from the point to the door's opening mechanism.
+    *
+    * @param pointInWorld The point (in world frame) around which to find doors
+    * @param radius       Radius (from the point) within which to find doors
+    * @return A list of currently detected doors which are within the radius of the passed in point (might be empty).
+    */
+   public synchronized List<DetectedDoor> getDetectedDoorsInRange(Point3DReadOnly pointInWorld, double radius)
    {
       double radiusSquared = radius * radius;
       List<DetectedDoor> doorsInRange = detectedDoors.stream().filter(detectedDoor ->
@@ -67,7 +80,13 @@ public List<DetectedDoor> getDetectedDoorsInRange(Point3DReadOnly pointInWorld,
       return new LinkedList<>(doorsInRange);
    }
 
-   public DetectedDoor getClosestDoor(Point3DReadOnly pointInWorld)
+   /**
+    * Get the detected door that is closest to some point.
+    *
+    * @param pointInWorld The point (in world frame)
+    * @return The closest detected door to the point. Null if no door is found.
+    */
+   public synchronized DetectedDoor getClosestDoor(Point3DReadOnly pointInWorld)
    {
       return detectedDoors.stream()
                           .filter(detectedDoor -> detectedDoor.getOpeningMechanism().isPositionKnown())