added continuous hiking publisher to xbox one plugin

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/joystickBasedJavaFXController/XBoxOneCSGPluginGDX.java → ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/joystickBasedJavaFXController/XBoxOneJoystickWalkingPlugin.java

@@ -1,5 +1,6 @@
 package us.ihmc.avatar.joystickBasedJavaFXController;
 
+import behavior_msgs.msg.dds.ContinuousHikingCommandMessage;
 import com.badlogic.gdx.controllers.Controller;
 import com.badlogic.gdx.controllers.ControllerListener;
 import com.badlogic.gdx.controllers.Controllers;
@@ -13,8 +14,10 @@
 import us.ihmc.commonWalkingControlModules.desiredFootStep.footstepGenerator.ContinuousStepGeneratorParametersBasics;
 import us.ihmc.commons.DeadbandTools;
 import us.ihmc.communication.HumanoidControllerAPI;
+import us.ihmc.footstepPlanning.communication.ContinuousHikingAPI;
 import us.ihmc.ros2.QueuedROS2Subscription;
 import us.ihmc.ros2.ROS2Node;
+import us.ihmc.ros2.ROS2Publisher;
 
 /**
  * Plugin for using an xbox controller to send commands and receive status info to/from the
@@ -26,11 +29,14 @@
  *
  * @author Stefan Fasano
  */
