Begin FP integration.

Author: calvertdw

ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/scene/BehaviorTreeSceneExecutor.java

@@ -5,6 +5,7 @@
 import us.ihmc.communication.crdt.CRDTInfo;
 import us.ihmc.perception.detections.InstantDetection;
 import us.ihmc.perception.detections.PersistentDetection;
+import us.ihmc.perception.detections.foundationPose.IsaacROSFoundationPoseCommunicator;
 import us.ihmc.perception.detections.foundationPose.IsaacROSFoundationPoseCommunicatorMap;
 import us.ihmc.perception.detections.foundationPose.IsaacROSFoundationPoseObject;
 import us.ihmc.perception.detections.yolo.YOLOv8DetectionExecutor;
@@ -45,6 +46,13 @@ public BehaviorTreeSceneExecutor(CRDTInfo crdtInfo,
 
       if (yolo != null)
          yolo.addDetectionConsumerCallback(instantDetectionQueue::add);
+
+      if (foundationPose != null)
+      {
+         IsaacROSFoundationPoseCommunicator mustardCommunicator = foundationPose.get(IsaacROSFoundationPoseObject.MUSTARD);
+         mustardCommunicator.enable(true);
+//         mustardCommunicator.addResultCallback(instantDetectionQueue::add);
+      }
    }
 
    public void update()