Polishing

ihmcrobotics · Dec 18, 2024 · 3bdc53b · 3bdc53b
1 parent b12d847
commit 3bdc53b
Show file tree

Hide file tree

Showing 12 changed files with 16 additions and 26 deletions.
diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/PerceptionAPI.java b/ihmc-communication/src/main/java/us/ihmc/communication/PerceptionAPI.java
@@ -109,7 +109,7 @@ public final class PerceptionAPI
                                                                                   .withType(BigVideoPacket.class)
                                                                                   .withSuffix("debug_extraction");
    public static final ROS2Topic<Empty> REQUEST_REALSENSE = PERCEPTION_MODULE.withSuffix("request_realsense").withType(Empty.class);
-   public static final ROS2Topic<Empty> REQUEST_REALSENSE_POINT_CLOUD = PERCEPTION_MODULE.withSuffix("request_realsense_point_cloud").withType(Empty.class);
+   public static final ROS2Topic<Empty> REQUEST_REALSENSE_PUBLICATION = PERCEPTION_MODULE.withSuffix("request_realsense_publication").withType(Empty.class);
    public static final SideDependentList<ROS2Topic<BigVideoPacket>> BLACKFLY_VIDEO = new SideDependentList<>(BEST_EFFORT.withModule(BLACKFLY_NAME + "left")
                                                                                                                         .withType(BigVideoPacket.class)
                                                                                                                         .withSuffix("video"),

diff --git a/...-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphDemo.java b/...-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphDemo.java
@@ -104,22 +104,22 @@ public void create()
                      = new RDXROS2ImageMessageVisualizer("ZED 2 Color %s".formatted(side.getPascalCaseName()),
                                                          ros2Node,
                                                          PerceptionAPI.ZED2_COLOR_IMAGES.get(side));
-               zedColorImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_COLOR);
+               zedColorImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION);
                perceptionVisualizerPanel.addVisualizer(zedColorImageVisualizer);
             }
 
             RDXROS2ImageMessageVisualizer zed2DepthImageVisualizer = new RDXROS2ImageMessageVisualizer("ZED 2 Depth Image",
                                                                                                        ros2Node,
                                                                                                        PerceptionAPI.ZED2_DEPTH);
-            zed2DepthImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_DEPTH);
+            zed2DepthImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION);
             perceptionVisualizerPanel.addVisualizer(zed2DepthImageVisualizer);
 
             RDXROS2ColoredPointCloudVisualizer zed2ColoredPointCloudVisualizer
                   = new RDXROS2ColoredPointCloudVisualizer("ZED 2 Colored Point Cloud",
                                                            ros2Node,
                                                            PerceptionAPI.ZED2_DEPTH,
                                                            PerceptionAPI.ZED2_COLOR_IMAGES.get(RobotSide.LEFT));
-            zed2ColoredPointCloudVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_POINT_CLOUD);
+            zed2ColoredPointCloudVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION);
             zed2ColoredPointCloudVisualizer.setActive(true);
             perceptionVisualizerPanel.addVisualizer(zed2ColoredPointCloudVisualizer);
 

diff --git a/...viors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeUpdateThread.java b/...viors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeUpdateThread.java
@@ -23,7 +23,7 @@ public ROS2BehaviorTreeUpdateThread(ROS2Node ros2Node, DRCRobotModel robotModel,
       setFrequencyLimit(ROS2BehaviorTreeState.SYNC_FREQUENCY);
 
       ROS2ControllerHelper ros2ControllerHelper = new ROS2ControllerHelper(ros2Node, robotModel);
-      syncedRobot = new ROS2SyncedRobotModel(robotModel, ros2ControllerHelper.getROS2NodeInterface());
+      syncedRobot = new ROS2SyncedRobotModel(robotModel, ros2ControllerHelper.getROS2Node());
 
       ReferenceFrameLibrary referenceFrameLibrary = new ReferenceFrameLibrary();
       referenceFrameLibrary.addAll(Collections.singleton(ReferenceFrame.getWorldFrame()));

diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/IterativeClosestPointManager.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/IterativeClosestPointManager.java
@@ -92,7 +92,7 @@ else if (!requestMessage.getRunIcp())           // ICP Worker no longer needed (
          }
       });
 