-public class XBoxOneCSGPluginGDX
+public class XBoxOneJoystickWalkingPlugin
 {
    public static final double DEFAULT_PARAMETER_INCREMENT = 0.01;
    private static final boolean DEFAULT_USE_DEADMAN_SWITCH = true;
 
+   private enum JoystickWalkingMode {CSG, CONTINUOUS_HIKING}
+   private JoystickWalkingMode currentJoystickWalkingMode = JoystickWalkingMode.CSG;
+
    private final double parameterIncrement;
    private final boolean useDeadmanSwitch;
 
@@ -39,30 +45,39 @@ public class XBoxOneCSGPluginGDX
    private boolean currentControllerConnected = false;
    private boolean controllerListenerHasBeenAdded = false;
 
-   private final ROS2ControllerPublisherMap ros2ControllerPublisherMap;
+   private final ROS2ControllerPublisherMap ros2CSGPublisherMap;
    private final ContinuousStepGeneratorInputMessage csgInputCommand;
    private final ContinuousStepGeneratorParametersMessage csgParametersCommand;
 
    private final QueuedROS2Subscription<ContinuousStepGeneratorStatusMessage> csgStatusSubscription;
    private final ContinuousStepGeneratorStatusMessage csgStatusMessage = new ContinuousStepGeneratorStatusMessage();
 
-   public XBoxOneCSGPluginGDX(DRCRobotModel robotModel, ROS2Node ros2Node)
+   private final ROS2Publisher<ContinuousHikingCommandMessage> continuousHikingCommandPublisher;
+   private final ContinuousHikingCommandMessage continuousHikingCommand = new ContinuousHikingCommandMessage();
+
+   public XBoxOneJoystickWalkingPlugin(DRCRobotModel robotModel, ROS2Node ros2Node)
    {
       this(robotModel, ros2Node, DEFAULT_PARAMETER_INCREMENT, DEFAULT_USE_DEADMAN_SWITCH);
    }
 
-   public XBoxOneCSGPluginGDX(DRCRobotModel robotModel, ROS2Node ros2Node, double parameterIncrement, boolean useDeadmanSwitch)
+   public XBoxOneJoystickWalkingPlugin(DRCRobotModel robotModel, ROS2Node ros2Node, double parameterIncrement, boolean useDeadmanSwitch)
    {
       this.parameterIncrement = parameterIncrement;
       this.useDeadmanSwitch = useDeadmanSwitch;
 
-      ros2ControllerPublisherMap = new ROS2ControllerPublisherMap(ros2Node, robotModel.getSimpleRobotName());
+      // Set up CSG commands, publisher, and status subscriber
+      ros2CSGPublisherMap = new ROS2ControllerPublisherMap(ros2Node, robotModel.getSimpleRobotName());
+      csgStatusSubscription = ros2Node.createQueuedSubscription(HumanoidControllerAPI.getTopic(ContinuousStepGeneratorStatusMessage.class, robotModel.getSimpleRobotName()), 10);
       csgInputCommand = new ContinuousStepGeneratorInputMessage();
       csgParametersCommand = new ContinuousStepGeneratorParametersMessage();
-      csgStatusSubscription = ros2Node.createQueuedSubscription(HumanoidControllerAPI.getTopic(ContinuousStepGeneratorStatusMessage.class, robotModel.getSimpleRobotName()), 10);
 
-      configureCSGParameters(csgParametersCommand, robotModel.getWalkingControllerParameters());
+      // Set up CH publisher
+      continuousHikingCommandPublisher = ros2Node.createPublisher(ContinuousHikingAPI.CONTINUOUS_HIKING_COMMAND);
 
+      // Initialize CSG parameter command from default initial robot parameters
+      initializeCSGParametersCommand(csgParametersCommand, robotModel.getWalkingControllerParameters());
+
+      // Set up controller listener
       controllerListener = new ControllerListener()
       {
          @Override
@@ -75,12 +90,7 @@ public void disconnected(Controller controller)
          {
             currentControllerConnected = false;
 
-            csgInputCommand.setWalk(false);
-            csgInputCommand.setForwardVelocity(0.0);
-            csgInputCommand.setLateralVelocity(0.0);
-            csgInputCommand.setTurnVelocity(0.0);
-
-            ros2ControllerPublisherMap.publish(csgInputCommand);
+            sendStopWalkingCommands();
          }
 
          @Override
@@ -103,7 +113,10 @@ else if (buttonCode == controller.getMapping().buttonDpadDown)
                else if (buttonCode == controller.getMapping().buttonDpadUp)
                   csgParametersCommand.setSwingHeight(csgStatusMessage.getCurrentSwingHeight() + parameterIncrement);
 
-               ros2ControllerPublisherMap.publish(csgParametersCommand);
+               else if (buttonCode == controller.getMapping().buttonL1)
+                  continuousHikingCommand.setSquareUpToGoal(true);
+
+               ros2CSGPublisherMap.publish(csgParametersCommand);
             }
             return false;
          }
@@ -124,22 +137,53 @@ public boolean axisMoved(Controller controller, int axisCode, float value)
 
    public void update()
    {
+      // Get latest CSG status message, and reset CSG parameters command from that
       if (csgStatusSubscription.flushAndGetLatest(csgStatusMessage))
          setCSGCommandsToCurrentValues(csgStatusMessage);
 
+      // Check if new joystick has been connected
       boolean newControllerConnected = false;
       if (currentController != null && currentController != Controllers.getCurrent())
          newControllerConnected = true;
 
       currentController = Controllers.getCurrent();
       currentControllerConnected = currentController != null;
 
+      // Check if controller listener has been added
       if ((!controllerListenerHasBeenAdded || newControllerConnected) && currentControllerConnected)
       {
          currentController.addListener(controllerListener);
          controllerListenerHasBeenAdded = true;
       }
 
+      // Assemble our command from joystick inputs and publish it
+      switch (currentJoystickWalkingMode)
+      {
+         case CSG ->
+         {
+            // Assemble our CSG input command
+            configureCSGInputCommand();
+
+            // Publish our CSG input command
+            ros2CSGPublisherMap.publish(csgInputCommand);
+         }
+
+         case CONTINUOUS_HIKING ->
+         {
+            // Assemble our continuous hiking command
+            configureContinuousHikingCommand();
+
+            // Publish our continuous hiking command
+            continuousHikingCommandPublisher.publish(continuousHikingCommand);
+
+            // Reset this to false each time
+            continuousHikingCommand.setSquareUpToGoal(false);
+         }
+      }
+   }
+
+   private void configureCSGInputCommand()
+   {
       // Default CSG input values
       boolean requestWalking = false;
       double forwardJoystickValue = 0.0;
@@ -156,22 +200,81 @@ public void update()
          turningJoystickValue = DeadbandTools.applyDeadband(deadband, -currentController.getAxis(currentController.getMapping().axisRightX));
       }
 
+      configureCSGInputCommand(requestWalking, forwardJoystickValue, lateralJoystickValue, turningJoystickValue);
+   }
+
+   private void configureCSGInputCommand(boolean requestWalking, double desiredForwardVelocity, double desiredLateralVelocity, double desiredTurningVelocity)
+   {
       csgInputCommand.setWalk(requestWalking);
       csgInputCommand.setUnitVelocities(false);
-      csgInputCommand.setForwardVelocity(forwardJoystickValue);
-      csgInputCommand.setLateralVelocity(lateralJoystickValue);
-      csgInputCommand.setTurnVelocity(turningJoystickValue);
-      ros2ControllerPublisherMap.publish(csgInputCommand);
+      csgInputCommand.setForwardVelocity(desiredForwardVelocity);
+      csgInputCommand.setLateralVelocity(desiredLateralVelocity);
+      csgInputCommand.setTurnVelocity(desiredTurningVelocity);
    }
 
