Merge branch 'develop' into feature/update_standing

Author: rjgriffin42

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/wholeBodyHardwareControl/AvatarMultiThreadingFactory.java

@@ -69,6 +69,8 @@ public class AvatarMultiThreadingFactory
    private static final double GRAVITY = -9.81;
    public static final boolean RUN_AUTO_DIAGNOSTIC = false;
    private static final int ROS2_PRIORITY = 25;
+   private static final int JVM_STATISTICS_PRIORITY = 5;
+
 
    private final YoRegistry rootRegistry;
 
@@ -116,7 +118,6 @@ public class AvatarMultiThreadingFactory
    private final boolean useRealtimeThreads;
    private final boolean useMultiThreading;
    private final YoVariableServer yoVariableServer;
-   private JVMStatisticsGenerator jvmStatisticsGenerator;
 
    public AvatarMultiThreadingFactory(DRCRobotModel robotModel,
                                       FullHumanoidRobotModel fullRobotModel,
@@ -225,6 +226,17 @@ public AvatarMultiThreadingManager buildThreadsAndThreadingManager()
       if (ikStreamingThread.hasValue())
          yoVariableServer.addRegistry(ikStreamingThread.get().getYoVariableRegistry(), ikStreamingThread.get().getSCS1YoGraphicsListRegistry());
 
+
+      // Setup JVM statistics
+      PeriodicThreadSchedulerFactory jvmSchedulerFactory;
+      if (useRealtimeThreads)
+         jvmSchedulerFactory = new PeriodicRealtimeThreadSchedulerFactory(new PriorityParameters(JVM_STATISTICS_PRIORITY));
+      else
+         jvmSchedulerFactory = new PeriodicNonRealtimeThreadSchedulerFactory();
+
+      JVMStatisticsGenerator jvmStatisticsGenerator = new JVMStatisticsGenerator(yoVariableServer, jvmSchedulerFactory);
+      jvmStatisticsGenerator.addVariablesToStatisticsGenerator(yoVariableServer);
+
       // Create threading manager
       threadingManager.set(new AvatarMultiThreadingManager(robotModel.getSimpleRobotName().toLowerCase(),
                                                            robotModel,
@@ -245,6 +257,7 @@ public AvatarMultiThreadingManager buildThreadsAndThreadingManager()
                                                            useRealtimeThreads,
                                                            useMultiThreading,
                                                            yoVariableServer,
+                                                           jvmStatisticsGenerator,
                                                            rootRegistry));
 
       // Set up the block to prevent execution whenever there is no new state message.
@@ -257,11 +270,15 @@ public AvatarMultiThreadingManager buildThreadsAndThreadingManager()
          builders.add(new RegistrySendBufferBuilder(rootRegistry, null));
          builders.add(new RegistrySendBufferBuilder(controllerThread.get().getYoVariableRegistry(), null, controllerThread.get().getSCS2YoGraphics()));
          if (stepGeneratorThread.hasValue())
+         {
             builders.add(new RegistrySendBufferBuilder(stepGeneratorThread.get().getYoVariableRegistry(), null, stepGeneratorThread.get().getSCS2YoGraphics()));
+         }
          if (ikStreamingThread.hasValue())
+         {
             builders.add(new RegistrySendBufferBuilder(ikStreamingThread.get().getYoVariableRegistry(), null, ikStreamingThread.get().getSCS2YoGraphics()));
-         if (jvmStatisticsGenerator != null)
-            builders.add(new RegistrySendBufferBuilder(jvmStatisticsGenerator.getYoRegistry(), null));
+         }
+
+         builders.add(new RegistrySendBufferBuilder(jvmStatisticsGenerator.getYoRegistry(), null));
 
          // Logging locally on the robot
          IntraprocessYoVariableLogger intraprocessYoVariableLogger = new IntraprocessYoVariableLogger(getClass().getSimpleName(),
@@ -581,11 +598,6 @@ public void addSmoothTransitionState(String transitionName, HighLevelControllerN
       controllerFactory.addCustomSmoothTransitionControlState(transitionName, transitionStateEnum, currentControlStateEnum, nextControlStateEnum);
    }
 
-   public void addJVMStatisticsForLoggingLocally(JVMStatisticsGenerator jvmStatisticsGenerator)
-   {
-      this.jvmStatisticsGenerator = jvmStatisticsGenerator;
-   }
-
    public void setLogLocally(boolean logLocally)
    {
       this.logLocally = logLocally;

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/wholeBodyHardwareControl/AvatarMultiThreadingManager.java

@@ -8,6 +8,7 @@
 import us.ihmc.avatar.factory.HumanoidRobotControlTask;
 import us.ihmc.avatar.factory.SingleThreadedRobotController;
 import us.ihmc.avatar.networkProcessor.kinematicsStreamingToolboxModule.IKStreamingRTPluginFactory;
+import us.ihmc.avatar.networkProcessor.kinematicsStreamingToolboxModule.IKStreamingRTPluginFactory.IKStreamingRTThread;
 import us.ihmc.commonWalkingControlModules.barrierScheduler.context.HumanoidRobotContextData;
 import us.ihmc.commons.Conversions;
 import us.ihmc.commons.exception.DefaultExceptionHandler;
@@ -20,6 +21,7 @@
 import us.ihmc.realtime.PeriodicParameters;
 import us.ihmc.realtime.RealtimeThread;
 import us.ihmc.robotDataLogger.YoVariableServer;
+import us.ihmc.robotDataVisualizer.logger.JVMStatisticsGenerator;
 import us.ihmc.robotModels.FullHumanoidRobotModel;
 import us.ihmc.ros2.RealtimeROS2Node;
 import us.ihmc.scs2.definition.yoGraphic.YoGraphicGroupDefinition;
@@ -45,6 +47,7 @@
 public class AvatarMultiThreadingManager
 {
    private final YoRegistry registry = new YoRegistry(getClass().getSimpleName());
+   private final JVMStatisticsGenerator jvmStatisticsGenerator;
    private final YoRegistry rootRegistry;
    private final YoVariableServer yoVariableServer;
 
@@ -114,14 +117,15 @@ public AvatarMultiThreadingManager(String prefix,
                                       AvatarEstimatorThread estimatorThread,
                                       AvatarControllerThread controllerThread,
                                       AvatarStepGeneratorThread stepGeneratorThread,
-                                      IKStreamingRTPluginFactory.IKStreamingRTThread ikStreamingThread,
+                                      IKStreamingRTThread ikStreamingThread,
                                       AvatarAffinityInterface affinity,
                                       double masterThreadDt,
                                       MonotonicTime period,
                                       TimestampProvider monotonicTimeProvider,
                                       boolean useRealtimeThreads,
                                       boolean useMultiThreading,
                                       YoVariableServer yoVariableServer,
+                                      JVMStatisticsGenerator jvmStatisticsGenerator,
                                       YoRegistry rootRegistry)
    {
       this.estimatorROS2Node = estimatorROS2Node;
@@ -135,6 +139,7 @@ public AvatarMultiThreadingManager(String prefix,
       this.monotonicTimeProvider = monotonicTimeProvider;
       this.useRealtimeThreads = useRealtimeThreads;
       this.useMultiThreading = useMultiThreading;
+      this.jvmStatisticsGenerator = jvmStatisticsGenerator;
       this.rootRegistry = rootRegistry;
       this.yoVariableServer = yoVariableServer;
 
@@ -329,6 +334,7 @@ public void start()
       estimatorROS2Node.spin();
       controllerROS2Node.spin();
       hardwareCommunicationInterface.start();
+      jvmStatisticsGenerator.start();
       if (useRealtimeThreads)
       {
          running = true;
@@ -354,6 +360,7 @@ public void stop()
       estimatorROS2Node.stopSpinning();
       controllerROS2Node.stopSpinning();
       hardwareCommunicationInterface.stop();
+      jvmStatisticsGenerator.stop();
    }
 
    public void destroy()

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/behaviorTree/RDXAvailableBehaviorTreeDirectory.java

@@ -125,19 +125,27 @@ public void renderImGuiWidgets(RDXBehaviorTreeNode<?, ?> relativeNode, BehaviorT
             {
                if (ImGui.isMouseClicked(ImGuiMouseButton.Left))
                {
-                  RDXBehaviorTreeNode<?, ?> loadedNode = behaviorTree.getFileLoader().loadFromFile(indexedTreeFile.getTreeFile(), topologyOperationQueue);
+                  RDXBehaviorTreeRootNode rootNode;
+                  if (relativeNode == null) // Automatically add a root node if there isn't one
+                     rootNode = (RDXBehaviorTreeRootNode) behaviorTree.getNodeBuilder().createRootNode(behaviorTree.getAndIncrementNextID());
+                  else
+                     rootNode = behaviorTree.getRootNode();
+
+                  RDXBehaviorTreeNode<?, ?> loadedNode
+                        = behaviorTree.getFileLoader().loadFromFile(rootNode, indexedTreeFile.getTreeFile(), topologyOperationQueue);
 
                   if (loadedNode != null)
                   {
-                     RDXBehaviorTreeNode<?, ?> nodeToInsert = loadedNode;
-
-                     if (behaviorTree.getRootNode() == null) // Automatically add a root node if there isn't one
+                     RDXBehaviorTreeNode<?, ?> nodeToInsert;
+                     if (relativeNode == null) // Add as child of root node if we made one
+                     {
+                        nodeToInsert = rootNode;
+                        rootNode.getDefinition().modify();
+                        topologyOperationQueue.queueAppendChildModify(rootNode, loadedNode);
+                     }
+                     else
                      {
-                        nodeToInsert = new RDXBehaviorTreeRootNode(behaviorTree.getAndIncrementNextID(),
-                                                                   behaviorTree.getCRDTInfo(),
-                                                                   behaviorTree.getSaveFileDirectory());
-                        nodeToInsert.getDefinition().modify();
-                        topologyOperationQueue.queueAppendChildModify(nodeToInsert, loadedNode);
+                        nodeToInsert = loadedNode;
                      }
 
                      BehaviorTreeNodeInsertionDefinition<RDXBehaviorTreeNode<?, ?>> insertionDefinition

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/behaviorTree/RDXBehaviorTree.java

@@ -28,10 +28,9 @@
 import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary;
 import us.ihmc.tools.io.WorkspaceResourceDirectory;
 
-public class RDXBehaviorTree extends BehaviorTree<RDXBehaviorTreeNode<?, ?>>
+public class RDXBehaviorTree extends BehaviorTree<RDXBehaviorTreeRootNode, RDXBehaviorTreeNode<?, ?>>
 {
    public static final RDXBehaviorTreeSettings SETTINGS = new RDXBehaviorTreeSettings();
-   private RDXBehaviorTreeRootNode rootNode;
    /**
     * Useful for accessing nodes by ID instead of searching.
     * Also, sometimes, the tree will be disassembled and this is used in putting it
@@ -40,12 +39,12 @@ public class RDXBehaviorTree extends BehaviorTree<RDXBehaviorTreeNode<?, ?>>
    private transient final TLongObjectMap<RDXBehaviorTreeNode<?, ?>> idToNodeMap = new TLongObjectHashMap<>();
    private final RDXPanel panel = new RDXPanel("Behavior Tree", this::renderImGuiWidgets, false, true);
    private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass());
-   private final RDXBehaviorTreeFileMenu fileMenu;
    private final RDXBehaviorTreeNodeCreationMenu nodeCreationMenu;
    private final RDXBehaviorTreeWidgetsVerticalLayout treeWidgetsVerticalLayout;
    private boolean anyNodeSelected;
    private RDXBehaviorTreeNode<?, ?> selectedNode;
    private boolean draggingDivider;
+   private boolean shouldSave = false;
 
    public RDXBehaviorTree(WorkspaceResourceDirectory treeFilesDirectory,
                           DRCRobotModel robotModel,
@@ -59,14 +58,8 @@ public RDXBehaviorTree(WorkspaceResourceDirectory treeFilesDirectory,
       super(ROS2ActorDesignation.OPERATOR,
             peerClockEstimator,
             treeFilesDirectory,
-            new RDXBehaviorTreeNodeBuilder(robotModel,
-                                           syncedRobot,
-                                           selectionCollisionModel,
-                                           baseUI,
-                                           panel3D,
-                                           referenceFrameLibrary));
-
-      fileMenu = new RDXBehaviorTreeFileMenu();
+            new RDXBehaviorTreeNodeBuilder(robotModel, syncedRobot, referenceFrameLibrary, selectionCollisionModel, baseUI, panel3D));
+
       nodeCreationMenu = new RDXBehaviorTreeNodeCreationMenu(this, treeFilesDirectory, referenceFrameLibrary);
       treeWidgetsVerticalLayout = new RDXBehaviorTreeWidgetsVerticalLayout(this);
       baseUI.getImGuiPanelManager().addPanel(panel);
@@ -153,8 +146,31 @@ public void renderImGuiWidgets()
 
    protected void renderImGuiWidgetsPre()
    {
+      shouldSave = false;
       ImGui.beginMenuBar();
-      fileMenu.renderFileMenu(rootNode, nodeCreationMenu);
+      if (ImGui.beginMenu(labels.get("File")))
+      {
+         if (rootNode == null)
+         {
+            if (ImGui.menuItem(labels.get("Refresh File List")))
+               nodeCreationMenu.reindexDirectory();
+         }
+         else
+         {
+            if (ImGui.menuItem(labels.get("Save All"), "Ctrl + S"))
+            {
+               RDXBaseUI.pushNotification("Saving %s".formatted(rootNode.getDefinition().getName()));
+               rootNode.getDefinition().saveToFile();
+            }
+            if (ImGui.menuItem(labels.get("Undo All Non-topological Changes")))
+            {
+               RDXBaseUI.pushNotification("Undoing all non-topological behavior tree changes");
+               rootNode.getDefinition().undoAllNontopologicalChanges();
+            }
+         }
+
+         ImGui.endMenu();
+      }
       if (ImGui.beginMenu(labels.get("View")))
       {
          if (rootNode != null)
@@ -186,12 +202,15 @@ protected void renderImGuiWidgetsPost()
                selectedNode = node;
          });
 
+         shouldSave |= ImGui.isWindowHovered() && ImGui.getIO().getKeyCtrl() && ImGui.isKeyPressed('S');
+
          float remainingHeight = ImGui.getContentRegionAvailY();
          float treeExplorerPercentage = SETTINGS.getTreeExplorerHeightPercentage();
          float treeExplorerHeight = anyNodeSelected ? remainingHeight * treeExplorerPercentage : remainingHeight;
 
          ImGui.beginChild(labels.get("Tree Explorer Scroll Area"), 0.0f, treeExplorerHeight);
          treeWidgetsVerticalLayout.renderImGuiWidgets();
+         shouldSave |= ImGui.isWindowHovered() && ImGui.getIO().getKeyCtrl() && ImGui.isKeyPressed('S');
          ImGui.endChild();
 
          if (rootNode != null && anyNodeSelected) // It can become null above
@@ -216,22 +235,23 @@ protected void renderImGuiWidgetsPost()
 
             ImGui.beginChild(labels.get("Node Settings Scroll Area"), 0.0f, ImGui.getContentRegionAvailY());
             renderSelectedNodeSettingsWidgets(rootNode);
+            shouldSave |= ImGui.isWindowHovered() && ImGui.getIO().getKeyCtrl() && ImGui.isKeyPressed('S');
             ImGui.endChild();
-
-            if (ImGui.isWindowHovered() && ImGui.getIO().getKeyCtrl() && ImGui.isKeyPressed('S'))
-            {
-               RDXBaseUI.pushNotification("Saving %s".formatted(rootNode.getDefinition().getName()));
-               rootNode.getDefinition().saveToFile();
-            }
          }
       }
       else
       {
-         nodeCreationMenu.renderImGuiWidgets(rootNode, BehaviorTreeNodeInsertionType.INSERT_ROOT);
+         nodeCreationMenu.renderImGuiWidgets(null, BehaviorTreeNodeInsertionType.INSERT_ROOT);
       }
 
       // Perform any modifications that were made via user interaction.
       modifyTreeTopology();
+
+      if (shouldSave)
+      {
+         RDXBaseUI.pushNotification("Saving %s".formatted(rootNode.getDefinition().getName()));
+         rootNode.getDefinition().saveToFile();
+      }
    }
 
    private void renderSelectedNodeSettingsWidgets(RDXBehaviorTreeNode<?, ?> node)
@@ -302,18 +322,6 @@ public void destroy()
       RDXBaseUI.getInstance().getPrimary3DPanel().removeImGui3DViewInputProcessor(this);
    }
 
-   @Override
-   public void setRootNode(RDXBehaviorTreeNode<?, ?> rootNode)
-   {
-      this.rootNode = (RDXBehaviorTreeRootNode) rootNode;
-   }
-
-   @Override
-   public RDXBehaviorTreeRootNode getRootNode()
-   {
-      return rootNode;
-   }
-
    public RDXBehaviorTreeNodeCreationMenu getNodeCreationMenu()
    {
       return nodeCreationMenu;