Bugfix/walk to goal ch

ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousWalkingAPI.java

@@ -1,13 +1,11 @@
 package us.ihmc.footstepPlanning.communication;
 
-import behavior_msgs.msg.dds.ContinuousWalkingCommandMessage;
+import behavior_msgs.msg.dds.ContinuousHikingCommandMessage;
 import behavior_msgs.msg.dds.ContinuousWalkingStatusMessage;
 import controller_msgs.msg.dds.FootstepDataListMessage;
-import controller_msgs.msg.dds.RigidBodyTransformMessage;
 import ihmc_common_msgs.msg.dds.PoseListMessage;
-import std_msgs.Empty;
+import std_msgs.msg.dds.Empty;
 import us.ihmc.communication.property.StoredPropertySetROS2TopicPair;
-import us.ihmc.robotics.robotSide.SideDependentList;
 import us.ihmc.ros2.ROS2Topic;
 
 public class ContinuousWalkingAPI
@@ -17,8 +15,8 @@ public class ContinuousWalkingAPI
    private static final String ACTIVE_MODULE_NAME = "active_perception";
 
    // Commands supported for the Continuous Hiking Process
-   public static final ROS2Topic<ContinuousWalkingCommandMessage> CONTINUOUS_WALKING_COMMAND = IHMC_ROOT.withModule("continuous_walking").withType(ContinuousWalkingCommandMessage.class).withSuffix("command");
-   public static final ROS2Topic<Empty> CLEAR_GOAL_FOOTSTEPS = IHMC_ROOT.withModule("continuous_walking").withType(Empty.class).withSuffix("clear_goal_footsteps");
+   public static final ROS2Topic<ContinuousHikingCommandMessage> CONTINUOUS_WALKING_COMMAND = IHMC_ROOT.withModule("continuous_walking").withType(ContinuousHikingCommandMessage.class).withSuffix("command");
+   public static final ROS2Topic<std_msgs.msg.dds.Empty> CLEAR_GOAL_FOOTSTEPS = IHMC_ROOT.withModule("continuous_walking").withType(Empty.class).withSuffix("clear_goal_footsteps");
    public static final ROS2Topic<PoseListMessage> PLACED_GOAL_FOOTSTEPS = IHMC_ROOT.withModule("continuous_walking").withType(PoseListMessage.class).withSuffix("placed_goal_footsteps");
 
    // Statuses supported from the Continuous Hiking Process

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

@@ -1,6 +1,6 @@
 package us.ihmc.rdx.perception;
 
-import behavior_msgs.msg.dds.ContinuousWalkingCommandMessage;
+import behavior_msgs.msg.dds.ContinuousHikingCommandMessage;
 import com.badlogic.gdx.controllers.Controller;
 import com.badlogic.gdx.controllers.Controllers;
 import com.badlogic.gdx.graphics.g3d.Renderable;
@@ -40,6 +40,7 @@
 import us.ihmc.perception.comms.PerceptionComms;
 import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor;
 import us.ihmc.perception.heightMap.TerrainMapData;
+import us.ihmc.rdx.imgui.ImGuiSliderDouble;
 import us.ihmc.rdx.imgui.RDXPanel;
 import us.ihmc.rdx.input.ImGui3DViewInput;
 import us.ihmc.rdx.ui.ImGuiRemoteROS2StoredPropertySetGroup;
@@ -63,22 +64,23 @@ public class RDXContinuousHikingPanel extends RDXPanel implements RenderableProv
    private static final int numberOfKnotPoints = 12;
    private static final int maxIterationsOptimization = 100;
    private final ROS2Node ros2Node;
+   private final ROS2Helper ros2Helper;
    private final DRCRobotModel robotModel;
    private final ROS2SyncedRobotModel syncedRobotModel;
-   private final ROS2PublisherBasics<ContinuousWalkingCommandMessage> commandPublisher;
-   private final ContinuousWalkingCommandMessage commandMessage = new ContinuousWalkingCommandMessage();
+   private final ROS2PublisherBasics<ContinuousHikingCommandMessage> commandPublisher;
+   private final ContinuousHikingCommandMessage commandMessage = new ContinuousHikingCommandMessage();
    private final RDXStancePoseSelectionPanel stancePoseSelectionPanel;
    private final PositionOptimizedTrajectoryGenerator positionTrajectoryGenerator = new PositionOptimizedTrajectoryGenerator(numberOfKnotPoints,
                                                                                                                              maxIterationsOptimization);
    private final RDXTerrainPlanningDebugger terrainPlanningDebugger;
