Skip to content

Commit 7b46569

Browse files
calvertdwcalvertdw
authored
Setup behavior tree scene. (#1065)
1 parent fe9679a commit 7b46569

File tree

77 files changed

+1631
-529
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

77 files changed

+1631
-529
lines changed

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

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -2,22 +2,23 @@
22

33
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
44
import us.ihmc.robotics.referenceFrames.DetachableReferenceFrame;
5-
import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary;
5+
6+
import java.util.function.Function;
67

78
public class CRDTDetachableReferenceFrame
89
{
910
private final CRDTBidirectionalString parentFrameName;
1011
private final CRDTBidirectionalRigidBodyTransform transformToParent;
1112
private final DetachableReferenceFrame detachableReferenceFrame;
1213

13-
public CRDTDetachableReferenceFrame(ReferenceFrameLibrary referenceFrameLibrary,
14+
public CRDTDetachableReferenceFrame(Function<String, ReferenceFrame> frameFunction,
1415
CRDTBidirectionalString parentFrameName,
1516
CRDTBidirectionalRigidBodyTransform transformToParent)
1617
{
1718
this.parentFrameName = parentFrameName;
1819
this.transformToParent = transformToParent;
1920

20-
detachableReferenceFrame = new DetachableReferenceFrame(referenceFrameLibrary, transformToParent.getValueReadOnly());
21+
detachableReferenceFrame = new DetachableReferenceFrame(frameFunction, transformToParent.getValueReadOnly());
2122
}
2223

2324
public void update()

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

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,6 @@
33
import imgui.ImGui;
44
import imgui.flag.ImGuiCol;
55
import org.apache.commons.lang3.ArrayUtils;
6-
import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary;
76

87
import java.util.SortedSet;
98
import java.util.TreeSet;
@@ -14,12 +13,14 @@
1413
* Used to select between the reference frames in a library by human readable names.
1514
* It also includes any values that the user already might have and keeps those around
1615
* even if deselected. Also, this is done immediate mode style.
16+
*
17+
* TODO: Fix up with new design. Probably rename to be less general, like RDXBehaviorSceneFrameSelector...
1718
*/
1819
public class ImGuiReferenceFrameLibraryCombo
1920
{
2021
private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass());
2122
private final String comboName;
22-
private final ReferenceFrameLibrary referenceFrameLibrary;
23+
private final Consumer<Consumer<String>> frameSupplier;
2324
private final Supplier<String> currentFrameNameGetter;
2425
private final Consumer<String> currentFrameNameSetter;
2526
private final SortedSet<String> referenceFrameLibraryNames = new TreeSet<>();
@@ -29,20 +30,20 @@ public class ImGuiReferenceFrameLibraryCombo
2930
private transient String[] selectableReferenceFrameNameArray = new String[0];
3031

3132
public ImGuiReferenceFrameLibraryCombo(String comboName,
32-
ReferenceFrameLibrary referenceFrameLibrary,
33+
Consumer<Consumer<String>> frameSupplier,
3334
Supplier<String> currentFrameNameGetter,
3435
Consumer<String> currentFrameNameSetter)
3536
{
3637
this.comboName = comboName;
37-
this.referenceFrameLibrary = referenceFrameLibrary;
38+
this.frameSupplier = frameSupplier;
3839
this.currentFrameNameGetter = currentFrameNameGetter;
3940
this.currentFrameNameSetter = currentFrameNameSetter;
4041
}
4142

4243
public void render()
4344
{
4445
referenceFrameLibraryNames.clear();
45-
referenceFrameLibrary.getAllFrameNames(referenceFrameLibraryNames::add);
46+
frameSupplier.accept(referenceFrameLibraryNames::add);
4647

4748
selectableReferenceFrameNames.add(currentFrameNameGetter.get());
4849
selectableReferenceFrameNames.addAll(referenceFrameLibraryNames);

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

Lines changed: 27 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,7 @@
4343
import us.ihmc.rdx.mesh.RDXMeshBuilder;
4444
import us.ihmc.rdx.mesh.RDXMeshDataInterpreter;
4545
import us.ihmc.rdx.mesh.RDXMultiColorMeshBuilder;
46+
import us.ihmc.rdx.sceneManager.RDXRenderableAdapter;
4647
import us.ihmc.rdx.sceneManager.RDXSceneLevel;
4748
import us.ihmc.rdx.tools.LibGDXTools;
4849
import us.ihmc.rdx.ui.RDX3DPanel;
@@ -51,7 +52,7 @@
5152
import us.ihmc.robotics.referenceFrames.ReferenceFrameMissingTools;
5253
import us.ihmc.robotics.robotSide.RobotSide;
5354

54-
import java.util.Random;
55+
import java.util.function.Consumer;
5556

5657
import static us.ihmc.rdx.ui.gizmo.RDXGizmoTools.AXIS_COLORS;
5758
import static us.ihmc.rdx.ui.gizmo.RDXGizmoTools.AXIS_SELECTED_COLORS;
@@ -121,6 +122,10 @@ public class RDXPose3DGizmo implements RenderableProvider
121122
private boolean queuePopupToOpen = false;
122123
private boolean proportionsNeedUpdate = false;
123124
private FrameBasedGizmoModification frameBasedGizmoModification;
125+
private Runnable overlayAddition;
126+
private Consumer<ImGui3DViewInput> pickCalculator;
127+
private Consumer<ImGui3DViewInput> inputProcessor;
128+
private RDXRenderableAdapter renderableAdapter;
124129

125130
/** Maintains it's own frame in World. */
126131
public RDXPose3DGizmo()
@@ -225,16 +230,33 @@ public void changeParentFrameWithoutMoving(ReferenceFrame newParentFrame)
225230
public void createAndSetupDefault(RDX3DPanel panel3D)
226231
{
227232
create(panel3D);
228-
panel3D.addImGui3DViewPickCalculator(this::calculate3DViewPick);
229-
panel3D.addImGui3DViewInputProcessor(this::process3DViewInput);
230-
panel3D.getScene().addRenderableProvider(this, RDXSceneLevel.VIRTUAL);
233+
pickCalculator = this::calculate3DViewPick;
234+
panel3D.addImGui3DViewPickCalculator(pickCalculator);
235+
inputProcessor = this::process3DViewInput;
236+
panel3D.addImGui3DViewInputProcessor(inputProcessor);
237+
renderableAdapter = panel3D.getScene().addRenderableProvider(this, RDXSceneLevel.VIRTUAL);
238+
}
239+
240+
public void destroyDefault(RDX3DPanel panel3D)
241+
{
242+
panel3D.removeImGui3DViewInputProcessor(inputProcessor);
243+
panel3D.removeImGui3DViewPickCalculator(pickCalculator);
244+
panel3D.getScene().removeRenderableAdapter(renderableAdapter);
245+
panel3D.removeImGuiOverlayAddition(overlayAddition);
246+
247+
centerSphereModel.dispose();
248+
for (int i = 0; i < arrowModels.length; i++)
249+
arrowModels[i].dispose();
250+
for (int i = 0; i < torusModels.length; i++)
251+
torusModels[i].dispose();
231252
}
232253

233254
public void create(RDX3DPanel panel3D)
234255
{
235256
camera3D = panel3D.getCamera3D();
236257
frameBasedGizmoModification = new FrameBasedGizmoModification(this::getGizmoFrame, () -> gizmoFrame.getParent(), camera3D);
237-
panel3D.addImGuiOverlayAddition(this::renderTooltipAndContextMenu);
258+
overlayAddition = this::renderTooltipAndContextMenu;
259+
panel3D.addImGuiOverlayAddition(overlayAddition);
238260

239261
centerSphereModel.setMesh(meshBuilder -> meshBuilder.addSphere(centerSphereRadius.get(), RDXGizmoTools.CENTER_DEFAULT_COLOR));
240262

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

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -156,4 +156,9 @@ public void setSelected(boolean selected)
156156
{
157157
this.selected.set(selected);
158158
}
159+
160+
public void destroyDefault(RDX3DPanel panel3D)
161+
{
162+
poseGizmo.destroyDefault(panel3D);
163+
}
159164
}

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

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -7,9 +7,9 @@
77
import us.ihmc.behaviors.behaviorTree.topology.BehaviorTreeNodeInsertionType;
88
import us.ihmc.behaviors.behaviorTree.topology.BehaviorTreeTopologyOperationQueue;
99
import us.ihmc.log.LogTools;
10+
import us.ihmc.rdx.behaviorTree.scene.RDXBehaviorTreeScene;
1011
import us.ihmc.rdx.imgui.ImGuiExpandCollapseRenderer;
1112
import us.ihmc.rdx.imgui.ImGuiTools;
12-
import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary;
1313
import us.ihmc.tools.io.WorkspaceResourceDirectory;
1414
import us.ihmc.tools.io.WorkspaceResourceFile;
1515

@@ -23,7 +23,7 @@ public class RDXAvailableBehaviorTreeDirectory
2323
private final WorkspaceResourceDirectory treeFilesDirectory;
2424
private final RDXBehaviorTree behaviorTree;
2525
private final BehaviorTreeTopologyOperationQueue<RDXBehaviorTreeNode<?, ?>> topologyOperationQueue;
26-
private final ReferenceFrameLibrary referenceFrameLibrary;
26+
private final RDXBehaviorTreeScene scene;
2727
private final Consumer<BehaviorTreeNodeInsertionDefinition<RDXBehaviorTreeNode<?, ?>>> complete;
2828

2929
private final ImGuiExpandCollapseRenderer expandCollapseRenderer = new ImGuiExpandCollapseRenderer();
@@ -34,13 +34,13 @@ public class RDXAvailableBehaviorTreeDirectory
3434
public RDXAvailableBehaviorTreeDirectory(WorkspaceResourceDirectory treeFilesDirectory,
3535
RDXBehaviorTree behaviorTree,
3636
BehaviorTreeTopologyOperationQueue<RDXBehaviorTreeNode<?, ?>> topologyOperationQueue,
37-
ReferenceFrameLibrary referenceFrameLibrary,
37+
RDXBehaviorTreeScene scene,
3838
Consumer<BehaviorTreeNodeInsertionDefinition<RDXBehaviorTreeNode<?, ?>>> complete)
3939
{
4040
this.treeFilesDirectory = treeFilesDirectory;
4141
this.behaviorTree = behaviorTree;
4242
this.topologyOperationQueue = topologyOperationQueue;
43-
this.referenceFrameLibrary = referenceFrameLibrary;
43+
this.scene = scene;
4444
this.complete = complete;
4545
}
4646

@@ -58,7 +58,7 @@ public void reindexDirectory()
5858
}
5959
else
6060
{
61-
RDXAvailableBehaviorTreeFile treeFile = new RDXAvailableBehaviorTreeFile(queryContainedFile, referenceFrameLibrary);
61+
RDXAvailableBehaviorTreeFile treeFile = new RDXAvailableBehaviorTreeFile(queryContainedFile, scene);
6262
if (treeFile.getName() != null && treeFile.getNotes() != null)
6363
{
6464
indexedTreeFiles.add(treeFile);
@@ -75,7 +75,7 @@ public void reindexDirectory()
7575
RDXAvailableBehaviorTreeDirectory subtreeDirectory = new RDXAvailableBehaviorTreeDirectory(subdirectory,
7676
behaviorTree,
7777
topologyOperationQueue,
78-
referenceFrameLibrary,
78+
scene,
7979
complete);
8080
subtreeDirectory.reindexDirectory();
8181
indexedTreeDirectories.add(subtreeDirectory);

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

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22

33
import com.fasterxml.jackson.databind.JsonNode;
44
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
5-
import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary;
5+
import us.ihmc.rdx.behaviorTree.scene.RDXBehaviorTreeScene;
66
import us.ihmc.tools.io.JSONFileTools;
77
import us.ihmc.tools.io.JSONTools;
88
import us.ihmc.tools.io.WorkspaceResourceFile;
@@ -17,16 +17,16 @@ public class RDXAvailableBehaviorTreeFile
1717
public static final String[] FRAME_FIELD_NAMES = new String[] {"parentFrame", "objectFrame"};
1818

1919
private final WorkspaceResourceFile treeFile;
20-
private final ReferenceFrameLibrary referenceFrameLibrary;
20+
private final RDXBehaviorTreeScene scene;
2121
private String name;
2222
private String notes;
2323
private final SortedSet<String> referenceFrameNames = new TreeSet<>();
2424
private final Set<String> referenceFramesInWorld = new HashSet<>();
2525

26-
public RDXAvailableBehaviorTreeFile(WorkspaceResourceFile treeFile, ReferenceFrameLibrary referenceFrameLibrary)
26+
public RDXAvailableBehaviorTreeFile(WorkspaceResourceFile treeFile, RDXBehaviorTreeScene scene)
2727
{
2828
this.treeFile = treeFile;
29-
this.referenceFrameLibrary = referenceFrameLibrary;
29+
this.scene = scene;
3030

3131
JSONFileTools.load(treeFile.getFilesystemFile(), this::loadFromFile);
3232
}
@@ -36,7 +36,7 @@ public void update()
3636
referenceFramesInWorld.clear();
3737
for (String referenceFrameName : referenceFrameNames)
3838
{
39-
ReferenceFrame frameByName = referenceFrameLibrary.findFrameByName(referenceFrameName);
39+
ReferenceFrame frameByName = scene.findFrameByName(referenceFrameName);
4040
if (frameByName != null && frameByName.getRootFrame() == ReferenceFrame.getWorldFrame())
4141
{
4242
referenceFramesInWorld.add(referenceFrameName);

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

Lines changed: 17 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -8,13 +8,13 @@
88
import imgui.ImGui;
99
import imgui.flag.ImGuiMouseButton;
1010
import imgui.flag.ImGuiMouseCursor;
11-
import us.ihmc.avatar.drcRobot.DRCRobotModel;
1211
import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel;
1312
import us.ihmc.behaviors.behaviorTree.BehaviorTree;
1413
import us.ihmc.behaviors.behaviorTree.topology.BehaviorTreeNodeInsertionType;
1514
import us.ihmc.commons.MathTools;
1615
import us.ihmc.communication.ros2.ROS2ActorDesignation;
1716
import us.ihmc.communication.ros2.sync.ROS2PeerClockOffsetEstimator;
17+
import us.ihmc.rdx.behaviorTree.scene.RDXBehaviorTreeScene;
1818
import us.ihmc.rdx.imgui.ImGuiTools;
1919
import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap;
2020
import us.ihmc.rdx.imgui.RDXPanel;
@@ -25,7 +25,6 @@
2525
import us.ihmc.rdx.behaviorTree.actions.RDXActionProgressWidgetsManager.Type;
2626
import us.ihmc.rdx.vr.RDXVRContext;
2727
import us.ihmc.robotics.physics.RobotCollisionModel;
28-
import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary;
2928
import us.ihmc.tools.io.WorkspaceResourceDirectory;
3029

3130
public class RDXBehaviorTree extends BehaviorTree<RDXBehaviorTreeRootNode, RDXBehaviorTreeNode<?, ?>>
@@ -41,26 +40,33 @@ public class RDXBehaviorTree extends BehaviorTree<RDXBehaviorTreeRootNode, RDXBe
4140
private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass());
4241
private final RDXBehaviorTreeNodeCreationMenu nodeCreationMenu;
4342
private final RDXBehaviorTreeWidgetsVerticalLayout treeWidgetsVerticalLayout;
43+
private final RDXBehaviorTreeScene scene;
4444
private boolean anyNodeSelected;
4545
private RDXBehaviorTreeNode<?, ?> selectedNode;
4646
private boolean draggingDivider;
4747
private boolean shouldSave = false;
4848

4949
public RDXBehaviorTree(WorkspaceResourceDirectory treeFilesDirectory,
50-
DRCRobotModel robotModel,
5150
ROS2SyncedRobotModel syncedRobot,
5251
ROS2PeerClockOffsetEstimator peerClockEstimator,
5352
RobotCollisionModel selectionCollisionModel,
5453
RDXBaseUI baseUI,
55-
RDX3DPanel panel3D,
56-
ReferenceFrameLibrary referenceFrameLibrary)
54+
RDX3DPanel panel3D)
5755
{
58-
super(ROS2ActorDesignation.OPERATOR,
59-
peerClockEstimator,
60-
treeFilesDirectory,
61-
new RDXBehaviorTreeNodeBuilder(robotModel, syncedRobot, referenceFrameLibrary, selectionCollisionModel, baseUI, panel3D));
56+
super(syncedRobot, ROS2ActorDesignation.OPERATOR, peerClockEstimator, treeFilesDirectory, new RDXBehaviorTreeNodeBuilder());
6257

63-
nodeCreationMenu = new RDXBehaviorTreeNodeCreationMenu(this, treeFilesDirectory, referenceFrameLibrary);
58+
scene = new RDXBehaviorTreeScene(crdtInfo, this::getAndIncrementNextID, syncedRobot, baseUI, panel);
59+
setScene(scene);
60+
61+
((RDXBehaviorTreeNodeBuilder) getNodeBuilder()).initialize(crdtInfo,
62+
saveFileDirectory,
63+
syncedRobot,
64+
scene,
65+
selectionCollisionModel,
66+
baseUI,
67+
panel3D);
68+
69+
nodeCreationMenu = new RDXBehaviorTreeNodeCreationMenu(this, treeFilesDirectory, scene);
6470
treeWidgetsVerticalLayout = new RDXBehaviorTreeWidgetsVerticalLayout(this);
6571
baseUI.getImGuiPanelManager().addPanel(panel);
6672
}
@@ -76,6 +82,7 @@ public void createAndSetupDefault(RDXBaseUI baseUI)
7682

7783
public void update()
7884
{
85+
scene.update();
7986
idToNodeMap.clear();
8087

8188
if (rootNode != null)

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

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@
1313
import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel;
1414
import us.ihmc.behaviors.behaviorTree.*;
1515
import us.ihmc.behaviors.behaviorTree.log.BehaviorTreeNodeMessageLogger.LogMessage;
16+
import us.ihmc.rdx.behaviorTree.scene.RDXBehaviorTreeScene;
1617
import us.ihmc.rdx.imgui.ImGuiTools;
1718
import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap;
1819
import us.ihmc.rdx.input.ImGui3DViewInput;
@@ -21,7 +22,6 @@
2122
import us.ihmc.rdx.ui.tools.ImGuiScrollableLogArea;
2223
import us.ihmc.rdx.vr.RDXVRContext;
2324
import us.ihmc.robotics.physics.RobotCollisionModel;
24-
import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary;
2525

2626
import javax.annotation.Nullable;
2727
import java.util.ArrayList;
@@ -67,7 +67,7 @@ public class RDXBehaviorTreeNode<S extends BehaviorTreeNodeState<D>,
6767
private boolean dragReleasedAfter = false;
6868

6969
protected final DRCRobotModel robotModel;
70-
protected final ReferenceFrameLibrary referenceFrameLibrary;
70+
protected final RDXBehaviorTreeScene scene;
7171
protected final ROS2SyncedRobotModel syncedRobot;
7272
protected final RobotCollisionModel selectionCollisionModel;
7373
protected final RDXBaseUI baseUI;
@@ -86,7 +86,7 @@ public RDXBehaviorTreeNode(S state, RDXBehaviorTreeRootNode rootNode)
8686
this.state = state;
8787
this.rootNode = rootNode;
8888
this.robotModel = rootNode.getDefinition().getRobotModel();
89-
this.referenceFrameLibrary = rootNode.getState().getReferenceFrameLibrary();
89+
this.scene = rootNode.getScene();
9090
this.syncedRobot = rootNode.getSyncedRobot();
9191
this.selectionCollisionModel = rootNode.getSelectionCollisionModel();
9292
this.baseUI = rootNode.getBaseUI();
@@ -96,6 +96,7 @@ public RDXBehaviorTreeNode(S state, RDXBehaviorTreeRootNode rootNode)
9696
/** Root node constructor. */
9797
public RDXBehaviorTreeNode(S state,
9898
ROS2SyncedRobotModel syncedRobot,
99+
RDXBehaviorTreeScene scene,
99100
RobotCollisionModel selectionCollisionModel,
100101
RDXBaseUI baseUI,
101102
RDX3DPanel panel3D)
@@ -104,7 +105,7 @@ public RDXBehaviorTreeNode(S state,
104105
this.state = state;
105106
this.rootNode = (RDXBehaviorTreeRootNode) this;
106107
this.robotModel = rootNode.getDefinition().getRobotModel();
107-
this.referenceFrameLibrary = rootNode.getState().getReferenceFrameLibrary();
108+
this.scene = scene;
108109
this.syncedRobot = syncedRobot;
109110
this.selectionCollisionModel = selectionCollisionModel;
110111
this.baseUI = baseUI;

0 commit comments

Comments
 (0)