Fix insert root bug.

ihmcrobotics · Dec 28, 2024 · 25b088c · 25b088c
1 parent 7cebed5
commit 25b088c
Showing 1 changed file with 12 additions and 3 deletions.
diff --git a/...main/java/us/ihmc/behaviors/behaviorTree/topology/BehaviorTreeTopologyOperationQueue.java b/...main/java/us/ihmc/behaviors/behaviorTree/topology/BehaviorTreeTopologyOperationQueue.java
@@ -35,9 +35,18 @@ public boolean performAllQueuedOperations()
 
    public void queueInsertNode(BehaviorTreeNodeInsertionDefinition<HLT> insertionDefinition)
    {
-      queueAddAndModifyNode(insertionDefinition.getNodeToInsert(),
-                            insertionDefinition.getParent(),
-                            insertionDefinition.getInsertionIndex());
+      if (insertionDefinition.getInsertionType() == BehaviorTreeNodeInsertionType.INSERT_ROOT)
+      {
+         queueSetAndModifyRootNode(insertionDefinition.getNodeToInsert(),
+                                   insertionDefinition.getRootNodeSetter(),
+                                   insertionDefinition.getRootNodeModifiable());
+      }
+      else
+      {
+         queueAddAndModifyNode(insertionDefinition.getNodeToInsert(),
+                               insertionDefinition.getParent(),
+                               insertionDefinition.getInsertionIndex());
+      }
    }
 
    public void queueSetAndModifyRootNode(HLT node, Consumer<HLT> setter, LatestTimestampModifiable freezableRootHolder)