-   private final ContinuousHikingParameters continuousHikingParameters = new ContinuousHikingParameters();
    private final SwingTrajectoryParameters swingTrajectoryParameters;
    private final RDXStoredPropertySetTuner continuousHikingParametersPanel = new RDXStoredPropertySetTuner("Continuous Hiking Parameters (CH)");
    private final ImGuiRemoteROS2StoredPropertySetGroup hostStoredPropertySets;
-   private final ImBoolean localRenderMode = new ImBoolean(false);
-   private final ImBoolean useMonteCarloReference = new ImBoolean(false);
-   private final ImBoolean useHybridPlanner = new ImBoolean(false);
+   private final ImGuiSliderDouble stepsBeforeSafetyStop = new ImGuiSliderDouble("Steps Before Safety Stop", "%.2f");
+   private final ImBoolean enableContinuousHiking = new ImBoolean(false);
+   private final ImBoolean squareUpToGoal = new ImBoolean(false);
    private final ImBoolean useAStarFootstepPlanner = new ImBoolean(true);
+   private final ImBoolean useMonteCarloReference = new ImBoolean(false);
    private final ImBoolean useMonteCarloFootstepPlanner = new ImBoolean(false);
    private SideDependentList<FramePose3D> startStancePose = new SideDependentList<>(new FramePose3D(), new FramePose3D());
    private FootstepPlan latestFootstepPlan;
@@ -92,6 +94,7 @@ public class RDXContinuousHikingPanel extends RDXPanel implements RenderableProv
    public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, ROS2Helper ros2Helper, DRCRobotModel robotModel, ROS2SyncedRobotModel syncedRobotModel)
    {
       super("Continuous Hiking");
+      this.ros2Helper = ros2Helper;
       setRenderMethod(this::renderImGuiWidgets);
 
       this.ros2Node = ros2Node;
@@ -131,6 +134,7 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, ROS2Helper
                                       message -> terrainPlanningDebugger.reset());
 
       hostStoredPropertySets = new ImGuiRemoteROS2StoredPropertySetGroup(ros2Helper);
+      ContinuousHikingParameters continuousHikingParameters = new ContinuousHikingParameters();
       createParametersPanel(continuousHikingParameters,
                             continuousHikingParametersPanel,
                             hostStoredPropertySets,
@@ -220,7 +224,7 @@ public void update(TerrainMapData terrainMapData, HeightMapData heightMapData)
    /**
     * This method handles updating the stored property sets used in Continuous Hiking.
     * These are all the parameters that are getting synced back and forth between the remote process and the local process.
-    * There are three situations that can occur when tyring to use Continuous Hiking.
+    * There are three situations that can occur when trying to use Continuous Hiking.
     * <ul>
     *    <li>Case 1: The situation where we are simulating the process running on a remote machine but in reality its running locally.
     *    This is where we only want to update the property sets running on that process. Represented by {@link #clientStoredPropertySets}.
@@ -229,7 +233,7 @@ public void update(TerrainMapData terrainMapData, HeightMapData heightMapData)
     *    Here we want to publish, and subscribe in one place as everything is being run on the same machine.
     *    So we update {@link #clientStoredPropertySets} and {@link #hostStoredPropertySets}</li>
     *    <li>Case 3: Then the situation where we only want to publish the property sets to be sent to the remote process.
-    *    This is when we don't want to subscribe but we publish and changes to {@link #hostStoredPropertySets} so the remote process can receive these chagnes</li>
+    *    This is when we don't want to subscribe but we publish and changes to {@link #hostStoredPropertySets} so the remote process can receive these changes</li>
     *
     * </ul>
     */
@@ -258,13 +262,41 @@ public void renderImGuiWidgets()
       ImGui.separator();
       continuousHikingParametersPanel.renderImGuiWidgets();
 
-      ImGui.checkbox("Local Render Mode", localRenderMode);
+      ImGui.separator();
+      ImGui.text("Options for Continuous Hiking Message");
+      ImGui.indent();
+      ImGui.checkbox("Enable Continuous Hiking", enableContinuousHiking);
+      ImGui.checkbox("Square Up To Goal", squareUpToGoal);
+      if (ImGui.button("Clear Planned footsteps"))
+      {
+         clearPlannedFootsteps();
+      }
+      ImGui.sameLine();
+      stepsBeforeSafetyStop.render(0.0, 50.0);
       ImGui.checkbox("Use A* Footstep Planner", useAStarFootstepPlanner);
       ImGui.checkbox("Use Monte-Carlo Footstep Planner", useMonteCarloFootstepPlanner);
       ImGui.checkbox("Use Monte-Carlo Reference", useMonteCarloReference);
+      ImGui.unindent();
       ImGui.separator();
       terrainPlanningDebugger.renderImGuiWidgets();
-      publishInputCommandMessage();
+
+      // Check to see if a controller is plugged into the computer
+      Controller joystickController = Controllers.getCurrent();
+      // Here we check against null rather then .isConnected() because if the controller is unplugged, that method won't work
+      boolean controllerConnected = joystickController != null;
+
+      if (ImGui.getIO().getKeyAlt())
+      {
+         publishStopContinuousHiking();
+      }
+      else if (controllerConnected)
+      {
+         publishJoystickStatus(joystickController);
+      }
+      else if (ImGui.getIO().getKeyCtrl() && ImGui.getIO().getKeyShift())
+      {
+         publishContinuousHikingCommand();
+      }
    }
 
    public void processImGui3DViewInput(ImGui3DViewInput input)
