Get mustard relative behavior going.

Author: calvertdw

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

@@ -46,6 +46,8 @@ public BehaviorTreeExecutor(ROS2SyncedRobotModel syncedRobot,
 
    public void update()
    {
+      scene.update();
+
       if (rootNode != null)
       {
          rootNode.clock();

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

@@ -6,7 +6,6 @@
 import us.ihmc.perception.detections.foundationPose.IsaacROSFoundationPoseCommunicatorMap;
 import us.ihmc.perception.detections.yolo.YOLOv8DetectionExecutor;
 
-import java.util.ArrayList;
 import java.util.List;
 import java.util.function.LongSupplier;
 
@@ -15,7 +14,7 @@ public class BehaviorTreeSceneExecutor extends BehaviorTreeSceneState
    private final YOLOv8DetectionExecutor yolo;
    private final IsaacROSFoundationPoseCommunicatorMap foundationPose;
 
-   private final List<BehaviorTreeSceneObjectExecutor> trackers = new ArrayList<>();
+   private final List<BehaviorTreeSceneObjectExecutor> objects;
 
    public BehaviorTreeSceneExecutor(CRDTInfo crdtInfo,
                                     LongSupplier idSupplier,
@@ -28,17 +27,22 @@ public BehaviorTreeSceneExecutor(CRDTInfo crdtInfo,
       this.yolo = yolo;
       this.foundationPose = foundationPose;
 
-
+      objects = (List) super.objects;
 
    }
 
    public void update()
    {
-      for (String availableModelName : yolo.getAvailableModelNames())
+      for (BehaviorTreeSceneObjectExecutor object : objects)
       {
-
+         object.update();
       }
 
+//      for (String availableModelName : yolo.getAvailableModelNames())
+//      {
+//
+//      }
+
 //      yolo.enableModel("door");
 
       //      yolo.

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

@@ -18,7 +18,7 @@ public BehaviorTreeSceneObjectExecutor(long id, CRDTInfo crdtInfo, String type)
 
    public void update()
    {
-
+      persistentDetection = null;
    }
 
    public void destroy()

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

@@ -31,7 +31,7 @@ public BehaviorTreeSceneObjectState(long id, CRDTInfo crdtInfo, String type)
 
    public String getName()
    {
-      return "";
+      return type;
    }
 
    public void clearOffset()
@@ -64,6 +64,7 @@ public void fromMessage(BehaviorTreeSceneObjectStateMessage message)
          LogTools.error("Types should match! {} != {}", id, message.getId());
 
       transform.fromMessage(message.getTransformToWorld());
+      referenceFrame.update();
    }
 
    public void destroy()

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

@@ -59,24 +59,28 @@ private void addAll(Collection<ReferenceFrame> referenceFrames)
 
    public boolean containsFrame(String referenceFrameName)
    {
-      for (ReferenceFrame frame : robotFrames)
-      {
-         if (referenceFrameName.equals(frame.getName()))
-            return true;
-      }
+      boolean contains = robotFrameMap.containsKey(referenceFrameName);
+
+      if (!contains)
+         for (BehaviorTreeSceneObjectState object : objects)
+            if (referenceFrameName.startsWith(object.getName())) // TODO Affordance frame
+               return true;
 
-      return false;
+      return contains;
    }
 
    @Nullable
    public ReferenceFrame findFrameByName(String referenceFrameName)
    {
       // Check map first, then dynamic collections
       ReferenceFrame referenceFrame = robotFrameMap.get(referenceFrameName);
-      boolean frameFound = referenceFrame != null;
 
+      if (referenceFrame == null)
+         for (BehaviorTreeSceneObjectState object : objects)
+            if (referenceFrameName.startsWith(object.getName())) // TODO Affordance frame
+               return object.getReferenceFrame();
 
-      return frameFound ? referenceFrame : null;
+      return referenceFrame;
    }
 
    public void getAllFrameNames(Consumer<String> frameNameConsumer)
@@ -85,6 +89,11 @@ public void getAllFrameNames(Consumer<String> frameNameConsumer)
       {
          frameNameConsumer.accept(frame.getName());
       }
+
+      for (BehaviorTreeSceneObjectState object : objects)
+      {
+         frameNameConsumer.accept(object.getName());
+      }
    }
 
    public void toMessage(BehaviorTreeSceneStateMessage message)