-      workerThread = new RepeatingTaskThread(this::runWorkers, "ICPWorkers").setFrequencyLimit(ICP_WORK_FREQUENCY);
+      workerThread = new RepeatingTaskThread("ICPWorkers", this::runWorkers).setFrequencyLimit(ICP_WORK_FREQUENCY);
    }
 
    /**
@@ -139,7 +139,6 @@ public boolean isDemanded()
 
    public void destroy()
    {
-      requestMessageSubscription.destroy();
       nodeIDToWorkerMap.clear();
       workerThread.blockingKill();
    }

diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java
@@ -61,7 +61,7 @@ public StandAloneRealsenseProcess(ROS2Helper ros2Helper, ROS2SyncedRobotModel sy
             soleFrameSuppliers.put(side, () -> syncedRobot.getReferenceFrames().getSoleFrame(side));
          }
       }
-      realsenseDemandNode = new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_REALSENSE_POINT_CLOUD);
+      realsenseDemandNode = new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_REALSENSE_PUBLICATION);
 
       realsenseImageRetriever = new RealsenseColorDepthImageRetriever(new RealsenseDeviceManager(),
                                                                       RealsenseConfiguration.D455_COLOR_720P_DEPTH_720P_30HZ,

diff --git a/ihmc-java-toolkit/src/main/java/us/ihmc/tools/thread/RestartableThread.java b/ihmc-java-toolkit/src/main/java/us/ihmc/tools/thread/RestartableThread.java
@@ -90,15 +90,9 @@ public void blockingStop()
          ExceptionTools.handle((RunnableThatThrows) thread::join, exceptionHandler);
    }
 
-   public void interrupt()
-   {
-      if (thread != null)
-         thread.interrupt();
-   }
-
    public boolean isRunning()
    {
-      return running.get();
+      return running.getPlain();
    }
 
    boolean isAlive()

diff --git a/...-perception/src/main/java/us/ihmc/perception/detections/yolo/YOLOv8DetectionExecutor.java b/...-perception/src/main/java/us/ihmc/perception/detections/yolo/YOLOv8DetectionExecutor.java
@@ -144,7 +144,7 @@ public void runYOLODetectionOnAllModels(RawImage colorImage, RawImage depthImage
 
    /**
     * Non-blocking call to run YOLO on the provided images
-    * @param colorImage RGB color image, used for YOLO detection
+    * @param colorImage BGR color image, used for YOLO detection
     * @param depthImage 16UC1 depth image, used to get points of detected objects
     */
    public void runYOLODetection(YOLOv8ObjectDetector yoloDetector, RawImage colorImage, RawImage depthImage)

diff --git a/...rception/src/main/java/us/ihmc/perception/sceneGraph/ros2/ROS2SceneGraphUpdateThread.java b/...rception/src/main/java/us/ihmc/perception/sceneGraph/ros2/ROS2SceneGraphUpdateThread.java
@@ -79,7 +79,5 @@ protected void runTask()
       ReferenceFrame pelvisFrame = robotPelvisFrameSupplier.get();
       sceneGraph.updateOnRobotOnly(pelvisFrame);
       sceneGraph.updatePublication();
-
-      // TODO: add behavior tree?
    }
 }
diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/streaming/ROS2SRTSensorStreamer.java b/ihmc-perception/src/main/java/us/ihmc/perception/streaming/ROS2SRTSensorStreamer.java
@@ -28,7 +28,7 @@ public class ROS2SRTSensorStreamer
 
    public ROS2SRTSensorStreamer()
    {
-      this(new ROS2NodeBuilder.build(ROS2SRTSensorStreamer.class.getSimpleName().toLowerCase() + "_node"));
+      this(new ROS2NodeBuilder().build(ROS2SRTSensorStreamer.class.getSimpleName().toLowerCase() + "_node"));
       destroyROS2Node = true;
    }
 

