Merge pull request #68 from ftc16072/debugBehaviorTrees

Adding debugging for behavior trees
ftc16072 · Feb 18, 2024 · 75e28e3 · 75e28e3
2 parents 1443ad9 + 8c4ea2d
commit 75e28e3
Show file tree

Hide file tree

Showing 6 changed files with 95 additions and 4 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Failover.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Failover.java
@@ -14,8 +14,12 @@ public Failover(Node ... a) {
 
     @Override
     public State tick(QQOpMode opmode) {
+        opmode.debug.startParent(this);
         for (Node child : children) {
+            opmode.debug.addNode(child);
             State state = child.tick(opmode);
+            opmode.debug.updateNode(child, state);
+
             if (state == State.SUCCESS) {
                 return State.SUCCESS;
             } else if (state == State.RUNNING) {

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Parallel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Parallel.java
@@ -18,8 +18,12 @@ public Parallel(int requiredSuccesses, Node ... a) {
     public State tick(QQOpMode opmode) {
         int numSuccessful = 0;
         boolean anyRunning = false;
+        opmode.debug.startParent(this);
         for (Node child : children) {
+            opmode.debug.addNode(child);
             State state = child.tick(opmode);
+            opmode.debug.updateNode(child, state);
+
             if (state == State.SUCCESS) {
                 numSuccessful += 1;
                 if (numSuccessful >= requiredSuccesses){

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Sequence.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/BehaviorTrees/Sequence.java
@@ -14,8 +14,13 @@ public Sequence(Node ... a) {
 
     @Override
     public State tick(QQOpMode opmode) {
+        opmode.debug.startParent(this);
+
         for (Node child : children) {
+            opmode.debug.addNode(child);
             State state = child.tick(opmode);
+            opmode.debug.updateNode(child, state);
+
             if (state == State.FAILURE) {
                 return State.FAILURE;
             } else if (state == State.RUNNING) {

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/CompTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/CompTeleOp.java
@@ -16,14 +16,15 @@ public void init_loop(){
     @Override
     public void loop() {
         if(!done){
+            debug.reset();
+            debug.addNode(root);
             Node.State state = root.tick(this);
+            debug.updateNode(root, state);
+            telemetry.addData("Tree", debug);
             if(state == Node.State.SUCCESS){
                 done = true;
             }
         }
-
     }
 
-
-
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/QQOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/QQOpMode.java
@@ -4,9 +4,9 @@
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 
-import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.ftc16072.Robot;
 import org.firstinspires.ftc.teamcode.ftc16072.Util.BlackBoard;
+import org.firstinspires.ftc.teamcode.ftc16072.Util.DebugTree;
 import org.firstinspires.ftc.teamcode.ftc16072.Util.LiftControl;
 
 /**
@@ -19,6 +19,8 @@ abstract public class QQOpMode extends OpMode {
     public LiftControl liftControl = new LiftControl(robot);
 
     public BlackBoard board = new BlackBoard();
+    public DebugTree debug = new DebugTree();
+
 
     /**
      * every opmode initializes robot with hardware map

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/DebugTree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/DebugTree.java
@@ -0,0 +1,75 @@
+package org.firstinspires.ftc.teamcode.ftc16072.Util;
+
+import androidx.annotation.NonNull;
+
+import org.firstinspires.ftc.teamcode.ftc16072.BehaviorTrees.Node;
+
+import java.util.LinkedList;
+import java.util.List;
+import java.util.Stack;
+
+public class DebugTree {
+    static class NodeInfo {
+        int indent;
+        public Node node;
+        Node.State state = Node.State.RUNNING;
+
+        NodeInfo(int indent, Node node){
+            this.indent = indent;
+            this.node = node;
+        }
+
+        @NonNull
+        public String toString(){
+            String prefix = new String(new char[indent]).replace("\0", "-");
+            String name = node.getClass().getSimpleName();
+
+            switch(state){
+                case FAILURE:
+                    return prefix + name + ":F\n";
+                case SUCCESS:
+                    return prefix + name + ":S\n";
+                case RUNNING:
+                    return prefix + name + ":R\n";
+            }
+            return "";
+        }
+    }
+    List<NodeInfo> nodeInfoList;
+    Stack<Node> parents = new Stack<>();
+
+    public DebugTree(){
+        reset();
+    }
+    public void reset(){
+        nodeInfoList = new LinkedList<>();
+        parents = new Stack<>();
+    }
+    public void startParent(Node parent){
+        parents.push(parent);
+    }
+
+    public void addNode(Node node){
+        nodeInfoList.add(new NodeInfo(parents.size(), node));
+    }
+
+    public void updateNode(Node node, Node.State newState){
+        if (node == parents.peek()){
+            parents.pop();
+        }
+        for (NodeInfo nodeInfo : nodeInfoList){
+            if(nodeInfo.node == node){
+                nodeInfo.state = newState;
+            }
+        }
+    }
+
+    @NonNull
+    public String toString(){
+        StringBuilder retString = new StringBuilder();
+        for(NodeInfo nodeInfo : nodeInfoList){
+            retString.append(nodeInfo.toString());
+        }
+        return retString.toString();
+    }
+}