-   public void shutDownXboxJoystick()
+   private void configureContinuousHikingCommand()
    {
+      // Setup variables to be published in the message
+      boolean requestWalking = false;
+      boolean walkForwards = false;
+      boolean walkBackwards = false;
+      double forwardJoystickValue = 0.0;
+      double lateralJoystickValue = 0.0;
+      double turningJoystickValue = 0.0;
+      double deadband = 0.09;
+
+      // Continuous hiking input values we get from the xbox controller
+      if (currentControllerConnected)
+      {
+         requestWalking = currentController.getAxis(4) == 1.0; // This is the left trigger value (1.0 = pressed in)
+         forwardJoystickValue = requestWalking ? DeadbandTools.applyDeadband(deadband, -currentController.getAxis(currentController.getMapping().axisLeftY)) : 0.0;
+         lateralJoystickValue = requestWalking ? DeadbandTools.applyDeadband(deadband, -currentController.getAxis(currentController.getMapping().axisLeftX)) : 0.0;
+         turningJoystickValue = requestWalking ? DeadbandTools.applyDeadband(deadband, -currentController.getAxis(currentController.getMapping().axisRightX)) : 0.0;
+         walkForwards = requestWalking && forwardJoystickValue > 0.0;
+         walkBackwards = requestWalking && forwardJoystickValue < 0.0;
+      }
+
+      configureContinuousHikingCommand(true, walkForwards, walkBackwards, forwardJoystickValue, lateralJoystickValue, turningJoystickValue);
+   }
+
+   private void configureContinuousHikingCommand(boolean enableContinuousHiking, boolean walkForwards, boolean walkBackwards, double forwardValue, double lateralValue, double turningValue)
+   {
+      continuousHikingCommand.setEnableContinuousHiking(enableContinuousHiking);
+      continuousHikingCommand.setUseJoystickController(true);
+      continuousHikingCommand.setWalkForwards(walkForwards);
+      continuousHikingCommand.setWalkBackwards(walkBackwards);
+      continuousHikingCommand.setForwardValue(forwardValue);
+      continuousHikingCommand.setLateralValue(lateralValue);
+      continuousHikingCommand.setTurningValue(turningValue);
+   }
+
+   public void sendStopWalkingCommands()
+   {
+      // Safety command for CSG
       csgInputCommand.setWalk(false);
       csgInputCommand.setForwardVelocity(0.0);
       csgInputCommand.setLateralVelocity(0.0);
       csgInputCommand.setTurnVelocity(0.0);
 
-      ros2ControllerPublisherMap.publish(csgInputCommand);
+      // Safety command for CH
+      continuousHikingCommand.setEnableContinuousHiking(false); //TODO what do we wanna do here?
+      continuousHikingCommand.setUseJoystickController(false);
+      continuousHikingCommand.setWalkForwards(false);
+      continuousHikingCommand.setWalkBackwards(false);
+      continuousHikingCommand.setForwardValue(0.0);
+      continuousHikingCommand.setLateralValue(0.0);
+      continuousHikingCommand.setSquareUpToGoal(true);
+
+      switch (currentJoystickWalkingMode)
+      {
+         case CSG -> ros2CSGPublisherMap.publish(csgInputCommand);
+         case CONTINUOUS_HIKING -> continuousHikingCommandPublisher.publish(continuousHikingCommand);
+      }
+   }
+
+   public void shutDownXboxJoystick()
+   {
+      sendStopWalkingCommands();
    }
 
    private void setCSGCommandsToCurrentValues(ContinuousStepGeneratorStatusMessage csgStatusMessage)
