Initial implementation for square up and walk-to-goal for CH process

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

@@ -4,6 +4,7 @@
 import behavior_msgs.msg.dds.ContinuousWalkingStatusMessage;
 import controller_msgs.msg.dds.FootstepDataListMessage;
 import ihmc_common_msgs.msg.dds.PoseListMessage;
+import std_msgs.msg.dds.Empty;
 import us.ihmc.communication.property.StoredPropertySetROS2TopicPair;
 import us.ihmc.ros2.ROS2Topic;
 
@@ -16,8 +17,10 @@ public class ContinuousHikingAPI
 
    // Commands supported for the Continuous Hiking Process
    public static final ROS2Topic<ContinuousHikingCommandMessage> CONTINUOUS_HIKING_COMMAND = IHMC_ROOT.withModule(moduleName).withType(ContinuousHikingCommandMessage.class).withSuffix("command");
-   public static final ROS2Topic<std_msgs.msg.dds.Empty> CLEAR_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(std_msgs.msg.dds.Empty.class).withSuffix("clear_goal_footsteps");
+   public static final ROS2Topic<Empty> CLEAR_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(Empty.class).withSuffix("clear_goal_footsteps");
    public static final ROS2Topic<PoseListMessage> PLACED_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(PoseListMessage.class).withSuffix("placed_goal_footsteps");
+   public static final ROS2Topic<PoseListMessage> ROTATE_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(PoseListMessage.class).withSuffix("rotate_goal_footsteps");
+   public static final ROS2Topic<Empty> SQUARE_UP_STEP = IHMC_ROOT.withModule(moduleName).withType(Empty.class).withSuffix("rotate_goal_footsteps");
 
    // Statuses supported from the Continuous Hiking Process
    public static final ROS2Topic<ContinuousWalkingStatusMessage> CONTINUOUS_WALKING_STATUS = IHMC_ROOT.withModule(moduleName).withType(ContinuousWalkingStatusMessage.class).withSuffix("status");

ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepPlanningTest.java

@@ -18,6 +18,7 @@
 import us.ihmc.perception.BytedecoImage;
 import us.ihmc.perception.camera.CameraIntrinsics;
 import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor;
+import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager;
 import us.ihmc.perception.heightMap.TerrainMapData;
 import us.ihmc.perception.opencl.OpenCLManager;
 import us.ihmc.robotics.robotSide.RobotSide;
@@ -39,12 +40,17 @@ public void testMonteCarloFootstepPlanning()
       CameraIntrinsics depthImageIntrinsics = new CameraIntrinsics();
       BytedecoImage heightMapBytedecoImage = new BytedecoImage(depthImageIntrinsics.getWidth(), depthImageIntrinsics.getHeight(), opencv_core.CV_16UC1);
       heightMapBytedecoImage.createOpenCLImage(openCLManager, OpenCL.CL_MEM_READ_WRITE);
-      RapidHeightMapExtractor heightMapExtractor = new RapidHeightMapExtractor(openCLManager, heightMapBytedecoImage, cameraIntrinsics, 1);
+
+      RapidHeightMapExtractor heightMapExtractor = new RapidHeightMapExtractor(openCLManager,
+                                                                               heightMapBytedecoImage,
+                                                                               cameraIntrinsics,
+                                                                               1,
+                                                                               RapidHeightMapManager.getHeightMapParameters());
 
       LogTools.info("Initializing");
 
-      RapidHeightMapExtractor.getHeightMapParameters().setInternalGlobalWidthInMeters(4.0);
-      RapidHeightMapExtractor.getHeightMapParameters().setInternalGlobalCellSizeInMeters(0.02);
+      RapidHeightMapManager.getHeightMapParameters().setInternalGlobalWidthInMeters(4.0);
+      RapidHeightMapManager.getHeightMapParameters().setInternalGlobalCellSizeInMeters(0.02);
       heightMapExtractor.initialize();
       heightMapExtractor.reset();
 
@@ -56,7 +62,7 @@ public void testMonteCarloFootstepPlanning()
       HeightMapTerrainGeneratorTools.fillWithSteppingStones(heightMap, 0.4f, 0.4f, 0.3f, 0.25f, 3);
 
       heightMapExtractor.getInternalGlobalHeightMapImage().writeOpenCLImage(openCLManager);
