Add goto node field to fallback node.

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/sequence/RDXFallbackNode.java

@@ -1,6 +1,9 @@
 package us.ihmc.rdx.ui.behavior.sequence;
 
 import imgui.ImGui;
+import us.ihmc.behaviors.behaviorTree.BehaviorTreeRootNodeState;
+import us.ihmc.behaviors.behaviorTree.BehaviorTreeTools;
+import us.ihmc.behaviors.sequence.ActionNodeState;
 import us.ihmc.behaviors.sequence.FallbackNodeDefinition;
 import us.ihmc.behaviors.sequence.FallbackNodeState;
 import us.ihmc.communication.crdt.CRDTInfo;
@@ -11,12 +14,14 @@
 public class RDXFallbackNode extends RDXBehaviorTreeNode<FallbackNodeState, FallbackNodeDefinition>
 {
    private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass());
+   private final FallbackNodeDefinition definition;
    private final FallbackNodeState state;
 
    public RDXFallbackNode(long id, CRDTInfo crdtInfo, WorkspaceResourceDirectory saveFileDirectory)
    {
       super(new FallbackNodeState(id, crdtInfo, saveFileDirectory));
 
+      definition = getDefinition();
       state = getState();
    }
 
@@ -37,6 +42,36 @@ public void renderNodeSettingsWidgets()
    {
       ImGui.text("Type: %s   ID: %d".formatted(getDefinition().getClass().getSimpleName(), getState().getID()));
 
+      BehaviorTreeRootNodeState actionSequence = BehaviorTreeTools.findRootNode(state);
+
+      if (actionSequence != null)
+      {
+         String selectedText = definition.getGotoActionName();
+
+         if (selectedText == null)
+            selectedText = "Not specified";
+
+         if (ImGui.beginCombo(labels.get("Go to"), selectedText))
+         {
+            if (ImGui.selectable(labels.get("Not specified"), definition.getGotoActionID().getValue() == 0))
+            {
+               definition.setGotoActionName(null);
+               definition.getGotoActionID().setValue(0);
+            }
+
+            for (ActionNodeState<?> actionChild : actionSequence.getActionChildren())
+            {
+               if (ImGui.selectable(labels.get(actionChild.getDefinition().getName()), definition.getGotoActionID().getValue() == actionChild.getID()))
+               {
+                  definition.setGotoActionName(actionChild.getDefinition().getName());
+                  definition.getGotoActionID().setValue(actionChild.getID());
+               }
+            }
+
+            ImGui.endCombo();
+         }
+      }
+
       super.renderNodeSettingsWidgets();
    }
 }

ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/FallbackNodeDefinition.java

@@ -1,24 +1,105 @@
 package us.ihmc.behaviors.sequence;
 
 import behavior_msgs.msg.dds.FallbackNodeDefinitionMessage;
+import com.fasterxml.jackson.databind.JsonNode;
+import com.fasterxml.jackson.databind.node.ObjectNode;
 import us.ihmc.behaviors.behaviorTree.BehaviorTreeNodeDefinition;
+import us.ihmc.communication.crdt.CRDTBidirectionalLong;
 import us.ihmc.communication.crdt.CRDTInfo;
 import us.ihmc.tools.io.WorkspaceResourceDirectory;
 