@@ -279,6 +311,11 @@ public void getRenderables(Array<Renderable> renderables, Pool<Renderable> pool)
       terrainPlanningDebugger.getRenderables(renderables, pool);
    }
 
+   public void clearPlannedFootsteps()
+   {
+      ros2Helper.publish(ContinuousWalkingAPI.CLEAR_GOAL_FOOTSTEPS);
+   }
+
    /**
     * We have received the start and goal pose from the process, lets unpack this message and visualize the start and goal on the UI.
     */
@@ -327,49 +364,70 @@ public void onMonteCarloPlanReceived(FootstepDataListMessage message)
       terrainPlanningDebugger.generateMonteCarloPlanGraphic(message);
    }
 
-   private void publishInputCommandMessage()
+   private void publishStopContinuousHiking()
    {
-      // Check to see if a controller is plugged into the computer
-      Controller joystickController = Controllers.getCurrent();
-      // Here we check against null rather then .isConnected() because if the controller is unplugged that method won't work
-      boolean controllerConnected = joystickController != null;
+      commandMessage.setEnableContinuousHiking(false);
+      commandMessage.setStepsBeforeSafetyStop(0);
+      commandMessage.setWalkForwards(false);
+      commandMessage.setSquareUpToGoal(false);
+      commandMessage.setUseAstarFootstepPlanner(useAStarFootstepPlanner.get());
+      commandMessage.setUseMonteCarloFootstepPlanner(useMonteCarloFootstepPlanner.get());
+      commandMessage.setUseMonteCarloPlanAsReference(useMonteCarloReference.get());
+      commandMessage.setUsePreviousPlanAsReference(!useMonteCarloReference.get());
+
+      commandMessage.setUseJoystickController(false);
+      commandMessage.setForwardValue(0.0);
+      commandMessage.setWalkBackwards(false);
+      commandMessage.setLateralValue(0.0);
+      commandMessage.setTurningValue(0.0);
+
+      commandPublisher.publish(commandMessage);
+   }
 
