diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/sequence/RDXFallbackNode.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/sequence/RDXFallbackNode.java index ad88c938d99..bfbbe5e20c5 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/behavior/sequence/RDXFallbackNode.java +++ b/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 { 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(); } } \ No newline at end of file diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/FallbackNodeDefinition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/FallbackNodeDefinition.java index 296a68e0b2b..a166fed625c 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/FallbackNodeDefinition.java +++ b/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; } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/FallbackNodeExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/FallbackNodeExecutor.java index 31c1d59ec71..5db541f5d1d 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/FallbackNodeExecutor.java +++ b/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; + } + } } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/FallbackNodeState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/FallbackNodeState.java index 23a47297017..47a7d04301d 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/FallbackNodeState.java +++ b/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 { + 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> 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) diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/FallbackNodeDefinitionMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/FallbackNodeDefinitionMessage_.idl index a28772686db..0b699e7c6a1 100644 --- a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/FallbackNodeDefinitionMessage_.idl +++ b/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; }; }; }; diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessagePubSubType.java index 86bdfaca45e..ccb91cb759d 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeStateMessagePubSubType.java +++ b/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 diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/FallbackNodeDefinitionMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/FallbackNodeDefinitionMessage.java index b58dbfec727..4d784e9e07a 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/FallbackNodeDefinitionMessage.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/FallbackNodeDefinitionMessage.java @@ -12,6 +12,10 @@ public class FallbackNodeDefinitionMessage extends Packet 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(); } diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/FallbackNodeDefinitionMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/FallbackNodeDefinitionMessagePubSubType.java index d0edf3dd3f0..044ea5232b1 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/FallbackNodeDefinitionMessagePubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/FallbackNodeDefinitionMessagePubSubType.java @@ -15,7 +15,7 @@ public class FallbackNodeDefinitionMessagePubSubType implements us.ihmc.pubsub.T @Override public final java.lang.String getDefinitionChecksum() { - return "24e413272f81e80dab766a7f9b72617448a7a4d4b7e7ab7277fb962c8cebdc46"; + return "3c6311ce6f239ea71498a54868ba17eeb3a57e922c69778ac028eaa5c7aee0ce"; } @Override @@ -54,6 +54,8 @@ public static int getMaxCdrSerializedSize(int current_alignment) current_alignment += behavior_msgs.msg.dds.BehaviorTreeNodeDefinitionMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + return current_alignment - initial_alignment; } @@ -69,17 +71,25 @@ public final static int getCdrSerializedSize(behavior_msgs.msg.dds.FallbackNodeD current_alignment += behavior_msgs.msg.dds.BehaviorTreeNodeDefinitionMessagePubSubType.getCdrSerializedSize(data.getDefinition(), current_alignment); + current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); + + return current_alignment - initial_alignment; } public static void write(behavior_msgs.msg.dds.FallbackNodeDefinitionMessage data, us.ihmc.idl.CDR cdr) { - behavior_msgs.msg.dds.BehaviorTreeNodeDefinitionMessagePubSubType.write(data.getDefinition(), cdr); } + behavior_msgs.msg.dds.BehaviorTreeNodeDefinitionMessagePubSubType.write(data.getDefinition(), cdr); + cdr.write_type_4(data.getGotoActionId()); + + } public static void read(behavior_msgs.msg.dds.FallbackNodeDefinitionMessage data, us.ihmc.idl.CDR cdr) { behavior_msgs.msg.dds.BehaviorTreeNodeDefinitionMessagePubSubType.read(data.getDefinition(), cdr); + data.setGotoActionId(cdr.read_type_4()); + } @@ -88,12 +98,15 @@ public final void serialize(behavior_msgs.msg.dds.FallbackNodeDefinitionMessage { ser.write_type_a("definition", new behavior_msgs.msg.dds.BehaviorTreeNodeDefinitionMessagePubSubType(), data.getDefinition()); + ser.write_type_4("goto_action_id", data.getGotoActionId()); } @Override public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_msgs.msg.dds.FallbackNodeDefinitionMessage data) { ser.read_type_a("definition", new behavior_msgs.msg.dds.BehaviorTreeNodeDefinitionMessagePubSubType(), data.getDefinition()); + + data.setGotoActionId(ser.read_type_4("goto_action_id")); } public static void staticCopy(behavior_msgs.msg.dds.FallbackNodeDefinitionMessage src, behavior_msgs.msg.dds.FallbackNodeDefinitionMessage dest) diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/FallbackNodeStateMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/FallbackNodeStateMessagePubSubType.java index a5e9e57564b..89528f06a8b 100644 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/FallbackNodeStateMessagePubSubType.java +++ b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/FallbackNodeStateMessagePubSubType.java @@ -15,7 +15,7 @@ public class FallbackNodeStateMessagePubSubType implements us.ihmc.pubsub.TopicD @Override public final java.lang.String getDefinitionChecksum() { - return "023b5cdcbaef45938ae9eb59206fd88592e40492ece93cdffa7fa884c966c61c"; + return "06275d8f808af538e33236fe245e50a4d604d711604c68cc85b17bd914c0ec02"; } @Override diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/FallbackNodeDefinitionMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/FallbackNodeDefinitionMessage.msg index abaad466b55..5ca3e634b22 100644 --- a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/FallbackNodeDefinitionMessage.msg +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/FallbackNodeDefinitionMessage.msg @@ -1,3 +1,6 @@ # Parent definition fields behavior_msgs/BehaviorTreeNodeDefinitionMessage definition +# The ID of the action to retry +uint32 goto_action_id + diff --git a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/FallbackNodeDefinitionMessage.msg b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/FallbackNodeDefinitionMessage.msg index bd9ccfaf459..88eca739a1d 100644 --- a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/FallbackNodeDefinitionMessage.msg +++ b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/FallbackNodeDefinitionMessage.msg @@ -2,4 +2,7 @@ # Parent definition fields behavior_msgs/BehaviorTreeNodeDefinitionMessage definition +# The ID of the action to retry +uint32 goto_action_id +