diff --git a/ihmc-perception/src/main/java/us/ihmc/sensors/ImageSensor.java b/ihmc-perception/src/main/java/us/ihmc/sensors/ImageSensor.java
@@ -2,9 +2,9 @@
 
 import us.ihmc.commons.Conversions;
 import us.ihmc.commons.thread.RepeatingTaskThread;
+import us.ihmc.commons.thread.ThreadTools;
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.perception.RawImage;
-import us.ihmc.tools.thread.MissingThreadTools;
 
 import java.util.function.Supplier;
 
@@ -22,7 +22,7 @@ public abstract class ImageSensor implements AutoCloseable
    public ImageSensor(String sensorName)
    {
       this.sensorName = sensorName;
-      grabThread = new RepeatingTaskThread(this::grabAndNotify, sensorName + "Grabber");
+      grabThread = new RepeatingTaskThread(sensorName + "Grabber", this::grabAndNotify);
    }
 
    /**
@@ -114,7 +114,7 @@ private void grabAndNotify()
       // If the sensor is not running, try to start the sensor
       if (!isSensorRunning() && !startSensor())
       {  // if sensor failed to start, sleep a bit and retry
-         MissingThreadTools.sleep(SECONDS_BETWEEN_RETRIES);
+         ThreadTools.park(SECONDS_BETWEEN_RETRIES);
          return;
       }
 

diff --git a/ihmc-perception/src/main/java/us/ihmc/sensors/RealsenseImageSensor.java b/ihmc-perception/src/main/java/us/ihmc/sensors/RealsenseImageSensor.java
@@ -2,17 +2,16 @@
 
 import org.bytedeco.opencv.global.opencv_core;
 import org.bytedeco.opencv.opencv_core.Mat;
+import us.ihmc.commons.thread.Throttler;
 import us.ihmc.euclid.referenceFrame.FramePose3D;
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.log.LogTools;
 import us.ihmc.perception.RawImage;
 import us.ihmc.perception.realsense.RealsenseConfiguration;
 import us.ihmc.perception.realsense.RealsenseDevice;
 import us.ihmc.perception.realsense.RealsenseDeviceManager;
-import us.ihmc.tools.thread.Throttler;
 
 import java.time.Instant;
-import java.util.function.Supplier;
 
 public class RealsenseImageSensor extends ImageSensor
 {

diff --git a/ihmc-perception/src/main/java/us/ihmc/sensors/ZEDSVOPlaybackSensor.java b/ihmc-perception/src/main/java/us/ihmc/sensors/ZEDSVOPlaybackSensor.java
@@ -39,15 +39,15 @@ public ZEDSVOPlaybackSensor(ROS2PublishSubscribeAPI ros2, int cameraID, ZEDModel
       // Subscribe to play message
       ros2.subscribeViaCallback(PerceptionAPI.ZED_SVO_PLAY, () -> getGrabThread().startRepeating());
 
-      publishInfoThread = new RepeatingTaskThread(() ->
+      publishInfoThread = new RepeatingTaskThread("PublishSVOInfoThread", () ->
       {
          svoStatusMessage.setCurrentFileName(svoFileName);
          svoStatusMessage.setRecordMode((byte) 1); // playback
          svoStatusMessage.setCurrentPosition(getCurrentPosition());
          svoStatusMessage.setLength(getLength());
 
          ros2.publish(PerceptionAPI.ZED_SVO_CURRENT_FILE, svoStatusMessage);
-      }, "PublishSVOInfoThread").setFrequencyLimit(ZEDImageSensor.CAMERA_FPS);
+      }).setFrequencyLimit(ZEDImageSensor.CAMERA_FPS);
       publishInfoThread.start();
    }