diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXGPUHeightMapBodyPathPlanningDemo.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXGPUHeightMapBodyPathPlanningDemo.java index 837cdb35409..baa533d6e43 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXGPUHeightMapBodyPathPlanningDemo.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXGPUHeightMapBodyPathPlanningDemo.java @@ -35,7 +35,7 @@ import us.ihmc.rdx.ui.affordances.RDXInteractableReferenceFrame; import us.ihmc.rdx.ui.gizmo.RDXSelectablePose3DGizmo; import us.ihmc.rdx.ui.graphics.RDXBodyPathPlanGraphic; -import us.ihmc.rdx.ui.graphics.RDXHeightMapGraphicNew; +import us.ihmc.rdx.ui.graphics.RDXGridMapGraphic; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.sensorProcessing.heightMap.HeightMapData; @@ -55,7 +55,7 @@ public class RDXGPUHeightMapBodyPathPlanningDemo private RDXInteractableReferenceFrame robotInteractableReferenceFrame; private RDXSelectablePose3DGizmo ousterPoseGizmo; private RDXEnvironmentBuilder environmentBuilder; - private RDXHeightMapGraphicNew heightMapGraphic; + private RDXGridMapGraphic heightMapGraphic; private SimpleGPUHeightMapParameters simpleGPUHeightMapParameters; private SimpleGPUHeightMapUpdater simpleGPUHeightMapUpdater; private RosMainNode ros1Node; @@ -138,7 +138,7 @@ public void create() heightMapUpdater = new AlternateHeightMapUpdater(); } - heightMapGraphic = new RDXHeightMapGraphicNew(); + heightMapGraphic = new RDXGridMapGraphic(); baseUI.getPrimaryScene().addRenderableProvider(heightMapGraphic, RDXSceneLevel.VIRTUAL); baseUI.getImGuiPanelManager().addPanel("Height Map", this::renderHeightMapImGuiWidgets); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXGlobalHeightMapGraphic.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXGlobalHeightMapGraphic.java index 325446d62c0..7b2e368f1b3 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXGlobalHeightMapGraphic.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXGlobalHeightMapGraphic.java @@ -5,46 +5,38 @@ import com.badlogic.gdx.utils.IntMap; import com.badlogic.gdx.utils.Pool; import perception_msgs.msg.dds.HeightMapMessage; -import us.ihmc.sensorProcessing.globalHeightMap.GlobalHeightMap; -import us.ihmc.sensorProcessing.globalHeightMap.GlobalMapTile; -import us.ihmc.sensorProcessing.heightMap.HeightMapData; -import us.ihmc.sensorProcessing.heightMap.HeightMapMessageTools; import us.ihmc.tools.thread.MissingThreadTools; import us.ihmc.tools.thread.ResettableExceptionHandlingExecutorService; -import java.util.ArrayList; -import java.util.Collection; -import java.util.HashMap; - /** * Creates a graphic for the GPU Height Map to be visualized in the RDX UI. Each height value from the height map is turned into a 2cm polygon that is then * visualized on the UI. The height value location will be in the center of the 2cm polygon that is visualized. */ public class RDXGlobalHeightMapGraphic implements RenderableProvider { - private final IntMap globalMapRenderables = new IntMap<>(); + private final IntMap globalMapRenderables = new IntMap<>(); private final ResettableExceptionHandlingExecutorService executorService = MissingThreadTools.newSingleThreadExecutor(getClass().getSimpleName(), true, 1); public void update() { - for (RDXHeightMapGraphicNew mapRenderables : globalMapRenderables.values()) + for (RDXGridMapGraphic mapRenderables : globalMapRenderables.values()) mapRenderables.update(); } public void generateMeshesAsync(HeightMapMessage heightMapMessage, int tileKey) { - RDXHeightMapGraphicNew graphic = getOrCreateHeightMapGraphic(tileKey); + RDXGridMapGraphic graphic = getOrCreateHeightMapGraphic(tileKey); graphic.generateMeshesAsync(heightMapMessage); } - private RDXHeightMapGraphicNew getOrCreateHeightMapGraphic(int tileKey) + private RDXGridMapGraphic getOrCreateHeightMapGraphic(int tileKey) { - RDXHeightMapGraphicNew graphic = globalMapRenderables.get(tileKey); + RDXGridMapGraphic graphic = globalMapRenderables.get(tileKey); if (graphic == null) { - graphic = new RDXHeightMapGraphicNew(); + graphic = new RDXGridMapGraphic(); globalMapRenderables.put(tileKey, graphic); } @@ -54,14 +46,14 @@ private RDXHeightMapGraphicNew getOrCreateHeightMapGraphic(int tileKey) @Override public void getRenderables(Array renderables, Pool pool) { - for (RDXHeightMapGraphicNew mapRenderables : globalMapRenderables.values()) + for (RDXGridMapGraphic mapRenderables : globalMapRenderables.values()) mapRenderables.getRenderables(renderables, pool); } public void destroy() { executorService.destroy(); - for (RDXHeightMapGraphicNew mapRenderables : globalMapRenderables.values()) + for (RDXGridMapGraphic mapRenderables : globalMapRenderables.values()) mapRenderables.destroy(); } } diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXGridMapGraphic.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXGridMapGraphic.java index 707f48d7959..1565c4a178d 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXGridMapGraphic.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXGridMapGraphic.java @@ -31,10 +31,6 @@ import java.util.function.IntFunction; import java.util.function.IntToDoubleFunction; -/** - * This class has been replaced with a newer implementation called {@link RDXHeightMapGraphicNew}, please use that going forward and use this one only for - * reference - */ @Deprecated public class RDXGridMapGraphic implements RenderableProvider { @@ -59,10 +55,6 @@ public class RDXGridMapGraphic implements RenderableProvider private static final double zForInPainting = 0.95; - public void clear() - { - } - public void update() { createModelFromMeshBuilders(); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXHeightMapGraphicNew.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXHeightMapGraphicNew.java deleted file mode 100644 index b7fc4687c87..00000000000 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXHeightMapGraphicNew.java +++ /dev/null @@ -1,232 +0,0 @@ -package us.ihmc.rdx.ui.graphics; - -import com.badlogic.gdx.graphics.Color; -import com.badlogic.gdx.graphics.Mesh; -import com.badlogic.gdx.graphics.Texture; -import com.badlogic.gdx.graphics.g3d.*; -import com.badlogic.gdx.graphics.g3d.attributes.ColorAttribute; -import com.badlogic.gdx.graphics.g3d.attributes.TextureAttribute; -import com.badlogic.gdx.graphics.g3d.model.MeshPart; -import com.badlogic.gdx.graphics.g3d.utils.MeshBuilder; -import com.badlogic.gdx.graphics.g3d.utils.ModelBuilder; -import com.badlogic.gdx.utils.Array; -import com.badlogic.gdx.utils.Pool; -import net.mgsx.gltf.scene3d.attributes.PBRColorAttribute; -import net.mgsx.gltf.scene3d.attributes.PBRTextureAttribute; -import org.lwjgl.opengl.GL41; -import perception_msgs.msg.dds.HeightMapMessage; -import us.ihmc.euclid.transform.RigidBodyTransform; -import us.ihmc.euclid.tuple3D.Point3D; -import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; -import us.ihmc.rdx.mesh.RDXMultiColorMeshBuilder; -import us.ihmc.sensorProcessing.heightMap.HeightMapTools; -import us.ihmc.tools.thread.MissingThreadTools; -import us.ihmc.tools.thread.ResettableExceptionHandlingExecutorService; - -import java.util.ArrayList; -import java.util.Arrays; -import java.util.List; -import java.util.concurrent.atomic.AtomicReference; -import java.util.function.IntFunction; -import java.util.function.IntToDoubleFunction; - -/** - * Creates a graphic for the GPU Height Map to be visualized in the RDX UI. Each height value from the height map is turned into a 2cm polygon that is then - * visualized on the UI. The height value location will be in the center of the 2cm polygon that is visualized. - */ -public class RDXHeightMapGraphicNew implements RenderableProvider -{ - private static final int MAX_INDEX_MESH_BUILDER = MeshBuilder.MAX_INDEX / 4; // To prevent over flowing, stay far away from MAX_INDEX - - private final ResettableExceptionHandlingExecutorService executorService = MissingThreadTools.newSingleThreadExecutor(getClass().getSimpleName(), true, 1); - private final ModelBuilder modelBuilder = new ModelBuilder(); - private final RigidBodyTransform transformToWorld = new RigidBodyTransform(); - - private final AtomicReference> latestMeshBuilder = new AtomicReference<>(null); - private final AtomicReference latestModel = new AtomicReference<>(null); - - private Model lastModel; - private Texture paletteTexture = null; - - public void update() - { - createModelFromMeshBuilders(); - } - - public void generateMeshesAsync(HeightMapMessage heightMapMessage) - { - executorService.clearQueueAndExecute(() -> generateMeshes(heightMapMessage)); - } - - public void generateMeshes(HeightMapMessage heightMapMessage) - { - IntToDoubleFunction heightProvider = (d) -> (double) heightMapMessage.getHeights().get(d); - IntFunction keyProvider = (d) -> heightMapMessage.getKeys().get(d); - - generateMeshes(heightProvider, - keyProvider, - heightMapMessage.getHeights().size(), - heightMapMessage.getXyResolution(), - heightMapMessage.getGridSizeXy(), - heightMapMessage.getGridCenterX(), - heightMapMessage.getGridCenterY(), - heightMapMessage.getEstimatedGroundHeight()); - } - - private void generateMeshes(IntToDoubleFunction heightsProvider, - IntFunction keysProvider, - int numberOfOccupiedCells, - double gridResolutionXY, - double gridSizeXy, - double gridCenterX, - double gridCenterY, - double groundHeight) - { - List meshBuilders = generateHeightCells(heightsProvider, - keysProvider, - numberOfOccupiedCells, - gridResolutionXY, - gridSizeXy, - gridCenterX, - gridCenterY, - groundHeight); - latestMeshBuilder.set(meshBuilders); - } - - private static List generateHeightCells(IntToDoubleFunction heightsProvider, - IntFunction keysProvider, - int numberOfOccupiedCells, - double gridResolutionXY, - double gridSizeXy, - double gridCenterX, - double gridCenterY, - double groundHeight) - { - List meshBuilders = new ArrayList<>(); - - int centerIndex = HeightMapTools.computeCenterIndex(gridSizeXy, gridResolutionXY); - int cellsPerAxis = 2 * centerIndex + 1; - - Point3D[][] vertices = new Point3D[cellsPerAxis][]; - for (int i = 0; i < cellsPerAxis; i++) - { - vertices[i] = new Point3D[cellsPerAxis]; - } - - for (int i = 0; i < numberOfOccupiedCells; i++) - { - int key = keysProvider.apply(i); - int xIndex = HeightMapTools.keyToXIndex(key, centerIndex); - int yIndex = HeightMapTools.keyToYIndex(key, centerIndex); - double x = HeightMapTools.indexToCoordinate(xIndex, gridCenterX, gridResolutionXY, centerIndex); - double y = HeightMapTools.indexToCoordinate(yIndex, gridCenterY, gridResolutionXY, centerIndex); - double height = heightsProvider.applyAsDouble(i); - - vertices[xIndex][yIndex] = new Point3D(x, y, height); - } - - // Fill in all the vertices for the mesh texture - for (int xIndex = 0; xIndex < cellsPerAxis; xIndex++) - { - for (int yIndex = 0; yIndex < cellsPerAxis; yIndex++) - { - if (vertices[xIndex][yIndex] == null) - { - double x = HeightMapTools.indexToCoordinate(xIndex, gridCenterX, gridResolutionXY, centerIndex); - double y = HeightMapTools.indexToCoordinate(yIndex, gridCenterY, gridResolutionXY, centerIndex); - vertices[xIndex][yIndex] = new Point3D(x, y, groundHeight); - } - } - } - - RDXMultiColorMeshBuilder meshBuilder = new RDXMultiColorMeshBuilder(); - int currentIndexForMeshBuilder = 0; // This is used to prevent us from over filling a MeshBuilder - - for (int xIndex = 0; xIndex < cellsPerAxis; xIndex++) - { - for (int yIndex = 0; yIndex < cellsPerAxis; yIndex++) - { - Point3D topLeft = new Point3D(); - Point3D topRight = new Point3D(); - Point3D bottomLeft = new Point3D(); - Point3D bottomRight = new Point3D(); - - // Take each vertex and create a 2cm polygon where the vertex is in the center - double halfWidth = gridResolutionXY / 2.0; - Point3DReadOnly vertexCenter = vertices[xIndex][yIndex]; - topLeft.set(vertexCenter.getX() + halfWidth, vertexCenter.getY() + halfWidth, vertexCenter.getZ()); - topRight.set(vertexCenter.getX() - halfWidth, vertexCenter.getY() + halfWidth, vertexCenter.getZ()); - bottomLeft.set(vertexCenter.getX() - halfWidth, vertexCenter.getY() - halfWidth, vertexCenter.getZ()); - bottomRight.set(vertexCenter.getX() + halfWidth, vertexCenter.getY() - halfWidth, vertexCenter.getZ()); - - double[] redGreenBlue = HeightMapTools.getRedGreenBlue(vertexCenter.getZ()); - Color color = new Color((float) redGreenBlue[0], (float) redGreenBlue[1], (float) redGreenBlue[2], 1.0f); - - meshBuilder.addPolygon(Arrays.asList(topLeft, topRight, bottomLeft, bottomRight), color); - currentIndexForMeshBuilder++; - - // Create a new MeshBuilder if this one gets filled up - if (currentIndexForMeshBuilder >= MAX_INDEX_MESH_BUILDER) - { - meshBuilders.add(meshBuilder); - meshBuilder = new RDXMultiColorMeshBuilder(); - currentIndexForMeshBuilder = 0; - } - } - } - meshBuilders.add(meshBuilder); - - return meshBuilders; - } - - private void createModelFromMeshBuilders() - { - List meshBuilders = latestMeshBuilder.getAndSet(null); - - if (meshBuilders == null) - return; - - modelBuilder.begin(); - Material material = new Material(); - if (paletteTexture == null) - paletteTexture = RDXMultiColorMeshBuilder.loadPaletteTexture(); - material.set(PBRTextureAttribute.createBaseColorTexture(paletteTexture)); - material.set(PBRColorAttribute.createBaseColorFactor(new Color(0.7f, 0.7f, 0.7f, 1.0f))); - - for (int i = 0; i < meshBuilders.size(); i++) - { - Mesh mesh = meshBuilders.get(i).generateMesh(); - MeshPart meshPart = new MeshPart("xyz", mesh, 0, mesh.getNumIndices(), GL41.GL_TRIANGLES); - modelBuilder.part(meshPart, material); - } - - if (lastModel != null) - lastModel.dispose(); - - lastModel = modelBuilder.end(); - ModelInstance modelInstance = new ModelInstance(lastModel); // TODO: Clean up garbage and look into reusing the Model - - latestModel.set(modelInstance); - } - - @Override - public void getRenderables(Array renderables, Pool pool) - { - ModelInstance modelInstance = latestModel.get(); - // Sync over current and add - if (modelInstance != null) - { - modelInstance.getRenderables(renderables, pool); - } - } - - public void destroy() - { - executorService.destroy(); - } - - public RigidBodyTransform getTransformToWorld() - { - return transformToWorld; - } -} \ No newline at end of file diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXSteppableRegionGraphic.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXSteppableRegionGraphic.java index 66bf53000b6..5f5dcc1028c 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXSteppableRegionGraphic.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXSteppableRegionGraphic.java @@ -36,9 +36,6 @@ public class RDXSteppableRegionGraphic implements RenderableProvider public void clear() { - for (int i = 0; i < gridMapGraphics.size(); i++) - gridMapGraphics.get(i).clear(); - regionGraphics.clear(); gridMapGraphics.clear(); } diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2HeightMapVisualizer.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2HeightMapVisualizer.java index 9ff7f37c035..5f5b703614f 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2HeightMapVisualizer.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2HeightMapVisualizer.java @@ -10,7 +10,6 @@ import org.bytedeco.opencv.opencv_core.Mat; import perception_msgs.msg.dds.GlobalMapMessage; import perception_msgs.msg.dds.GlobalMapTileMessage; -import perception_msgs.msg.dds.HeightMapMessage; import perception_msgs.msg.dds.ImageMessage; import us.ihmc.communication.PerceptionAPI; import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; @@ -24,11 +23,9 @@ import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap; import us.ihmc.rdx.sceneManager.RDXSceneLevel; import us.ihmc.rdx.ui.graphics.RDXGlobalHeightMapGraphic; -import us.ihmc.rdx.ui.graphics.RDXHeightMapGraphicNew; import us.ihmc.ros2.ROS2Topic; import us.ihmc.sensorProcessing.globalHeightMap.GlobalLattice; import us.ihmc.sensorProcessing.heightMap.HeightMapData; -import us.ihmc.sensorProcessing.heightMap.HeightMapMessageTools; import us.ihmc.tools.thread.MissingThreadTools; import us.ihmc.tools.thread.ResettableExceptionHandlingExecutorService; @@ -41,21 +38,17 @@ public class RDXROS2HeightMapVisualizer extends RDXROS2MultiTopicVisualizer private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass()); private final RDXHeightMapRenderer heightMapRenderer = new RDXHeightMapRenderer(); - private final RDXHeightMapGraphicNew heightMapGraphicNew = new RDXHeightMapGraphicNew(); private final RDXGlobalHeightMapGraphic globalHeightMapGraphic = new RDXGlobalHeightMapGraphic(); private final ResettableExceptionHandlingExecutorService executorService; - private final ImBoolean enableHeightMapVisualizer = new ImBoolean(false); private final ImBoolean enableGlobalHeightMapVisualizer = new ImBoolean(false); private final ImBoolean enableHeightMapRenderer = new ImBoolean(true); - private final ImBoolean displayGlobalHeightMapImage = new ImBoolean(false); private final RigidBodyTransform zUpToWorldTransform = new RigidBodyTransform(); private final TerrainMapData terrainMapData = new TerrainMapData(RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), RapidHeightMapManager.getHeightMapParameters().getCropWindowSize()); private ROS2PublishSubscribeAPI ros2; - private HeightMapMessage latestHeightMapMessage = new HeightMapMessage(); private HeightMapData latestHeightMapData; private Mat heightMapImage; private Mat compressedBytesMat; @@ -80,12 +73,6 @@ public List> getTopics() return List.of(PerceptionAPI.HEIGHT_MAP_OUTPUT, PerceptionAPI.HEIGHT_MAP_CROPPED); } - public void setupForHeightMapMessage(ROS2PublishSubscribeAPI ros2) - { - this.ros2 = ros2; - ros2.subscribeViaCallback(PerceptionAPI.HEIGHT_MAP_OUTPUT, this::acceptHeightMapMessage); - } - public void setupForImageMessage(ROS2PublishSubscribeAPI ros2) { this.ros2 = ros2; @@ -106,14 +93,6 @@ public void create() heightMapRenderer.create(cellsPerAxis * cellsPerAxis); } - public void acceptHeightMapMessage(HeightMapMessage heightMapMessage) - { - if (isActive()) - { - updateGridMapGraphic(heightMapMessage); - } - } - public void acceptGlobalMapMessage(GlobalMapMessage globalMapMessage) { if (enableGlobalHeightMapVisualizer.get()) @@ -134,19 +113,6 @@ public void acceptGlobalMapTileMessage(GlobalMapTileMessage globalMapTileMessage } } - private void updateGridMapGraphic(HeightMapMessage heightMapMessage) - { - executorService.clearQueueAndExecute(() -> - { - if (enableHeightMapVisualizer.get()) - { - heightMapGraphicNew.generateMeshesAsync(heightMapMessage); - } - }); - - getFrequency(PerceptionAPI.HEIGHT_MAP_OUTPUT).ping(); - } - public void acceptImageMessage(ImageMessage imageMessage) { if (isActive()) @@ -180,19 +146,6 @@ public void acceptImageMessage(ImageMessage imageMessage) incomingCompressedImageBuffer, incomingCompressedImageBytePointer, compressedBytesMat); - - if (!heightMapMessageGenerated) - { - PerceptionMessageTools.convertToHeightMapData(heightMapImage, - latestHeightMapData, - imageMessage.getPosition(), - (float) RapidHeightMapManager.getHeightMapParameters() - .getGlobalWidthInMeters(), - (float) RapidHeightMapManager.getHeightMapParameters() - .getGlobalCellSizeInMeters()); - latestHeightMapMessage = HeightMapMessageTools.toMessage(latestHeightMapData); - heightMapMessageGenerated = true; - } }); } @@ -217,10 +170,8 @@ public void renderImGuiWidgets() if (ImGui.collapsingHeader(labels.get("Visualization Options"))) { - ImGui.checkbox(labels.get("Enable Height Map Visualizer"), enableHeightMapVisualizer); ImGui.checkbox(labels.get("Enable Global Height Map Visualizer"), enableGlobalHeightMapVisualizer); ImGui.checkbox(labels.get("Enable Height Map Renderer"), enableHeightMapRenderer); - ImGui.checkbox(labels.get("Display Global Height Map Image"), displayGlobalHeightMapImage); } } @@ -232,17 +183,11 @@ public void update() if (heightMapMessageGenerated) { heightMapMessageGenerated = false; - updateGridMapGraphic(latestHeightMapMessage); } boolean isActive = isActive(); - if (isActive && enableHeightMapVisualizer.get()) - { - heightMapGraphicNew.update(); - } if (enableGlobalHeightMapVisualizer.get()) { - heightMapGraphicNew.update(); globalHeightMapGraphic.update(); } @@ -267,15 +212,9 @@ public void getRenderables(Array renderables, Pool pool, { if (isActive() && sceneLevelCheck(sceneLevels)) { - if (enableHeightMapVisualizer.get()) - { - heightMapGraphicNew.getRenderables(renderables, pool); - } - if (enableGlobalHeightMapVisualizer.get()) { globalHeightMapGraphic.getRenderables(renderables, pool); - heightMapGraphicNew.getRenderables(renderables, pool); } if (enableHeightMapRenderer.get()) @@ -290,7 +229,6 @@ public void destroy() { super.destroy(); executorService.destroy(); - heightMapGraphicNew.destroy(); globalHeightMapGraphic.destroy(); } @@ -305,9 +243,4 @@ public TerrainMapData getTerrainMapData() terrainMapData.setSensorOrigin(zUpToWorldTransform.getTranslation().getX(), zUpToWorldTransform.getTranslation().getY()); return terrainMapData; } - - public ImBoolean getDisplayGlobalHeightMapImage() - { - return displayGlobalHeightMapImage; - } } diff --git a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXHeightMapExtractionDemo.java b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXHeightMapExtractionDemo.java deleted file mode 100644 index 3b68ae22cae..00000000000 --- a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXHeightMapExtractionDemo.java +++ /dev/null @@ -1,293 +0,0 @@ -package us.ihmc.rdx.perception; - -import imgui.ImGui; -import imgui.type.ImBoolean; -import imgui.type.ImFloat; -import imgui.type.ImInt; -import org.bytedeco.javacpp.BytePointer; -import org.bytedeco.opencl.global.OpenCL; -import org.bytedeco.opencv.global.opencv_core; -import us.ihmc.commons.thread.Notification; -import us.ihmc.euclid.geometry.Pose3D; -import us.ihmc.euclid.referenceFrame.FramePose3D; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.transform.RigidBodyTransform; -import us.ihmc.euclid.tuple3D.Point3D; -import us.ihmc.euclid.tuple4D.Quaternion; -import us.ihmc.log.LogTools; -import us.ihmc.perception.BytedecoImage; -import us.ihmc.perception.depthData.PointCloudData; -import us.ihmc.perception.heightMap.HeightMapInputData; -import us.ihmc.perception.heightMap.HeightMapUpdater; -import us.ihmc.perception.logging.PerceptionDataLoader; -import us.ihmc.perception.logging.PerceptionLoggerConstants; -import us.ihmc.perception.opencl.OpenCLManager; -import us.ihmc.perception.tools.PerceptionDataTools; -import us.ihmc.rdx.Lwjgl3ApplicationAdapter; -import us.ihmc.rdx.imgui.RDXPanel; -import us.ihmc.rdx.tools.LibGDXTools; -import us.ihmc.rdx.tools.RDXModelBuilder; -import us.ihmc.rdx.tools.RDXModelInstance; -import us.ihmc.rdx.ui.RDXBaseUI; -import us.ihmc.rdx.ui.RDXStoredPropertySetTuner; -import us.ihmc.rdx.ui.graphics.ros2.RDXROS2HeightMapVisualizer; -import us.ihmc.robotics.referenceFrames.PoseReferenceFrame; -import us.ihmc.tools.IHMCCommonPaths; -import us.ihmc.tools.thread.MissingThreadTools; -import us.ihmc.tools.thread.ResettableExceptionHandlingExecutorService; - -import java.nio.FloatBuffer; -import java.time.Instant; -import java.util.ArrayList; - -public class RDXHeightMapExtractionDemo -{ - private final String perceptionLogFile = IHMCCommonPaths.PERCEPTION_LOGS_DIRECTORY.resolve("20230117_161540_GoodPerceptionLog.hdf5").toString(); - - private final RDXBaseUI baseUI = new RDXBaseUI(); - private RDXPanel navigationPanel; - - private String sensorTopicName; - - private final ArrayList sensorPositionBuffer = new ArrayList<>(); - private final ArrayList sensorOrientationBuffer = new ArrayList<>(); - - private final HeightMapUpdater heightMapUpdater = new HeightMapUpdater(); - private final RDXROS2HeightMapVisualizer heightMapVisualizer = new RDXROS2HeightMapVisualizer("Height Map"); - private final RDXStoredPropertySetTuner heightMapParameters; - private final RDXStoredPropertySetTuner heightMapFitlerParameters; - - private final Notification userChangedIndex = new Notification(); - - private final ResettableExceptionHandlingExecutorService loadAndDecompressThreadExecutor = MissingThreadTools.newSingleThreadExecutor("LoadAndDecompress", - true, - 1); - - private final ImInt frameIndex = new ImInt(0); - private final ImFloat planeHeight = new ImFloat(1.5f); // 2.133f - private final ImBoolean autoAdvance = new ImBoolean(false); - - private final Pose3D previousPose = new Pose3D(); - private final Pose3D cameraPose = new Pose3D(); - private final PoseReferenceFrame cameraFrame = new PoseReferenceFrame("l515ReferenceFrame", ReferenceFrame.getWorldFrame()); - - private final Notification heightMapUpdateNotification = new Notification(); - - private BytedecoImage loadedDepthImage; - private final BytePointer depthBytePointer = new BytePointer(1000000); - private double translation = Double.NaN; - - private OpenCLManager openCLManager; - private PerceptionDataLoader perceptionDataLoader; - - private boolean initialized = false; - - public RDXHeightMapExtractionDemo() - { - perceptionDataLoader = new PerceptionDataLoader(); - previousPose.setToNaN(); - - heightMapParameters = new RDXStoredPropertySetTuner("heightMapParameters"); - heightMapFitlerParameters = new RDXStoredPropertySetTuner("Filter Parameters"); - - baseUI.launchRDXApplication(new Lwjgl3ApplicationAdapter() - { - @Override - public void create() - { - baseUI.create(); - - openCLManager = new OpenCLManager(); - - heightMapVisualizer.create(); - heightMapVisualizer.setActive(true); - heightMapUpdater.attachHeightMapConsumer(heightMapVisualizer::acceptHeightMapMessage); - - heightMapParameters.create(heightMapUpdater.getHeightMapParameters()); - heightMapFitlerParameters.create(heightMapUpdater.getHeightMapFilterParameters()); - - navigationPanel = new RDXPanel("Dataset Navigation Panel"); - baseUI.getImGuiPanelManager().addPanel(navigationPanel); - baseUI.getImGuiPanelManager().addPanel(heightMapParameters); - baseUI.getImGuiPanelManager().addPanel(heightMapFitlerParameters); - - baseUI.getPrimaryScene().addRenderableProvider(heightMapVisualizer); - - createForOuster(128, 2048); - - updateHeightMap(); - - // testProjection(loadedDepthImage.getBytedecoOpenCVMat()); - - navigationPanel.setRenderMethod(this::renderNavigationPanel); - } - - private void createForOuster(int depthHeight, int depthWidth) - { - sensorTopicName = PerceptionLoggerConstants.OUSTER_DEPTH_NAME; - perceptionDataLoader.openLogFile(perceptionLogFile); - - loadedDepthImage = new BytedecoImage(depthWidth, depthHeight, opencv_core.CV_16UC1); - - perceptionDataLoader.loadPoint3DList(PerceptionLoggerConstants.OUSTER_SENSOR_POSITION, sensorPositionBuffer); - perceptionDataLoader.loadQuaternionList(PerceptionLoggerConstants.OUSTER_SENSOR_ORIENTATION, sensorOrientationBuffer); - - perceptionDataLoader.loadCompressedDepth(PerceptionLoggerConstants.OUSTER_DEPTH_NAME, - frameIndex.get(), - depthBytePointer, - loadedDepthImage.getBytedecoOpenCVMat()); - loadedDepthImage.createOpenCLImage(openCLManager, OpenCL.CL_MEM_READ_ONLY); - } - - @Override - public void render() - { - if (autoAdvance.get()) - { - frameIndex.set(frameIndex.get() + 1); - userChangedIndex.set(); - if (frameIndex.get() == (perceptionDataLoader.getHDF5Manager().getCount(sensorTopicName) - 1)) - autoAdvance.set(false); - } - - if (userChangedIndex.poll()) - { - loadAndDecompressThreadExecutor.clearQueueAndExecute(() -> perceptionDataLoader.loadCompressedDepth(sensorTopicName, - frameIndex.get(), - depthBytePointer, - loadedDepthImage.getBytedecoOpenCVMat())); - updateHeightMap(); - } - - baseUI.renderBeforeOnScreenUI(); - baseUI.renderEnd(); - } - - private void renderNavigationPanel() - { - boolean changed = ImGui.sliderInt("Frame Index", frameIndex.getData(), 0, (perceptionDataLoader.getHDF5Manager().getCount(sensorTopicName) - 1)); - - changed |= ImGui.sliderFloat("Plane Height", planeHeight.getData(), -3.0f, 3.0f); - if (ImGui.button("AutoAdvance")) - autoAdvance.set(true); - if (ImGui.button("Stop Advancing")) - autoAdvance.set(false); - - if (ImGui.button("Load Previous")) - { - frameIndex.set(Math.max(0, frameIndex.get() - 1)); - changed = true; - } - ImGui.sameLine(); - if (ImGui.button("Load Next")) - { - frameIndex.set(frameIndex.get() + 1); - changed = true; - } - - if (changed) - { - userChangedIndex.set(); - } - imgui.internal.ImGui.text("Distance " + translation); - - ImGui.separator(); - - heightMapVisualizer.renderImGuiWidgets(); - } - - @Override - public void dispose() - { - perceptionDataLoader.closeLogFile(); - openCLManager.destroy(); - baseUI.dispose(); - } - }); - } - - public void updateWithDataBuffer(ReferenceFrame groundFrame, - ReferenceFrame sensorFrame, - BytedecoImage depthImage, - int numberOfPoints, - Instant instant, - boolean isMoving) - { - - FramePose3D sensorPose = new FramePose3D(sensorFrame); - sensorPose.changeFrame(ReferenceFrame.getWorldFrame()); - Point3D gridCenter = new Point3D(sensorPose.getX(), sensorPose.getY(), 0.0); - FloatBuffer pointCloudInSensorFrame = PerceptionDataTools.convertSphericalDepthImageToPointCloudInSensorFrame(depthImage, Math.PI / 2.0, 2.0 * Math.PI); - PointCloudData pointCloudData = new PointCloudData(instant, numberOfPoints, pointCloudInSensorFrame); - HeightMapInputData inputData = new HeightMapInputData(); - inputData.pointCloud = pointCloudData; - inputData.gridCenter = gridCenter; - // submitting the world frame for the sensor pose, as that's the frame the data is in. - inputData.sensorPose = sensorPose; - // TODO add variance - if (isMoving) - { - inputData.verticalMeasurementVariance = heightMapUpdater.getHeightMapParameters().getSensorVarianceWhenMoving(); - } - else - { - inputData.verticalMeasurementVariance = heightMapUpdater.getHeightMapParameters().getSensorVarianceWhenStanding(); - } - - double size = isMoving ? 0.05 : 0.1; - RDXModelInstance coordinateFrame = new RDXModelInstance(RDXModelBuilder.createCoordinateFrameInstance(size)); - LibGDXTools.toLibGDX(new RigidBodyTransform(sensorPose), coordinateFrame.transform); - baseUI.getPrimaryScene().addRenderableProvider(coordinateFrame); - - heightMapUpdater.addPointCloudToQueue(inputData); - } - - private void updateHeightMap() - { - LogTools.info("Update Height Map: " + frameIndex.get()); - Point3D position = sensorPositionBuffer.get(frameIndex.get()); - Quaternion orientation = sensorOrientationBuffer.get(frameIndex.get()); - cameraPose.set(position, orientation); - cameraFrame.setPoseAndUpdate(cameraPose); - - boolean validTranslation = cameraPose.getTranslation().distance(previousPose.getTranslation()) < 5.0; - boolean validRotation = cameraPose.getRotation().distance(previousPose.getOrientation()) < 0.1; - if (previousPose.containsNaN() || validTranslation && validRotation) - { - translation = cameraPose.getTranslation().distance(previousPose.getTranslation()); - boolean isMoving = translation > 0.005; - updateWithDataBuffer(ReferenceFrame.getWorldFrame(), - cameraFrame, - loadedDepthImage, - loadedDepthImage.getImageHeight() * loadedDepthImage.getImageWidth(), - Instant.now(), - isMoving); - heightMapUpdater.runUpdateThread(); - - heightMapVisualizer.update(); - previousPose.set(cameraPose); - } - - // if (heightMapUpdateNotification.poll()) - // { - // heightMapRenderer.update(rapidHeightMapUpdater.getOutputHeightMapImage().getPointerForAccessSpeed(), - // rapidHeightMapUpdater.getGridLength(), - // rapidHeightMapUpdater.getGridWidth(), - // rapidHeightMapUpdater.getCellSizeXYInMeters()); - // - // rapidHeightMapUpdater.setModified(false); - // rapidHeightMapUpdater.setProcessing(false); - // - // PerceptionDebugTools.displayHeightMap("Output Height Map", - // rapidHeightMapUpdater.getOutputHeightMapImage().getBytedecoOpenCVMat(), - // 1, - // 1 / (0.3f + 0.20f * rapidHeightMapUpdater.getCellSizeXYInMeters())); - // } - - } - - public static void main(String[] args) - { - new RDXHeightMapExtractionDemo(); - } -} diff --git a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXHighLevelDepthSensorDemo.java b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXHighLevelDepthSensorDemo.java index e6749e1ebaf..0eed65ec483 100644 --- a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXHighLevelDepthSensorDemo.java +++ b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXHighLevelDepthSensorDemo.java @@ -15,7 +15,7 @@ import us.ihmc.rdx.ui.RDX3DPanel; import us.ihmc.rdx.ui.RDXBaseUI; import us.ihmc.rdx.ui.gizmo.RDXPose3DGizmo; -import us.ihmc.rdx.ui.graphics.RDXHeightMapGraphicNew; +import us.ihmc.rdx.ui.graphics.RDXGridMapGraphic; import us.ihmc.rdx.visualizers.RDXFrustumGraphic; import us.ihmc.perception.BytedecoImage; import us.ihmc.sensorProcessing.heightMap.HeightMapMessageTools; @@ -35,7 +35,7 @@ public class RDXHighLevelDepthSensorDemo private RDXFrustumGraphic frustumVisualizer; private RDXBytedecoImagePanel mainViewDepthPanel; private BytedecoImage image; - private RDXHeightMapGraphicNew heightMap; + private RDXGridMapGraphic heightMap; public RDXHighLevelDepthSensorDemo() { @@ -123,7 +123,7 @@ public void create() baseUI.getPrimaryScene().addRenderableProvider(box, RDXSceneLevel.MODEL); baseUI.getPrimaryScene().addRenderableProvider(box, RDXSceneLevel.GROUND_TRUTH); - heightMap = new RDXHeightMapGraphicNew(); + heightMap = new RDXGridMapGraphic(); HeightMapMessage heightMapMessage = new HeightMapMessage(); heightMapMessage.setXyResolution(0.1); heightMapMessage.setGridSizeXy(2.0); diff --git a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXRapidHeightMapExtractorCUDADemo.java b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXRapidHeightMapExtractorCUDADemo.java index 9532d552dfe..db628db6f67 100644 --- a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXRapidHeightMapExtractorCUDADemo.java +++ b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXRapidHeightMapExtractorCUDADemo.java @@ -12,7 +12,6 @@ import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools; import us.ihmc.euclid.transform.RigidBodyTransform; import us.ihmc.perception.camera.CameraIntrinsics; -import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractorCUDA; import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.tools.PerceptionMessageTools; @@ -22,7 +21,7 @@ import us.ihmc.rdx.simulation.sensors.RDXSensorSimulator; import us.ihmc.rdx.ui.RDXBaseUI; import us.ihmc.rdx.ui.gizmo.RDXPose3DGizmo; -import us.ihmc.rdx.ui.graphics.RDXHeightMapGraphicNew; +import us.ihmc.rdx.ui.graphics.RDXGridMapGraphic; import us.ihmc.robotics.referenceFrames.ZUpFrame; import us.ihmc.sensorProcessing.heightMap.HeightMapData; import us.ihmc.sensorProcessing.heightMap.HeightMapMessageTools; @@ -42,7 +41,7 @@ public class RDXRapidHeightMapExtractorCUDADemo private final RigidBodyTransform sensorToSensorZUp = new RigidBodyTransform(); private final RigidBodyTransform sensorZUpToWorld = new RigidBodyTransform(); private GpuMat heightMapImage; - private final RDXHeightMapGraphicNew heightMapGraphic = new RDXHeightMapGraphicNew(); + private final RDXGridMapGraphic heightMapGraphic = new RDXGridMapGraphic(); private final BytePointer compressedCroppedHeightMapPointer = new BytePointer(); private final ImageMessage croppedHeightMapImageMessage = new ImageMessage(); private final FramePose3D cameraPoseForHeightMap = new FramePose3D(); diff --git a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXSteppableRegionCalculatorDemo.java b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXSteppableRegionCalculatorDemo.java index 29d5d14d677..645bb8bd7b7 100644 --- a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXSteppableRegionCalculatorDemo.java +++ b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXSteppableRegionCalculatorDemo.java @@ -88,7 +88,7 @@ public void create() ros2Node.createSubscription2(PerceptionAPI.HEIGHT_MAP_OUTPUT, message -> { - heightMapVisualizer.acceptHeightMapMessage(message); + heightMapVisualizer.setupForImageMessage(ros2Helper); heightMapUI.acceptHeightMapMessage(message); steppableRegionsUpdater.submitLatestHeightMapMessage(message);