-      heightMapExtractor.populateParameterBuffers(RapidHeightMapExtractor.getHeightMapParameters(), cameraIntrinsics, new Point3D());
+      heightMapExtractor.populateParameterBuffers(RapidHeightMapManager.getHeightMapParameters(), cameraIntrinsics, new Point3D());
       heightMapExtractor.computeContactMap();
       heightMapExtractor.readContactMapImage();
 

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

@@ -9,8 +9,8 @@
 import imgui.type.ImFloat;
 import org.bytedeco.opencv.opencv_core.Mat;
 import us.ihmc.communication.ros2.ROS2Helper;
+import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager;
 import us.ihmc.perception.gpuHeightMap.HeatMapGenerator;
-import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor;
 import us.ihmc.perception.HumanoidPerceptionModule;
 import us.ihmc.perception.heightMap.TerrainMapData;
 import us.ihmc.rdx.imgui.RDXPanel;
@@ -219,16 +219,16 @@ public void initializeHeightMapUI(ROS2Helper ros2Helper)
          //                                                   RDXImagePanel.DO_NOT_FLIP_Y);
 
          depthImagePanel = new RDXBytedecoImagePanel("Depth Image",
-                                                     RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(),
-                                                     RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(),
+                                                     RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(),
+                                                     RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(),
                                                      RDXImagePanel.DO_NOT_FLIP_Y);
          localHeightMapPanel = new RDXBytedecoImagePanel("Local Height Map",
-                                                         RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(),
-                                                         RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(),
+                                                         RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(),
+                                                         RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(),
                                                          RDXImagePanel.FLIP_Y);
          croppedHeightMapPanel = new RDXBytedecoImagePanel("Cropped Height Map",
-                                                           RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(),
-                                                           RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(),
+                                                           RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(),
+                                                           RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(),
                                                            RDXImagePanel.FLIP_Y);
          snapNormalZMapPanel = new RDXBytedecoImagePanel("Normal Z",
                                                                   humanoidPerception.getRapidHeightMapExtractor().getSnapNormalZImage().getBytedecoOpenCVMat().cols(),
@@ -251,12 +251,12 @@ public void initializeHeightMapUI(ROS2Helper ros2Helper)
          //                                                        humanoidPerception.getRapidHeightMapExtractor().getSensorCroppedHeightMapImage().rows(),
          //                                                        RDXImagePanel.DO_NOT_FLIP_Y);
          terrainCostImagePanel = new RDXBytedecoImagePanel("Terrain Cost Image",
-                                                           RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(),
-                                                           RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(),
+                                                           RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(),
+                                                           RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(),
                                                            RDXImagePanel.FLIP_Y);
          contactMapImagePanel = new RDXBytedecoImagePanel("Contact Map Image",
-                                                          RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(),
-                                                          RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(),
+                                                          RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(),
+                                                          RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(),
                                                           RDXImagePanel.FLIP_Y);
 
          addChild(localHeightMapPanel.getImagePanel());

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

@@ -46,6 +46,7 @@ public class RDXStancePoseSelectionPanel extends RDXPanel implements RenderableP
    private final RDXBaseUI baseUI;
    private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass());
    private final ROS2Publisher<PoseListMessage> publisher;
+   private final ROS2Publisher<PoseListMessage> turningPublisher;
 
    private ModelInstance pickPointSphere;
 
@@ -73,6 +74,7 @@ public RDXStancePoseSelectionPanel(RDXBaseUI baseUI, ROS2Node ros2Node, StancePo
       stancePoseCalculatorParametersTuner.create(stancePoseCalculator.getStancePoseParameters());
 
       publisher = ros2Node.createPublisher(ContinuousHikingAPI.PLACED_GOAL_FOOTSTEPS);
+      turningPublisher = ros2Node.createPublisher(ContinuousHikingAPI.ROTATE_GOAL_FOOTSTEPS);
 
       SegmentDependentList<RobotSide, ArrayList<Point2D>> contactPoints = new SideDependentList<>();
       contactPoints.set(RobotSide.LEFT, PlannerTools.createFootContactPoints(0.2, 0.1, 0.08));
@@ -210,6 +212,11 @@ else if (dScroll < 0.0)
             latestPickPoint.getOrientation().setYawPitchRoll(latestFootstepYaw + deltaYaw, 0.0, 0.0);
          }
       }