+import javax.annotation.Nullable;
+
 public class FallbackNodeDefinition extends BehaviorTreeNodeDefinition
 {
+   private final CRDTBidirectionalLong gotoActionID;
+   /** We use this to save the action name to file instead of the number for human readability. */
+   @Nullable private String gotoActionName = null;
+
+   // On disk fields
+   private String onDiskGotoActionName;
+
    public FallbackNodeDefinition(CRDTInfo crdtInfo, WorkspaceResourceDirectory saveFileDirectory)
    {
       super(crdtInfo, saveFileDirectory);
+
+      gotoActionID = new CRDTBidirectionalLong(this, 0);
+   }
+
+   @Override
+   public void saveToFile(ObjectNode jsonNode)
+   {
+      super.saveToFile(jsonNode);
+
+      if (gotoActionName != null)
+         jsonNode.put("gotoActionName", gotoActionName);
+   }
+
+   @Override
+   public void loadFromFile(JsonNode jsonNode)
+   {
+      super.loadFromFile(jsonNode);
+
+      if (jsonNode.has("gotoAction"))
+         gotoActionName = jsonNode.get("gotoAction").textValue();
+      gotoActionID.setValue(0); // Invalidate until we can find it
+   }
+
+   @Override
+   public void setOnDiskFields()
+   {
+      super.setOnDiskFields();
+
+      onDiskGotoActionName = gotoActionName;
+   }
+
+   @Override
+   public void undoAllNontopologicalChanges()
+   {
+      super.undoAllNontopologicalChanges();
+
+      gotoActionName = onDiskGotoActionName;
+      gotoActionID.setValue(0); // Invalidate until we can find it
+   }
+
+   @Override
+   public boolean hasChanges()
+   {
+      boolean unchanged = !super.hasChanges();
+
+      if (gotoActionName != null)
+         unchanged &= gotoActionName.equals(onDiskGotoActionName);
+
+      return !unchanged;
    }
 
    public void toMessage(FallbackNodeDefinitionMessage message)
    {
       super.toMessage(message.getDefinition());
+
+      message.setGotoActionId(gotoActionID.toMessage());
    }
 
    public void fromMessage(FallbackNodeDefinitionMessage message)
    {
       super.fromMessage(message.getDefinition());
+
+      gotoActionID.fromMessage(message.getGotoActionId());
+   }
+
+   public CRDTBidirectionalLong getGotoActionID()
+   {
+      return gotoActionID;
+   }
+
+   public void setGotoActionName(@Nullable String gotoActionName)
+   {
+      this.gotoActionName = gotoActionName;
+   }
+
+   /** Only used for finding the ID after loading */
+   @Nullable public String getGotoActionName()
+   {
+      return gotoActionName;
    }
 }

ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/FallbackNodeExecutor.java

@@ -1,6 +1,8 @@
 package us.ihmc.behaviors.sequence;
 
 import us.ihmc.behaviors.behaviorTree.BehaviorTreeNodeExecutor;
+import us.ihmc.behaviors.behaviorTree.BehaviorTreeRootNodeExecutor;
+import us.ihmc.behaviors.behaviorTree.BehaviorTreeTools;
 import us.ihmc.communication.crdt.CRDTInfo;
 import us.ihmc.tools.io.WorkspaceResourceDirectory;
 
@@ -21,5 +23,15 @@ public FallbackNodeExecutor(long id, CRDTInfo crdtInfo, WorkspaceResourceDirecto
    public void update()
    {
       super.update();
+
+      BehaviorTreeRootNodeExecutor actionSequence = BehaviorTreeTools.findRootNode(this);
+      ActionNodeExecutor<?, ?> gotoNode = null;
+      for (ActionNodeExecutor<?, ?> executorChild : actionSequence.getExecutorChildren())
+      {
+         if (executorChild.getState().getID() == definition.getGotoActionID().getValue())
+         {
+            gotoNode = executorChild;
+         }
+      }
    }
 }

ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/FallbackNodeState.java

@@ -2,20 +2,63 @@
 
 import behavior_msgs.msg.dds.FallbackNodeStateMessage;
 import us.ihmc.behaviors.behaviorTree.BehaviorTreeNodeState;
+import us.ihmc.behaviors.behaviorTree.BehaviorTreeRootNodeState;
+import us.ihmc.behaviors.behaviorTree.BehaviorTreeTools;
 import us.ihmc.communication.crdt.CRDTInfo;
+import us.ihmc.log.LogTools;
 import us.ihmc.tools.io.WorkspaceResourceDirectory;
 