@@ -188,7 +291,7 @@ private void setCSGCommandsToCurrentValues(ContinuousStepGeneratorStatusMessage
       csgParametersCommand.setTurnMaxAngleOutward(csgStatusMessage.getCurrentTurnMaxAngleOutward());
    }
 
-   private void configureCSGParameters(ContinuousStepGeneratorParametersMessage csgParametersCommand, WalkingControllerParameters walkingControllerParameters)
+   private void initializeCSGParametersCommand(ContinuousStepGeneratorParametersMessage csgParametersCommand, WalkingControllerParameters walkingControllerParameters)
    {
       csgParametersCommand.setNumberOfFootstepsToPlan(ContinuousStepGeneratorParametersBasics.DEFAULT_NUMBER_OF_FOOTSTEPS_TO_PLAN);
       csgParametersCommand.setNumberOfFixedFootsteps(ContinuousStepGeneratorParametersBasics.DEFAULT_NUMBER_OF_FIXED_FOOTSTEPS);
@@ -204,4 +307,33 @@ private void configureCSGParameters(ContinuousStepGeneratorParametersMessage csg
       csgParametersCommand.setTurnMaxAngleInward(steppingParameters.getMaxAngleTurnInwards());
       csgParametersCommand.setTurnMaxAngleOutward(steppingParameters.getMaxAngleTurnOutwards());
    }
+
+   public void enableCSGPublishing()
+   {
+      // Stop walking before we switch to CSG
+      if (currentJoystickWalkingMode.equals(JoystickWalkingMode.CONTINUOUS_HIKING))
+      {
+         configureContinuousHikingCommand(false, false, false, 0.0, 0.0, 0.0);
+         continuousHikingCommandPublisher.publish(continuousHikingCommand);
+      }
+
+      currentJoystickWalkingMode = JoystickWalkingMode.CSG;
+   }
+
+   public void enableContinuousHikingPublishing()
+   {
+      // Stop walking before we switch to CH
+      if (currentJoystickWalkingMode.equals(JoystickWalkingMode.CSG))
+      {
+         configureCSGInputCommand(false, 0.0, 0.0, 0.0);
+         ros2CSGPublisherMap.publish(csgInputCommand);
+      }
+
+      currentJoystickWalkingMode = JoystickWalkingMode.CONTINUOUS_HIKING;
+   }
+
+   public JoystickWalkingMode getCurrentJoystickWalkingMode()
+   {
+      return currentJoystickWalkingMode;
+   }
 }

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java

@@ -355,10 +355,10 @@ public void renderImGuiWidgets()
       // Here we check against null rather then .isConnected() because if the controller is unplugged, that method won't work
       boolean controllerConnected = joystickController != null;
 
-      if (controllerConnected)
-      {
-         performJoystickControllerAction(joystickController);
-      }
+//      if (controllerConnected)
+//      {
+//         performJoystickControllerAction(joystickController);
+//      }
    }
 
    private void performJoystickControllerAction(Controller joystickController)

ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/ReadyToPlanState.java

@@ -217,7 +217,7 @@ else if (commandMessage.get().getUseJoystickController())
          // Here we assume the joystick isn't being turned at all, so we give a direction of straight forward
          // The number here it to account for drift in the controller where that value is never actually zero.
          // Cause the Joystick is drifting constantly
-         else if (Math.abs(commandMessage.get().getLateralValue()) < 0.05)
+         else if (commandMessage.get().getWalkForwards())
          {
             previousLateralValue = 0;
             goalPoses = ContinuousPlannerTools.setStraightForwardGoalPoses(continuousPlanner.getWalkingStartMidPose(),