Feature/tufts integration

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

@@ -136,7 +136,7 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, ROS2Helper
                                       message -> terrainPlanningDebugger.reset());
 
       remotePropertySets = new ImGuiRemoteROS2StoredPropertySetGroup(ros2Helper);
-      createParametersPanel(continuousHikingParameters, continuousHikingParametersPanel, remotePropertySets, ContinuousWalkingAPI.CONTINUOUS_HIKING_PARAMETERS);
+//      createParametersPanel(continuousHikingParameters, continuousHikingParametersPanel, remotePropertySets, ContinuousWalkingAPI.CONTINUOUS_HIKING_PARAMETERS);
       RDXStoredPropertySetTuner monteCarloPlannerParametersPanel = new RDXStoredPropertySetTuner("Monte Carlo Footstep Planner Parameters (CH)");
       createParametersPanel(monteCarloPlannerParameters,
                             monteCarloPlannerParametersPanel,
@@ -204,13 +204,15 @@ public void startContinuousPlannerSchedulingTask()
 
    public void update(TerrainMapData terrainMapData, HeightMapData heightMapData)
    {
-      remotePropertySets.setPropertyChanged();
-
-      // When running on the process we don't want to create the parameters locally, this gets done on the remote side
+      // When running on the process, we don't want to create the parameters locally; this gets done on the remote side
       if (runningLocally)
       {
          ros2PropertySetGroup.update();
       }
+      else
+      {
+         remotePropertySets.setPropertyChanged();
+      }
 
       if (latestFootstepPlan != null)
       {
@@ -227,7 +229,7 @@ public void renderImGuiWidgets()
       ImGui.text("And the enabled checkbox must be checked");
       ImGui.text("By holding CTRL the robot will walk forward");
       ImGui.separator();
-      continuousHikingParametersPanel.renderImGuiWidgets();
+//      continuousHikingParametersPanel.renderImGuiWidgets();
 
       ImGui.checkbox("Local Render Mode", localRenderMode);
       ImGui.checkbox("Use A* Footstep Planner", useAStarFootstepPlanner);
@@ -317,7 +319,7 @@ private void publishInputCommandMessage()
       }
 
       // Only allow Continuous Walking if the CTRL key is held and the checkbox is checked
-      if (continuousHikingParameters.getEnableContinuousHiking())
+//      if (continuousHikingParameters.getEnableContinuousHiking())
       {
          commandMessage.setEnableContinuousWalking(walkingEnabled);
          commandMessage.setPublishToController(ImGui.getIO().getKeyAlt());

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

@@ -122,6 +122,20 @@ public void update(TerrainMapData latestTerrainMapData, HeightMapData latestHeig
             }
          }
       }
+      else
+      {
+         if (selectionActive)
+         {
+            stancePoses.set(new SideDependentList<>(new FramePose3D(), new FramePose3D()));
+            for (RobotSide robotSide : RobotSide.values)
+            {
+               FramePose3D pose = new FramePose3D(latestPickPoint);
+               pose.appendTranslation(0, robotSide == RobotSide.LEFT ? 0.12 : -0.12, 0);
+               stancePoses.get(robotSide).set(pose);
+               footstepGraphics.get(robotSide).setPose(stancePoses.get(robotSide));
+            }
+         }
+      }
 
       // NOTE: This is very important for making sure that the collision with the height map is correct
       if (selectionActive)

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

@@ -164,6 +164,9 @@ public SideDependentList<FramePose3D> getGoalPosesBasedOnPlanningMode()
          // This allows for walking to a goal that isn't straight forward, its assumed that if there is no goal we will just resume walking straight forward
          case WALK_TO_GOAL ->
          {
+            // Allow for more planning time with this one, just plan for one step length
+            continuousHikingParameters.setPlannerTimeoutFraction(1.0);
+
             // Set the goalPoses here so that we return a good value regardless of what happens next
             goalPoses = walkToGoalWayPointPoses.get(0);
 
@@ -202,6 +205,9 @@ public void addWayPointPoseToList(PoseListMessage poseListMessage)
       FramePose3D leftFootPose = new FramePose3D();
       FramePose3D rightFootPose = new FramePose3D();
 
+      //TODO remove later or fix for long term, this is temporary fix for Tufts
+      continuousHikingParameters.setEnableContinuousHiking(true);
+
       leftFootPose.set(poses.get(0));
       rightFootPose.set(poses.get(1));
 

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

@@ -110,6 +110,11 @@ public ContinuousPlannerSchedulingTask(DRCRobotModel robotModel,
       stateMachineFactory.addState(ContinuousHikingState.READY_TO_PLAN, readyToPlanState);
       stateMachineFactory.addState(ContinuousHikingState.WAITING_TO_LAND, waitingtoLandState);
 
+      // TODO Set these to true for Tufts integration, need a better long term solution
+      continuousHikingParameters.setEnableContinuousHiking(true);
+      continuousHikingParameters.setStepPublisherEnabled(true);
+      // -------------------------------------------------------
+
       // Create different conditions
       StartContinuousHikingTransitionCondition startContinuousHikingTransitionCondition = new StartContinuousHikingTransitionCondition(commandMessage,
                                                                                                                                        continuousHikingParameters);