+import java.util.List;
+
 public class FallbackNodeState extends BehaviorTreeNodeState<FallbackNodeDefinition>
 {
+   private final FallbackNodeDefinition definition;
+
    public FallbackNodeState(long id, CRDTInfo crdtInfo, WorkspaceResourceDirectory saveFileDirectory)
    {
       super(id, new FallbackNodeDefinition(crdtInfo, saveFileDirectory), crdtInfo);
+
+      definition = getDefinition();
    }
 
    @Override
    public void update()
    {
       super.update();
+
+      BehaviorTreeRootNodeState rootNode = BehaviorTreeTools.findRootNode(this);
+      List<ActionNodeState<?>> actionChildren = rootNode.getActionChildren();
+
+      // Need to find action ID
+      if (definition.getGotoActionName() != null && definition.getGotoActionID().getValue() == 0)
+      {
+         int matchedActions = 0;
+         for (ActionNodeState<?> action : actionChildren)
+         {
+            if (action.getDefinition().getName().equals(definition.getGotoActionName()))
+            {
+               definition.getGotoActionID().setValue(action.getActionIndex());
+               ++matchedActions;
+            }
+         }
+
+         if (matchedActions > 1)
+            LogTools.error("There were {} actions with the name {}.", matchedActions, definition.getGotoActionName());
+      }
+
+      // Update action name
+      if (definition.getGotoActionID().getValue() != 0)
+      {
+         int matchedActions = 0;
+         for (ActionNodeState<?> action : actionChildren)
+         {
+            definition.setGotoActionName(action.getDefinition().getName());
+            ++matchedActions;
+         }
+
+         if (matchedActions > 1)
+            LogTools.error("There were {} actions with the name {}.", matchedActions, definition.getGotoActionName());
+      }
    }
 
    public void toMessage(FallbackNodeStateMessage message)

ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/FallbackNodeDefinitionMessage_.idl

@@ -16,6 +16,10 @@ module behavior_msgs
          * Parent definition fields
          */
         behavior_msgs::msg::dds::BehaviorTreeNodeDefinitionMessage definition;
+        /**
+         * The ID of the action to retry
+         */
+        unsigned long goto_action_id;
       };
     };
   };

ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessagePubSubType.java

@@ -15,7 +15,7 @@ public class BehaviorTreeStateMessagePubSubType implements us.ihmc.pubsub.TopicD
    @Override
    public final java.lang.String getDefinitionChecksum()
    {
-   		return "6d296496df2b561c0c0f126052808de9cc6b2f3d43707dc1cb6aa90a5fdb08d4";
+   		return "367918dba12642255786d7950fa172a2bc63d3fc0cf32455cfad0c6a86276361";
    }
 
    @Override

ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/FallbackNodeDefinitionMessage.java

@@ -12,6 +12,10 @@ public class FallbackNodeDefinitionMessage extends Packet<FallbackNodeDefinition
             * Parent definition fields
             */
    public behavior_msgs.msg.dds.BehaviorTreeNodeDefinitionMessage definition_;
+   /**
+            * The ID of the action to retry
+            */
+   public long goto_action_id_;
 
    public FallbackNodeDefinitionMessage()
    {
@@ -26,7 +30,10 @@ public FallbackNodeDefinitionMessage(FallbackNodeDefinitionMessage other)
 
    public void set(FallbackNodeDefinitionMessage other)
    {
-      behavior_msgs.msg.dds.BehaviorTreeNodeDefinitionMessagePubSubType.staticCopy(other.definition_, definition_);   }
+      behavior_msgs.msg.dds.BehaviorTreeNodeDefinitionMessagePubSubType.staticCopy(other.definition_, definition_);
+      goto_action_id_ = other.goto_action_id_;
+
+   }
 
 
    /**
@@ -37,6 +44,21 @@ public behavior_msgs.msg.dds.BehaviorTreeNodeDefinitionMessage getDefinition()
       return definition_;
    }
 
+   /**
+            * The ID of the action to retry
+            */
+   public void setGotoActionId(long goto_action_id)
+   {
+      goto_action_id_ = goto_action_id;
+   }
+   /**
+            * The ID of the action to retry
+            */
+   public long getGotoActionId()
+   {
+      return goto_action_id_;
+   }
+
 
    public static Supplier<FallbackNodeDefinitionMessagePubSubType> getPubSubType()
    {
@@ -56,6 +78,8 @@ public boolean epsilonEquals(FallbackNodeDefinitionMessage other, double epsilon
       if(other == this) return true;
 
       if (!this.definition_.epsilonEquals(other.definition_, epsilon)) return false;
+      if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.goto_action_id_, other.goto_action_id_, epsilon)) return false;
+
 
       return true;
    }
@@ -70,6 +94,8 @@ public boolean equals(Object other)
       FallbackNodeDefinitionMessage otherMyClass = (FallbackNodeDefinitionMessage) other;
 
       if (!this.definition_.equals(otherMyClass.definition_)) return false;
+      if(this.goto_action_id_ != otherMyClass.goto_action_id_) return false;
+
 
       return true;
    }
@@ -81,7 +107,9 @@ public java.lang.String toString()
 
       builder.append("FallbackNodeDefinitionMessage {");
       builder.append("definition=");
-      builder.append(this.definition_);
+      builder.append(this.definition_);      builder.append(", ");
+      builder.append("goto_action_id=");
+      builder.append(this.goto_action_id_);
       builder.append("}");
       return builder.toString();
    }