Skip to content

Commit 4840c7b

Browse files
calvertdwcalvertdw
committed
Implement behavior scene object synchronization.
1 parent b33b9d5 commit 4840c7b

18 files changed

+262
-45
lines changed

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -55,7 +55,7 @@ public RDXBehaviorTree(WorkspaceResourceDirectory treeFilesDirectory,
5555
{
5656
super(syncedRobot, ROS2ActorDesignation.OPERATOR, peerClockEstimator, treeFilesDirectory, new RDXBehaviorTreeNodeBuilder());
5757

58-
scene = new RDXBehaviorTreeScene(syncedRobot, baseUI, panel);
58+
scene = new RDXBehaviorTreeScene(crdtInfo, this::getAndIncrementNextID, syncedRobot, baseUI, panel);
5959
setScene(scene);
6060

6161
((RDXBehaviorTreeNodeBuilder) getNodeBuilder()).initialize(crdtInfo,

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

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -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.scene = rootNode.getScene();
108+
this.scene = scene;
108109
this.syncedRobot = syncedRobot;
109110
this.selectionCollisionModel = selectionCollisionModel;
110111
this.baseUI = baseUI;

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -49,6 +49,7 @@ public RDXBehaviorTreeRootNode(long id,
4949
{
5050
super(new BehaviorTreeRootNodeState(id, crdtInfo, saveFileDirectory, syncedRobot.getRobotModel(), scene),
5151
syncedRobot,
52+
scene,
5253
selectionCollisionModel,
5354
baseUI,
5455
panel3D);

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

Lines changed: 19 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -1,17 +1,21 @@
11
package us.ihmc.rdx.behaviorTree.scene;
22

3+
import behavior_msgs.msg.dds.BehaviorTreeSceneObjectStateMessage;
34
import imgui.ImGui;
45
import imgui.type.ImBoolean;
56
import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel;
7+
import us.ihmc.behaviors.behaviorTree.scene.BehaviorTreeSceneObjectState;
68
import us.ihmc.behaviors.behaviorTree.scene.BehaviorTreeSceneState;
9+
import us.ihmc.communication.crdt.CRDTInfo;
710
import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames;
11+
import us.ihmc.perception.detections.foundationPose.IsaacROSFoundationPoseObject;
812
import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap;
913
import us.ihmc.rdx.imgui.RDXPanel;
1014
import us.ihmc.rdx.input.ImGui3DViewInput;
1115
import us.ihmc.rdx.ui.RDXBaseUI;
1216

13-
import java.util.ArrayList;
1417
import java.util.List;
18+
import java.util.function.LongSupplier;
1519

1620
public class RDXBehaviorTreeScene extends BehaviorTreeSceneState
1721
{
@@ -20,17 +24,19 @@ public class RDXBehaviorTreeScene extends BehaviorTreeSceneState
2024
private final RDXPanel panel = new RDXPanel("Scene", this::renderImGuiWidgets);
2125
private final ImBoolean mustard = new ImBoolean(false);
2226

23-
private final List<RDXBehaviorTreeSceneObject> objects = new ArrayList<>();
27+
private final List<RDXBehaviorTreeSceneObject> objects;
2428

2529
private boolean needToInitializePlacementHeight = false;
2630
private RDXBehaviorTreeSceneObject beingPlaced;
2731

28-
public RDXBehaviorTreeScene(ROS2SyncedRobotModel syncedRobot, RDXBaseUI baseUI, RDXPanel parentPanel)
32+
public RDXBehaviorTreeScene(CRDTInfo crdtInfo, LongSupplier idSupplier, ROS2SyncedRobotModel syncedRobot, RDXBaseUI baseUI, RDXPanel parentPanel)
2933
{
30-
super(syncedRobot);
34+
super(crdtInfo, idSupplier, syncedRobot);
3135

3236
this.baseUI = baseUI;
3337

38+
this.objects = (List) super.objects;
39+
3440
parentPanel.addChild(panel);
3541

3642
baseUI.getPrimary3DPanel().addImGui3DViewInputProcessor(this::processImGui3DViewInput);
@@ -56,25 +62,30 @@ private void processImGui3DViewInput(ImGui3DViewInput input)
5662

5763
private void renderImGuiWidgets()
5864
{
59-
// place mustard in scene?
60-
6165
if (ImGui.checkbox(labels.get("Mustard"), mustard))
6266
{
6367
if (mustard.get())
6468
{
65-
beingPlaced = new RDXBehaviorTreeSceneObject("environmentObjects/mustard/mustard.glb", baseUI);
69+
beingPlaced = new RDXBehaviorTreeSceneObject(idSupplier.getAsLong(), crdtInfo, IsaacROSFoundationPoseObject.MUSTARD.name(), baseUI);
6670
objects.add(beingPlaced);
71+
objectsModifiable.modify();
6772
needToInitializePlacementHeight = true;
6873
}
6974
else
7075
{
7176
if (!objects.isEmpty())
7277
{
7378
RDXBehaviorTreeSceneObject remove = objects.remove(0);
79+
objectsModifiable.modify();
7480
remove.destroy();
7581
}
76-
// TODO Remove mustard
7782
}
7883
}
7984
}
85+
86+
@Override
87+
protected BehaviorTreeSceneObjectState buildObject(BehaviorTreeSceneObjectStateMessage message)
88+
{
89+
return new RDXBehaviorTreeSceneObject(message.getId(), crdtInfo, message.getTypeAsString(), baseUI);
90+
}
8091
}

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

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

33
import com.badlogic.gdx.graphics.g3d.Model;
44
import us.ihmc.behaviors.behaviorTree.scene.BehaviorTreeSceneObjectState;
5-
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
5+
import us.ihmc.communication.crdt.CRDTInfo;
6+
import us.ihmc.perception.detections.foundationPose.IsaacROSFoundationPoseObject;
7+
import us.ihmc.rdx.behaviorTree.RDXCRDTTools;
68
import us.ihmc.rdx.sceneManager.RDXRenderableAdapter;
79
import us.ihmc.rdx.tools.RDXModelInstance;
810
import us.ihmc.rdx.tools.RDXModelLoader;
@@ -18,14 +20,20 @@ public class RDXBehaviorTreeSceneObject extends BehaviorTreeSceneObjectState
1820
private final RDXModelInstance modelInstance;
1921
private final RDXRenderableAdapter modelRenderableAdapter;
2022

21-
// TODO: Instead give an object type with the object name stored centrally
22-
public RDXBehaviorTreeSceneObject(String modelName, RDXBaseUI baseUI)
23+
public RDXBehaviorTreeSceneObject(long id, CRDTInfo crdtInfo, String type, RDXBaseUI baseUI)
2324
{
25+
super(id, crdtInfo, type);
26+
2427
this.baseUI = baseUI;
25-
gizmo = new RDXSelectablePose3DGizmo(transform, ReferenceFrame.getWorldFrame());
2628

29+
gizmo = new RDXSelectablePose3DGizmo();
2730
gizmo.createAndSetupDefault(baseUI.getPrimary3DPanel());
2831
gizmo.setSelected(true);
32+
gizmo.getPoseGizmo().setGizmoFrame(referenceFrame);
33+
34+
String modelName = "";
35+
if (type.equals(IsaacROSFoundationPoseObject.MUSTARD.name()))
36+
modelName = "environmentObjects/mustard/mustard.glb";
2937

3038
model = RDXModelLoader.load(modelName);
3139
modelInstance = new RDXModelInstance(model);
@@ -34,7 +42,8 @@ public RDXBehaviorTreeSceneObject(String modelName, RDXBaseUI baseUI)
3442

3543
public void update()
3644
{
37-
modelInstance.setTransformToWorldFrame(transform);
45+
RDXCRDTTools.syncGizmoWithBidirectionalField(gizmo.getPoseGizmo(), transform, this);
46+
modelInstance.setTransformToWorldFrame(transform.getValueUnsafe());
3847
}
3948

4049
public void destroy()

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ public BehaviorTreeExecutor(ROS2SyncedRobotModel syncedRobot,
3333
new BehaviorTreeExecutorNodeBuilder());
3434

3535
controllerStatusTracker = new ControllerStatusTracker(new LogToolsLogger(), ros2ControllerHelper.getROS2Node(), robotModel.getSimpleRobotName());
36-
scene = new BehaviorTreeSceneExecutor(syncedRobot, yolo, foundationPose);
36+
scene = new BehaviorTreeSceneExecutor(crdtInfo, this::getAndIncrementNextID, syncedRobot, yolo, foundationPose);
3737
setScene(scene);
3838

3939
((BehaviorTreeExecutorNodeBuilder) getNodeBuilder()).initialize(crdtInfo,

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

Lines changed: 17 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,22 +1,29 @@
11
package us.ihmc.behaviors.behaviorTree.scene;
22

3+
import behavior_msgs.msg.dds.BehaviorTreeSceneObjectStateMessage;
34
import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel;
5+
import us.ihmc.communication.crdt.CRDTInfo;
46
import us.ihmc.perception.detections.foundationPose.IsaacROSFoundationPoseCommunicatorMap;
57
import us.ihmc.perception.detections.yolo.YOLOv8DetectionExecutor;
68

79
import java.util.ArrayList;
810
import java.util.List;
11+
import java.util.function.LongSupplier;
912

1013
public class BehaviorTreeSceneExecutor extends BehaviorTreeSceneState
1114
{
1215
private final YOLOv8DetectionExecutor yolo;
1316
private final IsaacROSFoundationPoseCommunicatorMap foundationPose;
1417

15-
private final List<BehaviorTreeSceneObjectTracker> trackers = new ArrayList<>();
18+
private final List<BehaviorTreeSceneObjectExecutor> trackers = new ArrayList<>();
1619

17-
public BehaviorTreeSceneExecutor(ROS2SyncedRobotModel syncedRobot, YOLOv8DetectionExecutor yolo, IsaacROSFoundationPoseCommunicatorMap foundationPose)
20+
public BehaviorTreeSceneExecutor(CRDTInfo crdtInfo,
21+
LongSupplier idSupplier,
22+
ROS2SyncedRobotModel syncedRobot,
23+
YOLOv8DetectionExecutor yolo,
24+
IsaacROSFoundationPoseCommunicatorMap foundationPose)
1825
{
19-
super(syncedRobot);
26+
super(crdtInfo, idSupplier, syncedRobot);
2027

2128
this.yolo = yolo;
2229
this.foundationPose = foundationPose;
@@ -32,8 +39,14 @@ public void update()
3239

3340
}
3441

35-
yolo.enableModel("door");
42+
// yolo.enableModel("door");
3643

3744
// yolo.
3845
}
46+
47+
@Override
48+
protected BehaviorTreeSceneObjectState buildObject(BehaviorTreeSceneObjectStateMessage message)
49+
{
50+
return new BehaviorTreeSceneObjectExecutor(message.getId(), crdtInfo, message.getTypeAsString());
51+
}
3952
}

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

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
package us.ihmc.behaviors.behaviorTree.scene;
2+
3+
import us.ihmc.communication.crdt.CRDTInfo;
4+
import us.ihmc.perception.detections.PersistentDetection;
5+
6+
/**
7+
* Also known as the "Tracker"
8+
*/
9+
public class BehaviorTreeSceneObjectExecutor extends BehaviorTreeSceneObjectState
10+
{
11+
private PersistentDetection persistentDetection;
12+
13+
public BehaviorTreeSceneObjectExecutor(long id, CRDTInfo crdtInfo, String type)
14+
{
15+
super(id, crdtInfo, type);
16+
//new PersistentDetection()
17+
}
18+
19+
public void update()
20+
{
21+
22+
}
23+
24+
public void destroy()
25+
{
26+
27+
}
28+
}

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

Lines changed: 58 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,33 @@
11
package us.ihmc.behaviors.behaviorTree.scene;
22

3+
import behavior_msgs.msg.dds.BehaviorTreeSceneObjectStateMessage;
4+
import us.ihmc.communication.crdt.CRDTBidirectionalRigidBodyTransform;
5+
import us.ihmc.communication.crdt.CRDTInfo;
6+
import us.ihmc.communication.crdt.LatestTimestampModifiable;
37
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
4-
import us.ihmc.euclid.transform.RigidBodyTransform;
8+
import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools;
59
import us.ihmc.euclid.transform.interfaces.RigidBodyTransformReadOnly;
10+
import us.ihmc.log.LogTools;
611

7-
public class BehaviorTreeSceneObjectState
12+
public class BehaviorTreeSceneObjectState extends LatestTimestampModifiable
813
{
9-
protected final RigidBodyTransform transform = new RigidBodyTransform();
14+
private final long id;
15+
private final String type;
16+
protected final CRDTBidirectionalRigidBodyTransform transform;
17+
protected final ReferenceFrame referenceFrame;
18+
19+
public BehaviorTreeSceneObjectState(long id, CRDTInfo crdtInfo, String type)
20+
{
21+
super(crdtInfo);
22+
23+
this.id = id;
24+
this.type = type;
25+
26+
transform = new CRDTBidirectionalRigidBodyTransform(this);
27+
referenceFrame = ReferenceFrameTools.constructFrameWithChangingTransformToParent("%s_%d".formatted(type, id),
28+
ReferenceFrame.getWorldFrame(),
29+
transform.getValueReadOnly());
30+
}
1031

1132
public String getName()
1233
{
@@ -23,13 +44,45 @@ public void freeze()
2344

2445
}
2546

47+
public void toMessage(BehaviorTreeSceneObjectStateMessage message)
48+
{
49+
toMessage(message.getLatestModificationToData());
50+
message.setId(id);
51+
message.setType(type);
52+
transform.toMessage(message.getTransformToWorld());
53+
}
54+
55+
public void fromMessage(BehaviorTreeSceneObjectStateMessage message)
56+
{
57+
// Needs to be done first to detect incoming modification
58+
fromMessage(message.getLatestModificationToData());
59+
60+
if (id != message.getId())
61+
LogTools.error("IDs should match! {} != {}", id, message.getId());
62+
63+
if (!type.equals(message.getTypeAsString()))
64+
LogTools.error("Types should match! {} != {}", id, message.getId());
65+
66+
transform.fromMessage(message.getTransformToWorld());
67+
}
68+
69+
public void destroy()
70+
{
71+
72+
}
73+
74+
public long getID()
75+
{
76+
return id;
77+
}
78+
2679
public ReferenceFrame getReferenceFrame()
2780
{
28-
return null;
81+
return referenceFrame;
2982
}
3083

3184
public RigidBodyTransformReadOnly getTransformToWorld()
3285
{
33-
return transform;
86+
return transform.getValueReadOnly();
3487
}
3588
}

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

Lines changed: 0 additions & 14 deletions
This file was deleted.

0 commit comments

Comments
 (0)