Lay out the concurrency.

ihmcrobotics · Oct 15, 2024 · a175f15 · a175f15
1 parent d111965
commit a175f15
Show file tree

Hide file tree

Showing 2 changed files with 41 additions and 7 deletions.
diff --git a/...igh-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeSVGNode.java b/...igh-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeSVGNode.java
@@ -3,8 +3,10 @@
 import org.apache.commons.lang.WordUtils;
 import org.jfree.svg.SVGGraphics2D;
 import us.ihmc.behaviors.door.DoorTraversalDefinition;
+import us.ihmc.behaviors.sequence.ActionNodeDefinition;
 import us.ihmc.behaviors.sequence.ActionNodeState;
 import us.ihmc.behaviors.sequence.ActionSequenceDefinition;
+import us.ihmc.behaviors.sequence.ActionSequenceState;
 import us.ihmc.behaviors.sequence.actions.ChestOrientationActionDefinition;
 import us.ihmc.behaviors.sequence.actions.FootstepPlanActionDefinition;
 import us.ihmc.behaviors.sequence.actions.HandPoseActionDefinition;
@@ -38,16 +40,47 @@ public BehaviorTreeSVGNode(SVGGraphics2D svgGraphics2D,
       this.originX = originX;
       this.originY = originY;
 
-      x = originX;
-      y = originY;
+      BehaviorTreeSVGNode actionSequenceNode = null;
+      for (BehaviorTreeSVGNode allNode : allNodes)
+      {
+         if (allNode.node instanceof ActionSequenceState)
+         {
+            actionSequenceNode = allNode;
+         }
+      }
 
       if (node instanceof ActionNodeState actionNode)
       {
-//         index = actionNode.getActionIndex();
-//         node.
+         if (actionNode.getDefinition() instanceof ActionNodeDefinition actionNodeDefinition)
+         {
+            if (actionNodeDefinition.getExecuteAfterPrevious().getValue())
+            {
+               originX = allNodes.get(allNodes.size() - 1).x;
+            }
+            else if (actionSequenceNode != null && actionNodeDefinition.getExecuteAfterBeginning().getValue())
+            {
+               originX = actionSequenceNode.x;
+            }
+            else
+            {
+               long afterID = actionNodeDefinition.getExecuteAfterNodeID().getValue();
+               for (BehaviorTreeSVGNode otherNode : allNodes)
+               {
+                  if (otherNode.node instanceof ActionNodeState existingActionNode)
+                  {
+                     if (existingActionNode.getID() == afterID)
+                     {
+                        originX = otherNode.x;
+                     }
+                  }
+               }
+            }
+         }
       }
 
-//      svgGraphics2D.setStroke();
+
+      x = originX;
+      y = originY;
 
       svgGraphics2D.setColor(new Color((int) (Math.random() * 256), (int) (Math.random() * 256), (int) (Math.random() * 256), 100));
       svgGraphics2D.setStroke(new BasicStroke(0.5f));
@@ -60,7 +93,7 @@ public BehaviorTreeSVGNode(SVGGraphics2D svgGraphics2D,
       svgGraphics2D.setFont(new Font("Arial", Font.PLAIN, 8));
       String indexString = "%d".formatted(index);
       svgGraphics2D.drawString(indexString, x + 4 * (2 - indexString.length()), y);
-      x += 12;
+      x += 14;
       y += 4;
       svgGraphics2D.setColor(Color.BLACK);
       svgGraphics2D.setFont(new Font("Arial", Font.PLAIN, 12));
@@ -70,6 +103,7 @@ public BehaviorTreeSVGNode(SVGGraphics2D svgGraphics2D,
       svgGraphics2D.setFont(new Font("Arial", Font.PLAIN, 10));
       svgGraphics2D.drawString("%s".formatted(getTypeName(node.getDefinition())), x, y);
 
+      x = originX + 80;
    }
 
    public int getHeight()

diff --git a/...h-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeSVGWriter.java b/...h-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeSVGWriter.java
@@ -18,7 +18,7 @@ public class BehaviorTreeSVGWriter
 
    public BehaviorTreeSVGWriter(BehaviorTreeNodeState node)
    {
-      double documentSize = 1500.0;
+      double documentSize = 2000.0;
       SVGGraphics2D svgGraphics2D = new SVGGraphics2D(documentSize, documentSize);
 
       BehaviorTreeTools.runForSubtreeNodes(node, child ->