+      if (input.isWindowHovered() && input.mouseReleasedWithoutDrag(ImGuiMouseButton.Middle) && calculateStancePose.get() && selectionActive)
+      {
+         setRotateGoalFootsteps();
+         selectionActive = false;
+      }
 
       if (input.isWindowHovered() & input.mouseReleasedWithoutDrag(ImGuiMouseButton.Left) && calculateStancePose.get() && selectionActive)
       {
@@ -254,6 +261,18 @@ public void destroy()
       rightSpheres.clear();
    }
 
+   private void setRotateGoalFootsteps()
+   {
+      List<Pose3D> poses = new ArrayList<>();
+      poses.add(new Pose3D(stancePoses.get(RobotSide.LEFT)));
+      poses.add(new Pose3D(stancePoses.get(RobotSide.RIGHT)));
+
+      PoseListMessage poseListMessage = new PoseListMessage();
+      MessageTools.packPoseListMessage(poses, poseListMessage);
+
+      turningPublisher.publish(poseListMessage);
+   }
+
    private void setGoalFootsteps()
    {
       List<Pose3D> poses = new ArrayList<>();

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java

@@ -161,6 +161,7 @@ public class RDXHighLevelDepthSensorSimulator extends RDXPanel
 
    private Thread publishImagesThread;
    private volatile boolean publishImagesRunning;
+   private ROS2Publisher<ImageMessage> depthImagePublisher;
 
    public RDXHighLevelDepthSensorSimulator(String sensorName,
                                            ReferenceFrame sensorFrame,
@@ -233,6 +234,9 @@ public void setupForROS2ImageMessages(ROS2Node ros2Node, ROS2Topic<ImageMessage>
    {
       this.ros2Node = ros2Node;
       this.ros2Helper = new ROS2Helper(ros2Node);
+
+      depthImagePublisher = ros2Node.createPublisher(ros2DepthTopic);
+
       this.ros2DepthTopic = ros2DepthTopic;
       this.ros2ColorTopic = ros2ColorTopic;
 
@@ -572,7 +576,8 @@ public void publishROS2DepthImageMessage()
             sensorPose.setToZero(sensorFrame);
             sensorPose.changeFrame(ReferenceFrame.getWorldFrame());
             OpenCVTools.compressImagePNG(depthImageMat, compressedDepthPointer);
-            PerceptionMessageTools.publishCompressedDepthImage(compressedDepthPointer, ros2DepthTopic, depthImageMessage, ros2Helper, sensorPose, now, depthSequenceNumber++,
+
+            PerceptionMessageTools.publishCompressedDepthImage(compressedDepthPointer, depthImageMessage, depthImagePublisher, sensorPose, now, depthSequenceNumber++,
                                                                    depthSensorSimulator.getImageHeight(), depthSensorSimulator.getImageWidth(), 0.001f);
 
          });

ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXHeightMapRenderer.java

@@ -20,7 +20,6 @@
 import us.ihmc.rdx.shader.RDXShader;
 import us.ihmc.rdx.shader.RDXUniform;
 
-import java.nio.FloatBuffer;
 
 /**
  * Renders a height map as a point cloud. The height map is stored as a 16-bit grayscale image.
@@ -29,13 +28,6 @@
  * 0 is the metric -3.2768f height, and 65536 is the 3.2768f height.
  * The height is scaled up by 10,000 for storage as 16-bit value (short)
  */
-
-/**
- * This has been updated to use {@link us.ihmc.rdx.ui.graphics.RDXHeightMapGraphicNew}, please use that going forward, this implementation has bugs with
- * interacting with collisions
- * from the mouse
- */
-@Deprecated
 public class RDXHeightMapRenderer implements RenderableProvider
 {
    private Renderable renderable;
@@ -142,11 +134,6 @@ public void update(RigidBodyTransform zUpFrameToWorld,
       renderable.meshPart.mesh.setVertices(intermediateVertexBuffer, 0, totalCells * FLOATS_PER_CELL);
    }
 
-   public FloatBuffer getVertexBuffer()
-   {
-      return renderable.meshPart.mesh.getVerticesBuffer();
-   }
-
    @Override
    public void getRenderables(Array<Renderable> renderables, Pool<Renderable> pool)
    {