Refactor hand code to simplify to help find bugs and issues

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

@@ -5,8 +5,8 @@
 import us.ihmc.behaviors.behaviorTree.action.actions.AbilityHandActionDefinition;
 import us.ihmc.behaviors.behaviorTree.action.actions.AbilityHandActionDefinition.SuccessCriteria;
 import us.ihmc.behaviors.behaviorTree.action.actions.AbilityHandActionState;
-import us.ihmc.handsros2.abilityHand.AbilityHandManager;
-import us.ihmc.handsros2.abilityHand.AbilityHandManager.ControlMode;
+import us.ihmc.handsros2.abilityHand.AbilityHandGrip;
+import us.ihmc.handsros2.abilityHand.AbilityHandControlMode;
 import us.ihmc.rdx.behaviorTree.RDXBehaviorTreeRootNode;
 import us.ihmc.rdx.imgui.ImBooleanWrapper;
 import us.ihmc.rdx.imgui.ImFloatWrapper;
@@ -23,7 +23,7 @@ public class RDXAbilityHandAction extends RDXActionNode<AbilityHandActionState,
 
    private final ImFloat[] sliderPositions = new ImFloat[6];
    private final ImFloat[] sliderVelocities = new ImFloat[6];
-   private final ControlMode[] modes = { ControlMode.GRIP, ControlMode.POSITION };
+   private final AbilityHandControlMode[] modes = {AbilityHandControlMode.GRIP, AbilityHandControlMode.POSITION };
    private final ImFloatWrapper ultimateTimeoutWidget;
    private final ImBooleanWrapper enableWiggleOnFailureWidget;
    private final ImFloatWrapper timeToWiggleWidget;
@@ -66,19 +66,19 @@ protected void renderImGuiWidgetsInternal()
       ImGui.text("Ability Hand action for " + definition.getSide().getLowerCaseName() + " side");
 
       ImGui.text("Control Mode:");
-      for (ControlMode mode : modes)
+      for (AbilityHandControlMode mode : modes)
       {
          if (ImGui.radioButton(labels.get(mode.name()), definition.getControlMode() == mode))
             definition.setControlMode(mode);
-         if (mode == ControlMode.GRIP)
+         if (mode == AbilityHandControlMode.GRIP)
             ImGui.sameLine();
       }
 
