General cleanup from PR comments

Author: PotatoPeeler3000

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

@@ -13,6 +13,7 @@
 import perception_msgs.msg.dds.HeightMapMessage;
 import perception_msgs.msg.dds.HeightMapMessagePubSubType;
 import us.ihmc.idl.serializers.extra.JSONSerializer;
+import us.ihmc.log.LogTools;
 import us.ihmc.perception.heightMap.HeightMapLogReader;
 import us.ihmc.rdx.imgui.RDXPanel;
 import us.ihmc.rdx.sceneManager.RDXSceneLevel;
@@ -26,11 +27,11 @@
 
 public class RDXHeightMapPanel extends RDXPanel implements RenderableProvider
 {
-   RDXROS2HeightMapVisualizer heightMapVisualizer;
+   private final RDXROS2HeightMapVisualizer heightMapVisualizer;
 
    private final HeightMapMessage heightMapMessage = new HeightMapMessage();
    private HeightMapLogReader logReader;
-   int currentFrameIndex = 0;
+   private int currentFrameIndex = 0;
 
    private boolean isPlaying = false;
    private final float[] playbackSpeedSeconds = new float[] {0.1f};
@@ -66,7 +67,7 @@ public void renderImGuiWidgets()
          if (ImGuiFileDialog.isOk())
          {
             String selectedFile = ImGuiFileDialog.getFilePathName();
-            System.out.println("Selected file: " + selectedFile);
+            LogTools.info("Selected file: " + selectedFile);
 
             if (selectedFile.endsWith(".json"))
             {
@@ -237,6 +238,14 @@ public void getRenderablesFull(Array<Renderable> renderables, Pool<Renderable> p
    public void destroy()
    {
       heightMapVisualizer.destroy();
+      try
+      {
+         logReader.close();
+      }
+      catch (IOException e)
+      {
+         e.printStackTrace();
+      }
    }
 
    @Override

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

@@ -34,21 +34,15 @@ public class ContinuousHikingProcess
    private final EnvironmentHandler environmentHandler = new EnvironmentHandler();
    private final ActiveMappingParameterToolBox activeMappingParameterToolBox;
    private final ContinuousPlannerSchedulingTask continuousPlannerSchedulingTask;
-
    private final SnappingTerrainManager snappingTerrainManager;
-
    private final RapidHeightMapThread rapidHeightMapThread;
-   @org.jetbrains.annotations.NotNull
-   private final ROS2ImageSensors ros2ImageSensors;
 
    public ContinuousHikingProcess(DRCRobotModel robotModel,
                                   RobotCollisionModel robotCollisionModel,
                                   ROS2Node ros2Node,
                                   ROS2ImageSensors ros2ImageSensors,
                                   ROS2SyncedRobotModel ros2SyncedRobot)
    {
-      this.ros2ImageSensors = ros2ImageSensors;
-
       // Create a bunch of overhead for the ROS2 communication and the robot
       ros2SyncedRobot.initializeToDefaultRobotInitialSetup(0.0, 0.0, 0.0, 0.0);
       ControllerFootstepQueueMonitor controllerFootstepQueueMonitor = new ControllerFootstepQueueMonitor(ros2Node, robotModel.getSimpleRobotName());
@@ -118,7 +112,6 @@ public void update()
 
    public void destroy()
    {
-      ros2ImageSensors.destroy();
       rapidHeightMapThread.blockingKill();
       continuousPlannerSchedulingTask.destroy();
       snappingTerrainManager.close();

ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java

@@ -149,8 +149,9 @@ private void publishHeightMap(Mat globalHeightMap, Point3D heightMapCenter)
          {
             throw new RuntimeException(e);
          }
-         catch (IOException ignored)
+         catch (IOException e)
          {
+            e.printStackTrace();
          }
 
          try

ihmc-perception/src/main/java/us/ihmc/perception/heightMap/HeightMapMessageTools.java

@@ -46,6 +46,9 @@ public static Mat unpackMessageToMat(HeightMapMessage heightMapMessage)
       Mat heightMap = new Mat(cellsPerAxis, cellsPerAxis, opencv_core.CV_16UC1);
       ShortBuffer shortBuffer = heightMap.createBuffer();
 
+      int totalCells = cellsPerAxis * cellsPerAxis;
+      short[] heights = new short[totalCells];
+
       for (int i = 0; i < heightMapMessage.getHeights().size(); i++)
       {
          short height = (short) heightMapMessage.getHeights().get(i);
@@ -55,9 +58,11 @@ public static Mat unpackMessageToMat(HeightMapMessage heightMapMessage)
          int yIndex = key / cellsPerAxis;
 
          int index = yIndex * cellsPerAxis + xIndex;
-         shortBuffer.put(index, height);
+         heights[index] = height;
       }
 
+      shortBuffer.put(heights);
+
       return heightMap;
    }
 
@@ -93,7 +98,7 @@ public static void toMessage(HeightMapData heightMapData, HeightMapMessage messa
       for (int i = 0; i < heightMapData.getNumberOfOccupiedCells() && i < messageToPack.getKeys().capacity(); i++)
       {
          int key = heightMapData.getKey(i);
-         messageToPack.getKeys().add((short) key);
+         messageToPack.getKeys().add((key));
          messageToPack.getHeights().add((short) heightMapData.getHeightAt(key));
       }
    }

ihmc-perception/src/main/java/us/ihmc/perception/heightMap/HeightMapUpdater.java

@@ -1,11 +1,8 @@
 package us.ihmc.perception.heightMap;
 
 import com.google.common.util.concurrent.AtomicDouble;
-import gnu.trove.list.TShortList;
-import gnu.trove.list.array.TShortArrayList;
 import perception_msgs.msg.dds.HeightMapMessage;
 import perception_msgs.msg.dds.HeightMapMessagePubSubType;
-import gnu.trove.list.array.TFloatArrayList;
 import gnu.trove.list.array.TIntArrayList;
 import us.ihmc.commons.MathTools;
 import us.ihmc.euclid.tuple2D.interfaces.Point2DBasics;

ihmc-perception/src/test/java/us/ihmc/perception/heightmap/HeightMapMessageToolsTest.java

@@ -14,7 +14,7 @@
 
 public class HeightMapMessageToolsTest
 {
-   private final int iterations = 10000;
+   private final int iterations = 1000;
 
    @Test
    public void testHeightMapMessaging()
@@ -31,7 +31,7 @@ public void testHeightMapMessaging()
       {
          for (int j = 0; j < cellsPerAxis; j++)
          {
-            heightMap.ptr(i, j).putInt(i + j);
+            heightMap.ptr(i, j).putShort((short) (i + j));
          }
       }