Setup behavior tree scene.

ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTDetachableReferenceFrame.java

@@ -2,22 +2,23 @@
 
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.robotics.referenceFrames.DetachableReferenceFrame;
-import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary;
+
+import java.util.function.Function;
 
 public class CRDTDetachableReferenceFrame
 {
    private final CRDTBidirectionalString parentFrameName;
    private final CRDTBidirectionalRigidBodyTransform transformToParent;
    private final DetachableReferenceFrame detachableReferenceFrame;
 
-   public CRDTDetachableReferenceFrame(ReferenceFrameLibrary referenceFrameLibrary,
+   public CRDTDetachableReferenceFrame(Function<String, ReferenceFrame> frameFunction,
                                        CRDTBidirectionalString parentFrameName,
                                        CRDTBidirectionalRigidBodyTransform transformToParent)
    {
       this.parentFrameName = parentFrameName;
       this.transformToParent = transformToParent;
 
-      detachableReferenceFrame = new DetachableReferenceFrame(referenceFrameLibrary, transformToParent.getValueReadOnly());
+      detachableReferenceFrame = new DetachableReferenceFrame(frameFunction, transformToParent.getValueReadOnly());
    }
 
    public void update()

ihmc-graphics/src/libgdx/java/us/ihmc/rdx/imgui/ImGuiReferenceFrameLibraryCombo.java

@@ -3,7 +3,6 @@
 import imgui.ImGui;
 import imgui.flag.ImGuiCol;
 import org.apache.commons.lang3.ArrayUtils;
-import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary;
 
 import java.util.SortedSet;
 import java.util.TreeSet;
@@ -14,12 +13,14 @@
  * Used to select between the reference frames in a library by human readable names.
  * It also includes any values that the user already might have and keeps those around
  * even if deselected. Also, this is done immediate mode style.
+ *
+ * TODO: Fix up with new design. Probably rename to be less general, like RDXBehaviorSceneFrameSelector...
  */
 public class ImGuiReferenceFrameLibraryCombo
 {
    private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass());
    private final String comboName;
-   private final ReferenceFrameLibrary referenceFrameLibrary;
+   private final Consumer<Consumer<String>> frameSupplier;
    private final Supplier<String> currentFrameNameGetter;
    private final Consumer<String> currentFrameNameSetter;
    private final SortedSet<String> referenceFrameLibraryNames = new TreeSet<>();
@@ -29,20 +30,20 @@ public class ImGuiReferenceFrameLibraryCombo
    private transient String[] selectableReferenceFrameNameArray = new String[0];
 
    public ImGuiReferenceFrameLibraryCombo(String comboName,
-                                          ReferenceFrameLibrary referenceFrameLibrary,
+                                          Consumer<Consumer<String>> frameSupplier,
                                           Supplier<String> currentFrameNameGetter,
                                           Consumer<String> currentFrameNameSetter)
    {
       this.comboName = comboName;
-      this.referenceFrameLibrary = referenceFrameLibrary;
+      this.frameSupplier = frameSupplier;
       this.currentFrameNameGetter = currentFrameNameGetter;
       this.currentFrameNameSetter = currentFrameNameSetter;
    }
 
    public void render()
    {
       referenceFrameLibraryNames.clear();
-      referenceFrameLibrary.getAllFrameNames(referenceFrameLibraryNames::add);
+      frameSupplier.accept(referenceFrameLibraryNames::add);
 
       selectableReferenceFrameNames.add(currentFrameNameGetter.get());
       selectableReferenceFrameNames.addAll(referenceFrameLibraryNames);

ihmc-graphics/src/libgdx/java/us/ihmc/rdx/ui/gizmo/RDXPose3DGizmo.java

@@ -43,6 +43,7 @@
 import us.ihmc.rdx.mesh.RDXMeshBuilder;
 import us.ihmc.rdx.mesh.RDXMeshDataInterpreter;
 import us.ihmc.rdx.mesh.RDXMultiColorMeshBuilder;
+import us.ihmc.rdx.sceneManager.RDXRenderableAdapter;
 import us.ihmc.rdx.sceneManager.RDXSceneLevel;
 import us.ihmc.rdx.tools.LibGDXTools;
 import us.ihmc.rdx.ui.RDX3DPanel;
@@ -51,7 +52,7 @@
 import us.ihmc.robotics.referenceFrames.ReferenceFrameMissingTools;
 import us.ihmc.robotics.robotSide.RobotSide;
 
-import java.util.Random;
+import java.util.function.Consumer;
 
 import static us.ihmc.rdx.ui.gizmo.RDXGizmoTools.AXIS_COLORS;
 import static us.ihmc.rdx.ui.gizmo.RDXGizmoTools.AXIS_SELECTED_COLORS;
@@ -121,6 +122,10 @@ public class RDXPose3DGizmo implements RenderableProvider
    private boolean queuePopupToOpen = false;
    private boolean proportionsNeedUpdate = false;
    private FrameBasedGizmoModification frameBasedGizmoModification;
+   private Runnable overlayAddition;
+   private Consumer<ImGui3DViewInput> pickCalculator;
+   private Consumer<ImGui3DViewInput> inputProcessor;
+   private RDXRenderableAdapter renderableAdapter;
 
    /** Maintains it's own frame in World. */
    public RDXPose3DGizmo()
@@ -225,16 +230,33 @@ public void changeParentFrameWithoutMoving(ReferenceFrame newParentFrame)
    public void createAndSetupDefault(RDX3DPanel panel3D)
    {
       create(panel3D);
-      panel3D.addImGui3DViewPickCalculator(this::calculate3DViewPick);
-      panel3D.addImGui3DViewInputProcessor(this::process3DViewInput);
-      panel3D.getScene().addRenderableProvider(this, RDXSceneLevel.VIRTUAL);
+      pickCalculator = this::calculate3DViewPick;
+      panel3D.addImGui3DViewPickCalculator(pickCalculator);
+      inputProcessor = this::process3DViewInput;
+      panel3D.addImGui3DViewInputProcessor(inputProcessor);
+      renderableAdapter = panel3D.getScene().addRenderableProvider(this, RDXSceneLevel.VIRTUAL);
+   }
+
+   public void destroyDefault(RDX3DPanel panel3D)
+   {
+      panel3D.removeImGui3DViewInputProcessor(inputProcessor);
+      panel3D.removeImGui3DViewPickCalculator(pickCalculator);
+      panel3D.getScene().removeRenderableAdapter(renderableAdapter);
+      panel3D.removeImGuiOverlayAddition(overlayAddition);
+
+      centerSphereModel.dispose();
+      for (int i = 0; i < arrowModels.length; i++)
+         arrowModels[i].dispose();
+      for (int i = 0; i < torusModels.length; i++)
+         torusModels[i].dispose();
    }
 
    public void create(RDX3DPanel panel3D)
    {
       camera3D = panel3D.getCamera3D();
       frameBasedGizmoModification = new FrameBasedGizmoModification(this::getGizmoFrame, () -> gizmoFrame.getParent(), camera3D);
-      panel3D.addImGuiOverlayAddition(this::renderTooltipAndContextMenu);
+      overlayAddition = this::renderTooltipAndContextMenu;
+      panel3D.addImGuiOverlayAddition(overlayAddition);
 
       centerSphereModel.setMesh(meshBuilder -> meshBuilder.addSphere(centerSphereRadius.get(), RDXGizmoTools.CENTER_DEFAULT_COLOR));
 

ihmc-graphics/src/libgdx/java/us/ihmc/rdx/ui/gizmo/RDXSelectablePose3DGizmo.java

@@ -156,4 +156,9 @@ public void setSelected(boolean selected)
    {
       this.selected.set(selected);
    }
+
+   public void destroyDefault(RDX3DPanel panel3D)
+   {
+      poseGizmo.destroyDefault(panel3D);
+   }
 }

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/behaviorTree/RDXAvailableBehaviorTreeDirectory.java

@@ -7,9 +7,9 @@
 import us.ihmc.behaviors.behaviorTree.topology.BehaviorTreeNodeInsertionType;
 import us.ihmc.behaviors.behaviorTree.topology.BehaviorTreeTopologyOperationQueue;
 import us.ihmc.log.LogTools;
+import us.ihmc.rdx.behaviorTree.scene.RDXBehaviorTreeScene;
 import us.ihmc.rdx.imgui.ImGuiExpandCollapseRenderer;
 import us.ihmc.rdx.imgui.ImGuiTools;
-import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary;
 import us.ihmc.tools.io.WorkspaceResourceDirectory;
 import us.ihmc.tools.io.WorkspaceResourceFile;
 
@@ -23,7 +23,7 @@ public class RDXAvailableBehaviorTreeDirectory
    private final WorkspaceResourceDirectory treeFilesDirectory;
    private final RDXBehaviorTree behaviorTree;
    private final BehaviorTreeTopologyOperationQueue<RDXBehaviorTreeNode<?, ?>> topologyOperationQueue;
-   private final ReferenceFrameLibrary referenceFrameLibrary;
+   private final RDXBehaviorTreeScene scene;
    private final Consumer<BehaviorTreeNodeInsertionDefinition<RDXBehaviorTreeNode<?, ?>>> complete;
 
    private final ImGuiExpandCollapseRenderer expandCollapseRenderer = new ImGuiExpandCollapseRenderer();
@@ -34,13 +34,13 @@ public class RDXAvailableBehaviorTreeDirectory
    public RDXAvailableBehaviorTreeDirectory(WorkspaceResourceDirectory treeFilesDirectory,
                                             RDXBehaviorTree behaviorTree,
                                             BehaviorTreeTopologyOperationQueue<RDXBehaviorTreeNode<?, ?>> topologyOperationQueue,
-                                            ReferenceFrameLibrary referenceFrameLibrary,
+                                            RDXBehaviorTreeScene scene,
                                             Consumer<BehaviorTreeNodeInsertionDefinition<RDXBehaviorTreeNode<?, ?>>> complete)
    {
       this.treeFilesDirectory = treeFilesDirectory;
       this.behaviorTree = behaviorTree;
       this.topologyOperationQueue = topologyOperationQueue;
-      this.referenceFrameLibrary = referenceFrameLibrary;
+      this.scene = scene;
       this.complete = complete;
    }
 
@@ -58,7 +58,7 @@ public void reindexDirectory()
             }
             else
             {
-               RDXAvailableBehaviorTreeFile treeFile = new RDXAvailableBehaviorTreeFile(queryContainedFile, referenceFrameLibrary);
+               RDXAvailableBehaviorTreeFile treeFile = new RDXAvailableBehaviorTreeFile(queryContainedFile, scene);
                if (treeFile.getName() != null && treeFile.getNotes() != null)
                {
                   indexedTreeFiles.add(treeFile);
@@ -75,7 +75,7 @@ public void reindexDirectory()
          RDXAvailableBehaviorTreeDirectory subtreeDirectory = new RDXAvailableBehaviorTreeDirectory(subdirectory,
                                                                                                     behaviorTree,
                                                                                                     topologyOperationQueue,
-                                                                                                    referenceFrameLibrary,
+                                                                                                    scene,
                                                                                                     complete);
          subtreeDirectory.reindexDirectory();
          indexedTreeDirectories.add(subtreeDirectory);

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/behaviorTree/RDXAvailableBehaviorTreeFile.java

@@ -2,7 +2,7 @@
 
 import com.fasterxml.jackson.databind.JsonNode;
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
-import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary;
+import us.ihmc.rdx.behaviorTree.scene.RDXBehaviorTreeScene;
 import us.ihmc.tools.io.JSONFileTools;
 import us.ihmc.tools.io.JSONTools;
 import us.ihmc.tools.io.WorkspaceResourceFile;
@@ -17,16 +17,16 @@ public class RDXAvailableBehaviorTreeFile
    public static final String[] FRAME_FIELD_NAMES = new String[] {"parentFrame", "objectFrame"};
 
    private final WorkspaceResourceFile treeFile;
-   private final ReferenceFrameLibrary referenceFrameLibrary;
+   private final RDXBehaviorTreeScene scene;
    private String name;
    private String notes;
    private final SortedSet<String> referenceFrameNames = new TreeSet<>();
    private final Set<String> referenceFramesInWorld = new HashSet<>();
 
-   public RDXAvailableBehaviorTreeFile(WorkspaceResourceFile treeFile, ReferenceFrameLibrary referenceFrameLibrary)
+   public RDXAvailableBehaviorTreeFile(WorkspaceResourceFile treeFile, RDXBehaviorTreeScene scene)
    {
       this.treeFile = treeFile;
-      this.referenceFrameLibrary = referenceFrameLibrary;
+      this.scene = scene;
 
       JSONFileTools.load(treeFile.getFilesystemFile(), this::loadFromFile);
    }
@@ -36,7 +36,7 @@ public void update()
       referenceFramesInWorld.clear();
       for (String referenceFrameName : referenceFrameNames)
       {
-         ReferenceFrame frameByName = referenceFrameLibrary.findFrameByName(referenceFrameName);
+         ReferenceFrame frameByName = scene.findFrameByName(referenceFrameName);
          if (frameByName != null && frameByName.getRootFrame() == ReferenceFrame.getWorldFrame())
          {
             referenceFramesInWorld.add(referenceFrameName);

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/behaviorTree/RDXBehaviorTree.java

@@ -8,13 +8,13 @@
 import imgui.ImGui;
 import imgui.flag.ImGuiMouseButton;
 import imgui.flag.ImGuiMouseCursor;
-import us.ihmc.avatar.drcRobot.DRCRobotModel;
 import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel;
 import us.ihmc.behaviors.behaviorTree.BehaviorTree;
 import us.ihmc.behaviors.behaviorTree.topology.BehaviorTreeNodeInsertionType;
 import us.ihmc.commons.MathTools;
 import us.ihmc.communication.ros2.ROS2ActorDesignation;
 import us.ihmc.communication.ros2.sync.ROS2PeerClockOffsetEstimator;
+import us.ihmc.rdx.behaviorTree.scene.RDXBehaviorTreeScene;
 import us.ihmc.rdx.imgui.ImGuiTools;
 import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap;
 import us.ihmc.rdx.imgui.RDXPanel;
@@ -25,7 +25,6 @@
 import us.ihmc.rdx.behaviorTree.actions.RDXActionProgressWidgetsManager.Type;
 import us.ihmc.rdx.vr.RDXVRContext;
 import us.ihmc.robotics.physics.RobotCollisionModel;
-import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary;
 import us.ihmc.tools.io.WorkspaceResourceDirectory;
 
 public class RDXBehaviorTree extends BehaviorTree<RDXBehaviorTreeRootNode, RDXBehaviorTreeNode<?, ?>>
@@ -41,26 +40,33 @@ public class RDXBehaviorTree extends BehaviorTree<RDXBehaviorTreeRootNode, RDXBe
    private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass());
    private final RDXBehaviorTreeNodeCreationMenu nodeCreationMenu;
    private final RDXBehaviorTreeWidgetsVerticalLayout treeWidgetsVerticalLayout;
+   private final RDXBehaviorTreeScene scene;
    private boolean anyNodeSelected;
    private RDXBehaviorTreeNode<?, ?> selectedNode;
    private boolean draggingDivider;
    private boolean shouldSave = false;
 
    public RDXBehaviorTree(WorkspaceResourceDirectory treeFilesDirectory,
-                          DRCRobotModel robotModel,
                           ROS2SyncedRobotModel syncedRobot,
                           ROS2PeerClockOffsetEstimator peerClockEstimator,
                           RobotCollisionModel selectionCollisionModel,
                           RDXBaseUI baseUI,
-                          RDX3DPanel panel3D,
-                          ReferenceFrameLibrary referenceFrameLibrary)
+                          RDX3DPanel panel3D)
    {
-      super(ROS2ActorDesignation.OPERATOR,
-            peerClockEstimator,
-            treeFilesDirectory,
-            new RDXBehaviorTreeNodeBuilder(robotModel, syncedRobot, referenceFrameLibrary, selectionCollisionModel, baseUI, panel3D));
+      super(syncedRobot, ROS2ActorDesignation.OPERATOR, peerClockEstimator, treeFilesDirectory, new RDXBehaviorTreeNodeBuilder());
 
-      nodeCreationMenu = new RDXBehaviorTreeNodeCreationMenu(this, treeFilesDirectory, referenceFrameLibrary);
+      scene = new RDXBehaviorTreeScene(crdtInfo, this::getAndIncrementNextID, syncedRobot, baseUI, panel);
+      setScene(scene);
+
+      ((RDXBehaviorTreeNodeBuilder) getNodeBuilder()).initialize(crdtInfo,
+                                                                 saveFileDirectory,
+                                                                 syncedRobot,
+                                                                 scene,
+                                                                 selectionCollisionModel,
+                                                                 baseUI,
+                                                                 panel3D);
+
+      nodeCreationMenu = new RDXBehaviorTreeNodeCreationMenu(this, treeFilesDirectory, scene);
       treeWidgetsVerticalLayout = new RDXBehaviorTreeWidgetsVerticalLayout(this);
       baseUI.getImGuiPanelManager().addPanel(panel);
    }
@@ -76,6 +82,7 @@ public void createAndSetupDefault(RDXBaseUI baseUI)
 
    public void update()
    {
+      scene.update();
       idToNodeMap.clear();
 
       if (rootNode != null)

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/behaviorTree/RDXBehaviorTreeNode.java

@@ -13,6 +13,7 @@
 import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel;
 import us.ihmc.behaviors.behaviorTree.*;
 import us.ihmc.behaviors.behaviorTree.log.BehaviorTreeNodeMessageLogger.LogMessage;
+import us.ihmc.rdx.behaviorTree.scene.RDXBehaviorTreeScene;
 import us.ihmc.rdx.imgui.ImGuiTools;
 import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap;
 import us.ihmc.rdx.input.ImGui3DViewInput;
@@ -21,7 +22,6 @@
 import us.ihmc.rdx.ui.tools.ImGuiScrollableLogArea;
 import us.ihmc.rdx.vr.RDXVRContext;
 import us.ihmc.robotics.physics.RobotCollisionModel;
-import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary;
 
 import javax.annotation.Nullable;
 import java.util.ArrayList;
@@ -67,7 +67,7 @@ public class RDXBehaviorTreeNode<S extends BehaviorTreeNodeState<D>,
    private boolean dragReleasedAfter = false;
 
    protected final DRCRobotModel robotModel;
-   protected final ReferenceFrameLibrary referenceFrameLibrary;
+   protected final RDXBehaviorTreeScene scene;
    protected final ROS2SyncedRobotModel syncedRobot;
    protected final RobotCollisionModel selectionCollisionModel;
    protected final RDXBaseUI baseUI;
@@ -86,7 +86,7 @@ public RDXBehaviorTreeNode(S state, RDXBehaviorTreeRootNode rootNode)
       this.state = state;
       this.rootNode = rootNode;
       this.robotModel = rootNode.getDefinition().getRobotModel();
-      this.referenceFrameLibrary = rootNode.getState().getReferenceFrameLibrary();
+      this.scene = rootNode.getScene();
       this.syncedRobot = rootNode.getSyncedRobot();
       this.selectionCollisionModel = rootNode.getSelectionCollisionModel();
       this.baseUI = rootNode.getBaseUI();
@@ -96,6 +96,7 @@ public RDXBehaviorTreeNode(S state, RDXBehaviorTreeRootNode rootNode)
    /** Root node constructor. */
    public RDXBehaviorTreeNode(S state,
                               ROS2SyncedRobotModel syncedRobot,
+                              RDXBehaviorTreeScene scene,
                               RobotCollisionModel selectionCollisionModel,
                               RDXBaseUI baseUI,
                               RDX3DPanel panel3D)
@@ -104,7 +105,7 @@ public RDXBehaviorTreeNode(S state,
       this.state = state;
       this.rootNode = (RDXBehaviorTreeRootNode) this;
       this.robotModel = rootNode.getDefinition().getRobotModel();
-      this.referenceFrameLibrary = rootNode.getState().getReferenceFrameLibrary();
+      this.scene = scene;
       this.syncedRobot = syncedRobot;
       this.selectionCollisionModel = selectionCollisionModel;
       this.baseUI = baseUI;