-      if (definition.getControlMode() == ControlMode.GRIP)
+      if (definition.getControlMode() == AbilityHandControlMode.GRIP)
       {
          if (ImGui.beginCombo(labels.get("Grip"), definition.getGrip().name()))
          {
-            for (AbilityHandManager.Grip grip : AbilityHandManager.Grip.values)
+            for (AbilityHandGrip grip : AbilityHandGrip.values)
             {
                boolean isSelected = definition.getGrip() == grip;
                if (ImGui.selectable(grip.name(), isSelected))

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXArmManager.java

@@ -121,7 +121,7 @@ public RDXArmManager(CommunicationHelper communicationHelper,
          armConfigurationNames[i] = PresetArmConfiguration.values[i].name();
       }
 
-      handManager = new RDXHandManager(robotModel);
+      handManager = new RDXHandManager(robotModel, communicationHelper.getROS2Node());
    }
 
    public void create(RDXBaseUI baseUI)

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/RDXHandManager.java

@@ -5,18 +5,14 @@
 import us.ihmc.avatar.drcRobot.DRCRobotModel;
 import us.ihmc.avatar.drcRobot.RobotVersion;
 import us.ihmc.handsros2.HandInterface;
-import us.ihmc.handsros2.HandROS2HardwareCommunication;
 import us.ihmc.handsros2.HandType;
-import us.ihmc.handsros2.abilityHand.AbilityHandROS2HardwareCommunication;
 import us.ihmc.handsros2.ezGripper.EZGripperROS2HardwareCommunication;
 import us.ihmc.rdx.ui.RDXBaseUI;
 import us.ihmc.rdx.ui.hands.psyonicAbilityHand.RDXAbilityHand;
 import us.ihmc.rdx.ui.hands.sakeEZGripper.RDXEZGripper;
 import us.ihmc.robotics.robotSide.RobotSide;
 import us.ihmc.robotics.robotSide.SideDependentList;
-
-import java.util.HashMap;
-import java.util.Map;
+import us.ihmc.ros2.ROS2Node;
 
 /**
  * Manages the UI for a humanoid robot's hands. A hand configuration is like "open", "closed", etc.
@@ -26,9 +22,9 @@ public class RDXHandManager
    private final SideDependentList<RDXHandInterface> rdxHands = new SideDependentList<>();
    private final SideDependentList<RDXHandQuickAccessButtons> quickAccessButtons = new SideDependentList<>();
 
-   private final Map<HandType, HandROS2HardwareCommunication<?, ?>> handCommunications = new HashMap<>();
+   private EZGripperROS2HardwareCommunication ezGripperCommunication = null;
 
-   public RDXHandManager(DRCRobotModel robotModel)
+   public RDXHandManager(DRCRobotModel robotModel, ROS2Node ros2Node)
    {
       RobotVersion robotVersion = robotModel.getRobotVersion();
       String robotName = robotModel.getSimpleRobotName();
@@ -39,31 +35,30 @@ public RDXHandManager(DRCRobotModel robotModel)
          if (!robotVersion.hasHandWithFingers(side) || handType == null)
             continue;
 
-         if (!handCommunications.containsKey(handType))
+         switch (handType)
          {
-            HandROS2HardwareCommunication<?, ?> handCommunication = switch (handType)
+            case EZ_GRIPPER ->
             {
-               case EZ_GRIPPER -> new EZGripperROS2HardwareCommunication(getClass().getSimpleName() + "EZGripperCommunication");
-               case ABILITY_HAND -> new AbilityHandROS2HardwareCommunication(getClass().getSimpleName() + "AbilityHandCommunication");
-            };
-            handCommunications.put(handType, handCommunication);
+               if (ezGripperCommunication == null)
+                  ezGripperCommunication = new EZGripperROS2HardwareCommunication(getClass().getSimpleName() + "EZGripperCommunication");
+            }
          }
 
          String handIdentifier = HandInterface.getSimpleIdentifier(robotName, side, handType);
          switch (handType)
          {
             case EZ_GRIPPER ->
-                  rdxHands.put(side, new RDXEZGripper(handIdentifier, side, (EZGripperROS2HardwareCommunication) handCommunications.get(handType)));
+                  rdxHands.put(side, new RDXEZGripper(handIdentifier, side, ezGripperCommunication));
             case ABILITY_HAND ->
-                  rdxHands.put(side, new RDXAbilityHand(handIdentifier, side, (AbilityHandROS2HardwareCommunication) handCommunications.get(handType)));
+                  rdxHands.put(side, new RDXAbilityHand(handIdentifier, side, ros2Node));
          }
       }
    }
 
    public void create(RDXBaseUI baseUI)
    {
-      for (HandROS2HardwareCommunication<?, ?> handCommunication : handCommunications.values())
-         handCommunication.start();
+      if (ezGripperCommunication != null)
+         ezGripperCommunication.start();
 
       for (RobotSide side : rdxHands.sides())
          quickAccessButtons.put(side, new RDXHandQuickAccessButtons(baseUI, rdxHands.get(side)));
@@ -99,7 +94,7 @@ public RDXHandInterface getHand(RobotSide handSide)
 
    public void destroy()
    {
-      for (HandROS2HardwareCommunication<?, ?> handCommunication : handCommunications.values())
-         handCommunication.shutdown();
+      if (ezGripperCommunication != null)
+         ezGripperCommunication.shutdown();
    }
 }

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/psyonicAbilityHand/RDXAbilityHand.java

@@ -2,19 +2,24 @@
 
 import com.badlogic.gdx.graphics.Color;
 import ihmc_hands_ros2.msg.dds.AbilityHandCommand;
+import ihmc_hands_ros2.msg.dds.AbilityHandState;
 import imgui.ImGui;
 import imgui.flag.ImGuiCol;
 import imgui.type.ImFloat;
 import us.ihmc.commons.thread.Throttler;
-import us.ihmc.handsros2.abilityHand.AbilityHandManager.ControlMode;
-import us.ihmc.handsros2.abilityHand.AbilityHandManager.Grip;
-import us.ihmc.handsros2.abilityHand.AbilityHandROS2HardwareCommunication;
+import us.ihmc.commons.thread.TypedNotification;
+import us.ihmc.handsros2.abilityHand.AbilityHandControlMode;
+import us.ihmc.handsros2.abilityHand.AbilityHandGrip;
+import us.ihmc.handsros2.abilityHand.AbilityHandROS2API;
 import us.ihmc.log.LogTools;
 import us.ihmc.rdx.imgui.ImGuiTools;
 import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap;
 import us.ihmc.rdx.ui.hands.RDXHandInterface;
 import us.ihmc.robotics.EuclidCoreMissingTools;
 import us.ihmc.robotics.robotSide.RobotSide;
+import us.ihmc.ros2.ROS2Node;
+import us.ihmc.ros2.ROS2Publisher;
+import us.ihmc.tools.Timer;
 
 public class RDXAbilityHand implements RDXHandInterface
 {
@@ -30,59 +35,73 @@ public class RDXAbilityHand implements RDXHandInterface
 
    private final String identifier;
    private final RobotSide handSide;
-   private final AbilityHandROS2HardwareCommunication communication;
 
    private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass());
    private final ImFloat[] desiredPositions = new ImFloat[6];
    private final ImFloat[] desiredVelocities = new ImFloat[6];
-
-   private float[] currentPositions = new float[6];
-   private Grip executeGrip = null;
+   private AbilityHandGrip executeGrip = null;
    private boolean executeVelToPos = false;
-   private final Throttler publishThrottler = new Throttler().setFrequency(30.0);
 
-   public RDXAbilityHand(String identifier, RobotSide handSide, AbilityHandROS2HardwareCommunication communication)
+   private final TypedNotification<AbilityHandState> stateNotification = new TypedNotification<>();
+   private AbilityHandState latestState = null;
+   private final AbilityHandCommand command = new AbilityHandCommand();
+   private final ROS2Publisher<AbilityHandCommand> commandPublisher;
+   private final Throttler commandThrottler = new Throttler().setFrequency(30.0);
+   private final Timer connectedTimer = new Timer();
+
+   public RDXAbilityHand(String identifier, RobotSide handSide, ROS2Node ros2Node)
    {
       this.identifier = identifier;
       this.handSide = handSide;
-      this.communication = communication;
 
       for (int i = 0; i < 6; i++)
       {
          desiredPositions[i] = new ImFloat(START_POSITION);
          desiredVelocities[i] = new ImFloat(DEFAULT_VELOCITY);
       }
+
+      ros2Node.createSubscription2(AbilityHandROS2API.STATE_TOPIC, stateNotification::set);
+      command.setIdentifier(identifier);
+      commandPublisher = ros2Node.createPublisher(AbilityHandROS2API.COMMAND_TOPIC);
    }
 
    @Override
    public void update()
    {
-      if (!communication.getAvailableHands().contains(identifier))
+      if (stateNotification.poll())
       {
-         executeGrip = null; // Clear so they don't get executed later
-         executeVelToPos = false;
-         return;
+         AbilityHandState read = stateNotification.read();
+         if (read.getIdentifierAsString().equals(identifier) && read.getHandSide() == handSide.toByte())
+         {
+            latestState = read;
+            connectedTimer.reset();
+         }
       }
 
-      currentPositions = communication.readState(identifier).getActuatorPositions();
+      if (!connectedTimer.isRunning(0.5))
+         latestState = null;
 
-      if ((executeGrip != null || executeVelToPos) && publishThrottler.run())
+      if (latestState == null)
+      {
+         executeGrip = null;
+         executeVelToPos = false;
+      }
+      else if ((executeGrip != null || executeVelToPos) && commandThrottler.run())
       {
-         AbilityHandCommand command = communication.getCommand(identifier);
          if (executeGrip != null)
          {
-            command.setControlMode(ControlMode.GRIP.toByte());
+            command.setControlMode(AbilityHandControlMode.GRIP.toByte());
             command.setGrip(executeGrip.toByte());
          }
          else
          {
-            command.setControlMode(ControlMode.POSITION.toByte());
+            command.setControlMode(AbilityHandControlMode.POSITION.toByte());
             for (int i = 0; i < 6; i++)
                command.getGoalPositions()[i] = desiredPositions[i].get();
          }
          for (int i = 0; i < 6; i++)
             command.getGoalVelocities()[i] = desiredVelocities[i].get();
-         communication.publishCommand(identifier);
+         commandPublisher.publish(command);
 
          executeGrip = null;
          executeVelToPos = false;
@@ -92,20 +111,18 @@ public void update()
    @Override
    public void renderImGuiWidgets()
    {
-      boolean connected = communication.getAvailableHands().contains(identifier);
-
       ImGuiTools.separatorText(getSide().toString() + " Ability Hand", ImGuiTools.getSmallBoldFont());
 
-      if (!connected)
+      if (latestState == null)
          ImGuiTools.textColored(Color.RED, "Not connected");
 
-      ImGui.beginDisabled(!connected);
+      ImGui.beginDisabled(latestState == null);
 
-      for (Grip grip : Grip.values)
+      for (AbilityHandGrip grip : AbilityHandGrip.values)
       {
          if (ImGui.button(labels.get(grip.name())))
             executeGrip = grip;
-         if (grip != Grip.values[5] && grip != Grip.values[7] && grip != Grip.values[Grip.values.length - 1])
+         if (grip != AbilityHandGrip.values[5] && grip != AbilityHandGrip.values[7] && grip != AbilityHandGrip.values[AbilityHandGrip.values.length - 1])
             ImGui.sameLine();
       }
 
@@ -114,15 +131,16 @@ public void renderImGuiWidgets()
       {
          float sliderMin = 0.0f;
          float sliderMax = i == 5 ? -120.0f : 120.0f; // thumb rotator moves negative
-         float currentNotch = (currentPositions[i] - sliderMin) / (sliderMax - sliderMin);
+         float actuatorPosition = latestState == null ? Float.NaN : latestState.getActuatorPositions()[i];
+         float currentNotch = (actuatorPosition - sliderMin) / (sliderMax - sliderMin);
          float sliderWidth = ImGui.getColumnWidth() * 0.6f;
          ImGuiTools.renderSliderOrProgressNotch(currentNotch * sliderWidth, ImGui.getColorU32(ImGuiCol.Text));
 
          ImGui.pushItemWidth(sliderWidth);
          scheduleExecuteVelToPos |= ImGui.sliderFloat(labels.getHidden(FINGER_NAMES[i]), desiredPositions[i].getData(), sliderMin, sliderMax,
-                               "%s: %.2f%s flexion".formatted(FINGER_NAMES[i], currentPositions[i], EuclidCoreMissingTools.DEGREE_SYMBOL));
-         if (!ImGui.isItemActive() && !executeVelToPos) // Prevent overriding externally submitted positions too
-            desiredPositions[i].set(currentPositions[i]);
+                               "%s: %.2f%s flexion".formatted(FINGER_NAMES[i], actuatorPosition, EuclidCoreMissingTools.DEGREE_SYMBOL));
+         if (!ImGui.isItemActive() && !executeVelToPos && latestState != null) // Prevent overriding externally submitted positions too
+            desiredPositions[i].set(latestState.getGoalPositions()[i]);
          ImGui.popItemWidth();
          ImGui.sameLine();
          ImGui.pushItemWidth(ImGui.getColumnWidth());
@@ -163,9 +181,9 @@ public boolean needsReset()
    public void sendCommand(HandAction handAction)
    {
       if (handAction == HandAction.OPEN)
-         executeGrip = Grip.OPEN;
+         executeGrip = AbilityHandGrip.OPEN;
       else if (handAction == HandAction.CLOSE || handAction == HandAction.GRIP)
-         executeGrip = Grip.CLOSE;
+         executeGrip = AbilityHandGrip.CLOSE;
       else
          LogTools.warn("Attempted to send an unsupported hand action command: {}", handAction.name());
    }
@@ -180,6 +198,6 @@ public void sendFingerPosition(int index, float angleDegrees)
    @Override
    public float getFingerPosition(int index)
    {
-      return currentPositions[index];
+      return latestState.getActuatorPositions()[index];
    }
 }

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/hands/sakeEZGripper/RDXEZGripper.java

@@ -20,8 +20,7 @@
 import us.ihmc.robotics.EuclidCoreMissingTools;
 import us.ihmc.robotics.robotSide.RobotSide;
 import us.ihmc.handsros2.ezGripper.EZGripperError;
-import us.ihmc.handsros2.ezGripper.EZGripperManager;
-import us.ihmc.handsros2.ezGripper.EZGripperManager.OperationMode;
+import us.ihmc.handsros2.ezGripper.EZGripper.OperationMode;
 import us.ihmc.handsros2.ezGripper.EZGripperROS2HardwareCommunication;
 
 public class RDXEZGripper implements RDXHandInterface
@@ -49,7 +48,7 @@ public class RDXEZGripper implements RDXHandInterface
    private final ImGuiFlashingText needResetStatusText = new ImGuiFlashingText(ImGuiTools.RED);
    private final ImGuiFlashingText sakeErrorStatusText = new ImGuiFlashingText(ImGuiTools.RED);
 
-   private EZGripperManager.OperationMode previousOperationMode = null;
+   private OperationMode previousOperationMode = null;
 
    public RDXEZGripper(String identifier, RobotSide handSide, EZGripperROS2HardwareCommunication communication)
    {

ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeExecutor.java

@@ -2,25 +2,27 @@
 
 import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel;
 import us.ihmc.avatar.ros2.ROS2ControllerHelper;
+import us.ihmc.behaviors.behaviorTree.action.actions.AbilityHandActionComms;
 import us.ihmc.behaviors.behaviorTree.scene.BehaviorTreeSceneExecutor;
 import us.ihmc.behaviors.behaviorTree.topology.BehaviorTreeTopologyOperationQueue;
 import us.ihmc.behaviors.behaviorTree.condition.LLMConditionExecutor;
 import us.ihmc.behaviors.tools.interfaces.LogToolsLogger;
 import us.ihmc.behaviors.tools.walkingController.ControllerStatusTracker;
 import us.ihmc.communication.ros2.ROS2ActorDesignation;
 import us.ihmc.communication.ros2.sync.ROS2PeerClockOffsetEstimator;
-import us.ihmc.handsros2.abilityHand.AbilityHandROS2HardwareCommunication;
 import us.ihmc.log.LogTools;
 import us.ihmc.perception.detections.foundationPose.IsaacROSFoundationPoseCommunicatorMap;
 import us.ihmc.perception.detections.yolo.YOLOv8DetectionExecutor;
+import us.ihmc.robotics.robotSide.RobotSide;
+import us.ihmc.robotics.robotSide.SideDependentList;
 import us.ihmc.tools.io.WorkspaceResourceDirectory;
 import us.ihmc.tools.io.WorkspaceResourceFile;
 
 public class BehaviorTreeExecutor extends BehaviorTree<BehaviorTreeRootNodeExecutor, BehaviorTreeNodeExecutor<?, ?>>
 {
    private final ControllerStatusTracker controllerStatusTracker;
    private final BehaviorTreeSceneExecutor scene;
-   private final AbilityHandROS2HardwareCommunication abilityHandCommunication;
+   private final SideDependentList<AbilityHandActionComms> abilityHandComms = new SideDependentList<>();
 
    public BehaviorTreeExecutor(ROS2SyncedRobotModel syncedRobot,
                                ROS2PeerClockOffsetEstimator peerClockEstimator,
@@ -35,8 +37,8 @@ public BehaviorTreeExecutor(ROS2SyncedRobotModel syncedRobot,
             new BehaviorTreeExecutorNodeBuilder());
 
       controllerStatusTracker = new ControllerStatusTracker(new LogToolsLogger(), ros2ControllerHelper.getROS2Node(), robotModel.getSimpleRobotName());
-      abilityHandCommunication = new AbilityHandROS2HardwareCommunication("bt_abilty_hand");
-      abilityHandCommunication.start();
+      for (RobotSide robotSide : RobotSide.values)
+         abilityHandComms.put(robotSide, new AbilityHandActionComms(robotSide, ros2ControllerHelper.getROS2Node()));
       scene = new BehaviorTreeSceneExecutor(crdtInfo, this::getAndIncrementNextID, syncedRobot, yolo, foundationPose);
       setScene(scene);
 
@@ -45,7 +47,7 @@ public BehaviorTreeExecutor(ROS2SyncedRobotModel syncedRobot,
                                                                       ros2ControllerHelper,
                                                                       syncedRobot,
                                                                       controllerStatusTracker,
-                                                                      abilityHandCommunication,
+                                                                      abilityHandComms,
                                                                       scene);
    }
 
@@ -75,7 +77,6 @@ private void update(BehaviorTreeNodeExecutor<?, ?> node)
 
    public void destroy()
    {
-      abilityHandCommunication.shutdown();
       modifyTreeTopology(BehaviorTreeTopologyOperationQueue::queueDestroyEntireTree);
       LLMConditionExecutor.destroy();
    }