-      // Setup a bunch of variables to be published in the message
-      boolean walkWithKeyboard = ImGui.getIO().getKeyCtrl();
-      boolean walkWithController = false;
+   private void publishJoystickStatus(Controller joystickController)
+   {
+
+      // Setup variables to be published in the message
       boolean walkBackwards = false;
-      double lateralJoystickValue = 0.0;
       double forwardJoystickValue = 0.0;
+      double lateralJoystickValue = 0.0;
       double turningJoystickValue = 0.0;
 
-      if (controllerConnected)
-      {
-         walkBackwards = joystickController.getButton(joystickController.getMapping().buttonB);
-         walkWithController = joystickController.getButton(joystickController.getMapping().buttonA);
-         walkWithController |= walkBackwards;
-         forwardJoystickValue = -joystickController.getAxis(joystickController.getMapping().axisLeftY);
-         lateralJoystickValue = -joystickController.getAxis(joystickController.getMapping().axisLeftX);
-         turningJoystickValue = -joystickController.getAxis(joystickController.getMapping().axisRightX);
-      }
+      walkBackwards = joystickController.getButton(joystickController.getMapping().buttonB);
+      forwardJoystickValue = -joystickController.getAxis(joystickController.getMapping().axisLeftY);
+      lateralJoystickValue = -joystickController.getAxis(joystickController.getMapping().axisLeftX);
+      turningJoystickValue = -joystickController.getAxis(joystickController.getMapping().axisRightX);
 
-      // Only allow Continuous Walking if the CTRL key is held and the checkbox is checked
-      // We publish this all the time to prevent any of the values from staying true all the time
-      if (continuousHikingParameters.getEnableContinuousHiking())
-      {
-         commandMessage.setEnableContinuousHikingWithKeyboard(walkWithKeyboard);
-         commandMessage.setEnableContinuousHikingWithJoystickController(walkWithController);
-         commandMessage.setWalkBackwards(walkBackwards);
-         commandMessage.setForwardValue(forwardJoystickValue);
-         commandMessage.setLateralValue(lateralJoystickValue);
-         commandMessage.setTurningValue(turningJoystickValue);
-         commandMessage.setUseMonteCarloFootstepPlanner(useMonteCarloFootstepPlanner.get());
-         commandMessage.setUseAstarFootstepPlanner(useAStarFootstepPlanner.get());
-         commandMessage.setUseMonteCarloPlanAsReference(useMonteCarloReference.get());
-         commandMessage.setUsePreviousPlanAsReference(!useMonteCarloReference.get());
-         commandMessage.setUseHybridPlanner(useHybridPlanner.get());
-
-         commandPublisher.publish(commandMessage);
-      }
+      commandMessage.setUseJoystickController(true);
+      commandMessage.setForwardValue(forwardJoystickValue);
+      commandMessage.setWalkBackwards(walkBackwards);
+      commandMessage.setLateralValue(lateralJoystickValue);
+      commandMessage.setTurningValue(turningJoystickValue);
+
+      commandPublisher.publish(commandMessage);
+   }
+
+   /**
+    * Here we want to publish and walk forward, that is the point of pressing the keys on the keyboard
+    */
+   private void publishContinuousHikingCommand()
+   {
+      commandMessage.setEnableContinuousHiking(enableContinuousHiking.get());
+      commandMessage.setStepsBeforeSafetyStop((int) stepsBeforeSafetyStop.getDoubleValue());
+      commandMessage.setWalkForwards(true);
+      commandMessage.setSquareUpToGoal(squareUpToGoal.get());
+      commandMessage.setUseAstarFootstepPlanner(useAStarFootstepPlanner.get());
+      commandMessage.setUseMonteCarloFootstepPlanner(useMonteCarloFootstepPlanner.get());
+      commandMessage.setUseMonteCarloPlanAsReference(useMonteCarloReference.get());
+      commandMessage.setUsePreviousPlanAsReference(!useMonteCarloReference.get());
+
+      commandMessage.setUseJoystickController(false);
+      commandMessage.setForwardValue(0.0);
+      commandMessage.setWalkBackwards(false);
+      commandMessage.setLateralValue(0.0);
+      commandMessage.setTurningValue(0.0);
+
+      commandPublisher.publish(commandMessage);
    }
 
    public void destroy()

ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.java

@@ -18,7 +18,6 @@ public class ContinuousHikingParameters extends StoredPropertySet implements Con
 {
    public static final StoredPropertyKeyList keys = new StoredPropertyKeyList();
 
-   public static final BooleanStoredPropertyKey enableContinuousHiking = keys.addBooleanKey("Enable continuous hiking");
    public static final BooleanStoredPropertyKey stepPublisherEnabled = keys.addBooleanKey("Step publisher enabled");
    public static final BooleanStoredPropertyKey overrideEntireQueueEachStep = keys.addBooleanKey("Override entire queue each step");
    public static final IntegerStoredPropertyKey numberOfStepsToSend = keys.addIntegerKey("Number of steps to send");

ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersBasics.java

@@ -8,11 +8,6 @@
  */
 public interface ContinuousHikingParametersBasics extends ContinuousHikingParametersReadOnly, StoredPropertySetBasics
 {
-   default void setEnableContinuousHiking(boolean enableContinuousHiking)
-   {
-      set(ContinuousHikingParameters.enableContinuousHiking, enableContinuousHiking);
-   }
-
    default void setStepPublisherEnabled(boolean stepPublisherEnabled)
    {
       set(ContinuousHikingParameters.stepPublisherEnabled, stepPublisherEnabled);

ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersReadOnly.java

@@ -10,11 +10,6 @@
  */
 public interface ContinuousHikingParametersReadOnly extends StoredPropertySetReadOnly
 {
-   default boolean getEnableContinuousHiking()
-   {
-      return get(enableContinuousHiking);
-   }
-
    default boolean getStepPublisherEnabled()
    {
       return get(stepPublisherEnabled);