From c5dd8ed6dd3f294992f5ad4defc308b878f6d47f Mon Sep 17 00:00:00 2001 From: nkitchel Date: Thu, 1 May 2025 11:41:10 -0500 Subject: [PATCH 1/9] Split the autonomy height map thread and the continuous hiking height map thread --- ...> RapidHeightMapAutonomyUpdateThread.java} | 20 ++-- .../ihmc/perception/RapidHeightMapThread.java | 110 ++++++++++++++++++ .../StandAloneRealsenseProcess.java | 37 +++--- .../gpuHeightMap/RapidHeightMapManager.java | 54 +++++---- 4 files changed, 163 insertions(+), 58 deletions(-) rename ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/{RapidHeightMapUpdateThread.java => RapidHeightMapAutonomyUpdateThread.java} (78%) create mode 100644 ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapThread.java diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapAutonomyUpdateThread.java similarity index 78% rename from ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java rename to ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapAutonomyUpdateThread.java index cdde8f20b45..234751817cf 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapAutonomyUpdateThread.java @@ -14,7 +14,7 @@ import us.ihmc.sensorProcessing.heightMap.HeightMapParameters; import us.ihmc.sensors.ImageSensor; -public class RapidHeightMapUpdateThread extends RepeatingTaskThread +public class RapidHeightMapAutonomyUpdateThread extends RepeatingTaskThread { private final RapidHeightMapManager heightMapManager; @@ -25,15 +25,15 @@ public class RapidHeightMapUpdateThread extends RepeatingTaskThread private final ReferenceFrame zUpSensorFrame; private final int depthImageKey; - public RapidHeightMapUpdateThread(ROS2Node ros2Node, - ROS2SyncedRobotModel syncedRobotModel, - RobotCollisionModel robotCollisionModel, - ControllerFootstepQueueMonitor controllerFootstepQueueMonitor, - ImageSensor imageSensor, - int depthImageKey, - HeightMapParameters heightMapParameters) + public RapidHeightMapAutonomyUpdateThread(ROS2Node ros2Node, + ROS2SyncedRobotModel syncedRobotModel, + RobotCollisionModel robotCollisionModel, + ControllerFootstepQueueMonitor controllerFootstepQueueMonitor, + ImageSensor imageSensor, + int depthImageKey, + HeightMapParameters heightMapParameters) { - super(imageSensor.getSensorName() + RapidHeightMapUpdateThread.class.getSimpleName()); + super(imageSensor.getSensorName() + RapidHeightMapAutonomyUpdateThread.class.getSimpleName()); this.imageSensor = imageSensor; this.depthImageKey = depthImageKey; @@ -65,7 +65,7 @@ protected void runTask() // Update height map synchronized (heightMapLock) { - heightMapManager.update(depthImage, sensorFrame, zUpSensorFrame); + heightMapManager.updateAndPublishHeightMap(depthImage, sensorFrame, zUpSensorFrame); } depthImage.release(); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapThread.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapThread.java new file mode 100644 index 00000000000..112ded3eb42 --- /dev/null +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapThread.java @@ -0,0 +1,110 @@ +package us.ihmc.perception; + +import com.google.common.util.concurrent.ThreadFactoryBuilder; +import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; +import us.ihmc.log.LogTools; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; +import us.ihmc.perception.heightMap.TerrainMapData; +import us.ihmc.robotics.physics.RobotCollisionModel; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.ros2.ROS2Node; +import us.ihmc.sensorProcessing.heightMap.HeightMapData; +import us.ihmc.sensorProcessing.heightMap.HeightMapParameters; +import us.ihmc.sensors.ImageSensor; + +import java.util.concurrent.Executors; +import java.util.concurrent.ScheduledExecutorService; +import java.util.concurrent.ThreadFactory; +import java.util.concurrent.TimeUnit; + +public class RapidHeightMapThread +{ + private final RapidHeightMapManager heightMapManager; + private final Object heightMapLock = new Object(); + + private final ImageSensor imageSensor; + private final ReferenceFrame sensorFrame; + private final ReferenceFrame zUpSensorFrame; + private final int depthImageKey; + private ScheduledExecutorService scheduler; + + public RapidHeightMapThread(ROS2Node ros2Node, + ROS2SyncedRobotModel syncedRobotModel, + RobotCollisionModel robotCollisionModel, + ImageSensor imageSensor, + int depthImageKey, + ControllerFootstepQueueMonitor controllerFootstepQueueMonitor, + HeightMapParameters heightMapParameters) + { + this.imageSensor = imageSensor; + this.depthImageKey = depthImageKey; + + sensorFrame = syncedRobotModel.getReferenceFrames().getSteppingCameraFrame(); + zUpSensorFrame = syncedRobotModel.getReferenceFrames().getSteppingCameraZUpFrame(); + + ReferenceFrame leftFootFrame = syncedRobotModel.getReferenceFrames().getSoleFrame(RobotSide.LEFT); + ReferenceFrame rightFootFrame = syncedRobotModel.getReferenceFrames().getSoleFrame(RobotSide.LEFT); + + heightMapManager = new RapidHeightMapManager(ros2Node, + robotCollisionModel, + syncedRobotModel.getFullRobotModel(), + syncedRobotModel.getRobotModel().getSimpleRobotName(), + leftFootFrame, + rightFootFrame, + controllerFootstepQueueMonitor, + heightMapParameters); + } + + public void start() + { + // We create a ThreadFactory here so that when profiling the thread, there is a user-friendly name to identify it with + ThreadFactory threadFactory = new ThreadFactoryBuilder().setNameFormat("RemoteHeightMapUpdateThread").build(); + scheduler = Executors.newScheduledThreadPool(1, threadFactory); + scheduler.scheduleWithFixedDelay(this::update, 100, 10, TimeUnit.MILLISECONDS); + } + + public void update() + { + try + { + imageSensor.waitForGrab(); + RawImage depthImage = imageSensor.getImage(depthImageKey); + + // Update height map + synchronized (heightMapLock) + { + heightMapManager.updateAndPublishHeightMap(depthImage, sensorFrame, zUpSensorFrame); + } + + depthImage.release(); + } + catch (Exception e) + { + LogTools.error(e); + } + } + + public HeightMapData getLatestHeightMapData() + { + synchronized (heightMapLock) + { + return heightMapManager.getLatestHeightMapData(); + } + } + + public TerrainMapData getLatestTerrainMapData() + { + synchronized (heightMapLock) + { + return heightMapManager.getTerrainMapData(); + } + } + + public void destroy() + { + scheduler.shutdown(); + heightMapManager.destroy(); + } +} diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java index 350ff574087..7ef74f7e3d7 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java @@ -8,10 +8,8 @@ import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.communication.ros2.ROS2TunedRigidBodyTransform; import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; -import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.robotics.physics.RobotCollisionModel; -import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.ros2.ROS2Node; import us.ihmc.sensorProcessing.heightMap.HeightMapData; import us.ihmc.sensorProcessing.heightMap.HeightMapParameters; @@ -32,17 +30,12 @@ public class StandAloneRealsenseProcess public static final String STAND_ALONE_REALSENSE_PROCESS = "StandAloneRealsenseProcess"; private final ROS2DemandGraphNode realsenseDemandNode; - private final HeightMapParameters heightMapParameters; private final ROS2DemandGraphNode realsensePublishDemandNode; - private final ROS2Helper ros2Helper; - private final ROS2SyncedRobotModel syncedRobot; private final RealSenseImageSensor d455Sensor; private final ImageSensorPublishThread d455PublishThread; - private final ROS2DemandGraphNode heightMapDemandNode; - private RapidHeightMapUpdateThread heightMapUpdateThread; - private final RobotCollisionModel robotCollisionModel; + private final RapidHeightMapThread rapidHeightMapThread; public StandAloneRealsenseProcess(ROS2Node ros2Node, ROS2Helper ros2Helper, @@ -60,15 +53,11 @@ public StandAloneRealsenseProcess(ROS2Node ros2Node, HeightMapParameters heightMapParameters, ControllerFootstepQueueMonitor controllerFootstepQueueMonitor) { - this.ros2Helper = ros2Helper; - this.syncedRobot = syncedRobot; - this.robotCollisionModel = robotCollisionModel; realsensePublishDemandNode = new ROS2DemandGraphNode(ros2Node, PerceptionAPI.REQUEST_REALSENSE_PUBLICATION); - heightMapDemandNode = new ROS2DemandGraphNode(ros2Node, PerceptionAPI.REQUEST_HEIGHT_MAP); + ROS2DemandGraphNode heightMapDemandNode = new ROS2DemandGraphNode(ros2Node, PerceptionAPI.REQUEST_HEIGHT_MAP); realsenseDemandNode = new ROS2DemandGraphNode(ros2Node, PerceptionAPI.REQUEST_REALSENSE); - this.heightMapParameters = heightMapParameters; realsenseDemandNode.addDependents(realsensePublishDemandNode, heightMapDemandNode); d455Sensor = new RealSenseImageSensor(RealSenseConfiguration.D455_COLOR_720P_DEPTH_720P_30HZ); @@ -92,29 +81,29 @@ public StandAloneRealsenseProcess(ROS2Node ros2Node, d455PublishThread.addTopic(PerceptionAPI.D455_DEPTH_IMAGE, RealSenseImageSensor.DEPTH_IMAGE_KEY); loopOnDemand(d455PublishThread, realsensePublishDemandNode); - heightMapUpdateThread = new RapidHeightMapUpdateThread(ros2Helper.getROS2Node(), - syncedRobot, - robotCollisionModel, - controllerFootstepQueueMonitor, - d455Sensor, - RealSenseImageSensor.DEPTH_IMAGE_KEY, - heightMapParameters); - loopOnDemand(heightMapUpdateThread, heightMapDemandNode); + rapidHeightMapThread = new RapidHeightMapThread(ros2Helper.getROS2Node(), + syncedRobot, + robotCollisionModel, + d455Sensor, + RealSenseImageSensor.DEPTH_IMAGE_KEY, + controllerFootstepQueueMonitor, + heightMapParameters); + rapidHeightMapThread.start(); } public HeightMapData getLatestHeightMapData() { - return heightMapUpdateThread.getLatestHeightMapData(); + return rapidHeightMapThread.getLatestHeightMapData(); } public TerrainMapData getLatestTerrainMapData() { - return heightMapUpdateThread.getLatestTerrainMapData(); + return rapidHeightMapThread.getLatestTerrainMapData(); } public void destroy() { - heightMapUpdateThread.blockingKill(); + rapidHeightMapThread.destroy(); realsenseDemandNode.destroy(); realsensePublishDemandNode.destroy(); d455Sensor.close(); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java index f7dc7b2e7a8..88e151be25d 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java @@ -96,23 +96,41 @@ public RapidHeightMapManager(ROS2Node ros2Node, } } - public void update(RawImage depthImage, ReferenceFrame cameraFrame, ReferenceFrame cameraZUpFrame) throws Exception + public void updateAndPublishHeightMap(RawImage depthImage, ReferenceFrame cameraFrame, ReferenceFrame cameraZUpFrame) throws Exception { - // -------- Update the Height Map with the latest depth image from the sensor -------------- RawImage depthImageCopy = depthImage.get(); Mat latestDepthImage = depthImageCopy.getCpuImageMat(); Instant acquisitionTime = depthImageCopy.getAcquisitionTime(); CameraIntrinsics depthIntrinsicsCopy = depthImageCopy.getIntrinsicsCopy(); - update(latestDepthImage, acquisitionTime, depthIntrinsicsCopy, cameraFrame, cameraZUpFrame); + // -------- Update the Height Map with the latest depth image from the sensor -------------- + GpuMat deviceCroppedHeightMap = new GpuMat(latestDepthImage); + updateInternal(latestDepthImage, deviceCroppedHeightMap, depthIntrinsicsCopy, cameraFrame, cameraZUpFrame); + + // Publish the height map to anyone who is subscribing + Mat hostCroppedHeightMap = new Mat(); + deviceCroppedHeightMap.download(hostCroppedHeightMap); + OpenCVTools.compressImagePNG(hostCroppedHeightMap, compressedCroppedHeightMapPointer); + PerceptionMessageTools.publishCompressedDepthImage(compressedCroppedHeightMapPointer, + croppedHeightMapImageMessage, + heightMapPublisher, + cameraPose, + acquisitionTime, + rapidHeightMapExtractor.getSequenceNumber(), + hostCroppedHeightMap.rows(), + hostCroppedHeightMap.cols(), + (float) heightMapParameters.getHeightScaleFactor()); + + deviceCroppedHeightMap.close(); + hostCroppedHeightMap.close(); depthImageCopy.release(); } - public void update(Mat latestDepthImage, - Instant acquisitionTime, - CameraIntrinsics depthIntrinsicsCopy, - ReferenceFrame cameraFrame, - ReferenceFrame cameraZUpFrame) throws Exception + private void updateInternal(Mat latestDepthImage, + GpuMat deviceHeightMapToPack, + CameraIntrinsics depthIntrinsicsCopy, + ReferenceFrame cameraFrame, + ReferenceFrame cameraZUpFrame) throws Exception { // Option that gets triggered from a message sent from the user if (lowerHeightMapBackdropRequested.poll()) @@ -188,24 +206,11 @@ public void update(Mat latestDepthImage, deviceOutputImage.close(); } + // Don't close this mat as its being used in the extractor till that finish's + deviceCroppedHeightMap.convertTo(deviceHeightMapToPack, deviceCroppedHeightMap.type()); + // Now extract the maps to be used in the footstep planning algorithm snappedFootstepsExtractor.update(rapidHeightMapExtractor.getTerrainHeightMapImage(), sensorOrigin); - - // Publish the height map to anyone who is subscribing - - Mat hostCroppedHeightMap = new Mat(); - deviceCroppedHeightMap.download(hostCroppedHeightMap); - OpenCVTools.compressImagePNG(hostCroppedHeightMap, compressedCroppedHeightMapPointer); - PerceptionMessageTools.publishCompressedDepthImage(compressedCroppedHeightMapPointer, - croppedHeightMapImageMessage, - heightMapPublisher, - cameraPose, - acquisitionTime, - rapidHeightMapExtractor.getSequenceNumber(), - hostCroppedHeightMap.rows(), - hostCroppedHeightMap.cols(), - (float) heightMapParameters.getHeightScaleFactor()); - hostCroppedHeightMap.close(); } public TerrainMapData getTerrainMapData() @@ -253,5 +258,6 @@ public void destroy() rapidHeightMapExtractor.destroy(); snappedFootstepsExtractor.destroy(); flyingPointsFilter.destroy(); + compressedCroppedHeightMapPointer.close(); } } \ No newline at end of file From 22fa69d184ff9a7229501c0bb47fd08415ce0909 Mon Sep 17 00:00:00 2001 From: nkitchel Date: Thu, 1 May 2025 12:00:42 -0500 Subject: [PATCH 2/9] Close kernel before the rest of the objeccts --- .../perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java index 738c283a5e0..a27d3c717d9 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java @@ -398,10 +398,10 @@ public void destroy() updateKernel.close(); registerKernel.close(); croppingKernel.close(); + planOffsetKernel.close(); emptyGlobalHeightMapImage.close(); planOffsetKernelGridDim.close(); - planOffsetKernel.close(); // Clean up each resource deallocateFloatPointer(groundToSensorTransformHostPointer, groundToSensorTransformDevicePointer); From 3d0de81f909d5f664083de880c5afb7a1aaa8f94 Mon Sep 17 00:00:00 2001 From: nkitchel Date: Thu, 1 May 2025 17:20:53 -0500 Subject: [PATCH 3/9] Change things to use the EnvironmentHandler rather then setting the HeightMapData and the TerrainMapData individually --- .../footstep/StepReachabilityVisualizer.java | 4 +- .../AStarFootstepPlanner.java | 16 +++--- .../FootstepPlannerRequest.java | 46 ++++++++--------- .../FootstepPlanningModule.java | 8 +-- .../bodyPath/AStarBodyPathPlanner.java | 2 +- .../BodyPathLSTraversibilityCalculator.java | 6 +-- .../bodyPath/GPUAStarBodyPathPlanner.java | 2 +- ...HeightMapLeastSquaresNormalCalculator.java | 6 +-- ...ntHandler.java => EnvironmentHandler.java} | 8 +-- .../FootstepSnapAndWiggler.java | 10 ++-- .../HeightMapFootstepChecker.java | 12 ++--- .../stepExpansion/IdealStepCalculator.java | 6 +-- .../MonteCarloFootstepNode.java | 6 ++- .../MonteCarloFootstepPlannerRequest.java | 49 +++++++++++-------- .../MonteCarloPlannerTools.java | 15 +++--- .../HeightMapPolygonSnapper.java | 18 +++---- .../polygonSnapping/HeightMapSnapWiggler.java | 9 ++-- .../HeightMapFootstepPlanner.java | 10 ++-- .../simplePlanners/PlanThenSnapPlanner.java | 6 +-- .../HeightMapDataVisualizer.java | 6 +-- ...ootstepPlannerLogVisualizerController.java | 6 +-- .../RDXStancePoseSelectionPanel.java | 6 +-- .../ui/affordances/RDXFootstepChecker.java | 4 +- .../ContinuousHikingProcess.java | 15 ++++-- .../JustWaitState.java | 25 +++------- .../ReadyToPlanState.java | 19 +++---- .../activeMapping/ContinuousPlanner.java | 25 +++++----- .../ContinuousPlannerSchedulingTask.java | 30 +++--------- .../activeMapping/StancePoseCalculator.java | 8 +-- .../TerrainPlanningDebugger.java | 10 ++-- 30 files changed, 183 insertions(+), 210 deletions(-) rename ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/{FootstepPlannerEnvironmentHandler.java => EnvironmentHandler.java} (84%) diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/reachabilityMap/footstep/StepReachabilityVisualizer.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/reachabilityMap/footstep/StepReachabilityVisualizer.java index 9a389a8caf4..d980298f068 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/reachabilityMap/footstep/StepReachabilityVisualizer.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/reachabilityMap/footstep/StepReachabilityVisualizer.java @@ -6,7 +6,7 @@ import us.ihmc.euclid.geometry.Pose3D; import us.ihmc.euclid.transform.RigidBodyTransform; import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapAndWiggler; import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapData; import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnappingTools; @@ -38,7 +38,7 @@ private enum Mode private final YoRegistry registry = new YoRegistry(getClass().getSimpleName()); private final SideDependentList footPolygons = PlannerTools.createDefaultFootPolygons(); private final DefaultFootstepPlannerParameters parameters = new DefaultFootstepPlannerParameters(); - private final FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + private final EnvironmentHandler environmentHandler = new EnvironmentHandler(); private final FootstepSnapAndWiggler snapper = new FootstepSnapAndWiggler(footPolygons, parameters, environmentHandler); private final FootstepPoseHeuristicChecker checker = new FootstepPoseHeuristicChecker(parameters, snapper, registry); private final StepReachabilityData stepReachabilityData; diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/AStarFootstepPlanner.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/AStarFootstepPlanner.java index 77fdb0a94f2..c4d5dbecc6a 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/AStarFootstepPlanner.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/AStarFootstepPlanner.java @@ -10,7 +10,7 @@ import us.ihmc.euclid.referenceFrame.FramePose3D; import us.ihmc.footstepPlanning.graphSearch.AStarFootstepPlannerIterationConductor; import us.ihmc.footstepPlanning.graphSearch.AStarIterationData; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerHeuristicCalculator; import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapAndWiggler; import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapData; @@ -49,7 +49,7 @@ public class AStarFootstepPlanner private final AStarFootstepPlannerIterationConductor iterationConductor; private final DefaultFootstepPlannerParametersBasics footstepPlannerParameters; - private final FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + private final EnvironmentHandler environmentHandler = new EnvironmentHandler(); private final FootstepSnapAndWiggler snapper; private final ParameterBasedStepExpansion nominalExpansion; private final HeightMapFootstepChecker heightMapFootstepChecker; @@ -151,12 +151,12 @@ public void handleRequest(FootstepPlannerRequest request, FootstepPlannerOutput result = FootstepPlanningResult.PLANNING; // Update what we should use for planning - boolean hasHeightMap = request.getHeightMapData() != null && !request.getHeightMapData().isEmpty(); - boolean hasTerrainMap = request.getTerrainMapData() != null; + boolean hasHeightMap = request.getEnvironmentHandler().getHeightMapData() != null && !request.getEnvironmentHandler().getHeightMapData().isEmpty(); + boolean hasTerrainMap = request.getEnvironmentHandler().getTerrainMapData() != null; boolean flatGroundMode = request.getAssumeFlatGround() || (!hasHeightMap && !hasTerrainMap); - HeightMapData heightMapData = flatGroundMode ? null : request.getHeightMapData(); - TerrainMapData terrainMapData = flatGroundMode ? null : request.getTerrainMapData(); + HeightMapData heightMapData = flatGroundMode ? null : request.getEnvironmentHandler().getHeightMapData(); + TerrainMapData terrainMapData = flatGroundMode ? null : request.getEnvironmentHandler().getTerrainMapData(); if (flatGroundMode) { @@ -165,7 +165,7 @@ public void handleRequest(FootstepPlannerRequest request, FootstepPlannerOutput } snapper.clearSnapData(); - environmentHandler.setHeightMap(heightMapData); + environmentHandler.setHeightMapData(heightMapData); environmentHandler.setTerrainMapData(terrainMapData); double pathLength = bodyPathPlanHolder.computePathLength(0.0); @@ -317,7 +317,7 @@ private void reportStatus(FootstepPlannerRequest request, FootstepPlannerOutput outputToPack.getFootstepPlan().addFootstep(footstep); } - swingPlanningModule.computeSwingWaypoints(request.getHeightMapData(), + swingPlanningModule.computeSwingWaypoints(request.getEnvironmentHandler().getHeightMapData(), outputToPack.getFootstepPlan(), request.getStartFootPoses(), request.getSwingPlannerType()); diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/FootstepPlannerRequest.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/FootstepPlannerRequest.java index 2636cd1673c..3d13bd35737 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/FootstepPlannerRequest.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/FootstepPlannerRequest.java @@ -7,6 +7,7 @@ import us.ihmc.euclid.geometry.interfaces.Pose3DReadOnly; import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersReadOnly; import us.ihmc.footstepPlanning.swing.SwingPlannerType; import us.ihmc.perception.heightMap.TerrainMapData; @@ -96,10 +97,9 @@ public class FootstepPlannerRequest private double horizonLength; /** - * Height map. May be null to enable flat ground mode. + * Holds the data for the {@link HeightMapData} and the {@link TerrainMapData} */ - private HeightMapData heightMapData; - private TerrainMapData terrainMapData; + private final EnvironmentHandler environmentHandler = new EnvironmentHandler(); /** * If true, will ignore planar regions and plan on flat ground. @@ -149,8 +149,7 @@ private void clear() timeout = 5.0; maximumIterations = -1; horizonLength = Double.MAX_VALUE; - heightMapData = null; - terrainMapData = null; + environmentHandler.clear(); assumeFlatGround = false; bodyPathWaypoints.clear(); statusPublishPeriod = 1.0; @@ -275,12 +274,12 @@ public void setHorizonLength(double horizonLength) public void setHeightMapData(HeightMapData heightMapData) { - this.heightMapData = heightMapData; + environmentHandler.setHeightMapData(heightMapData); } public void setTerrainMapData(TerrainMapData terrainMapData) { - this.terrainMapData = terrainMapData; + environmentHandler.setTerrainMapData(terrainMapData); } public void setAssumeFlatGround(boolean assumeFlatGround) @@ -381,14 +380,9 @@ public double getHorizonLength() return horizonLength; } - public HeightMapData getHeightMapData() + public EnvironmentHandler getEnvironmentHandler() { - return heightMapData; - } - - public TerrainMapData getTerrainMapData() - { - return terrainMapData; + return environmentHandler; } public boolean getAssumeFlatGround() @@ -503,15 +497,15 @@ public void setPacket(FootstepPlanningRequestPacket requestPacket) requestPacket.getBodyPathWaypoints().add().set(bodyPathWaypoints.get(i)); } - if (getHeightMapData() != null) + if (getEnvironmentHandler().getHeightMapData() != null) { - HeightMapMessage heightMapMessage = HeightMapMessageTools.toMessage(getHeightMapData()); + HeightMapMessage heightMapMessage = HeightMapMessageTools.toMessage(getEnvironmentHandler().getHeightMapData()); requestPacket.getHeightMapMessage().set(heightMapMessage); } // TODO need to add a message for the terrain map. - if (getTerrainMapData() != null) + if (getEnvironmentHandler().getTerrainMapData() != null) { - TerrainMapMessage terrainMapMessage = TerrainMapTools.toMessage(terrainMapData); + TerrainMapMessage terrainMapMessage = TerrainMapTools.toMessage(getEnvironmentHandler().getTerrainMapData()); requestPacket.getTerrainMapMessage().set(terrainMapMessage); } @@ -553,15 +547,15 @@ public void set(FootstepPlannerRequest other) this.bodyPathWaypoints.add(new Pose3D(other.bodyPathWaypoints.get(i))); } - if (this.heightMapData != null) - this.heightMapData.set(other.heightMapData); - else if (other.heightMapData != null) - this.heightMapData = new HeightMapData(other.heightMapData); + if (environmentHandler.getHeightMapData() != null) + environmentHandler.setHeightMapData(other.getEnvironmentHandler().getHeightMapData()); + else if (other.getEnvironmentHandler().getTerrainMapData() != null) + environmentHandler.setHeightMapData(new HeightMapData(other.getEnvironmentHandler().getHeightMapData())); - if (this.terrainMapData != null) - this.terrainMapData.set(other.terrainMapData); - else if (other.terrainMapData != null) - this.terrainMapData = new TerrainMapData(other.terrainMapData); + if (environmentHandler.getTerrainMapData() != null) + environmentHandler.setTerrainMapData(other.getEnvironmentHandler().getTerrainMapData()); + else if (other.getEnvironmentHandler().getTerrainMapData() != null) + environmentHandler.setTerrainMapData(new TerrainMapData(other.getEnvironmentHandler().getTerrainMapData())); if (other.referencePlan != null) this.referencePlan = new FootstepPlan(other.referencePlan); diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/FootstepPlanningModule.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/FootstepPlanningModule.java index 83ac11a1d45..111a5a17424 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/FootstepPlanningModule.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/FootstepPlanningModule.java @@ -243,7 +243,7 @@ private void handleRequestInternal(FootstepPlannerRequest request) throws Except aStarFootstepPlanner.clearLoggedData(); bodyPathPlannerInterface.clearLoggedData(); - boolean heightMapAvailable = request.getHeightMapData() != null && !request.getHeightMapData().isEmpty(); + boolean heightMapAvailable = request.getEnvironmentHandler().getHeightMapData() != null && !request.getEnvironmentHandler().getHeightMapData().isEmpty(); startMidFootPose.interpolate(request.getStartFootPoses().get(RobotSide.LEFT), request.getStartFootPoses().get(RobotSide.RIGHT), 0.5); goalMidFootPose.interpolate(request.getGoalFootPoses().get(RobotSide.LEFT), request.getGoalFootPoses().get(RobotSide.RIGHT), 0.5); @@ -315,7 +315,7 @@ else if (request.getPlanFootsteps()) RobotSide initialStanceSide = request.getRequestedInitialStanceSide(); FramePose3D initialStancePose = new FramePose3D(request.getStartFootPoses().get(initialStanceSide)); planThenSnapPlanner.setInitialStanceFoot(initialStancePose, initialStanceSide); - planThenSnapPlanner.setHeightMapData(request.getHeightMapData()); + planThenSnapPlanner.setHeightMapData(request.getEnvironmentHandler().getHeightMapData()); FootstepPlannerGoal goal = new FootstepPlannerGoal(); goal.setGoalPoseBetweenFeet(goalMidFootPose); @@ -334,7 +334,7 @@ else if (request.getPlanFootsteps()) output.getPlannerTimings().setTimePlanningStepsSeconds(stopwatch.lap()); aStarFootstepPlanner.getSwingPlanningModule() - .computeSwingWaypoints(request.getHeightMapData(), + .computeSwingWaypoints(request.getEnvironmentHandler().getHeightMapData(), output.getFootstepPlan(), request.getStartFootPoses(), request.getSwingPlannerType()); @@ -375,7 +375,7 @@ else if (output.getFootstepPlan().isEmpty()) try { swingReplanRequestCallbacks.forEach(callback -> callback.accept(swingPlannerType)); - aStarFootstepPlanner.getSwingPlanningModule().computeSwingWaypoints(request.getHeightMapData(), + aStarFootstepPlanner.getSwingPlanningModule().computeSwingWaypoints(request.getEnvironmentHandler().getHeightMapData(), output.getFootstepPlan(), request.getStartFootPoses(), swingPlannerType); diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/bodyPath/AStarBodyPathPlanner.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/bodyPath/AStarBodyPathPlanner.java index 047043aa416..3acf6a4ee39 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/bodyPath/AStarBodyPathPlanner.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/bodyPath/AStarBodyPathPlanner.java @@ -243,7 +243,7 @@ public void handleRequest(FootstepPlannerRequest request, FootstepPlannerOutput iterationData.clear(); edgeDataMap.clear(); gridHeightMap.clear(); - setHeightMapData(request.getHeightMapData()); + setHeightMapData(request.getEnvironmentHandler().getHeightMapData()); packRadialOffsets(heightMapData, plannerParameters.getSnapRadius(), xSnapOffsets, ySnapOffsets); diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/bodyPath/BodyPathLSTraversibilityCalculator.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/bodyPath/BodyPathLSTraversibilityCalculator.java index 54e99d611ab..8ecf6e6f3a6 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/bodyPath/BodyPathLSTraversibilityCalculator.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/bodyPath/BodyPathLSTraversibilityCalculator.java @@ -4,7 +4,7 @@ import us.ihmc.euclid.geometry.ConvexPolygon2D; import us.ihmc.euclid.geometry.Pose2D; import us.ihmc.euclid.transform.RigidBodyTransform; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersReadOnly; import us.ihmc.footstepPlanning.polygonSnapping.HeightMapPolygonSnapper; import us.ihmc.robotics.robotSide.RobotSide; @@ -27,7 +27,7 @@ public class BodyPathLSTraversibilityCalculator private final Pose2D bodyPose = new Pose2D(); private final Pose2D stepPose = new Pose2D(); - private final FootstepPlannerEnvironmentHandler internalEnvironmentHandler = new FootstepPlannerEnvironmentHandler(); + private final EnvironmentHandler internalEnvironmentHandler = new EnvironmentHandler(); private final HeightMapPolygonSnapper snapper = new HeightMapPolygonSnapper(); private final Map gridHeightMap; @@ -169,7 +169,7 @@ private double compute(RobotSide side, double parentHeight) TDoubleArrayList inclineAlphas = new TDoubleArrayList(); Pose2D rotatedBodyPose = new Pose2D(); - internalEnvironmentHandler.setHeightMap(heightMapData); + internalEnvironmentHandler.setHeightMapData(heightMapData); for (int ti = 0; ti < yawOffsets.size(); ti++) { diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/bodyPath/GPUAStarBodyPathPlanner.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/bodyPath/GPUAStarBodyPathPlanner.java index 5d9f67c7cb7..0dffcbde871 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/bodyPath/GPUAStarBodyPathPlanner.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/bodyPath/GPUAStarBodyPathPlanner.java @@ -342,7 +342,7 @@ private enum RejectionReason @Override public void handleRequest(FootstepPlannerRequest request, FootstepPlannerOutput outputToPack) { - heightMapData = request.getHeightMapData(); + heightMapData = request.getEnvironmentHandler().getHeightMapData(); if (firstTick) { firstTickSetup(); diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/bodyPath/HeightMapLeastSquaresNormalCalculator.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/bodyPath/HeightMapLeastSquaresNormalCalculator.java index f9b131c4fa4..851db46fa2d 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/bodyPath/HeightMapLeastSquaresNormalCalculator.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/bodyPath/HeightMapLeastSquaresNormalCalculator.java @@ -6,7 +6,7 @@ import us.ihmc.euclid.transform.RigidBodyTransform; import us.ihmc.euclid.tuple3D.UnitVector3D; import us.ihmc.euclid.tuple3D.interfaces.UnitVector3DBasics; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.polygonSnapping.HeightMapPolygonSnapper; import us.ihmc.sensorProcessing.heightMap.HeightMapData; import us.ihmc.sensorProcessing.heightMap.HeightMapTools; @@ -16,7 +16,7 @@ public class HeightMapLeastSquaresNormalCalculator { private final HeightMapPolygonSnapper heightMapSnapper = new HeightMapPolygonSnapper(); - private final FootstepPlannerEnvironmentHandler internalEnvironmentHandler = new FootstepPlannerEnvironmentHandler(); + private final EnvironmentHandler internalEnvironmentHandler = new EnvironmentHandler(); private UnitVector3DBasics[] surfaceNormals; private double[] sampledHeights; @@ -55,7 +55,7 @@ public void computeSurfaceNormals(HeightMapData heightMapData, double patchWidth double maxIncline = Math.toRadians(45.0); double snapHeightThreshold = patchWidth * Math.sin(maxIncline); - internalEnvironmentHandler.setHeightMap(heightMapData); + internalEnvironmentHandler.setHeightMapData(heightMapData); for (int xIndex = 0; xIndex < gridWidth; xIndex++) { diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/FootstepPlannerEnvironmentHandler.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/EnvironmentHandler.java similarity index 84% rename from ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/FootstepPlannerEnvironmentHandler.java rename to ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/EnvironmentHandler.java index 725c2e9a390..616551b171d 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/FootstepPlannerEnvironmentHandler.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/EnvironmentHandler.java @@ -3,18 +3,18 @@ import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.sensorProcessing.heightMap.HeightMapData; -public class FootstepPlannerEnvironmentHandler +public class EnvironmentHandler { private HeightMapData heightMap; private TerrainMapData terrainMapData; - public void reset() + public void clear() { heightMap = null; terrainMapData = null; } - public void setHeightMap(HeightMapData heightMap) + public void setHeightMapData(HeightMapData heightMap) { this.heightMap = heightMap; } @@ -40,7 +40,7 @@ public boolean hasTerrainMapData() return terrainMapData != null; } - public HeightMapData getHeightMap() + public HeightMapData getHeightMapData() { return heightMap; } diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/footstepSnapping/FootstepSnapAndWiggler.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/footstepSnapping/FootstepSnapAndWiggler.java index db34565e07f..e4e20d7dff4 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/footstepSnapping/FootstepSnapAndWiggler.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/footstepSnapping/FootstepSnapAndWiggler.java @@ -9,7 +9,7 @@ import us.ihmc.euclid.tuple2D.interfaces.Point2DBasics; import us.ihmc.euclid.tuple3D.Point3D; import us.ihmc.euclid.tuple3D.interfaces.Point3DBasics; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.graph.DiscreteFootstep; import us.ihmc.footstepPlanning.graphSearch.graph.DiscreteFootstepTools; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersReadOnly; @@ -37,7 +37,7 @@ public class FootstepSnapAndWiggler implements FootstepSnapperReadOnly private final HashSet snappedFootsteps = new HashSet<>(); private final HashMap manuallySnappedFootsteps = new HashMap<>(); - private final FootstepPlannerEnvironmentHandler environmentHandler; + private final EnvironmentHandler environmentHandler; private final HeightMapPolygonSnapper heightMapSnapper = new HeightMapPolygonSnapper(); private final HeightMapSnapWiggler heightMapSnapWiggler; @@ -45,7 +45,7 @@ public class FootstepSnapAndWiggler implements FootstepSnapperReadOnly // Use this by default public FootstepSnapAndWiggler(SideDependentList footPolygonsInSoleFrame, DefaultFootstepPlannerParametersReadOnly parameters, - FootstepPlannerEnvironmentHandler environmentHandler) + EnvironmentHandler environmentHandler) { this(footPolygonsInSoleFrame, parameters, null, environmentHandler,null, null); } @@ -54,7 +54,7 @@ public FootstepSnapAndWiggler(SideDependentList footPolygonsInS public FootstepSnapAndWiggler(SideDependentList footPolygonsInSoleFrame, DefaultFootstepPlannerParametersReadOnly parameters, TickAndUpdatable tickAndUpdatable, - FootstepPlannerEnvironmentHandler environmentHandler, + EnvironmentHandler environmentHandler, YoGraphicsListRegistry graphicsListRegistry, YoRegistry parentRegistry) { @@ -207,7 +207,7 @@ protected void updateTheCroppedFoothold(DiscreteFootstep footstepToCrop) snappedFootPolygonInWorld.applyTransform(transformToFootPose, false); heightMapSnapper.computeFootPointsInTheEnvironment(snappedFootPolygonInWorld, - environmentHandler.getHeightMap(), + environmentHandler.getHeightMapData(), environmentHandler.getTerrainMapData(), parameters.getHeightMapSnapThreshold(), parameters.getMinSurfaceIncline(), diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/stepChecking/HeightMapFootstepChecker.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/stepChecking/HeightMapFootstepChecker.java index 121b2102b06..f0cee556676 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/stepChecking/HeightMapFootstepChecker.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/stepChecking/HeightMapFootstepChecker.java @@ -3,7 +3,7 @@ import us.ihmc.commonWalkingControlModules.staticReachability.StepReachabilityData; import us.ihmc.euclid.geometry.ConvexPolygon2D; import us.ihmc.euclid.transform.RigidBodyTransform; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.collision.FootstepPlannerBodyCollisionDetector; import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapData; import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapDataReadOnly; @@ -37,7 +37,7 @@ public class HeightMapFootstepChecker implements FootstepCheckerInterface private final SideDependentList footPolygons; private final ConvexPolygon2D tmpFootPolygon = new ConvexPolygon2D(); - private final FootstepPlannerEnvironmentHandler environmentHandler; + private final EnvironmentHandler environmentHandler; private final HeightMapCliffAvoider heightMapCliffAvoider; private final ObstacleBetweenStepsChecker obstacleBetweenStepsChecker; private final FootstepPlannerBodyCollisionDetector collisionDetector; @@ -57,7 +57,7 @@ public class HeightMapFootstepChecker implements FootstepCheckerInterface public HeightMapFootstepChecker(DefaultFootstepPlannerParametersReadOnly parameters, SideDependentList footPolygons, - FootstepPlannerEnvironmentHandler environmentHandler, + EnvironmentHandler environmentHandler, FootstepSnapperReadOnly snapper, StepReachabilityData stepReachabilityData, YoRegistry parentRegistry) @@ -142,7 +142,7 @@ private void doValidityCheck(DiscreteFootstep candidateStep, DiscreteFootstep st } // Check height map cliff avoidance - heightMapCliffAvoider.setHeightMapData(environmentHandler.getHeightMap()); + heightMapCliffAvoider.setHeightMapData(environmentHandler.getHeightMapData()); if (!heightMapCliffAvoider.isStepValid(candidateStep, stanceStep)) { rejectionReason.set(BipedalFootstepPlannerNodeRejectionReason.STEP_ON_CLIFF_EDGE); @@ -271,7 +271,7 @@ private void isCollisionFree(DiscreteFootstep candidateStep, DiscreteFootstep st { try { - obstacleBetweenStepsChecker.setHeightMapData(environmentHandler.getHeightMap()); + obstacleBetweenStepsChecker.setHeightMapData(environmentHandler.getHeightMapData()); if (!obstacleBetweenStepsChecker.isFootstepValid(candidateStep, stanceStep)) { @@ -305,7 +305,7 @@ private boolean boundingBoxCollisionDetected(DiscreteFootstep candidateStep, Dis double candidateStepHeight = DiscreteFootstepTools.getSnappedStepHeight(candidateStep, candidateStepSnapData.getSnapTransform()); double stanceStepHeight = DiscreteFootstepTools.getSnappedStepHeight(stanceStep, stanceStepSnapData.getSnapTransform()); - collisionDetector.setHeightMapData(environmentHandler.getHeightMap()); + collisionDetector.setHeightMapData(environmentHandler.getHeightMapData()); boolean collisionDetected = collisionDetector.checkForCollision(candidateStep, stanceStep, candidateStepHeight, diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/IdealStepCalculator.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/IdealStepCalculator.java index afed3846255..25798b132ec 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/IdealStepCalculator.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/IdealStepCalculator.java @@ -8,7 +8,7 @@ import us.ihmc.euclid.tuple2D.Vector2D; import us.ihmc.euclid.tuple2D.interfaces.Point2DBasics; import us.ihmc.footstepPlanning.FootstepPlanHeading; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.graph.DiscreteFootstep; import us.ihmc.footstepPlanning.graphSearch.stepChecking.FootstepCheckerInterface; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersReadOnly; @@ -39,7 +39,7 @@ private enum IdealStepMode private final HashMap idealStepMap = new HashMap<>(); private final DefaultFootstepPlannerParametersReadOnly parameters; private final WaypointDefinedBodyPathPlanHolder bodyPathPlanHolder; - private final FootstepPlannerEnvironmentHandler environmentHandler; + private final EnvironmentHandler environmentHandler; private final FootstepCheckerInterface nodeChecker; private SideDependentList goalSteps; @@ -65,7 +65,7 @@ private enum IdealStepMode public IdealStepCalculator(DefaultFootstepPlannerParametersReadOnly parameters, FootstepCheckerInterface nodeChecker, WaypointDefinedBodyPathPlanHolder bodyPathPlanHolder, - FootstepPlannerEnvironmentHandler environmentHandler, + EnvironmentHandler environmentHandler, YoRegistry registry) { this.parameters = parameters; diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepNode.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepNode.java index 2a60ea2f079..702bfa173b1 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepNode.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepNode.java @@ -57,8 +57,10 @@ public boolean checkReachability(MonteCarloFootstepPlannerRequest request, Monte newPosition.scale(1.0 / parameters.getNodesPerMeter()); previousPosition.scale(1.0 / parameters.getNodesPerMeter()); - double previousHeight = request.getTerrainMapData().getHeightInWorld((float) previousPosition.getX(), (float) previousPosition.getY()); - double currentHeight = request.getTerrainMapData().getHeightInWorld((float) newPosition.getX(), (float) newPosition.getY()); + double previousHeight = request.getEnvironmentHandler() + .getTerrainMapData() + .getHeightInWorld((float) previousPosition.getX(), (float) previousPosition.getY()); + double currentHeight = request.getEnvironmentHandler().getTerrainMapData().getHeightInWorld((float) newPosition.getX(), (float) newPosition.getY()); boolean valid = Math.abs(currentHeight - previousHeight) < parameters.getMaxTransferHeight(); return valid; diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepPlannerRequest.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepPlannerRequest.java index 7ebdff8c8b3..714cc958064 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepPlannerRequest.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepPlannerRequest.java @@ -4,6 +4,7 @@ import us.ihmc.euclid.geometry.interfaces.Pose3DReadOnly; import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; @@ -57,15 +58,9 @@ public class MonteCarloFootstepPlannerRequest private float snapHeightThreshold = 0.05f; /** - * Height Map image in OpenCV format + * Holds the data for the {@link HeightMapData} and the {@link TerrainMapData} */ - private TerrainMapData terrainMapData; - - /** - * TODO: Remove this variable - * Height Map data for legacy support - */ - private HeightMapData heightMapData; + private EnvironmentHandler environmentHandler; public MonteCarloFootstepPlannerRequest() { @@ -209,24 +204,25 @@ public float getSnapHeightThreshold() return snapHeightThreshold; } - public void setTerrainMapData(TerrainMapData terrainMap) + public void setEnvironmentHandler(EnvironmentHandler environmentHandler) { - this.terrainMapData = terrainMap; + this.environmentHandler = environmentHandler; } public void setHeightMapData(HeightMapData heightMapData) { - this.heightMapData = heightMapData; + environmentHandler.setHeightMapData(heightMapData); } - public TerrainMapData getTerrainMapData() + public void setTerrainMapData(TerrainMapData terrainMapData) { - return terrainMapData; + environmentHandler.setTerrainMapData(terrainMapData); } - public HeightMapData getHeightMapData() + + public EnvironmentHandler getEnvironmentHandler() { - return heightMapData; + return environmentHandler; } public void setPacket(MonteCarloFootstepPlannerRequest requestPacket) @@ -259,12 +255,23 @@ public String toString() StringBuilder builder = new StringBuilder(); builder.append("Monte-Carlo Footstep Planner Request: [") - .append("Stance Side: ").append(this.requestedInitialStanceSide).append(", ") - .append("Start Pose (Left): Position: ").append(startFootPoses.get(RobotSide.LEFT).getPosition()).append(", ") - .append("Start Pose (Right): Position: ").append(startFootPoses.get(RobotSide.RIGHT).getPosition()).append(", ") - .append("Goal Pose (Left): Position: ").append(goalFootPoses.get(RobotSide.LEFT).getPosition()).append(", ") - .append("Goal Pose (Right): Position: ").append(goalFootPoses.get(RobotSide.RIGHT).getPosition()).append(", ") - .append("Timeout: ").append(this.timeout); + .append("Stance Side: ") + .append(this.requestedInitialStanceSide) + .append(", ") + .append("Start Pose (Left): Position: ") + .append(startFootPoses.get(RobotSide.LEFT).getPosition()) + .append(", ") + .append("Start Pose (Right): Position: ") + .append(startFootPoses.get(RobotSide.RIGHT).getPosition()) + .append(", ") + .append("Goal Pose (Left): Position: ") + .append(goalFootPoses.get(RobotSide.LEFT).getPosition()) + .append(", ") + .append("Goal Pose (Right): Position: ") + .append(goalFootPoses.get(RobotSide.RIGHT).getPosition()) + .append(", ") + .append("Timeout: ") + .append(this.timeout); builder.append("]\n"); return builder.toString(); diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloPlannerTools.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloPlannerTools.java index 527df19cabf..eb8f1e2a66b 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloPlannerTools.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloPlannerTools.java @@ -20,13 +20,12 @@ import us.ihmc.euclid.tuple4D.Vector4D32; import us.ihmc.footstepPlanning.FootstepPlan; import us.ihmc.footstepPlanning.MonteCarloFootstepPlannerParameters; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.polygonSnapping.HeightMapPolygonSnapper; import us.ihmc.footstepPlanning.tools.PlannerTools; import us.ihmc.log.LogTools; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; -import us.ihmc.sensorProcessing.heightMap.HeightMapData; import java.util.ArrayList; import java.util.Arrays; @@ -327,9 +326,9 @@ public static FootstepPlan getFootstepPlanFromTree(MonteCarloFootstepNode root, LogTools.info("Optimal Path Size: {}", path.size()); HeightMapPolygonSnapper heightMapSnapper = new HeightMapPolygonSnapper(); - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); - environmentHandler.setHeightMap(request.getHeightMapData()); - environmentHandler.setTerrainMapData(request.getTerrainMapData()); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); + environmentHandler.setHeightMapData(request.getEnvironmentHandler().getHeightMapData()); + environmentHandler.setTerrainMapData(request.getEnvironmentHandler().getTerrainMapData()); FootstepPlan footstepPlan = new FootstepPlan(); for (MonteCarloTreeNode node : path) @@ -338,7 +337,7 @@ public static FootstepPlan getFootstepPlanFromTree(MonteCarloFootstepNode root, float nodeX = (float) (footstepNode.getState().getX32() / parameters.getNodesPerMeter()); float nodeY = (float) (footstepNode.getState().getY32() / parameters.getNodesPerMeter()); - float nodeZ = request.getTerrainMapData().getHeightInWorld(nodeX, nodeY); + float nodeZ = request.getEnvironmentHandler().getTerrainMapData().getHeightInWorld(nodeX, nodeY); float nodeYaw = footstepNode.getState().getZ32(); ConvexPolygon2D footPolygon = PlannerTools.createFootPolygon(0.25, 0.12, 0.08); @@ -353,7 +352,7 @@ public static FootstepPlan getFootstepPlanFromTree(MonteCarloFootstepNode root, return footstepPlan; } - public static void snapFootPoseToHeightMap(FootstepPlannerEnvironmentHandler environmentHandler, FramePose3D poseToSnap, HeightMapPolygonSnapper snapper, ConvexPolygon2D footPolygon) + public static void snapFootPoseToHeightMap(EnvironmentHandler environmentHandler, FramePose3D poseToSnap, HeightMapPolygonSnapper snapper, ConvexPolygon2D footPolygon) { ConvexPolygon2D footPolygonInWorld = new ConvexPolygon2D(footPolygon); footPolygonInWorld.applyTransform(poseToSnap); @@ -514,7 +513,7 @@ public static double scoreFootstepNode(MonteCarloFootstepNode oldNode, double goalReward = parameters.getGoalReward() * progressToGoal; - double contactValue = request.getTerrainMapData().getContactScoreInWorld(currentPosition.getX32(), currentPosition.getY32()); + double contactValue = request.getEnvironmentHandler().getTerrainMapData().getContactScoreInWorld(currentPosition.getX32(), currentPosition.getY32()); contactValue = MathTools.clamp(contactValue, parameters.getMinimumContactValue(), parameters.getMaximumContactValue()); contactValue /= parameters.getMaximumContactValue(); double contactReward = (contactValue) * parameters.getFeasibleContactReward(); diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/polygonSnapping/HeightMapPolygonSnapper.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/polygonSnapping/HeightMapPolygonSnapper.java index 21d45137e63..030b0000a7a 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/polygonSnapping/HeightMapPolygonSnapper.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/polygonSnapping/HeightMapPolygonSnapper.java @@ -13,7 +13,7 @@ import us.ihmc.euclid.tuple3D.interfaces.Point3DBasics; import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.UnitVector3DReadOnly; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapData; import us.ihmc.footstepPlanning.graphSearch.graph.DiscreteFootstep; import us.ihmc.footstepPlanning.graphSearch.graph.DiscreteFootstepTools; @@ -44,7 +44,7 @@ public void setSnapAreaResolution(double snapAreaResolution) public FootstepSnapData computeSnapData(DiscreteFootstep footstep, ConvexPolygon2DReadOnly polygonInStepFrame, - FootstepPlannerEnvironmentHandler environmentHandler, + EnvironmentHandler environmentHandler, double snapHeightThreshold, double minSurfaceIncline) { @@ -62,7 +62,7 @@ public FootstepSnapData computeSnapData(double stepX, double stepY, double stepYaw, ConvexPolygon2DReadOnly polygonInStepFrame, - FootstepPlannerEnvironmentHandler environmentHandler, + EnvironmentHandler environmentHandler, double snapHeightThreshold, double minSurfaceIncline) { @@ -71,7 +71,7 @@ public FootstepSnapData computeSnapData(double stepX, public FootstepSnapData computeSnapData(DiscreteFootstep footstep, ConvexPolygon2DReadOnly polygonInStepFrame, - FootstepPlannerEnvironmentHandler environmentHandler, + EnvironmentHandler environmentHandler, double snapHeightThreshold, double minSurfaceIncline, double minimumHeightToConsider) @@ -90,7 +90,7 @@ public FootstepSnapData computeSnapData(double stepX, double stepY, double yaw, ConvexPolygon2DReadOnly polygonInStepFrame, - FootstepPlannerEnvironmentHandler environmentHandler, + EnvironmentHandler environmentHandler, double snapHeightThreshold, double minSurfaceIncline, double minimumHeightToConsider) @@ -128,13 +128,13 @@ public FootstepSnapData computeSnapData(double stepX, } } - public RigidBodyTransform snapPolygonToHeightMap(ConvexPolygon2DReadOnly polygonToSnap, FootstepPlannerEnvironmentHandler environmentHandler) + public RigidBodyTransform snapPolygonToHeightMap(ConvexPolygon2DReadOnly polygonToSnap, EnvironmentHandler environmentHandler) { return snapPolygonToHeightMap(polygonToSnap, environmentHandler, Double.MAX_VALUE, Double.MAX_VALUE, -Double.MAX_VALUE); } public RigidBodyTransform snapPolygonToHeightMap(ConvexPolygon2DReadOnly polygonToSnap, - FootstepPlannerEnvironmentHandler environmentHandler, + EnvironmentHandler environmentHandler, double snapHeightThreshold, double minSurfaceIncline) { @@ -293,7 +293,7 @@ public boolean computeFootPointsInTheEnvironment(ConvexPolygon2DReadOnly polygon * - Any cells with heights below maxZ - snapHeightThreshold are ignored, where maxZ is the max height within the polygon */ public RigidBodyTransform snapPolygonToHeightMap(ConvexPolygon2DReadOnly polygonToSnap, - FootstepPlannerEnvironmentHandler environmentHandler, + EnvironmentHandler environmentHandler, double snapHeightThreshold, double minSurfaceIncline, double minimumHeightToConsider) @@ -301,7 +301,7 @@ public RigidBodyTransform snapPolygonToHeightMap(ConvexPolygon2DReadOnly polygon RigidBodyTransform transformToReturn; TerrainMapData terrainMapData = environmentHandler.getTerrainMapData(); - HeightMapData heightMapData = environmentHandler.getHeightMap(); + HeightMapData heightMapData = environmentHandler.getHeightMapData(); if (environmentHandler.hasTerrainMapData() && terrainMapData.hasSnapHeight() && terrainMapData.hasSnapNormal()) { diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/polygonSnapping/HeightMapSnapWiggler.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/polygonSnapping/HeightMapSnapWiggler.java index 1d82d2791b2..53ce3f407ad 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/polygonSnapping/HeightMapSnapWiggler.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/polygonSnapping/HeightMapSnapWiggler.java @@ -7,17 +7,14 @@ import us.ihmc.euclid.referenceFrame.FixedReferenceFrame; import us.ihmc.euclid.referenceFrame.FramePose3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.transform.RigidBodyTransform; import us.ihmc.euclid.tuple2D.Point2D; import us.ihmc.euclid.tuple2D.Vector2D; import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapData; import us.ihmc.footstepPlanning.graphSearch.graph.DiscreteFootstep; -import us.ihmc.footstepPlanning.graphSearch.graph.DiscreteFootstepTools; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; -import us.ihmc.sensorProcessing.heightMap.HeightMapData; public class HeightMapSnapWiggler { @@ -51,7 +48,7 @@ public HeightMapSnapWiggler(SideDependentList footPolygonsInSol } public void computeWiggleTransform(DiscreteFootstep footstepToWiggle, - FootstepPlannerEnvironmentHandler environmentHandler, + EnvironmentHandler environmentHandler, FootstepSnapData snapData, double snapHeightThreshold, double minSurfaceInclineRadians) @@ -139,7 +136,7 @@ private void computeWiggleOffsets(double stepYaw) private FootstepSnapData computeSnapData(Point2DReadOnly position, double yaw, RobotSide robotSide, - FootstepPlannerEnvironmentHandler environmentHandler, + EnvironmentHandler environmentHandler, double snapHeightThreshold, double minSurfaceInclineRadians) { diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/simplePlanners/HeightMapFootstepPlanner.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/simplePlanners/HeightMapFootstepPlanner.java index 72b643b904e..2f33fe7914b 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/simplePlanners/HeightMapFootstepPlanner.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/simplePlanners/HeightMapFootstepPlanner.java @@ -10,7 +10,7 @@ import us.ihmc.euclid.tuple2D.Vector2D; import us.ihmc.euclid.tuple3D.Point3D; import us.ihmc.footstepPlanning.FootstepPlan; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapData; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersReadOnly; import us.ihmc.footstepPlanning.polygonSnapping.HeightMapPolygonSnapper; @@ -31,8 +31,8 @@ public static FootstepPlan debug(SideDependentList footPolygons FootstepPlan footstepPlan = new FootstepPlan(); HeightMapPolygonSnapper snapper = new HeightMapPolygonSnapper(); - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); - environmentHandler.setHeightMap(heightMap); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); + environmentHandler.setHeightMapData(heightMap); for (int i = 0; i < stepsToDebug.size(); i++) { @@ -85,8 +85,8 @@ public static FootstepPlan plan(Pose3DReadOnly start, FootstepPlan footstepPlan = new FootstepPlan(); HeightMapPolygonSnapper snapper = new HeightMapPolygonSnapper(); - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); - environmentHandler.setHeightMap(heightMap); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); + environmentHandler.setHeightMapData(heightMap); List poses = generateTurnWalkTurnPoses(start, goal, parameters); RobotSide stepSide = RobotSide.LEFT; diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/simplePlanners/PlanThenSnapPlanner.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/simplePlanners/PlanThenSnapPlanner.java index f0a82ca542f..beb70576bd4 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/simplePlanners/PlanThenSnapPlanner.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/simplePlanners/PlanThenSnapPlanner.java @@ -5,7 +5,7 @@ import us.ihmc.euclid.referenceFrame.FramePose3D; import us.ihmc.euclid.transform.interfaces.RigidBodyTransformReadOnly; import us.ihmc.footstepPlanning.*; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapData; import us.ihmc.footstepPlanning.graphSearch.graph.DiscreteFootstep; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersBasics; @@ -20,7 +20,7 @@ public class PlanThenSnapPlanner { private final TurnWalkTurnPlanner turnWalkTurnPlanner; private final SideDependentList footPolygons; - private final FootstepPlannerEnvironmentHandler internalEnvironmentHandler = new FootstepPlannerEnvironmentHandler(); + private final EnvironmentHandler internalEnvironmentHandler = new EnvironmentHandler(); private HeightMapData heightMapData; private final HeightMapPolygonSnapper snapper; private final HeightMapSnapWiggler wiggler; @@ -47,7 +47,7 @@ public void setGoal(FootstepPlannerGoal goal) public void setHeightMapData(HeightMapData heightMapData) { - internalEnvironmentHandler.setHeightMap(heightMapData); + internalEnvironmentHandler.setHeightMapData(heightMapData); this.heightMapData = heightMapData; } diff --git a/ihmc-footstep-planning/src/visualizers/java/us/ihmc/footstepPlanning/HeightMapDataVisualizer.java b/ihmc-footstep-planning/src/visualizers/java/us/ihmc/footstepPlanning/HeightMapDataVisualizer.java index 1126c83a4f6..4b621d9b009 100644 --- a/ihmc-footstep-planning/src/visualizers/java/us/ihmc/footstepPlanning/HeightMapDataVisualizer.java +++ b/ihmc-footstep-planning/src/visualizers/java/us/ihmc/footstepPlanning/HeightMapDataVisualizer.java @@ -10,7 +10,7 @@ import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.euclid.tuple3D.interfaces.UnitVector3DReadOnly; import us.ihmc.footstepPlanning.bodyPath.HeightMapRANSACNormalCalculator; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParameters; import us.ihmc.footstepPlanning.polygonSnapping.HeightMapPolygonSnapper; import us.ihmc.graphicsDescription.Graphics3DObject; @@ -208,8 +208,8 @@ private static Graphics3DObject buildSnapGraphics(HeightMapData heightMapData, C Graphics3DObject graphics3DObject = new Graphics3DObject(); HeightMapPolygonSnapper snapper = new HeightMapPolygonSnapper(); DefaultFootstepPlannerParameters parameters = new DefaultFootstepPlannerParameters(); - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); - environmentHandler.setHeightMap(heightMapData); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); + environmentHandler.setHeightMapData(heightMapData); for (int i = 0; i < poses.length; i++) { diff --git a/ihmc-footstep-planning/src/visualizers/java/us/ihmc/footstepPlanning/ui/controllers/FootstepPlannerLogVisualizerController.java b/ihmc-footstep-planning/src/visualizers/java/us/ihmc/footstepPlanning/ui/controllers/FootstepPlannerLogVisualizerController.java index de39aaae562..83da6a834cf 100644 --- a/ihmc-footstep-planning/src/visualizers/java/us/ihmc/footstepPlanning/ui/controllers/FootstepPlannerLogVisualizerController.java +++ b/ihmc-footstep-planning/src/visualizers/java/us/ihmc/footstepPlanning/ui/controllers/FootstepPlannerLogVisualizerController.java @@ -49,7 +49,7 @@ import us.ihmc.footstepPlanning.FootstepPlan; import us.ihmc.footstepPlanning.FootstepPlanningResult; import us.ihmc.footstepPlanning.communication.FootstepPlannerMessagerAPI; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapAndWiggler; import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapData; import us.ihmc.footstepPlanning.graphSearch.graph.DiscreteFootstep; @@ -89,7 +89,7 @@ public class FootstepPlannerLogVisualizerController private List iterationDataList; private Map, FootstepPlannerEdgeData> edgeDataMap; private List variableDescriptors; - private final FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + private final EnvironmentHandler environmentHandler = new EnvironmentHandler(); private FootstepSnapAndWiggler snapper = new FootstepSnapAndWiggler(PlannerTools.createDefaultFootPolygons(), new DefaultFootstepPlannerParameters(), environmentHandler); // TODO private final AtomicBoolean loadingLog = new AtomicBoolean(); @@ -425,7 +425,7 @@ private void updateGraphData(HeightMapData heightMapData, this.iterationDataList = iterationData; this.edgeDataMap = edgeDataMap; this.variableDescriptors = variableDescriptors; - environmentHandler.setHeightMap(heightMapData); + environmentHandler.setHeightMapData(heightMapData); environmentHandler.setTerrainMapData(terrainMapData); parentNodeStack.clear(); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXStancePoseSelectionPanel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXStancePoseSelectionPanel.java index 74e23d736de..89ab18b6266 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXStancePoseSelectionPanel.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXStancePoseSelectionPanel.java @@ -18,7 +18,7 @@ import us.ihmc.euclid.tuple2D.Point2D; import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; import us.ihmc.footstepPlanning.communication.ContinuousHikingAPI; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.tools.PlannerTools; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.perception.tools.PerceptionDebugTools; @@ -58,7 +58,7 @@ public class RDXStancePoseSelectionPanel extends RDXPanel implements RenderableP private final SideDependentList footstepGraphics; private final StancePoseCalculator stancePoseCalculator; - private final FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + private final EnvironmentHandler environmentHandler = new EnvironmentHandler(); private boolean selectionActive = false; // Important for determining when to detect collisions or not private final ImBoolean calculateStancePose = new ImBoolean(false); @@ -90,7 +90,7 @@ public RDXStancePoseSelectionPanel(RDXBaseUI baseUI, ROS2Node ros2Node, StancePo public void update(TerrainMapData latestTerrainMapData, HeightMapData latestHeightMapData) { - environmentHandler.setHeightMap(latestHeightMapData); + environmentHandler.setHeightMapData(latestHeightMapData); environmentHandler.setTerrainMapData(latestTerrainMapData); if (environmentHandler.hasTerrainMapData() && environmentHandler.hasHeightMap()) diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXFootstepChecker.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXFootstepChecker.java index fb578232f9a..17761b7fe43 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXFootstepChecker.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/affordances/RDXFootstepChecker.java @@ -6,7 +6,7 @@ import us.ihmc.euclid.geometry.ConvexPolygon2D; import us.ihmc.euclid.referenceFrame.FramePose3D; import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapAndWiggler; import us.ihmc.footstepPlanning.graphSearch.graph.visualization.BipedalFootstepPlannerNodeRejectionReason; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersReadOnly; @@ -48,7 +48,7 @@ public RDXFootstepChecker(RDXBaseUI baseUI, this.syncedRobot = syncedRobot; this.controllerStatusTracker = controllerStatusTracker; baseUI.getPrimary3DPanel().addImGuiOverlayAddition(this::renderTooltips); - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); tooltip = new RDX3DPanelTooltip(baseUI.getPrimary3DPanel()); snapper = new FootstepSnapAndWiggler(footPolygons, footstepPlannerParameters, environmentHandler); stepChecker = new FootstepPoseHeuristicChecker(footstepPlannerParameters, snapper, registry); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java index f3124d32e4c..c351bfc3786 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java @@ -4,6 +4,7 @@ import us.ihmc.avatar.drcRobot.DRCRobotModel; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; import us.ihmc.communication.ros2.ROS2Helper; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; import us.ihmc.perception.StandAloneRealsenseProcess; import us.ihmc.robotics.physics.RobotCollisionModel; @@ -20,6 +21,7 @@ public class ContinuousHikingProcess public static final String CONTINUOUS_HIKING_THREAD = "ContinuousHikingThread"; public static final String SYNCED_ROBOT_THREAD = "SyncedRobotThread"; + private final EnvironmentHandler environmentHandler; private final ActiveMappingParameterToolBox activeMappingParameterToolBox; private final ContinuousPlannerSchedulingTask continuousPlannerSchedulingTask; private final StandAloneRealsenseProcess standAloneRealsenseProcess; @@ -39,6 +41,7 @@ public ContinuousHikingProcess(DRCRobotModel robotModel, RobotCollisionModel rob ControllerFootstepQueueMonitor controllerFootstepQueueMonitor = new ControllerFootstepQueueMonitor(ros2Node, robotModel.getSimpleRobotName()); + environmentHandler = new EnvironmentHandler(); activeMappingParameterToolBox = new ActiveMappingParameterToolBox(ros2Node, robotModel, "ForContinuousWalking"); standAloneRealsenseProcess = new StandAloneRealsenseProcess(ros2Node, ros2Helper, @@ -65,11 +68,13 @@ public void update() { activeMappingParameterToolBox.update(); - if (standAloneRealsenseProcess.getLatestHeightMapData() != null && standAloneRealsenseProcess.getLatestTerrainMapData() != null) - { - continuousPlannerSchedulingTask.setLatestHeightMapData(standAloneRealsenseProcess.getLatestHeightMapData()); - continuousPlannerSchedulingTask.setTerrainMapData(standAloneRealsenseProcess.getLatestTerrainMapData()); - } + if (standAloneRealsenseProcess.getLatestHeightMapData() != null) + environmentHandler.setHeightMapData(standAloneRealsenseProcess.getLatestHeightMapData()); + + if (standAloneRealsenseProcess.getLatestTerrainMapData() != null) + environmentHandler.setTerrainMapData(standAloneRealsenseProcess.getLatestTerrainMapData()); + + continuousPlannerSchedulingTask.setLatestEnvironmentHandler(environmentHandler); } public void destroy() diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java index b24bdfbd29d..88c1571006c 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java @@ -30,7 +30,7 @@ import us.ihmc.footstepPlanning.FootstepPlanningModule; import us.ihmc.footstepPlanning.PlannedFootstep; import us.ihmc.footstepPlanning.communication.ContinuousHikingAPI; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapAndWiggler; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersBasics; import us.ihmc.footstepPlanning.swing.SwingPlannerParametersBasics; @@ -38,12 +38,10 @@ import us.ihmc.footstepPlanning.tools.PlannerTools; import us.ihmc.log.LogTools; import us.ihmc.mecano.frames.MovingReferenceFrame; -import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.robotics.stateMachine.core.State; import us.ihmc.ros2.ROS2Topic; -import us.ihmc.sensorProcessing.heightMap.HeightMapData; import java.util.ArrayList; import java.util.List; @@ -59,7 +57,6 @@ public class JustWaitState implements State private final ROS2SyncedRobotModel syncedRobot; private final AtomicReference commandMessage; private final ControllerFootstepQueueMonitor controllerQueueMonitor; - private final FootstepPlannerEnvironmentHandler environmentHandler; private final ContinuousHikingParameters continuousHikingParameters; private final FootstepPlannerLogger logger; private boolean isDone; @@ -67,8 +64,7 @@ public class JustWaitState implements State private final TerrainPlanningDebugger terrainPlanningDebugger; private final DefaultFootstepPlannerParametersBasics footstepPlannerParameters; private final SwingPlannerParametersBasics swingPlannerParameters; - private final Supplier heightMapData; - private final Supplier terrainMapData; + private final Supplier environmentHandlerSupplier; private final ROS2Topic controllerFootstepDataTopic; private final FramePose3D midFeetZUpPose = new FramePose3D(); @@ -85,8 +81,7 @@ public JustWaitState(DRCRobotModel robotModel, ControllerFootstepQueueMonitor controllerQueueMonitor, TerrainPlanningDebugger terrainPlanningDebugger, ActiveMappingParameterToolBox activeMappingParameterToolBox, - Supplier heightMapData, - Supplier terrainMapData) + Supplier environmentHandlerSupplier) { this.ros2Helper = ros2Helper; this.syncedRobot = syncedRobot; @@ -100,11 +95,9 @@ public JustWaitState(DRCRobotModel robotModel, this.continuousHikingParameters = activeMappingParameterToolBox.getContinuousHikingParameters(); this.footstepPlannerParameters = activeMappingParameterToolBox.getFootstepPlannerParameters(); this.swingPlannerParameters = activeMappingParameterToolBox.getSwingPlannerParameters(); - this.heightMapData = heightMapData; - this.terrainMapData = terrainMapData; + this.environmentHandlerSupplier = environmentHandlerSupplier; - environmentHandler = new FootstepPlannerEnvironmentHandler(); - footstepSnapAndWiggler = new FootstepSnapAndWiggler(PlannerTools.createDefaultFootPolygons(), this.footstepPlannerParameters, environmentHandler); + footstepSnapAndWiggler = new FootstepSnapAndWiggler(PlannerTools.createDefaultFootPolygons(), this.footstepPlannerParameters, environmentHandlerSupplier.get()); midFeetZUpFrame = syncedRobot.getReferenceFrames().getMidFeetZUpFrame(); @@ -242,8 +235,9 @@ public void planToGoal(PoseListMessage poseListMessage) if (useEnvironmentData.get()) { - footstepPlannerRequest.setHeightMapData(heightMapData.get()); - footstepPlannerRequest.setTerrainMapData(terrainMapData.get()); + EnvironmentHandler environmentHandler = environmentHandlerSupplier.get(); + footstepPlannerRequest.setHeightMapData(environmentHandler.getHeightMapData()); + footstepPlannerRequest.setTerrainMapData(environmentHandler.getTerrainMapData()); footstepPlannerRequest.setSnapGoalSteps(useEnvironmentData.get()); } @@ -299,9 +293,6 @@ public void planToGoal(PoseListMessage poseListMessage) public void squareUpStep() { - environmentHandler.setHeightMap(heightMapData.get()); - environmentHandler.setTerrainMapData(terrainMapData.get()); - FramePose3DReadOnly firstStepInQueue; PlannedFootstep squareUpStep = null; ReferenceFrame leftFootFrame = syncedRobot.getReferenceFrames().getFootFrame(RobotSide.LEFT); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/ReadyToPlanState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/ReadyToPlanState.java index fdce113796f..06f564a5989 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/ReadyToPlanState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/ReadyToPlanState.java @@ -10,6 +10,7 @@ import us.ihmc.behaviors.activeMapping.ContinuousPlannerTools; import us.ihmc.commons.Conversions; import us.ihmc.commons.thread.ThreadTools; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; import us.ihmc.communication.packets.MessageTools; import us.ihmc.communication.ros2.ROS2Helper; @@ -22,11 +23,9 @@ import us.ihmc.behaviors.activeMapping.TerrainPlanningDebugger; import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames; import us.ihmc.log.LogTools; -import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.robotics.stateMachine.core.State; -import us.ihmc.sensorProcessing.heightMap.HeightMapData; import java.util.ArrayList; import java.util.List; @@ -46,8 +45,7 @@ public class ReadyToPlanState implements State private final ContinuousPlanner continuousPlanner; private final ControllerFootstepQueueMonitor controllerFootstepQueueMonitor; private final ContinuousHikingParameters continuousHikingParameters; - private final Supplier heightMapData; - private final Supplier terrainMapData; + private final Supplier environmentHandlerSupplier; private final TerrainPlanningDebugger debugger; private final ContinuousHikingLogger continuousHikingLogger; private final List> walkToGoalWayPointPoses = new ArrayList<>(); @@ -69,8 +67,7 @@ public ReadyToPlanState(ROS2Helper ros2Helper, ContinuousPlanner continuousPlanner, ControllerFootstepQueueMonitor controllerFootstepQueueMonitor, ContinuousHikingParameters continuousHikingParameters, - Supplier heightMapData, - Supplier terrainMapData, + Supplier environmentHandlerSupplier, TerrainPlanningDebugger debugger, ContinuousHikingLogger continuousHikingLogger) { @@ -79,8 +76,7 @@ public ReadyToPlanState(ROS2Helper ros2Helper, this.continuousPlanner = continuousPlanner; this.controllerFootstepQueueMonitor = controllerFootstepQueueMonitor; this.continuousHikingParameters = continuousHikingParameters; - this.heightMapData = heightMapData; - this.terrainMapData = terrainMapData; + this.environmentHandlerSupplier = environmentHandlerSupplier; this.debugger = debugger; this.continuousHikingLogger = continuousHikingLogger; @@ -135,11 +131,10 @@ public void doAction(double timeInState) } // Get the latest data from the perception pipeline to be used with the current footstep plan - TerrainMapData terrainMapData = this.terrainMapData.get(); - HeightMapData heightMapData = this.heightMapData.get(); + EnvironmentHandler environmentHandler = environmentHandlerSupplier.get(); // Plan to the goal and log the plan - continuousPlanner.planToGoal(goalPoses, heightMapData, terrainMapData); + continuousPlanner.planToGoal(goalPoses, environmentHandler); if (continuousHikingParameters.getStepPublisherEnabled()) { @@ -149,7 +144,7 @@ public void doAction(double timeInState) if (commandMessage.get().getUseMonteCarloFootstepPlanner() || commandMessage.get().getUseMonteCarloPlanAsReference()) { debugger.publishMonteCarloPlan(continuousPlanner.getMonteCarloFootstepDataListMessage()); - debugger.publishMonteCarloNodesForVisualization(continuousPlanner.getMonteCarloFootstepPlanner().getRoot(), terrainMapData); + debugger.publishMonteCarloNodesForVisualization(continuousPlanner.getMonteCarloFootstepPlanner().getRoot(), environmentHandler.getTerrainMapData()); } // We know that we have a plan, and this method only gets set to true when we have at least one step in the plan, so we know it's not empty, let's send it diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlanner.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlanner.java index aa0e24e966a..2f96bc4a18a 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlanner.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlanner.java @@ -18,6 +18,7 @@ import us.ihmc.footstepPlanning.FootstepPlanningModule; import us.ihmc.footstepPlanning.FootstepPlanningResult; import us.ihmc.footstepPlanning.PlannedFootstep; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersBasics; import us.ihmc.footstepPlanning.log.FootstepPlannerLogger; import us.ihmc.footstepPlanning.monteCarloPlanning.MonteCarloFootstepPlanner; @@ -27,10 +28,8 @@ import us.ihmc.footstepPlanning.swing.SwingPlannerType; import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames; import us.ihmc.log.LogTools; -import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; -import us.ihmc.sensorProcessing.heightMap.HeightMapData; import java.util.ArrayList; import java.util.List; @@ -124,24 +123,23 @@ public void setWalkingStartMidPose(FramePose3D startingPose) walkingStartMidPose.getOrientation().setToYawOrientation(startingPose.getRotation().getYaw()); } - public void planToGoal(SideDependentList goalPoses, HeightMapData heightMapData, TerrainMapData terrainMapData) + public void planToGoal(SideDependentList goalPoses, EnvironmentHandler environmentHandler) { if (commandMessage.get().getUseAstarFootstepPlanner()) { - latestFootstepPlan = generateAStarFootstepPlan(heightMapData, terrainMapData, commandMessage.get().getUsePreviousPlanAsReference(), false, goalPoses); + latestFootstepPlan = generateAStarFootstepPlan(environmentHandler, commandMessage.get().getUsePreviousPlanAsReference(), false, goalPoses); } else if (commandMessage.get().getUseMonteCarloFootstepPlanner()) { - latestFootstepPlan = generateMonteCarloFootstepPlan(goalPoses, heightMapData, terrainMapData); + latestFootstepPlan = generateMonteCarloFootstepPlan(goalPoses, environmentHandler); } else { - latestFootstepPlan = generateAStarFootstepPlan(heightMapData, terrainMapData, true, false, goalPoses); + latestFootstepPlan = generateAStarFootstepPlan(environmentHandler, true, false, goalPoses); } } - public FootstepPlan generateAStarFootstepPlan(HeightMapData heightMapData, - TerrainMapData terrainMapData, + public FootstepPlan generateAStarFootstepPlan(EnvironmentHandler environmentHandler, boolean usePreviousPlanAsReference, boolean useMonteCarloPlanAsReference, SideDependentList goalPoses) @@ -164,8 +162,8 @@ public FootstepPlan generateAStarFootstepPlan(HeightMapData heightMapData, request.setAssumeFlatGround(false); request.setPlanBodyPath(false); request.setRequestedInitialStanceSide(imminentFootstepSide); - request.setHeightMapData(heightMapData); - request.setTerrainMapData(terrainMapData); + request.setHeightMapData(environmentHandler.getHeightMapData()); + request.setTerrainMapData(environmentHandler.getTerrainMapData()); request.setSnapGoalSteps(true); request.setAbortIfGoalStepSnappingFails(true); @@ -288,7 +286,7 @@ public void logFootStePlan() }, "Footstep Logger Thead"); } - public FootstepPlan generateMonteCarloFootstepPlan(SideDependentList goalPoses, HeightMapData heightMapData, TerrainMapData terrainMapData) + public FootstepPlan generateMonteCarloFootstepPlan(SideDependentList goalPoses, EnvironmentHandler environmentHandler) { MonteCarloFootstepPlannerRequest monteCarloFootstepPlannerRequest = new MonteCarloFootstepPlannerRequest(); monteCarloFootstepPlannerRequest.setTimeout(monteCarloFootstepPlanner.getParameters().getTimeoutDuration()); @@ -297,8 +295,7 @@ public FootstepPlan generateMonteCarloFootstepPlan(SideDependentList stateMachine; - private TerrainMapData terrainMap; - private HeightMapData heightMapData; + private EnvironmentHandler environmentHandler; private final AtomicBoolean resetStateMachine = new AtomicBoolean(false); @@ -84,8 +82,7 @@ public ContinuousPlannerSchedulingTask(DRCRobotModel robotModel, continuousPlanner, controllerFootstepQueueMonitor, activeMappingParameterObject.getContinuousHikingParameters(), - this::getHeightMapData, - this::getTerrainMap, + this::getEnvironmentHandler, debugger, continuousHikingLogger); State waitingtoLandState = new WaitingToLandState(ros2Helper, @@ -101,8 +98,7 @@ public ContinuousPlannerSchedulingTask(DRCRobotModel robotModel, controllerFootstepQueueMonitor, debugger, activeMappingParameterObject, - this::getHeightMapData, - this::getTerrainMap); + this::getEnvironmentHandler); // Adding the different states stateMachineFactory.addState(ContinuousHikingState.DO_NOTHING, notStartedState); @@ -181,14 +177,9 @@ private void resetStateMachine() resetStateMachine.set(true); } - public TerrainMapData getTerrainMap() + public EnvironmentHandler getEnvironmentHandler() { - return terrainMap; - } - - public HeightMapData getHeightMapData() - { - return heightMapData; + return environmentHandler; } /** @@ -200,14 +191,9 @@ private void tickStateMachine() stateMachine.doActionAndTransition(); } - public void setLatestHeightMapData(HeightMapData heightMapData) - { - this.heightMapData = heightMapData; - } - - public void setTerrainMapData(TerrainMapData terrainMapData) + public void setLatestEnvironmentHandler(EnvironmentHandler environmentHandler) { - this.terrainMap = terrainMapData; + this.environmentHandler = environmentHandler; } public void destroy() diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/StancePoseCalculator.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/StancePoseCalculator.java index fe5d65ad6b2..06477e46acf 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/StancePoseCalculator.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/StancePoseCalculator.java @@ -10,7 +10,7 @@ import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; import us.ihmc.euclid.transform.RigidBodyTransform; import us.ihmc.euclid.tuple3D.interfaces.UnitVector3DReadOnly; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.polygonSnapping.HeightMapPolygonSnapper; import us.ihmc.footstepPlanning.polygonSnapping.PolygonSnapperTools; import us.ihmc.perception.heightMap.TerrainMapData; @@ -45,7 +45,7 @@ public StancePoseCalculator(SideDependentList footPolygons) public SideDependentList getStancePoses(FramePose3DReadOnly midStancePose, TerrainMapData terrainMap, - FootstepPlannerEnvironmentHandler environmentHandler) + EnvironmentHandler environmentHandler) { reset(); populationCandidatePoses(leftPoses, midStancePose, RobotSide.LEFT); @@ -183,7 +183,7 @@ private double evaluateCostOfPoses(FramePose3D leftPose, FramePose3D rightPose, return cost; } - public void snapPosesToEnvironment(FootstepPlannerEnvironmentHandler environmentHandler) + public void snapPosesToEnvironment(EnvironmentHandler environmentHandler) { for (RobotSide side : RobotSide.values) { @@ -191,7 +191,7 @@ public void snapPosesToEnvironment(FootstepPlannerEnvironmentHandler environment } } - private void snapToEnvironment(FootstepPlannerEnvironmentHandler environmentHandler, FramePose3D poseToSnap, RobotSide side) + private void snapToEnvironment(EnvironmentHandler environmentHandler, FramePose3D poseToSnap, RobotSide side) { // Transform the polygon to be surrounding the pose we want to step on ConvexPolygon2D footPolygon = new ConvexPolygon2D(footPolygons.get(side)); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/TerrainPlanningDebugger.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/TerrainPlanningDebugger.java index 174441d3f92..62a3e2f0e5f 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/TerrainPlanningDebugger.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/TerrainPlanningDebugger.java @@ -85,9 +85,9 @@ public void setRequest(MonteCarloFootstepPlannerRequest request) return; this.request = request; - this.offsetX = (int) (request.getTerrainMapData().getTerrainMapCenter().getX() * parameters.getNodesPerMeter()); - this.offsetY = (int) (request.getTerrainMapData().getTerrainMapCenter().getY() * parameters.getNodesPerMeter()); - refresh(request.getTerrainMapData()); + this.offsetX = (int) (request.getEnvironmentHandler().getTerrainMapData().getTerrainMapCenter().getX() * parameters.getNodesPerMeter()); + this.offsetY = (int) (request.getEnvironmentHandler().getTerrainMapData().getTerrainMapCenter().getY() * parameters.getNodesPerMeter()); + refresh(request.getEnvironmentHandler().getTerrainMapData()); } public void refresh(TerrainMapData terrainMapData) @@ -314,7 +314,7 @@ public void printContactMap() if (!enabled) return; - PerceptionDebugTools.printMat("Contact Map", request.getTerrainMapData().getContactMap(), 4); + PerceptionDebugTools.printMat("Contact Map", request.getEnvironmentHandler().getTerrainMapData().getContactMap(), 4); } public void printHeightMap() @@ -322,7 +322,7 @@ public void printHeightMap() if (!enabled) return; - PerceptionDebugTools.printMat("Height Map", request.getTerrainMapData().getHeightMap(), 4); + PerceptionDebugTools.printMat("Height Map", request.getEnvironmentHandler().getTerrainMapData().getHeightMap(), 4); } public void publishContinuousWalkingStatusMessage() From 863c4e74879c2ccf938f5f0a9d0f00f63fca0e1f Mon Sep 17 00:00:00 2001 From: nkitchel Date: Fri, 2 May 2025 09:06:48 -0500 Subject: [PATCH 4/9] Moved the snapping extractor out of the RapidHeightMapManager and to a higher level closer to where its used --- .../ContinuousHikingProcess.java | 14 +++-- .../RapidHeightMapAutonomyUpdateThread.java | 8 --- .../ihmc/perception/RapidHeightMapThread.java | 8 --- .../StandAloneRealsenseProcess.java | 6 --- .../RapidHeightMapExtractorCUDA.java | 28 +++------- .../gpuHeightMap/RapidHeightMapManager.java | 40 +++++++------- .../SnappingTerrainExtractor.java | 30 +++++++---- .../tools/PerceptionMessageTools.java | 4 +- .../gpuHeightMap/RapidHeightMapExtractor.cu | 54 +++++-------------- .../gpuHeightMap/SnappingTerrainExtractor.cu | 14 ++--- .../heightMap/HeightMapParameters.java | 1 - .../heightMap/HeightMapParametersBasics.java | 5 -- .../HeightMapParametersReadOnly.java | 5 -- .../heightMap/HeightMapParameters.json | 7 +-- .../heightMap/HeightMapParametersGPU.json | 7 +-- 15 files changed, 78 insertions(+), 153 deletions(-) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java index c351bfc3786..f4cb8c3156c 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java @@ -7,6 +7,7 @@ import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; import us.ihmc.perception.StandAloneRealsenseProcess; +import us.ihmc.perception.gpuHeightMap.SnappingTerrainExtractor; import us.ihmc.robotics.physics.RobotCollisionModel; import us.ihmc.ros2.ROS2Node; import us.ihmc.ros2.ROS2NodeBuilder; @@ -26,6 +27,8 @@ public class ContinuousHikingProcess private final ContinuousPlannerSchedulingTask continuousPlannerSchedulingTask; private final StandAloneRealsenseProcess standAloneRealsenseProcess; + private final SnappingTerrainExtractor snappingTerrainExtractor; + public ContinuousHikingProcess(DRCRobotModel robotModel, RobotCollisionModel robotCollisionModel) { ROS2Node ros2Node = new ROS2NodeBuilder().build("nadia_terrain_perception_node"); @@ -41,8 +44,9 @@ public ContinuousHikingProcess(DRCRobotModel robotModel, RobotCollisionModel rob ControllerFootstepQueueMonitor controllerFootstepQueueMonitor = new ControllerFootstepQueueMonitor(ros2Node, robotModel.getSimpleRobotName()); - environmentHandler = new EnvironmentHandler(); activeMappingParameterToolBox = new ActiveMappingParameterToolBox(ros2Node, robotModel, "ForContinuousWalking"); + environmentHandler = new EnvironmentHandler(); + snappingTerrainExtractor = new SnappingTerrainExtractor(activeMappingParameterToolBox.getHeightMapParameters()); standAloneRealsenseProcess = new StandAloneRealsenseProcess(ros2Node, ros2Helper, syncedRobot, @@ -69,10 +73,13 @@ public void update() activeMappingParameterToolBox.update(); if (standAloneRealsenseProcess.getLatestHeightMapData() != null) + { environmentHandler.setHeightMapData(standAloneRealsenseProcess.getLatestHeightMapData()); + snappingTerrainExtractor.update(environmentHandler.getHeightMapData()); + } - if (standAloneRealsenseProcess.getLatestTerrainMapData() != null) - environmentHandler.setTerrainMapData(standAloneRealsenseProcess.getLatestTerrainMapData()); + if (snappingTerrainExtractor.getTerrainMapData() != null) + environmentHandler.setTerrainMapData(snappingTerrainExtractor.getTerrainMapData()); continuousPlannerSchedulingTask.setLatestEnvironmentHandler(environmentHandler); } @@ -80,6 +87,7 @@ public void update() public void destroy() { continuousPlannerSchedulingTask.destroy(); + snappingTerrainExtractor.close(); standAloneRealsenseProcess.destroy(); } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapAutonomyUpdateThread.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapAutonomyUpdateThread.java index 234751817cf..c77086a987c 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapAutonomyUpdateThread.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapAutonomyUpdateThread.java @@ -87,14 +87,6 @@ public HeightMapData getLatestHeightMapData() } } - public TerrainMapData getLatestTerrainMapData() - { - synchronized (heightMapLock) - { - return heightMapManager.getTerrainMapData(); - } - } - @Override public void kill() { diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapThread.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapThread.java index 112ded3eb42..7f9744fcf08 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapThread.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapThread.java @@ -94,14 +94,6 @@ public HeightMapData getLatestHeightMapData() } } - public TerrainMapData getLatestTerrainMapData() - { - synchronized (heightMapLock) - { - return heightMapManager.getTerrainMapData(); - } - } - public void destroy() { scheduler.shutdown(); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java index 7ef74f7e3d7..aa7446651f8 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java @@ -8,7 +8,6 @@ import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.communication.ros2.ROS2TunedRigidBodyTransform; import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; -import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.robotics.physics.RobotCollisionModel; import us.ihmc.ros2.ROS2Node; import us.ihmc.sensorProcessing.heightMap.HeightMapData; @@ -96,11 +95,6 @@ public HeightMapData getLatestHeightMapData() return rapidHeightMapThread.getLatestHeightMapData(); } - public TerrainMapData getLatestTerrainMapData() - { - return rapidHeightMapThread.getLatestTerrainMapData(); - } - public void destroy() { rapidHeightMapThread.destroy(); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java index a27d3c717d9..05928282a12 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java @@ -35,8 +35,7 @@ public class RapidHeightMapExtractorCUDA private final GpuMat globalHeightMapImage; private final GpuMat terrainCostImage; private final GpuMat contactMapImage; - private final GpuMat sensorCroppedHeightMapImage; - private final GpuMat terrainHeightMapImage; + private final GpuMat croppedHeightMapImage; private final GpuMat emptyGlobalHeightMapImage; private final CUDAProgram heightMapCUDAProgram; @@ -65,11 +64,9 @@ public class RapidHeightMapExtractorCUDA private float gridOffsetX; private int centerIndex; private int cellsPerAxisLocal; - private int cellsPerAxisTerrain; private int globalCenterIndex; private int croppedCenterIndex; private int cellsPerAxisCropped; - private int terrainCenterIndex; private int globalCellsPerAxis; private dim3 blockSize; private dim3 updateKernelGridDim; @@ -117,8 +114,7 @@ public RapidHeightMapExtractorCUDA(int mode, HeightMapParameters heightMapParame globalHeightMapImage = new GpuMat(globalCellsPerAxis, globalCellsPerAxis, opencv_core.CV_16UC1); terrainCostImage = new GpuMat(globalCellsPerAxis, globalCellsPerAxis, opencv_core.CV_8UC1); contactMapImage = new GpuMat(globalCellsPerAxis, globalCellsPerAxis, opencv_core.CV_8UC1); - sensorCroppedHeightMapImage = new GpuMat(cellsPerAxisCropped, cellsPerAxisCropped, opencv_core.CV_16UC1); - terrainHeightMapImage = new GpuMat(cellsPerAxisTerrain, cellsPerAxisTerrain, opencv_core.CV_16UC1); + croppedHeightMapImage = new GpuMat(cellsPerAxisCropped, cellsPerAxisCropped, opencv_core.CV_16UC1); emptyGlobalHeightMapImage = new GpuMat(globalCellsPerAxis, globalCellsPerAxis, opencv_core.CV_16UC1); @@ -149,9 +145,6 @@ private void recomputeDerivedParameters() globalCenterIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getInternalGlobalWidthInMeters(), heightMapParameters.getCellSizeInMeters()); globalCellsPerAxis = 2 * globalCenterIndex + 1; - terrainCenterIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getTerrainWidthInMeters(), heightMapParameters.getCellSizeInMeters()); - cellsPerAxisTerrain = 2 * terrainCenterIndex + 1; - croppedCenterIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getCroppedWidthInMeters(), heightMapParameters.getCellSizeInMeters()); cellsPerAxisCropped = 2 * croppedCenterIndex + 1; @@ -269,11 +262,9 @@ public void update(GpuMat latestDepthImageGPU, // Run the cropping kernel croppingKernel.withPointer(globalHeightMapImage.data()).withLong(globalHeightMapImage.step()); - croppingKernel.withPointer(sensorCroppedHeightMapImage.data()).withLong(sensorCroppedHeightMapImage.step()); - croppingKernel.withPointer(terrainHeightMapImage.data()).withLong(terrainHeightMapImage.step()); + croppingKernel.withPointer(croppedHeightMapImage.data()).withLong(croppedHeightMapImage.step()); croppingKernel.withPointer(parametersDevicePointer); croppingKernel.withInt(cellsPerAxisCropped); - croppingKernel.withInt(terrainHeightMapImage.rows()); error = cudaStreamSynchronize(stream); CUDATools.checkCUDAError(error); @@ -377,7 +368,6 @@ public float[] populateParameterArray(HeightMapParameters parameters, (float) parameters.getSearchWindowHeight(), (float) parameters.getSearchWindowWidth(), (float) croppedCenterIndex, - (float) terrainCenterIndex, (float) parameters.getMinClampHeight(), (float) parameters.getMaxClampHeight(), (float) parameters.getHeightOffset(), @@ -418,8 +408,7 @@ public void destroy() globalHeightMapImage.close(); terrainCostImage.close(); contactMapImage.close(); - sensorCroppedHeightMapImage.close(); - terrainHeightMapImage.close(); + croppedHeightMapImage.close(); filteredRapidHeightMapExtractor.destroy(); verticalSurfacesExtractor.destroy(); @@ -449,13 +438,8 @@ public int getSequenceNumber() return sequenceNumber; } - public GpuMat getTerrainHeightMapImage() - { - return terrainHeightMapImage; - } - - public GpuMat getVisualizedHeightMap() + public GpuMat getCroppedHeightMap() { - return sensorCroppedHeightMapImage; + return croppedHeightMapImage; } } diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java index 1a9987b672a..9dcd81ccf4b 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java @@ -18,7 +18,6 @@ import us.ihmc.perception.camera.CameraIntrinsics; import us.ihmc.perception.cuda.CUDABodyCollisionFilter; import us.ihmc.perception.filters.CUDAFlyingPointsFilter; -import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.perception.opencv.OpenCVTools; import us.ihmc.perception.tools.PerceptionMessageTools; import us.ihmc.robotModels.FullHumanoidRobotModel; @@ -28,6 +27,7 @@ import us.ihmc.scs2.simulation.collision.Collidable; import us.ihmc.sensorProcessing.heightMap.HeightMapData; import us.ihmc.sensorProcessing.heightMap.HeightMapParameters; +import us.ihmc.sensorProcessing.heightMap.HeightMapTools; import java.time.Instant; import java.util.ArrayList; @@ -40,7 +40,6 @@ public class RapidHeightMapManager { private final HeightMapParameters heightMapParameters; private final RapidHeightMapExtractorCUDA rapidHeightMapExtractor; - private final SnappingTerrainExtractor snappedFootstepsExtractor; private final Point3D sensorOrigin = new Point3D(); private final FramePose3D cameraPose = new FramePose3D(); @@ -56,6 +55,7 @@ public class RapidHeightMapManager private final ROS2Publisher heightMapPublisher; private final ImageMessage croppedHeightMapImageMessage = new ImageMessage(); private final BytePointer compressedCroppedHeightMapPointer = new BytePointer(); + private final GpuMat deviceCroppedHeightMap; public RapidHeightMapManager(ROS2Node ros2Node, RobotCollisionModel robotCollisionModel, @@ -76,8 +76,10 @@ public RapidHeightMapManager(ROS2Node ros2Node, rapidHeightMapDriftOffset = new RapidHeightMapDriftOffset(controllerFootstepQueueMonitor); flyingPointsFilter = new CUDAFlyingPointsFilter(); + int croppedCenterIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getCroppedWidthInMeters(), heightMapParameters.getCellSizeInMeters()); + int cellsPerAxisCropped = 2 * croppedCenterIndex + 1; + deviceCroppedHeightMap = new GpuMat(cellsPerAxisCropped, cellsPerAxisCropped, opencv_core.CV_16UC1); rapidHeightMapExtractor = new RapidHeightMapExtractorCUDA(1, heightMapParameters); - snappedFootstepsExtractor = new SnappingTerrainExtractor(heightMapParameters); // We use a notification to only call resetting the height map in one place heightMapPublisher = ros2Node.createPublisher(PerceptionAPI.HEIGHT_MAP_CROPPED); @@ -96,15 +98,13 @@ public RapidHeightMapManager(ROS2Node ros2Node, } } - public void updateAndPublishHeightMap(RawImage depthImage, ReferenceFrame cameraFrame, ReferenceFrame cameraZUpFrame) throws Exception + public void updateAndPublishHeightMap(RawImage depthImage, ReferenceFrame cameraFrame, ReferenceFrame cameraZUpFrame) { RawImage depthImageCopy = depthImage.get(); Mat latestDepthImage = depthImageCopy.getCpuImageMat(); Instant acquisitionTime = depthImageCopy.getAcquisitionTime(); CameraIntrinsics depthIntrinsicsCopy = depthImageCopy.getIntrinsicsCopy(); - // -------- Update the Height Map with the latest depth image from the sensor -------------- - GpuMat deviceCroppedHeightMap = new GpuMat(latestDepthImage); updateInternal(latestDepthImage, deviceCroppedHeightMap, depthIntrinsicsCopy, cameraFrame, cameraZUpFrame); // Publish the height map to anyone who is subscribing @@ -121,16 +121,18 @@ public void updateAndPublishHeightMap(RawImage depthImage, ReferenceFrame camera hostCroppedHeightMap.cols(), (float) heightMapParameters.getHeightScaleFactor()); - deviceCroppedHeightMap.close(); hostCroppedHeightMap.close(); depthImageCopy.release(); } + /** + * Update the Height Map with the latest depth image from the sensor + */ private void updateInternal(Mat latestDepthImage, GpuMat deviceHeightMapToPack, CameraIntrinsics depthIntrinsicsCopy, ReferenceFrame cameraFrame, - ReferenceFrame cameraZUpFrame) throws Exception + ReferenceFrame cameraZUpFrame) { // Option that gets triggered from a message sent from the user if (lowerHeightMapBackdropRequested.poll()) @@ -193,7 +195,7 @@ private void updateInternal(Mat latestDepthImage, groundToWorld, sensorOrigin, computeFootHeight()); - GpuMat deviceCroppedHeightMap = rapidHeightMapExtractor.getVisualizedHeightMap(); + GpuMat deviceCroppedHeightMap = rapidHeightMapExtractor.getCroppedHeightMap(); // We have used the depth image without the robot, close this to avoid creating a memory leak depthImageWithoutRobot.close(); @@ -208,30 +210,24 @@ private void updateInternal(Mat latestDepthImage, // Don't close this mat as its being used in the extractor till that finish's deviceCroppedHeightMap.convertTo(deviceHeightMapToPack, deviceCroppedHeightMap.type()); - - // Now extract the maps to be used in the footstep planning algorithm - snappedFootstepsExtractor.update(rapidHeightMapExtractor.getTerrainHeightMapImage(), sensorOrigin); - } - - public TerrainMapData getTerrainMapData() - { - return snappedFootstepsExtractor.getTerrainMapData(); } public HeightMapData getLatestHeightMapData() { HeightMapData latestHeightMapData = new HeightMapData((float) heightMapParameters.getCellSizeInMeters(), - (float) heightMapParameters.getTerrainWidthInMeters(), + (float) heightMapParameters.getCroppedWidthInMeters(), sensorOrigin.getX(), sensorOrigin.getY()); - Mat heightMapMat = getTerrainMapData().getHeightMap(); - PerceptionMessageTools.convertToHeightMapData(heightMapMat, + Mat heightMap = new Mat(); + deviceCroppedHeightMap.download(heightMap); + PerceptionMessageTools.convertToHeightMapData(heightMap, latestHeightMapData, sensorOrigin, - (float) heightMapParameters.getTerrainWidthInMeters(), + (float) heightMapParameters.getCroppedWidthInMeters(), (float) heightMapParameters.getCellSizeInMeters(), heightMapParameters); + heightMap.close(); return latestHeightMapData; } @@ -256,8 +252,8 @@ private double computeFootHeight() public void destroy() { rapidHeightMapExtractor.destroy(); - snappedFootstepsExtractor.close(); flyingPointsFilter.destroy(); + deviceCroppedHeightMap.close(); compressedCroppedHeightMapPointer.close(); } } \ No newline at end of file diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.java index 59b55bb2872..17847a1b2f7 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.java @@ -6,14 +6,16 @@ import org.bytedeco.opencv.global.opencv_core; import org.bytedeco.opencv.opencv_core.GpuMat; import org.bytedeco.opencv.opencv_core.Mat; -import us.ihmc.euclid.tuple3D.Point3D; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.euclid.tuple2D.Point2D; +import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; import us.ihmc.perception.cuda.CUDAKernel; import us.ihmc.perception.cuda.CUDAProgram; import us.ihmc.perception.cuda.CUDAStreamManager; import us.ihmc.perception.cuda.CUDATools; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.perception.steppableRegions.SteppableRegionCalculatorParameters; +import us.ihmc.perception.tools.PerceptionMessageTools; +import us.ihmc.sensorProcessing.heightMap.HeightMapData; import us.ihmc.sensorProcessing.heightMap.HeightMapParameters; import us.ihmc.sensorProcessing.heightMap.HeightMapTools; @@ -130,16 +132,22 @@ public SnappingTerrainExtractor(HeightMapParameters heightMapParameters) */ private void computeDerivedParameters() { - int terrainCenterIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getTerrainWidthInMeters(), heightMapParameters.getCellSizeInMeters()); + int terrainCenterIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getCroppedWidthInMeters(), heightMapParameters.getCellSizeInMeters()); cellsPerAxisTerrain = 2 * terrainCenterIndex + 1; } - public void update(GpuMat terrainHeightMap, Point3D sensorOrigin) + public void update(HeightMapData heightMapData) { int error; + Point2D gridCenter = heightMapData.getGridCenter(); + + Mat heightMap = PerceptionMessageTools.convertHeightMapDataToMat(heightMapData, heightMapParameters); + GpuMat gpuHeightMap = new GpuMat(); + gpuHeightMap.upload(heightMap); + // Populate parameters buffer for the snapping kernel - float[] snappingParametersArray = populateSnappingParametersArray(sensorOrigin); + float[] snappingParametersArray = populateSnappingParametersArray(gridCenter); snappingParametersHostPointer.put(snappingParametersArray); // Handle memory allocation and copy values to the GPU @@ -148,7 +156,7 @@ public void update(GpuMat terrainHeightMap, Point3D sensorOrigin) checkCUDAError(); // Pass all the parameters to the kernel so that its setup to run correctly - snappingTerrainKernel.withPointer(terrainHeightMap.data()).withLong(terrainHeightMap.step()); + snappingTerrainKernel.withPointer(gpuHeightMap.data()).withLong(gpuHeightMap.step()); snappingTerrainKernel.withPointer(steppabilityMat.data()).withLong(steppabilityMat.step()); snappingTerrainKernel.withPointer(snapHeightMat.data()).withLong(snapHeightMat.step()); snappingTerrainKernel.withPointer(snapNormalXMat.data()).withLong(snapNormalXMat.step()); @@ -168,7 +176,7 @@ public void update(GpuMat terrainHeightMap, Point3D sensorOrigin) // --------------------- Run additional kernels for even more data to be used with the terrain map ------------------ - terrainCostKernel.withPointer(terrainHeightMap.data()).withLong(terrainHeightMap.step()); + terrainCostKernel.withPointer(gpuHeightMap.data()).withLong(gpuHeightMap.step()); terrainCostKernel.withPointer(terrainCostMat.data()).withLong(terrainCostMat.step()); terrainCostKernel.withPointer(snappingParametersDevicePointer); @@ -205,7 +213,7 @@ public void update(GpuMat terrainHeightMap, Point3D sensorOrigin) checkCUDAError(); // Update the terrain map data with the new results - terrainMapData.setSensorOrigin(sensorOrigin.getX(), sensorOrigin.getY()); + terrainMapData.setSensorOrigin(gridCenter.getX(), gridCenter.getY()); // This has to be done because we start to download to the CPU, so the data on the GPU needs to be finalized error = cudaStreamSynchronize(stream); @@ -223,7 +231,7 @@ public void update(GpuMat terrainHeightMap, Point3D sensorOrigin) cpuContactMap.close(); Mat cpuHeightMap = new Mat(); - terrainHeightMap.download(cpuHeightMap); + gpuHeightMap.download(cpuHeightMap); terrainMapData.setHeightMap(cpuHeightMap); cpuHeightMap.close(); @@ -285,12 +293,12 @@ private void checkCUDAError() * @param gridCenter is the location of the sensor origin, that will be the center of our map * @return a float array with the parameters for the snapping kernels. The order matters as it needs to match the order things are defined by in the kernel */ - public float[] populateSnappingParametersArray(Tuple3DReadOnly gridCenter) + public float[] populateSnappingParametersArray(Tuple2DReadOnly gridCenter) { return new float[] {(float) gridCenter.getX(), (float) gridCenter.getY(), (float) heightMapParameters.getCellSizeInMeters(), - (float) heightMapParameters.getTerrainWidthInMeters(), + (float) heightMapParameters.getCroppedWidthInMeters(), (float) heightMapParameters.getHeightScaleFactor(), (float) heightMapParameters.getHeightOffset(), (float) steppableRegionParameters.getFootLength(), diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java b/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java index 561950f50ba..98f02b65811 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java @@ -435,8 +435,8 @@ public static void convertToHeightMapData(Mat heightMapPointer, public static Mat convertHeightMapDataToMat(HeightMapData heightMapData, HeightMapParameters heightMapParameters) { - int centerIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getTerrainWidthInMeters(), heightMapParameters.getCellSizeInMeters()); - int cellsPerAxis = 2 * centerIndex + 1; + int cellsPerAxis = heightMapData.getCellsPerAxis(); + int centerIndex = heightMapData.getCenterIndex(); // Create a new Mat object to hold the height map data Mat heightMapMat = new Mat(cellsPerAxis, cellsPerAxis, opencv_core.CV_16UC1); diff --git a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.cu index 20cc668b058..855a0cd637c 100644 --- a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.cu +++ b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.cu @@ -28,19 +28,18 @@ extern "C" #define SEARCH_WINDOW_HEIGHT 23 #define SEARCH_WINDOW_WIDTH 24 #define CROPPED_WINDOW_CENTER_INDEX 25 -#define TERRAIN_WINDOW_CENTER_INDEX 26 -#define MIN_CLAMP_HEIGHT 27 -#define MAX_CLAMP_HEIGHT 28 -#define HEIGHT_OFFSET 29 -#define STEPPING_COSINE_THRESHOLD 30 -#define STEPPING_CONTACT_THRESHOLD 31 -#define CONTACT_WINDOW_SIZE 32 -#define SPATIAL_ALPHA 33 -#define SEARCH_SKIP_SIZE 34 -#define VERTICAL_SEARCH_SIZE 35 -#define VERTICAL_SEARCH_RESOLUTION 36 -#define FAST_SEARCH_SIZE 37 -#define GROUND_HEIGHT 38 +#define MIN_CLAMP_HEIGHT 26 +#define MAX_CLAMP_HEIGHT 27 +#define HEIGHT_OFFSET 28 +#define STEPPING_COSINE_THRESHOLD 29 +#define STEPPING_CONTACT_THRESHOLD 30 +#define CONTACT_WINDOW_SIZE 31 +#define SPATIAL_ALPHA 32 +#define SEARCH_SKIP_SIZE 33 +#define VERTICAL_SEARCH_SIZE 34 +#define VERTICAL_SEARCH_RESOLUTION 35 +#define FAST_SEARCH_SIZE 36 +#define GROUND_HEIGHT 37 #define VERTICAL_FOV 1.5707963267948966f #define HORIZONTAL_FOV 6.2831853f @@ -419,8 +418,7 @@ __global__ void heightMapRegistrationKernel(unsigned short *localMap, size_t pit extern "C" __global__ void croppingKernel(unsigned short *inputMap, size_t pitchInput, unsigned short *croppedMap, size_t pitchCropped, - unsigned short *terrainObjectMap, size_t pitchTerrain, - float *params, int croppedMapXY, int terrainObjectMapXY) + float *params, int croppedMapXY) { int xIndex = blockIdx.x * blockDim.x + threadIdx.x; int yIndex = blockIdx.y * blockDim.y + threadIdx.y; @@ -453,32 +451,6 @@ extern "C" __global__ void croppingKernel(unsigned short *inputMap, size_t pitch unsigned short *croppedRow = (unsigned short *)((char *)croppedMap + xIndex * pitchCropped); croppedRow[yIndex] = 0; // Assign 0 for out-of-bounds cells } - - if (xIndex >= terrainObjectMapXY || yIndex >= terrainObjectMapXY) - return; - - globalSensorIndex = coordinate_to_indices( - make_float2(params[HEIGHT_MAP_CENTER_X], params[HEIGHT_MAP_CENTER_Y]), - make_float2(0.0f, 0.0f), - params[GLOBAL_CELL_SIZE], - static_cast(params[GLOBAL_CENTER_INDEX])); - - globalCellIndexX = globalSensorIndex.x + xIndex - (params[TERRAIN_WINDOW_CENTER_INDEX]); - globalCellIndexY = globalSensorIndex.y + yIndex - (params[TERRAIN_WINDOW_CENTER_INDEX]); - - // Check if global cell index is within bounds - if (globalCellIndexX >= 0 && globalCellIndexX < globalMapSizeX && - globalCellIndexY >= 0 && globalCellIndexY < globalMapSizeY) - { - unsigned short *inputRow = (unsigned short *)((char *)inputMap + globalCellIndexX * pitchInput); - unsigned short *visualizedRow = (unsigned short *)((char *)terrainObjectMap + xIndex * pitchTerrain); - visualizedRow[yIndex] = inputRow[globalCellIndexY]; - } - else - { - unsigned short *visualizedRow = (unsigned short *)((char *)terrainObjectMap + xIndex * pitchTerrain); - visualizedRow[yIndex] = 0; // Assign 0 for out-of-bounds cells - } } extern "C" diff --git a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.cu index 3e6ab10f0d0..8c5ada22a1b 100644 --- a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.cu +++ b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.cu @@ -6,7 +6,7 @@ extern "C" #define HEIGHT_MAP_CENTER_X 0 #define HEIGHT_MAP_CENTER_Y 1 #define CELL_SIZE_IN_CENTIMETERS 2 -#define TERRAIN_WIDTH_IN_METERS 3 +#define HEIGHT_MAP_WIDTH_IM_METERS 3 #define HEIGHT_SCALING_FACTOR 4 #define HEIGHT_OFFSET 5 #define FOOT_LENGTH 6 @@ -51,8 +51,8 @@ __global__ void computeTerrainData(unsigned short *heightMap, size_t pitchHeight int x_index = blockIdx.x * blockDim.x + threadIdx.x; int y_index = blockIdx.y * blockDim.y + threadIdx.y; - if (x_index >= terrainMapXY || y_index >= terrainMapXY) - return; +// if (x_index >= terrainMapXY || y_index >= terrainMapXY) +// return; float foot_width = params[FOOT_WIDTH]; float foot_length = params[FOOT_LENGTH]; @@ -60,7 +60,7 @@ __global__ void computeTerrainData(unsigned short *heightMap, size_t pitchHeight float map_resolution = params[CELL_SIZE_IN_CENTIMETERS]; float max_dimension = fmaxf(params[FOOT_WIDTH], params[FOOT_LENGTH]); - int terrain_map_center_index = compute_center_index(params[TERRAIN_WIDTH_IN_METERS], map_resolution); + int terrain_map_center_index = compute_center_index(params[HEIGHT_MAP_WIDTH_IM_METERS], map_resolution); float2 terrain_map_center = make_float2(params[HEIGHT_MAP_CENTER_Y], params[HEIGHT_MAP_CENTER_X]); int cells_per_axis = 2 * terrain_map_center_index + 1; @@ -322,7 +322,7 @@ __global__ void computeSteppabilityConnections(unsigned short *steppableMap, siz int x_index = blockIdx.x * blockDim.x + threadIdx.x; int y_index = blockIdx.y * blockDim.y + threadIdx.y; - int cells_per_side = 2 * compute_center_index(params[TERRAIN_WIDTH_IN_METERS], params[CELL_SIZE_IN_CENTIMETERS]) + 1; + int cells_per_side = 2 * compute_center_index(params[HEIGHT_MAP_WIDTH_IM_METERS], params[CELL_SIZE_IN_CENTIMETERS]) + 1; int2 key = make_int2(x_index, y_index); @@ -377,7 +377,7 @@ __global__ void computeTerrainCost(unsigned short *heightMap, size_t pitchHeight { int xIndex = blockIdx.x * blockDim.x + threadIdx.x; int yIndex = blockIdx.y * blockDim.y + threadIdx.y; - int cells_per_axis = 2 * compute_center_index(params[TERRAIN_WIDTH_IN_METERS], params[CELL_SIZE_IN_CENTIMETERS]) + 1; + int cells_per_axis = 2 * compute_center_index(params[HEIGHT_MAP_WIDTH_IM_METERS], params[CELL_SIZE_IN_CENTIMETERS]) + 1; // Bounds check if (xIndex >= cells_per_axis || yIndex >= cells_per_axis) @@ -452,7 +452,7 @@ __global__ void computeContactMap(unsigned char *terrainCost, size_t pitchTerrai { int xIndex = blockIdx.x * blockDim.x + threadIdx.x; int yIndex = blockIdx.y * blockDim.y + threadIdx.y; - int cells_per_axis = 2 * compute_center_index(params[TERRAIN_WIDTH_IN_METERS], params[CELL_SIZE_IN_CENTIMETERS]) + 1; + int cells_per_axis = 2 * compute_center_index(params[HEIGHT_MAP_WIDTH_IM_METERS], params[CELL_SIZE_IN_CENTIMETERS]) + 1; if (xIndex >= cells_per_axis || yIndex >= cells_per_axis) return; diff --git a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java index a45442e3624..45c282150b1 100644 --- a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java +++ b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java @@ -36,7 +36,6 @@ public class HeightMapParameters extends StoredPropertySet implements HeightMapP public static final DoubleStoredPropertyKey maxClampHeight = keys.addDoubleKey("Max Clamp Height"); public static final DoubleStoredPropertyKey cellSizeInMeters = keys.addDoubleKey("Cell size in meters"); public static final DoubleStoredPropertyKey localWidthInMeters = keys.addDoubleKey("Local width in meters"); - public static final DoubleStoredPropertyKey terrainWidthInMeters = keys.addDoubleKey("Terrain width in meters"); public static final DoubleStoredPropertyKey croppedWidthInMeters = keys.addDoubleKey("Cropped width in meters"); public static final DoubleStoredPropertyKey internalGlobalWidthInMeters = keys.addDoubleKey("Internal global width in meters"); public static final DoubleStoredPropertyKey robotCollisionCylinderRadius = keys.addDoubleKey("Robot collision cylinder radius"); diff --git a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java index 9c999f5f342..b89bfa09544 100644 --- a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java +++ b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java @@ -98,11 +98,6 @@ default void setLocalWidthInMeters(double localWidthInMeters) set(HeightMapParameters.localWidthInMeters, localWidthInMeters); } - default void setTerrainWidthInMeters(double terrainWidthInMeters) - { - set(HeightMapParameters.terrainWidthInMeters, terrainWidthInMeters); - } - default void setCroppedWidthInMeters(double croppedWidthInMeters) { set(HeightMapParameters.croppedWidthInMeters, croppedWidthInMeters); diff --git a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java index 79d8d2ec687..9d44e634b87 100644 --- a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java +++ b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java @@ -100,11 +100,6 @@ default double getLocalWidthInMeters() return get(localWidthInMeters); } - default double getTerrainWidthInMeters() - { - return get(terrainWidthInMeters); - } - default double getCroppedWidthInMeters() { return get(croppedWidthInMeters); diff --git a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json index 592068af0bd..b6ccb2c8370 100644 --- a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json +++ b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json @@ -74,13 +74,8 @@ "lowerBound": 1.0, "upperBound": 4.5 }, - "Terrain width in meters": { - "value": 4.0, - "lowerBound": 2.0, - "upperBound": 8.0 - }, "Cropped width in meters": { - "value": 6.0, + "value": 4.0, "lowerBound": 2.0, "upperBound": 8.0 }, diff --git a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json index f43c80a27d4..7ec43ebd91b 100644 --- a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json +++ b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json @@ -74,13 +74,8 @@ "lowerBound": 1.0, "upperBound": 4.5 }, - "Terrain width in meters": { - "value": 4.0, - "lowerBound": 2.0, - "upperBound": 8.0 - }, "Cropped width in meters": { - "value": 6.0, + "value": 4.0, "lowerBound": 2.0, "upperBound": 8.0 }, From 288ced8eda9bea803c05a107d8e7ac7277f79f8e Mon Sep 17 00:00:00 2001 From: nkitchel Date: Fri, 2 May 2025 09:18:57 -0500 Subject: [PATCH 5/9] Renaming the methods didn't catch everything --- .../FootstepPlannerLogLoaderTest.java | 16 +----- .../FootstepSnapAndWigglerTest.java | 22 ++++---- .../footstepSnapping/FootstepSnapperTest.java | 10 ++-- .../CliffHeightMapCostExampleCode.java | 6 +- .../HeightMapFootstepCheckerTest.java | 56 +++++++++---------- .../IdealStepCalculatorTest.java | 4 +- .../HeightMapPolygonSnapperTest.java | 14 ++--- .../HeightMapSnapWigglerTest.java | 6 +- .../swing/SwingOverHeightMapTest.java | 2 +- .../SwingOverPlanarRegionsLogViewer.java | 2 +- 10 files changed, 62 insertions(+), 76 deletions(-) diff --git a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/FootstepPlannerLogLoaderTest.java b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/FootstepPlannerLogLoaderTest.java index 8c8f4609a48..c5265c2f029 100644 --- a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/FootstepPlannerLogLoaderTest.java +++ b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/FootstepPlannerLogLoaderTest.java @@ -1,27 +1,13 @@ package us.ihmc.footstepPlanning; -import org.junit.jupiter.api.Assertions; import org.junit.jupiter.api.Disabled; import org.junit.jupiter.api.Test; -import us.ihmc.euclid.geometry.ConvexPolygon2D; -import us.ihmc.euclid.tuple3D.Point3D; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; -import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapAndWiggler; -import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapData; -import us.ihmc.footstepPlanning.graphSearch.graph.DiscreteFootstep; -import us.ihmc.footstepPlanning.graphSearch.graph.LatticePoint; -import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParameters; import us.ihmc.footstepPlanning.log.FootstepPlannerLog; import us.ihmc.footstepPlanning.log.FootstepPlannerLogLoader; -import us.ihmc.footstepPlanning.tools.PlannerTools; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.perception.tools.PerceptionDebugTools; -import us.ihmc.perception.tools.PerceptionMessageTools; -import us.ihmc.robotics.robotSide.RobotSide; -import us.ihmc.robotics.robotSide.SideDependentList; import java.io.File; -import java.net.URL; public class FootstepPlannerLogLoaderTest { @@ -41,7 +27,7 @@ public void testLoadingLogWithTerrainMapData() FootstepPlannerRequest request = new FootstepPlannerRequest(); request.setFromPacket(log.getRequestPacket()); - TerrainMapData terrainMapData = request.getTerrainMapData(); + TerrainMapData terrainMapData = request.getEnvironmentHandler().getTerrainMapData(); PerceptionDebugTools.printMat("TerrainCost", terrainMapData.getTerrainCostMap(), 10); PerceptionDebugTools.printMat("ContactMap", terrainMapData.getContactMap(), 10); diff --git a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/footstepSnapping/FootstepSnapAndWigglerTest.java b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/footstepSnapping/FootstepSnapAndWigglerTest.java index 1bfc7fcfb8c..21bb7d99148 100644 --- a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/footstepSnapping/FootstepSnapAndWigglerTest.java +++ b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/footstepSnapping/FootstepSnapAndWigglerTest.java @@ -12,7 +12,7 @@ import us.ihmc.euclid.transform.RigidBodyTransform; import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.euclid.tuple4D.Quaternion; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.graph.DiscreteFootstep; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParameters; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersBasics; @@ -46,7 +46,7 @@ public class FootstepSnapAndWigglerTest private SimulationConstructionSet scs; private YoGraphicsListRegistry graphicsListRegistry; private YoRegistry registry; - private FootstepPlannerEnvironmentHandler environmentHandler; + private EnvironmentHandler environmentHandler; private FootstepSnapAndWiggler snapAndWiggler; private final DefaultFootstepPlannerParametersBasics parameters = new DefaultFootstepPlannerParameters(); private YoDouble achievedDeltaInside; @@ -63,7 +63,7 @@ public void setup() scs.setGroundVisible(false); registry = new YoRegistry(getClass().getSimpleName()); graphicsListRegistry = new YoGraphicsListRegistry(); - environmentHandler = new FootstepPlannerEnvironmentHandler(); + environmentHandler = new EnvironmentHandler(); snapAndWiggler = new FootstepSnapAndWiggler(footPolygons, parameters, environmentHandler); graphicsListRegistry.addArtifactListsToPlotter(scs.createSimulationOverheadPlotterFactory().createOverheadPlotter().getPlotter()); @@ -79,7 +79,7 @@ public void setup() } else { - environmentHandler = new FootstepPlannerEnvironmentHandler(); + environmentHandler = new EnvironmentHandler(); snapAndWiggler = new FootstepSnapAndWiggler(footPolygons, parameters, environmentHandler); } } @@ -130,11 +130,11 @@ public void testMaximumSnapHeightOnFlatRegions() PlanarRegionsList planarRegionsList = planarRegionsListGenerator.getPlanarRegionsList(); DefaultFootstepPlannerParameters footstepPlannerParameters = new DefaultFootstepPlannerParameters(); footstepPlannerParameters.setMaximumSnapHeight(maximumSnapHeight); - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); FootstepSnapAndWiggler snapper = new FootstepSnapAndWiggler(PlannerTools.createDefaultFootPolygons(), footstepPlannerParameters, environmentHandler); HeightMapMessage heightMapMessage = PlanarRegionToHeightMapConverter.convertFromPlanarRegionsToHeightMap(planarRegionsList); - environmentHandler.setHeightMap(HeightMapMessageTools.unpackMessage(heightMapMessage)); + environmentHandler.setHeightMapData(HeightMapMessageTools.unpackMessage(heightMapMessage)); RigidBodyTransform expectedTransform = new RigidBodyTransform(); double epsilon = 1e-5; @@ -187,11 +187,11 @@ public void testMaximumSnapHeightOnSlopedRegion() PlanarRegionsList planarRegionsList = planarRegionsListGenerator.getPlanarRegionsList(); DefaultFootstepPlannerParameters footstepPlannerParameters = new DefaultFootstepPlannerParameters(); footstepPlannerParameters.setMaximumSnapHeight(maximumSnapHeight); - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); FootstepSnapAndWiggler snapper = new FootstepSnapAndWiggler(PlannerTools.createDefaultFootPolygons(), footstepPlannerParameters, environmentHandler); HeightMapMessage heightMapMessage = PlanarRegionToHeightMapConverter.convertFromPlanarRegionsToHeightMap(planarRegionsList); - environmentHandler.setHeightMap(HeightMapMessageTools.unpackMessage(heightMapMessage)); + environmentHandler.setHeightMapData(HeightMapMessageTools.unpackMessage(heightMapMessage)); RigidBodyTransform expectedTransform = new RigidBodyTransform(); double epsilon = 1e-5; @@ -229,7 +229,7 @@ public void testOverlapDetection() DefaultFootstepPlannerParameters parameters = new DefaultFootstepPlannerParameters(); parameters.setMinClearanceFromStance(0.0); - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); FootstepSnapAndWiggler snapper = new FootstepSnapAndWiggler(footPolygons, parameters, environmentHandler); FootstepSnapData snapData1 = new FootstepSnapData(); @@ -269,7 +269,7 @@ public void testSnappingToFlatGroundHeight() { double flatGroundHeight = 0.7; snapAndWiggler.setFlatGroundHeight(flatGroundHeight); - environmentHandler.setHeightMap(null); + environmentHandler.setHeightMapData(null); DiscreteFootstep footstep = new DiscreteFootstep(3, -2, 5, RobotSide.LEFT); FootstepSnapData snapData = snapAndWiggler.snapFootstep(footstep); @@ -292,7 +292,7 @@ public void testStanceFootClearance() snapAndWiggler.initialize(); HeightMapMessage heightMapMessage = PlanarRegionToHeightMapConverter.convertFromPlanarRegionsToHeightMap(planarRegionsList); - environmentHandler.setHeightMap(HeightMapMessageTools.unpackMessage(heightMapMessage)); + environmentHandler.setHeightMapData(HeightMapMessageTools.unpackMessage(heightMapMessage)); DiscreteFootstep stanceStep = new DiscreteFootstep(105, 82, 3, RobotSide.LEFT); DiscreteFootstep candidateStep = new DiscreteFootstep(109, 80, 2, RobotSide.RIGHT); diff --git a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/footstepSnapping/FootstepSnapperTest.java b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/footstepSnapping/FootstepSnapperTest.java index 4d52b5d5eb0..928b9ea3fbf 100644 --- a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/footstepSnapping/FootstepSnapperTest.java +++ b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/footstepSnapping/FootstepSnapperTest.java @@ -2,7 +2,7 @@ import org.junit.jupiter.api.Test; import us.ihmc.euclid.transform.RigidBodyTransform; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.graph.DiscreteFootstep; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParameters; import us.ihmc.footstepPlanning.tools.PlanarRegionToHeightMapConverter; @@ -28,7 +28,7 @@ public class FootstepSnapperTest @Test public void testFootstepCacheing() { - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); TestSnapper testSnapper = new TestSnapper(environmentHandler); PlanarRegionsList planarRegionsList = PlanarRegionsList.flatGround(1.0); // Applying this transform is necessary to make the height map get populated. Otherwise, the height map is left with unoccupied cells, as it's at zero @@ -37,7 +37,7 @@ public void testFootstepCacheing() transform.appendTranslation(0.0, 0.0, 0.10); planarRegionsList.getPlanarRegion(0).applyTransform(transform); HeightMapData heightMapData = HeightMapMessageTools.unpackMessage(PlanarRegionToHeightMapConverter.convertFromPlanarRegionsToHeightMap(planarRegionsList)); - environmentHandler.setHeightMap(heightMapData); + environmentHandler.setHeightMapData(heightMapData); for (int i = 0; i < xIndices.length; i++) { @@ -66,7 +66,7 @@ public void testFootstepCacheing() @Test public void testWithoutPlanarRegions() { - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); TestSnapper testSnapper = new TestSnapper(environmentHandler); for (int i = 0; i < xIndices.length; i++) @@ -91,7 +91,7 @@ private class TestSnapper extends FootstepSnapAndWiggler { boolean dirtyBit = false; - public TestSnapper(FootstepPlannerEnvironmentHandler environmentHandler) + public TestSnapper(EnvironmentHandler environmentHandler) { super(PlannerTools.createDefaultFootPolygons(), new DefaultFootstepPlannerParameters(), environmentHandler); } diff --git a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/stepChecking/CliffHeightMapCostExampleCode.java b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/stepChecking/CliffHeightMapCostExampleCode.java index ad3c29a5698..5f4c0ec2008 100644 --- a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/stepChecking/CliffHeightMapCostExampleCode.java +++ b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/stepChecking/CliffHeightMapCostExampleCode.java @@ -6,7 +6,7 @@ import us.ihmc.euclid.geometry.Plane3D; import us.ihmc.euclid.geometry.Pose3D; import us.ihmc.euclid.transform.RigidBodyTransform; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.graph.DiscreteFootstep; import us.ihmc.footstepPlanning.graphSearch.graph.DiscreteFootstepTools; import us.ihmc.footstepPlanning.polygonSnapping.HeightMapPolygonSnapper; @@ -57,8 +57,8 @@ public CliffHeightMapCostExampleCode() ConvexPolygon2D candidateFootPolygon = new ConvexPolygon2D(); DiscreteFootstepTools.getFootPolygon(footstep, footPolygon, candidateFootPolygon); - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); - environmentHandler.setHeightMap(heightMapData); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); + environmentHandler.setHeightMapData(heightMapData); RigidBodyTransform snapTransform = heightMapSnapper.snapPolygonToHeightMap(candidateFootPolygon, environmentHandler); System.out.println("snap transform"); diff --git a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/stepChecking/HeightMapFootstepCheckerTest.java b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/stepChecking/HeightMapFootstepCheckerTest.java index 90c9d981648..0025cdf3d32 100644 --- a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/stepChecking/HeightMapFootstepCheckerTest.java +++ b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/stepChecking/HeightMapFootstepCheckerTest.java @@ -12,7 +12,7 @@ import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics; import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapAndWiggler; import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapData; import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapDataReadOnly; @@ -62,10 +62,10 @@ public void testSwingingThroughObstacle0() PlanarRegionsList planarRegions = generator.getPlanarRegionsList(); HeightMapData heightMapData = HeightMapMessageTools.unpackMessage(PlanarRegionToHeightMapConverter.convertFromPlanarRegionsToHeightMap(planarRegions)); - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); FootstepSnapAndWiggler snapper = new TestSnapper(environmentHandler); HeightMapFootstepChecker checker = new HeightMapFootstepChecker(parameters, footPolygons, environmentHandler, snapper, null, registry); - environmentHandler.setHeightMap(heightMapData); + environmentHandler.setHeightMapData(heightMapData); DiscreteFootstep step0 = new DiscreteFootstep(-0.65, 0.15, 0.0, RobotSide.RIGHT); DiscreteFootstep step1 = new DiscreteFootstep(-0.65, -0.1, 0.0, RobotSide.LEFT); @@ -146,11 +146,11 @@ public void testSwingingThroughObstacle1() PlanarRegionsList planarRegions = generator.getPlanarRegionsList(); HeightMapData heightMapData = HeightMapMessageTools.unpackMessage(PlanarRegionToHeightMapConverter.convertFromPlanarRegionsToHeightMap(planarRegions)); - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); FootstepSnapAndWiggler snapper = new TestSnapper(environmentHandler); HeightMapFootstepChecker checker = new HeightMapFootstepChecker(parameters, footPolygons, environmentHandler, snapper, null, registry); - environmentHandler.setHeightMap(heightMapData); + environmentHandler.setHeightMapData(heightMapData); DiscreteFootstep step0 = new DiscreteFootstep(-0.1, -0.2, 0.0, RobotSide.RIGHT); DiscreteFootstep step1 = new DiscreteFootstep(-0.1, 0.25, 0.0, RobotSide.LEFT); @@ -216,7 +216,7 @@ public void testSwingingThroughObstacle1() @Test public void testValidNode() { - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); FootstepSnapAndWiggler snapper = new TestSnapper(environmentHandler); DefaultFootstepPlannerParametersReadOnly parameters = new DefaultFootstepPlannerParameters(); HeightMapFootstepChecker checker = new HeightMapFootstepChecker(parameters, footPolygons, environmentHandler, snapper, null, registry); @@ -237,7 +237,7 @@ public void testValidNode() @Test public void testManuallyAddedSnapDataIsValid() { - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); FootstepSnapAndWiggler snapper = new TestSnapper(environmentHandler); DefaultFootstepPlannerParametersReadOnly parameters = new DefaultFootstepPlannerParameters(); HeightMapFootstepChecker checker = new HeightMapFootstepChecker(parameters, footPolygons, environmentHandler, snapper, null, registry); @@ -254,7 +254,7 @@ public void testManuallyAddedSnapDataIsValid() @Test public void testNodesOnSameSides() { - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); DefaultFootstepPlannerParametersReadOnly parameters = new DefaultFootstepPlannerParameters(); HeightMapFootstepChecker checker = new HeightMapFootstepChecker(parameters, footPolygons, @@ -287,7 +287,7 @@ public double getMaxStepZ() } }; - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); FootstepSnapAndWiggler snapper = new TestSnapper(environmentHandler); HeightMapFootstepChecker checker = new HeightMapFootstepChecker(parameters, footPolygons, environmentHandler, snapper, null, registry); @@ -296,7 +296,7 @@ public double getMaxStepZ() planarRegionsListGenerator.addRectangle(1.0, 1.0); PlanarRegionsList dummyRegions = planarRegionsListGenerator.getPlanarRegionsList(); HeightMapData heightMapData = HeightMapMessageTools.unpackMessage(PlanarRegionToHeightMapConverter.convertFromPlanarRegionsToHeightMap(dummyRegions)); - environmentHandler.setHeightMap(heightMapData); + environmentHandler.setHeightMapData(heightMapData); DiscreteFootstep step0 = new DiscreteFootstep(0.2, -0.2, 0.0, RobotSide.RIGHT); DiscreteFootstep step1 = new DiscreteFootstep(0.2, 0.2, 0.0, RobotSide.LEFT); @@ -329,7 +329,7 @@ public double getMaxStepZ() public void testTooSmallFoothold() { DefaultFootstepPlannerParametersReadOnly parameters = new DefaultFootstepPlannerParameters(); - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); FootstepSnapAndWiggler snapper = new TestSnapper(environmentHandler); HeightMapFootstepChecker checker = new HeightMapFootstepChecker(parameters, footPolygons, environmentHandler, snapper, null, registry); @@ -338,7 +338,7 @@ public void testTooSmallFoothold() planarRegionsListGenerator.addRectangle(1.0, 1.0); PlanarRegionsList dummyRegions = planarRegionsListGenerator.getPlanarRegionsList(); HeightMapData heightMapData = HeightMapMessageTools.unpackMessage(PlanarRegionToHeightMapConverter.convertFromPlanarRegionsToHeightMap(dummyRegions)); - environmentHandler.setHeightMap(heightMapData); + environmentHandler.setHeightMapData(heightMapData); double minFoothold = parameters.getMinFootholdPercent(); @@ -400,7 +400,7 @@ public boolean getWiggleWhilePlanning() } }; - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); FootstepSnapAndWiggler snapper = new FootstepSnapAndWiggler(footPolygons, parameters, environmentHandler); testSnappingToInclinedPlane(parameters, snapper, environmentHandler); } @@ -424,14 +424,14 @@ public boolean getWiggleWhilePlanning() } }; - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); FootstepSnapAndWiggler snapper = new FootstepSnapAndWiggler(footPolygons, parameters, environmentHandler); testSnappingToInclinedPlane(parameters, snapper, environmentHandler); } public void testSnappingToInclinedPlane(DefaultFootstepPlannerParametersBasics parameters, FootstepSnapAndWiggler snapper, - FootstepPlannerEnvironmentHandler environmentHandler) + EnvironmentHandler environmentHandler) { RigidBodyTransform transformToWorld = new RigidBodyTransform(); ConvexPolygon2D polygon = new ConvexPolygon2D(); @@ -454,7 +454,7 @@ public void testSnappingToInclinedPlane(DefaultFootstepPlannerParametersBasics p SideDependentList footPolygons = PlannerTools.createFootPolygons(footLength, footWidth); HeightMapFootstepChecker nodeChecker = new HeightMapFootstepChecker(parameters, footPolygons, environmentHandler, snapper, null, registry); - environmentHandler.setHeightMap(heightMapData); + environmentHandler.setHeightMapData(heightMapData); snapper.clearSnapData(); DiscreteFootstep step0 = new DiscreteFootstep(-0.1, -0.1, 0.0, RobotSide.RIGHT); // the previous right foot position @@ -476,7 +476,7 @@ public void testSnappingToInclinedPlane(DefaultFootstepPlannerParametersBasics p transformToWorld.appendRollRotation(rotationAngle); planarRegion.set(transformToWorld, polygons); heightMapData = HeightMapMessageTools.unpackMessage(PlanarRegionToHeightMapConverter.convertFromPlanarRegionsToHeightMap(planarRegionsList)); - environmentHandler.setHeightMap(heightMapData); + environmentHandler.setHeightMapData(heightMapData); snapper.clearSnapData(); assertTrue(nodeChecker.isStepValid(step2, step1, step0), "Rejected because " + nodeChecker.getRejectionReason()); @@ -486,7 +486,7 @@ public void testSnappingToInclinedPlane(DefaultFootstepPlannerParametersBasics p transformToWorld.appendPitchRotation(rotationAngle); planarRegion.set(transformToWorld, polygons); heightMapData = HeightMapMessageTools.unpackMessage(PlanarRegionToHeightMapConverter.convertFromPlanarRegionsToHeightMap(planarRegionsList)); - environmentHandler.setHeightMap(heightMapData); + environmentHandler.setHeightMapData(heightMapData); snapper.clearSnapData(); assertTrue(nodeChecker.isStepValid(step2, step1, step0)); @@ -500,7 +500,7 @@ public void testSnappingToInclinedPlane(DefaultFootstepPlannerParametersBasics p transformToWorld.appendRollRotation(rotationAngle); planarRegion.set(transformToWorld, polygons); heightMapData = HeightMapMessageTools.unpackMessage(PlanarRegionToHeightMapConverter.convertFromPlanarRegionsToHeightMap(planarRegionsList)); - environmentHandler.setHeightMap(heightMapData); + environmentHandler.setHeightMapData(heightMapData); snapper.clearSnapData(); boolean isValid = nodeChecker.isStepValid(step2, step1, step0); @@ -514,7 +514,7 @@ public void testSnappingToInclinedPlane(DefaultFootstepPlannerParametersBasics p transformToWorld.appendPitchRotation(rotationAngle); planarRegion.set(transformToWorld, polygons); heightMapData = HeightMapMessageTools.unpackMessage(PlanarRegionToHeightMapConverter.convertFromPlanarRegionsToHeightMap(planarRegionsList)); - environmentHandler.setHeightMap(heightMapData); + environmentHandler.setHeightMapData(heightMapData); snapper.clearSnapData(); assertFalse(nodeChecker.isStepValid(step2, step1, step0), "rotation = " + rotationAngle); @@ -527,7 +527,7 @@ public void testSnappingToInclinedPlane(DefaultFootstepPlannerParametersBasics p transformToWorld.appendRollRotation(rotationAngle); planarRegion.set(transformToWorld, polygons); heightMapData = HeightMapMessageTools.unpackMessage(PlanarRegionToHeightMapConverter.convertFromPlanarRegionsToHeightMap(planarRegionsList)); - environmentHandler.setHeightMap(heightMapData); + environmentHandler.setHeightMapData(heightMapData); snapper.clearSnapData(); assertFalse(nodeChecker.isStepValid(step2, step1, step0), "rotation = " + rotationAngle); @@ -537,7 +537,7 @@ public void testSnappingToInclinedPlane(DefaultFootstepPlannerParametersBasics p transformToWorld.appendPitchRotation(rotationAngle); planarRegion.set(transformToWorld, polygons); heightMapData = HeightMapMessageTools.unpackMessage(PlanarRegionToHeightMapConverter.convertFromPlanarRegionsToHeightMap(planarRegionsList)); - environmentHandler.setHeightMap(heightMapData); + environmentHandler.setHeightMapData(heightMapData); snapper.clearSnapData(); assertFalse(nodeChecker.isStepValid(step2, step1, step0), "rotation = " + rotationAngle); @@ -556,7 +556,7 @@ public void testSnappingToInclinedPlane(DefaultFootstepPlannerParametersBasics p heightMapData = HeightMapMessageTools.unpackMessage(PlanarRegionToHeightMapConverter.convertFromPlanarRegionsToHeightMap(planarRegionsList, PlanarRegionToHeightMapConverter.defaultResolution, Double.NaN)); - environmentHandler.setHeightMap(heightMapData); + environmentHandler.setHeightMapData(heightMapData); snapper.clearSnapData(); Vector3D vertical = new Vector3D(0.0, 0.0, 1.0); @@ -607,7 +607,7 @@ public void testWiggleMinimumThreshold() double footWidth = 0.1; SideDependentList footPolygons = PlannerTools.createFootPolygons(footLength, footWidth); - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); FootstepSnapAndWiggler snapper = new FootstepSnapAndWiggler(footPolygons, footstepPlannerParameters, environmentHandler); HeightMapFootstepChecker checker = new HeightMapFootstepChecker(footstepPlannerParameters, footPolygons, @@ -631,7 +631,7 @@ public void testWiggleMinimumThreshold() planarRegionsListGenerator.addRectangle(regionX, regionY); PlanarRegionsList regions = planarRegionsListGenerator.getPlanarRegionsList(); HeightMapData heightMapData = HeightMapMessageTools.unpackMessage(PlanarRegionToHeightMapConverter.convertFromPlanarRegionsToHeightMap(regions)); - environmentHandler.setHeightMap(heightMapData); + environmentHandler.setHeightMapData(heightMapData); snapper.initialize(); snapper.addSnapData(step0, new FootstepSnapData(new RigidBodyTransform())); boolean valid = checker.isStepValid(step1, step0, null); @@ -647,7 +647,7 @@ public void testWiggleMinimumThreshold() planarRegionsListGenerator.addRectangle(regionX, regionY); regions = planarRegionsListGenerator.getPlanarRegionsList(); heightMapData = HeightMapMessageTools.unpackMessage(PlanarRegionToHeightMapConverter.convertFromPlanarRegionsToHeightMap(regions)); - environmentHandler.setHeightMap(heightMapData); + environmentHandler.setHeightMapData(heightMapData); snapper.initialize(); snapper.addSnapData(step0, new FootstepSnapData(new RigidBodyTransform())); valid = checker.isStepValid(step1, step0, null); @@ -663,7 +663,7 @@ public void testWiggleMinimumThreshold() planarRegionsListGenerator.addRectangle(regionX, regionY); regions = planarRegionsListGenerator.getPlanarRegionsList(); heightMapData = HeightMapMessageTools.unpackMessage(PlanarRegionToHeightMapConverter.convertFromPlanarRegionsToHeightMap(regions)); - environmentHandler.setHeightMap(heightMapData); + environmentHandler.setHeightMapData(heightMapData); snapper.initialize(); snapper.addSnapData(step0, new FootstepSnapData(new RigidBodyTransform())); valid = checker.isStepValid(step1, step0, null); @@ -672,7 +672,7 @@ public void testWiggleMinimumThreshold() private class TestSnapper extends FootstepSnapAndWiggler { - public TestSnapper(FootstepPlannerEnvironmentHandler environmentHandler) + public TestSnapper(EnvironmentHandler environmentHandler) { super(PlannerTools.createDefaultFootPolygons(), new DefaultFootstepPlannerParameters(), environmentHandler); } diff --git a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/IdealStepCalculatorTest.java b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/IdealStepCalculatorTest.java index d8f936ad51c..e2d88a7bff4 100644 --- a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/IdealStepCalculatorTest.java +++ b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/IdealStepCalculatorTest.java @@ -5,7 +5,7 @@ import us.ihmc.commons.MathTools; import us.ihmc.euclid.geometry.Pose3D; import us.ihmc.euclid.tools.EuclidCoreRandomTools; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.graph.DiscreteFootstep; import us.ihmc.footstepPlanning.graphSearch.stepChecking.FootstepCheckerInterface; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParameters; @@ -37,7 +37,7 @@ public void testStanceWidthAndLength() Pose3D goalPose = new Pose3D(0.5 * pathLength, 0.0, 0.0, 0.0, 0.0, 0.0); bodyPathPlanHolder.setPoseWaypoints(Arrays.asList(startPose, goalPose)); - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); IdealStepCalculator idealStepCalculator = new IdealStepCalculator(footstepPlannerParameters, checker, bodyPathPlanHolder, diff --git a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/polygonSnapping/HeightMapPolygonSnapperTest.java b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/polygonSnapping/HeightMapPolygonSnapperTest.java index 02db7174432..4c053cd6119 100644 --- a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/polygonSnapping/HeightMapPolygonSnapperTest.java +++ b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/polygonSnapping/HeightMapPolygonSnapperTest.java @@ -11,7 +11,7 @@ import us.ihmc.euclid.transform.RigidBodyTransform; import us.ihmc.euclid.tuple3D.Point3D; import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.sensorProcessing.heightMap.HeightMapData; import us.ihmc.sensorProcessing.heightMap.HeightMapTools; @@ -62,8 +62,8 @@ public void testSnappingToPlane() } HeightMapPolygonSnapper snapper = new HeightMapPolygonSnapper(); - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); - environmentHandler.setHeightMap(heightMapData); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); + environmentHandler.setHeightMapData(heightMapData); RigidBodyTransform snapTransform = snapper.snapPolygonToHeightMap(polygonToSnap, environmentHandler, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY); // Check XY position of centroid isn't changed @@ -135,8 +135,8 @@ public void testBestFitSnap() heightMapData.setHeightAt(polygonToSnap.getVertex(3).getX(), polygonToSnap.getVertex(3).getY(), offsetZ3); HeightMapPolygonSnapper snapper = new HeightMapPolygonSnapper(); - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); - environmentHandler.setHeightMap(heightMapData); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); + environmentHandler.setHeightMapData(heightMapData); snapper.snapPolygonToHeightMap(polygonToSnap, environmentHandler, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY); Assertions.assertTrue(plane.getNormal().epsilonEquals(snapper.getBestFitPlane().getNormal(), 1e-10)); @@ -173,8 +173,8 @@ public void testAreaWhenFootOverhangs() double totalArea = footLength * footWidth; HeightMapPolygonSnapper snapper = new HeightMapPolygonSnapper(); - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); - environmentHandler.setHeightMap(heightMapData); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); + environmentHandler.setHeightMapData(heightMapData); snapper.snapPolygonToHeightMap(polygonToSnap, environmentHandler, 0.05, Math.toRadians(45.0)); Assertions.assertTrue(snapper.getAreaFraction() * totalArea >= polygonToSnap.getArea()); diff --git a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/polygonSnapping/HeightMapSnapWigglerTest.java b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/polygonSnapping/HeightMapSnapWigglerTest.java index 62ff46b5310..450e45a6502 100644 --- a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/polygonSnapping/HeightMapSnapWigglerTest.java +++ b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/polygonSnapping/HeightMapSnapWigglerTest.java @@ -4,7 +4,7 @@ import org.junit.jupiter.api.Test; import us.ihmc.commonWalkingControlModules.polygonWiggling.WiggleParameters; import us.ihmc.euclid.geometry.ConvexPolygon2D; -import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.EnvironmentHandler; import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapData; import us.ihmc.footstepPlanning.graphSearch.graph.DiscreteFootstep; import us.ihmc.robotics.robotSide.SideDependentList; @@ -39,10 +39,10 @@ public void testWiggleWithAndWithoutOverhang() polygonToSnap.update(); SideDependentList footPolygons = new SideDependentList<>(new ConvexPolygon2D(polygonToSnap), new ConvexPolygon2D(polygonToSnap)); - FootstepPlannerEnvironmentHandler environmentHandler = new FootstepPlannerEnvironmentHandler(); + EnvironmentHandler environmentHandler = new EnvironmentHandler(); HeightMapPolygonSnapper snapper = new HeightMapPolygonSnapper(); HeightMapSnapWiggler wiggler = new HeightMapSnapWiggler(footPolygons, new WiggleParameters()); - environmentHandler.setHeightMap(heightMapData); + environmentHandler.setHeightMapData(heightMapData); FootstepSnapData snapData = new FootstepSnapData(); DiscreteFootstep footstep = new DiscreteFootstep(0.0, 0.0); diff --git a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/swing/SwingOverHeightMapTest.java b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/swing/SwingOverHeightMapTest.java index f43cd4b770b..a839ad12ca7 100644 --- a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/swing/SwingOverHeightMapTest.java +++ b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/swing/SwingOverHeightMapTest.java @@ -606,7 +606,7 @@ public double getExtraSizeHigh(Axis3D axis) }; swingPlannerParameters.set(originalSwingPlannerParameters); - HeightMapData heightMapData = request.getHeightMapData(); + HeightMapData heightMapData = request.getEnvironmentHandler().getHeightMapData(); for (double time = 0.0; time <= 1.0; time += dt) { diff --git a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/swing/SwingOverPlanarRegionsLogViewer.java b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/swing/SwingOverPlanarRegionsLogViewer.java index 8322fccf30c..4c3dfafc210 100644 --- a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/swing/SwingOverPlanarRegionsLogViewer.java +++ b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/swing/SwingOverPlanarRegionsLogViewer.java @@ -105,7 +105,7 @@ public SwingOverPlanarRegionsLogViewer(String fileName) scs.setGroundVisible(false); scs.addStaticLinkGraphics(environment.getTerrainObject3D().getLinkGraphics()); - planningModule.getSwingPlanningModule().computeSwingWaypoints(request.getHeightMapData(), + planningModule.getSwingPlanningModule().computeSwingWaypoints(request.getEnvironmentHandler().getHeightMapData(), footstepPlan, request.getStartFootPoses(), SwingPlannerType.MULTI_WAYPOINT_POSITION); From ac5da78c861eeeb2a6e262c9eb8f67c4ac427d6f Mon Sep 17 00:00:00 2001 From: nkitchel Date: Fri, 2 May 2025 12:28:00 -0500 Subject: [PATCH 6/9] Fix snapping test --- .../SnappingTerrainExtractorTest.java | 21 +++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractorTest.java b/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractorTest.java index a84fe0d10ee..1ac22843b1d 100644 --- a/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractorTest.java +++ b/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractorTest.java @@ -2,10 +2,13 @@ import org.bytedeco.opencv.global.opencv_core; import org.bytedeco.opencv.opencv_core.GpuMat; +import org.bytedeco.opencv.opencv_core.Mat; import org.bytedeco.opencv.opencv_core.Scalar; import org.junit.jupiter.api.Disabled; import org.junit.jupiter.api.Test; import us.ihmc.euclid.tuple3D.Point3D; +import us.ihmc.perception.tools.PerceptionMessageTools; +import us.ihmc.sensorProcessing.heightMap.HeightMapData; import us.ihmc.sensorProcessing.heightMap.HeightMapParameters; public class SnappingTerrainExtractorTest @@ -21,13 +24,27 @@ public void testSnappingTerrainKernelRuns() HeightMapParameters heightMapParameters = new HeightMapParameters(); SnappingTerrainExtractor snappingTerrainExtractor = new SnappingTerrainExtractor(heightMapParameters); - GpuMat fakeHeightMap = new GpuMat(500, 500, opencv_core.CV_16UC1); + GpuMat fakeHeightMap = new GpuMat(401, 401, opencv_core.CV_16UC1); fakeHeightMap.setTo(new Scalar(100)); + Mat heightMap = new Mat(401, 401, opencv_core.CV_8UC1); + fakeHeightMap.download(heightMap); Point3D heightMapCenter = new Point3D(); heightMapCenter.set(new Point3D(0.0, 0.0, 0.0)); - snappingTerrainExtractor.update(fakeHeightMap, heightMapCenter); + HeightMapData heightMapData = new HeightMapData((float) heightMapParameters.getCellSizeInMeters(), + (float) heightMapParameters.getCroppedWidthInMeters(), + 0, + 0); + + PerceptionMessageTools.convertToHeightMapData(heightMap, + heightMapData, + new Point3D(0.0, 0.0, 0.0), + (float) 4.0, + 0.02F, + heightMapParameters); + + snappingTerrainExtractor.update(heightMapData); snappingTerrainExtractor.close(); } } From ae8c44bf7f7e767f5368db0d5851ade948ef9fee Mon Sep 17 00:00:00 2001 From: nkitchel Date: Mon, 5 May 2025 15:14:37 -0500 Subject: [PATCH 7/9] Remove duplicate variable --- .../us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java | 1 - 1 file changed, 1 deletion(-) diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java index 44d18c1cbb8..1c864862f68 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java @@ -106,7 +106,6 @@ public void updateAndPublishHeightMap(RawImage depthImage, ReferenceFrame camera CameraIntrinsics depthIntrinsicsCopy = depthImageCopy.getIntrinsicsCopy(); // -------- Update the Height Map with the latest depth image from the sensor -------------- - GpuMat deviceCroppedHeightMap = new GpuMat(); updateInternal(latestDepthImage, deviceCroppedHeightMap, depthIntrinsicsCopy, cameraFrame, cameraZUpFrame); // Publish the height map to anyone who is subscribing From 3fab42770e78f3a78c4d2577c8251aa756ec0f60 Mon Sep 17 00:00:00 2001 From: nkitchel Date: Tue, 6 May 2025 08:47:17 -0500 Subject: [PATCH 8/9] Uncomment bounds check --- .../ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.cu | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.cu index 8c5ada22a1b..93467cbfaee 100644 --- a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.cu +++ b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/SnappingTerrainExtractor.cu @@ -51,8 +51,8 @@ __global__ void computeTerrainData(unsigned short *heightMap, size_t pitchHeight int x_index = blockIdx.x * blockDim.x + threadIdx.x; int y_index = blockIdx.y * blockDim.y + threadIdx.y; -// if (x_index >= terrainMapXY || y_index >= terrainMapXY) -// return; + if (x_index >= terrainMapXY || y_index >= terrainMapXY) + return; float foot_width = params[FOOT_WIDTH]; float foot_length = params[FOOT_LENGTH]; From 128263b4b9d363ef0aab46ee5e0fe9f8720d2385 Mon Sep 17 00:00:00 2001 From: nkitchel Date: Tue, 6 May 2025 08:52:23 -0500 Subject: [PATCH 9/9] Need to increase window height to go down stairs --- .../ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json index 7ec43ebd91b..01d3cf23e0c 100644 --- a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json +++ b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json @@ -5,7 +5,7 @@ "Enable alpha filter": true, "Enable vertical filter": true, "Search window height": { - "value": 400, + "value": 500, "lowerBound": 100, "upperBound": 800 },