From 80d5aa575f5c71306c912aad8267e5d98505faf6 Mon Sep 17 00:00:00 2001 From: Nick Kitchel <59104880+PotatoPeeler3000@users.noreply.github.com> Date: Mon, 16 Dec 2024 13:23:01 -0600 Subject: [PATCH 01/16] Add OpenCLManager back to the RapidHeightMapManager (#535) * Add OpenCLManager back to the RapidHeightMapManager to support both nvidia and non-nvidia gpus --- .../src/main/java/us/ihmc/PerceptionAndAutonomyProcess.java | 3 ++- .../main/java/us/ihmc/perception/RapidHeightMapManager.java | 4 +++- .../java/us/ihmc/perception/StandAloneRealsenseProcess.java | 3 ++- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/PerceptionAndAutonomyProcess.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/PerceptionAndAutonomyProcess.java index d367e784bb3..b7f9e45619b 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/PerceptionAndAutonomyProcess.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/PerceptionAndAutonomyProcess.java @@ -605,7 +605,8 @@ private void updateHeightMap() if (heightMapManager == null) { - heightMapManager = new RapidHeightMapManager(syncedRobot == null ? null : syncedRobot.getRobotModel(), + heightMapManager = new RapidHeightMapManager(openCLManager, + syncedRobot == null ? null : syncedRobot.getRobotModel(), soleFrameSuppliers.get(RobotSide.LEFT).get(), soleFrameSuppliers.get(RobotSide.RIGHT).get(), latestRealsenseDepthImage.getIntrinsicsCopy(), diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java index 513b18db185..385cb0fe0b1 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java @@ -18,6 +18,7 @@ import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractorCUDA; import us.ihmc.perception.heightMap.TerrainMapData; +import us.ihmc.perception.opencl.OpenCLManager; import us.ihmc.perception.opencv.OpenCVTools; import us.ihmc.perception.tools.PerceptionMessageTools; import us.ihmc.sensorProcessing.heightMap.HeightMapData; @@ -40,7 +41,8 @@ public class RapidHeightMapManager private final Notification resetHeightMapRequested = new Notification(); private final BytePointer compressedCroppedHeightMapPointer = new BytePointer(); - public RapidHeightMapManager(DRCRobotModel robotModel, + public RapidHeightMapManager(OpenCLManager openCLManager, + DRCRobotModel robotModel, ReferenceFrame leftFootSoleFrame, ReferenceFrame rightFootSoleFrame, CameraIntrinsics depthImageIntrinsics, 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 a3311cd3c82..8342287ab1c 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 @@ -84,7 +84,8 @@ private void updateHeightMap() RawImage latestRealsenseDepthImage = realsenseDepthImage.get(); if (heightMapManager == null) // TODO: This should be able to instantiated earlier, but it doesn't reset correctly { - heightMapManager = new RapidHeightMapManager(syncedRobot == null ? null : syncedRobot.getRobotModel(), + heightMapManager = new RapidHeightMapManager(new OpenCLManager(), + syncedRobot == null ? null : syncedRobot.getRobotModel(), soleFrameSuppliers.get(RobotSide.LEFT).get(), soleFrameSuppliers.get(RobotSide.RIGHT).get(), latestRealsenseDepthImage.getIntrinsicsCopy(), From a16626b63a357225f3fbc350d56d1ee85ca2f7da Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Mon, 16 Dec 2024 14:03:33 -0600 Subject: [PATCH 02/16] Revert "Add OpenCLManager back to the RapidHeightMapManager (#535)" This reverts commit 80d5aa575f5c71306c912aad8267e5d98505faf6. --- .../src/main/java/us/ihmc/PerceptionAndAutonomyProcess.java | 3 +-- .../main/java/us/ihmc/perception/RapidHeightMapManager.java | 4 +--- .../java/us/ihmc/perception/StandAloneRealsenseProcess.java | 3 +-- 3 files changed, 3 insertions(+), 7 deletions(-) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/PerceptionAndAutonomyProcess.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/PerceptionAndAutonomyProcess.java index b7f9e45619b..d367e784bb3 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/PerceptionAndAutonomyProcess.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/PerceptionAndAutonomyProcess.java @@ -605,8 +605,7 @@ private void updateHeightMap() if (heightMapManager == null) { - heightMapManager = new RapidHeightMapManager(openCLManager, - syncedRobot == null ? null : syncedRobot.getRobotModel(), + heightMapManager = new RapidHeightMapManager(syncedRobot == null ? null : syncedRobot.getRobotModel(), soleFrameSuppliers.get(RobotSide.LEFT).get(), soleFrameSuppliers.get(RobotSide.RIGHT).get(), latestRealsenseDepthImage.getIntrinsicsCopy(), diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java index 385cb0fe0b1..513b18db185 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java @@ -18,7 +18,6 @@ import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractorCUDA; import us.ihmc.perception.heightMap.TerrainMapData; -import us.ihmc.perception.opencl.OpenCLManager; import us.ihmc.perception.opencv.OpenCVTools; import us.ihmc.perception.tools.PerceptionMessageTools; import us.ihmc.sensorProcessing.heightMap.HeightMapData; @@ -41,8 +40,7 @@ public class RapidHeightMapManager private final Notification resetHeightMapRequested = new Notification(); private final BytePointer compressedCroppedHeightMapPointer = new BytePointer(); - public RapidHeightMapManager(OpenCLManager openCLManager, - DRCRobotModel robotModel, + public RapidHeightMapManager(DRCRobotModel robotModel, ReferenceFrame leftFootSoleFrame, ReferenceFrame rightFootSoleFrame, CameraIntrinsics depthImageIntrinsics, 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 8342287ab1c..a3311cd3c82 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 @@ -84,8 +84,7 @@ private void updateHeightMap() RawImage latestRealsenseDepthImage = realsenseDepthImage.get(); if (heightMapManager == null) // TODO: This should be able to instantiated earlier, but it doesn't reset correctly { - heightMapManager = new RapidHeightMapManager(new OpenCLManager(), - syncedRobot == null ? null : syncedRobot.getRobotModel(), + heightMapManager = new RapidHeightMapManager(syncedRobot == null ? null : syncedRobot.getRobotModel(), soleFrameSuppliers.get(RobotSide.LEFT).get(), soleFrameSuppliers.get(RobotSide.RIGHT).get(), latestRealsenseDepthImage.getIntrinsicsCopy(), From 911f815c6aa1c532c0bde331e66c437f12b8ea19 Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Mon, 16 Dec 2024 14:56:40 -0600 Subject: [PATCH 03/16] Add message about hand pose action tolerances on failure. (#537) --- .../behaviors/sequence/actions/HandPoseActionExecutor.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/HandPoseActionExecutor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/HandPoseActionExecutor.java index bea37a61b2e..7ee9e02818b 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/HandPoseActionExecutor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/sequence/actions/HandPoseActionExecutor.java @@ -268,6 +268,9 @@ public void updateCurrentlyExecuting() { state.setIsExecuting(false); state.setFailed(true); + state.getLogger().error("%s %s" + .formatted("Position error: %.3f / %.3f".formatted(trackingCalculator.getPositionError(), definition.getPositionErrorTolerance()), + "Orientation error: %.3f / %.3f".formatted(trackingCalculator.getOrientationError(), definition.getOrientationErrorTolerance()))); return; } From f9d02b06aa5eb9344e47bf0678b43bcf976a9344 Mon Sep 17 00:00:00 2001 From: Nick Kitchel <59104880+PotatoPeeler3000@users.noreply.github.com> Date: Mon, 16 Dec 2024 15:49:23 -0600 Subject: [PATCH 04/16] Added the OpenCLManager back to the RapidHeightMapManager (#538) * Add OpenCLManager back to the RapidHeightMapManager to support both nvidia and non-nvidia gpus --- .../src/main/java/us/ihmc/PerceptionAndAutonomyProcess.java | 3 ++- .../main/java/us/ihmc/perception/RapidHeightMapManager.java | 4 +++- .../java/us/ihmc/perception/StandAloneRealsenseProcess.java | 3 ++- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/PerceptionAndAutonomyProcess.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/PerceptionAndAutonomyProcess.java index d367e784bb3..b7f9e45619b 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/PerceptionAndAutonomyProcess.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/PerceptionAndAutonomyProcess.java @@ -605,7 +605,8 @@ private void updateHeightMap() if (heightMapManager == null) { - heightMapManager = new RapidHeightMapManager(syncedRobot == null ? null : syncedRobot.getRobotModel(), + heightMapManager = new RapidHeightMapManager(openCLManager, + syncedRobot == null ? null : syncedRobot.getRobotModel(), soleFrameSuppliers.get(RobotSide.LEFT).get(), soleFrameSuppliers.get(RobotSide.RIGHT).get(), latestRealsenseDepthImage.getIntrinsicsCopy(), diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java index 513b18db185..385cb0fe0b1 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java @@ -18,6 +18,7 @@ import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractorCUDA; import us.ihmc.perception.heightMap.TerrainMapData; +import us.ihmc.perception.opencl.OpenCLManager; import us.ihmc.perception.opencv.OpenCVTools; import us.ihmc.perception.tools.PerceptionMessageTools; import us.ihmc.sensorProcessing.heightMap.HeightMapData; @@ -40,7 +41,8 @@ public class RapidHeightMapManager private final Notification resetHeightMapRequested = new Notification(); private final BytePointer compressedCroppedHeightMapPointer = new BytePointer(); - public RapidHeightMapManager(DRCRobotModel robotModel, + public RapidHeightMapManager(OpenCLManager openCLManager, + DRCRobotModel robotModel, ReferenceFrame leftFootSoleFrame, ReferenceFrame rightFootSoleFrame, CameraIntrinsics depthImageIntrinsics, 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 a3311cd3c82..8342287ab1c 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 @@ -84,7 +84,8 @@ private void updateHeightMap() RawImage latestRealsenseDepthImage = realsenseDepthImage.get(); if (heightMapManager == null) // TODO: This should be able to instantiated earlier, but it doesn't reset correctly { - heightMapManager = new RapidHeightMapManager(syncedRobot == null ? null : syncedRobot.getRobotModel(), + heightMapManager = new RapidHeightMapManager(new OpenCLManager(), + syncedRobot == null ? null : syncedRobot.getRobotModel(), soleFrameSuppliers.get(RobotSide.LEFT).get(), soleFrameSuppliers.get(RobotSide.RIGHT).get(), latestRealsenseDepthImage.getIntrinsicsCopy(), From 0dbd2412a223f622f8a68b018504c0eac992db68 Mon Sep 17 00:00:00 2001 From: ds58 <30220598+ds58@users.noreply.github.com> Date: Tue, 17 Dec 2024 12:46:06 -0600 Subject: [PATCH 05/16] :arrow_up: ihmc-ros2-library 1.1.4 (#543) --- ihmc-communication/build.gradle.kts | 4 ++-- ihmc-interfaces/build.gradle.kts | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ihmc-communication/build.gradle.kts b/ihmc-communication/build.gradle.kts index f453d9ddba9..225ae686858 100644 --- a/ihmc-communication/build.gradle.kts +++ b/ihmc-communication/build.gradle.kts @@ -13,7 +13,7 @@ ihmc { mainDependencies { api("us.ihmc:ihmc-realtime:1.6.0") api("us.ihmc:ihmc-video-codecs:2.1.6") - api("us.ihmc:ros2-library:1.1.3") + api("us.ihmc:ros2-library:1.1.4") api("commons-net:commons-net:3.6") api("org.lz4:lz4-java:1.8.0") @@ -24,5 +24,5 @@ mainDependencies { testDependencies { api("us.ihmc:ihmc-robotics-toolkit-test:source") - api("us.ihmc:ros2-library-test:1.1.3") + api("us.ihmc:ros2-library-test:1.1.4") } diff --git a/ihmc-interfaces/build.gradle.kts b/ihmc-interfaces/build.gradle.kts index afaaa797875..e5f2fbcb7d9 100644 --- a/ihmc-interfaces/build.gradle.kts +++ b/ihmc-interfaces/build.gradle.kts @@ -27,8 +27,8 @@ ihmc { mainDependencies { api("us.ihmc:euclid-geometry:0.21.0") - api("us.ihmc:ihmc-pub-sub:1.1.3") - api("us.ihmc:ros2-common-interfaces:1.1.3") { + api("us.ihmc:ihmc-pub-sub:1.1.4") + api("us.ihmc:ros2-common-interfaces:1.1.4") { exclude(group = "org.junit.jupiter", module = "junit-jupiter-api") exclude(group = "org.junit.jupiter", module = "junit-jupiter-engine") exclude(group = "org.junit.platform", module = "junit-platform-commons") @@ -38,13 +38,13 @@ mainDependencies { } testDependencies { - api("us.ihmc:ros2-library:1.1.3") + api("us.ihmc:ros2-library:1.1.4") } generatorDependencies { api("us.ihmc:euclid:0.21.0") api("us.ihmc:ihmc-commons:0.35.0") - api("us.ihmc:ros2-msg-to-pubsub-generator:1.1.3") + api("us.ihmc:ros2-msg-to-pubsub-generator:1.1.4") } val generator = us.ihmc.ros2.rosidl.ROS2InterfaceGenerator() From 21bbafe35a68191bbf38604dcdc686ba38389363 Mon Sep 17 00:00:00 2001 From: Nick Kitchel <59104880+PotatoPeeler3000@users.noreply.github.com> Date: Tue, 17 Dec 2024 14:05:08 -0600 Subject: [PATCH 06/16] Remove unused fields from the ReferenceBasedIdealStepCalculator (#540) --- .../ReferenceBasedIdealStepCalculator.java | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/ReferenceBasedIdealStepCalculator.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/ReferenceBasedIdealStepCalculator.java index 2810e43d222..efb6982fc4d 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/ReferenceBasedIdealStepCalculator.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/ReferenceBasedIdealStepCalculator.java @@ -1,14 +1,12 @@ package us.ihmc.footstepPlanning.graphSearch.stepExpansion; import us.ihmc.euclid.geometry.Pose2D; -import us.ihmc.euclid.tuple2D.Point2D; import us.ihmc.footstepPlanning.FootstepPlan; import us.ihmc.footstepPlanning.PlannedFootstep; import us.ihmc.footstepPlanning.graphSearch.graph.DiscreteFootstep; import us.ihmc.footstepPlanning.graphSearch.graph.FootstepGraphNode; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersBasics; import us.ihmc.pathPlanning.graph.structure.DirectedGraph; -import us.ihmc.robotics.geometry.AngleTools; import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; @@ -20,7 +18,6 @@ public class ReferenceBasedIdealStepCalculator implements IdealStepCalculatorInt private final YoBoolean stepSideIncorrect; // This will be used to query what reference alpha to use. private final DefaultFootstepPlannerParametersBasics footstepPlannerParameters; - private final YoBoolean usingReferenceStep; private FootstepPlan referenceFootstepPlan; private DirectedGraph footstepGraph; private DiscreteFootstep nominalIdealStep; @@ -32,7 +29,6 @@ public ReferenceBasedIdealStepCalculator(DefaultFootstepPlannerParametersBasics this.nominalIdealStepCalculator = nominalIdealStepCalculator; stepSideIncorrect = new YoBoolean("stepSideIncorrect", registry); this.footstepPlannerParameters = footstepPlannerParameters; - this.usingReferenceStep = new YoBoolean("usingReferenceStep", registry); } @Override @@ -60,11 +56,6 @@ public DiscreteFootstep computeIdealStep(DiscreteFootstep stanceNode, DiscreteFo throw new RuntimeException("Invalid side on reference plan"); } - double nominalIdealStepX = nominalIdealStep.getX(); - double nominalIdealStepY = nominalIdealStep.getY(); - double nominalIdealStepYaw = nominalIdealStep.getYaw(); - double interpolatedYaw = AngleTools.interpolateAngle(nominalIdealStepYaw, referenceFootstep.getFootstepPose().getYaw(), referenceAlpha); - Pose2D nominalStepPose = new Pose2D(nominalIdealStep.getX(), nominalIdealStep.getY(), nominalIdealStep.getYaw()); Pose2D referenceStepPose = new Pose2D(referenceFootstep.getFootstepPose()); Pose2D interpolatedPose = new Pose2D(nominalStepPose); @@ -82,11 +73,6 @@ public void setFootstepGraph(DirectedGraph footstepGraph) this.footstepGraph = footstepGraph; } - public boolean isUsingReferenceStep() - { - return usingReferenceStep.getBooleanValue(); - } - public PlannedFootstep getReferenceStep(FootstepGraphNode graphNode) { if (referenceFootstepPlan == null) From 6c77253f203ff67ad0b52fb04e67672b83467bac Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Tue, 17 Dec 2024 15:15:39 -0600 Subject: [PATCH 07/16] Remove old CRDT implementations. (#544) --- .../communication/crdt/CRDTStatusString.java | 2 +- .../crdt/CRDTUnidirectionalBoolean.java | 47 ---- .../crdt/CRDTUnidirectionalDouble.java | 47 ---- .../crdt/CRDTUnidirectionalDoubleArray.java | 52 ---- .../crdt/CRDTUnidirectionalEnumField.java | 29 -- .../crdt/CRDTUnidirectionalField.java | 40 --- .../CRDTUnidirectionalImmutableField.java | 49 ---- .../crdt/CRDTUnidirectionalInteger.java | 47 ---- .../crdt/CRDTUnidirectionalLong.java | 47 ---- .../crdt/CRDTUnidirectionalMutableField.java | 40 --- .../crdt/CRDTUnidirectionalNotification.java | 82 ------ ...idirectionalOneDoFJointTrajectoryList.java | 113 -------- .../crdt/CRDTUnidirectionalPoint3D.java | 36 --- .../crdt/CRDTUnidirectionalPose3D.java | 36 --- .../crdt/CRDTUnidirectionalPoseList.java | 53 ---- .../CRDTUnidirectionalRecyclingArrayList.java | 48 ---- .../CRDTUnidirectionalRigidBodyTransform.java | 52 ---- .../crdt/CRDTUnidirectionalSE3Trajectory.java | 87 ------ .../CRDTUnidirectionalStoredPropertySet.java | 39 --- .../crdt/CRDTUnidirectionalString.java | 14 - .../crdt/CRDTUnidirectionalVector3D.java | 36 --- .../communication/crdt/DurationFreezable.java | 45 --- .../us/ihmc/communication/crdt/Freezable.java | 13 - .../crdt/RequestConfirmFreezable.java | 4 +- .../behaviorTree/BehaviorTreeNodeState.java | 2 +- .../log/BehaviorTreeNodeMessageLogger.java | 12 +- .../BehaviorTreeNodeInsertionDefinition.java | 10 +- .../BehaviorTreeTopologyOperationQueue.java | 4 +- .../BehaviorTreeTopologyOperations.java | 4 +- .../msg/BehaviorTreeMessage_.idl | 27 -- .../msg/BehaviorTreeNodeMessage_.idl | 51 ---- .../msg/dds/BehaviorTreeMessage.java | 101 ------- .../dds/BehaviorTreeMessagePubSubType.java | 146 ---------- .../msg/dds/BehaviorTreeNodeMessage.java | 257 ------------------ .../BehaviorTreeNodeMessagePubSubType.java | 196 ------------- .../behavior_msgs/msg/BehaviorTreeMessage.msg | 5 - .../msg/BehaviorTreeNodeMessage.msg | 23 -- .../behavior_msgs/msg/BehaviorTreeMessage.msg | 8 - .../msg/BehaviorTreeNodeMessage.msg | 25 -- 39 files changed, 18 insertions(+), 1911 deletions(-) delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalBoolean.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalDouble.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalDoubleArray.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalEnumField.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalField.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalImmutableField.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalInteger.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalLong.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalMutableField.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalNotification.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalOneDoFJointTrajectoryList.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalPoint3D.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalPose3D.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalPoseList.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalRecyclingArrayList.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalRigidBodyTransform.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalSE3Trajectory.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalStoredPropertySet.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalString.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalVector3D.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/DurationFreezable.java delete mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/crdt/Freezable.java delete mode 100644 ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeMessage_.idl delete mode 100644 ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeNodeMessage_.idl delete mode 100644 ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeMessage.java delete mode 100644 ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeMessagePubSubType.java delete mode 100644 ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeNodeMessage.java delete mode 100644 ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeNodeMessagePubSubType.java delete mode 100644 ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/BehaviorTreeMessage.msg delete mode 100644 ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/BehaviorTreeNodeMessage.msg delete mode 100644 ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/BehaviorTreeMessage.msg delete mode 100644 ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/BehaviorTreeNodeMessage.msg diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTStatusString.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTStatusString.java index 122aaf97b44..d7bf0b07529 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTStatusString.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTStatusString.java @@ -3,7 +3,7 @@ import us.ihmc.communication.ros2.ROS2ActorDesignation; /** - * An unidirectional String field. See {@link CRDTUnidirectionalImmutableField}. + * An status String field. See {@link CRDTStatusImmutableField}. */ public class CRDTStatusString extends CRDTStatusImmutableField { diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalBoolean.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalBoolean.java deleted file mode 100644 index 70969210045..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalBoolean.java +++ /dev/null @@ -1,47 +0,0 @@ -package us.ihmc.communication.crdt; - -import us.ihmc.communication.ros2.ROS2ActorDesignation; - -/** - * Represents a data field that should only be modified by one actor type - * and read-only for the others. - */ -public class CRDTUnidirectionalBoolean extends CRDTUnidirectionalField -{ - private boolean value; - - public CRDTUnidirectionalBoolean(ROS2ActorDesignation sideThatCanModify, RequestConfirmFreezable requestConfirmFreezable, boolean initialValue) - { - super(sideThatCanModify, requestConfirmFreezable); - - value = initialValue; - } - - public boolean getValue() - { - return value; - } - - public void setValue(boolean value) - { - if (this.value != value) - { - checkActorCanModifyAndFreeze(); - - this.value = value; - } - } - - public boolean toMessage() - { - return value; - } - - public void fromMessage(boolean value) - { - if (isNotFrozen()) - { - this.value = value; - } - } -} diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalDouble.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalDouble.java deleted file mode 100644 index 61a59604c2f..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalDouble.java +++ /dev/null @@ -1,47 +0,0 @@ -package us.ihmc.communication.crdt; - -import us.ihmc.communication.ros2.ROS2ActorDesignation; - -/** - * Represents a data field that should only be modified by one actor type - * and read-only for the others. - */ -public class CRDTUnidirectionalDouble extends CRDTUnidirectionalField -{ - private double value; - - public CRDTUnidirectionalDouble(ROS2ActorDesignation sideThatCanModify, RequestConfirmFreezable requestConfirmFreezable, double initialValue) - { - super(sideThatCanModify, requestConfirmFreezable); - - value = initialValue; - } - - public double getValue() - { - return value; - } - - public void setValue(double value) - { - if (this.value != value) - { - checkActorCanModifyAndFreeze(); - - this.value = value; - } - } - - public double toMessage() - { - return value; - } - - public void fromMessage(double value) - { - if (isNotFrozen()) - { - this.value = value; - } - } -} diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalDoubleArray.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalDoubleArray.java deleted file mode 100644 index ae31d40ea17..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalDoubleArray.java +++ /dev/null @@ -1,52 +0,0 @@ -package us.ihmc.communication.crdt; - -import us.ihmc.communication.ros2.ROS2ActorDesignation; - -/** - * Represents a double array that should only be modified by one actor type - * and read-only for the others. The internal writeable instance is kept protected - * from unchecked modifications. - */ -public class CRDTUnidirectionalDoubleArray extends CRDTUnidirectionalMutableField -{ - public CRDTUnidirectionalDoubleArray(ROS2ActorDesignation sideThatCanModify, RequestConfirmFreezable requestConfirmFreezable, int arraySize) - { - super(sideThatCanModify, requestConfirmFreezable, () -> new double[arraySize]); - } - - public double getValueReadOnly(int index) - { - return getValueInternal()[index]; - } - - /** Use to prevent unecessary freezes. */ - public void setValue(int index, double value) - { - if (getValueReadOnly(index) != value) - getValueAndFreeze()[index] = value; - } - - public int getLength() - { - return getValueInternal().length; - } - - public void toMessage(double[] messageArray) - { - for (int i = 0; i < getValueInternal().length; i++) - { - messageArray[i] = getValueInternal()[i]; - } - } - - public void fromMessage(double[] messageArray) - { - if (isNotFrozen()) - { - for (int i = 0; i < getValueInternal().length; i++) - { - getValueInternal()[i] = messageArray[i]; - } - } - } -} diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalEnumField.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalEnumField.java deleted file mode 100644 index 91f84d5bca8..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalEnumField.java +++ /dev/null @@ -1,29 +0,0 @@ -package us.ihmc.communication.crdt; - -import us.ihmc.communication.ros2.ROS2ActorDesignation; - -/** - * Represents an enum field that should only be modified by one actor type - * and read-only for the others. - */ -public class CRDTUnidirectionalEnumField> extends CRDTUnidirectionalImmutableField -{ - public CRDTUnidirectionalEnumField(ROS2ActorDesignation sideThatCanModify, RequestConfirmFreezable requestConfirmFreezable, T initialValue) - { - super(sideThatCanModify, requestConfirmFreezable, initialValue); - } - - public int toMessageOrdinal() - { - return toMessage() == null ? -1 : toMessage().ordinal(); - } - - /** - * @param messageValue i.e. message.getFieldName() - * @param enumValues T.values() - */ - public void fromMessageOrdinal(int messageValue, T[] enumValues) - { - fromMessage(messageValue == -1 ? null : enumValues[messageValue]); - } -} diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalField.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalField.java deleted file mode 100644 index 74cc49d5f5a..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalField.java +++ /dev/null @@ -1,40 +0,0 @@ -package us.ihmc.communication.crdt; - -import us.ihmc.communication.ros2.ROS2ActorDesignation; - -/** - * Base class for a field that should only be modified by one actor type - * and read-only for the others. - */ -public abstract class CRDTUnidirectionalField -{ - private final ROS2ActorDesignation sideThatCanModify; - private final RequestConfirmFreezable requestConfirmFreezable; - private final CRDTInfo crdtInfo; - - public CRDTUnidirectionalField(ROS2ActorDesignation sideThatCanModify, RequestConfirmFreezable requestConfirmFreezable) - { - this.sideThatCanModify = sideThatCanModify; - this.requestConfirmFreezable = requestConfirmFreezable; - - crdtInfo = requestConfirmFreezable.getCRDTInfo(); - } - - protected void checkActorCanModifyAndFreeze() - { - if (isModificationDisallowed()) - throw new RuntimeException("%s is not allowed to modify this value.".formatted(crdtInfo.getActorDesignation())); - - requestConfirmFreezable.freeze(); - } - - protected boolean isModificationDisallowed() - { - return sideThatCanModify != crdtInfo.getActorDesignation(); - } - - protected boolean isNotFrozen() - { - return !requestConfirmFreezable.isFrozen(); - } -} diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalImmutableField.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalImmutableField.java deleted file mode 100644 index 4467dc6b024..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalImmutableField.java +++ /dev/null @@ -1,49 +0,0 @@ -package us.ihmc.communication.crdt; - -import us.ihmc.communication.ros2.ROS2ActorDesignation; - -import java.util.Objects; - -/** - * Represents a data field that should only be modified by one actor type - * and read-only for the others. - */ -public class CRDTUnidirectionalImmutableField extends CRDTUnidirectionalField -{ - private T value; - - public CRDTUnidirectionalImmutableField(ROS2ActorDesignation sideThatCanModify, RequestConfirmFreezable requestConfirmFreezable, T initialValue) - { - super(sideThatCanModify, requestConfirmFreezable); - - value = initialValue; - } - - public T getValue() - { - return value; - } - - public void setValue(T value) - { - if (!Objects.equals(this.value, value)) // Don't want to do anything in the case nothing changed - { - checkActorCanModifyAndFreeze(); - - this.value = value; - } - } - - public T toMessage() - { - return value; - } - - public void fromMessage(T value) - { - if (isNotFrozen()) - { - this.value = value; - } - } -} diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalInteger.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalInteger.java deleted file mode 100644 index 5822caae464..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalInteger.java +++ /dev/null @@ -1,47 +0,0 @@ -package us.ihmc.communication.crdt; - -import us.ihmc.communication.ros2.ROS2ActorDesignation; - -/** - * Represents a data field that should only be modified by one actor type - * and read-only for the others. - */ -public class CRDTUnidirectionalInteger extends CRDTUnidirectionalField -{ - private int value; - - public CRDTUnidirectionalInteger(ROS2ActorDesignation sideThatCanModify, RequestConfirmFreezable requestConfirmFreezable, int initialValue) - { - super(sideThatCanModify, requestConfirmFreezable); - - value = initialValue; - } - - public int getValue() - { - return value; - } - - public void setValue(int value) - { - if (this.value != value) - { - checkActorCanModifyAndFreeze(); - - this.value = value; - } - } - - public int toMessage() - { - return value; - } - - public void fromMessage(int value) - { - if (isNotFrozen()) - { - this.value = value; - } - } -} diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalLong.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalLong.java deleted file mode 100644 index 5b57ce6e6d1..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalLong.java +++ /dev/null @@ -1,47 +0,0 @@ -package us.ihmc.communication.crdt; - -import us.ihmc.communication.ros2.ROS2ActorDesignation; - -/** - * Represents a data field that should only be modified by one actor type - * and read-only for the others. - */ -public class CRDTUnidirectionalLong extends CRDTUnidirectionalField -{ - private long value; - - public CRDTUnidirectionalLong(ROS2ActorDesignation sideThatCanModify, RequestConfirmFreezable requestConfirmFreezable, long initialValue) - { - super(sideThatCanModify, requestConfirmFreezable); - - value = initialValue; - } - - public long getValue() - { - return value; - } - - public void setValue(long value) - { - if (this.value != value) - { - checkActorCanModifyAndFreeze(); - - this.value = value; - } - } - - public long toMessage() - { - return value; - } - - public void fromMessage(long value) - { - if (isNotFrozen()) - { - this.value = value; - } - } -} diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalMutableField.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalMutableField.java deleted file mode 100644 index dc301b4186a..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalMutableField.java +++ /dev/null @@ -1,40 +0,0 @@ -package us.ihmc.communication.crdt; - -import us.ihmc.communication.ros2.ROS2ActorDesignation; - -import java.util.function.Supplier; - -/** - * Represents a mutable field that should only be modified by one actor type - * and read-only for the others. The internal writeable instance is kept protected - * from unchecked modifications. - * - * This is abstract because there is a need to provide a read-only access to this - * mutable value, which will vary by type. - */ -public abstract class CRDTUnidirectionalMutableField extends CRDTUnidirectionalField -{ - private final T value; - - public CRDTUnidirectionalMutableField(ROS2ActorDesignation sideThatCanModify, RequestConfirmFreezable requestConfirmFreezable, Supplier valueSupplier) - { - super(sideThatCanModify, requestConfirmFreezable); - - value = valueSupplier.get(); - } - - /** - * Accessing this freezes the node and assumes a modification will be made. - * Only use this if modifying the value. - */ - public T getValueAndFreeze() - { - checkActorCanModifyAndFreeze(); - return value; - } - - protected T getValueInternal() - { - return value; - } -} diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalNotification.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalNotification.java deleted file mode 100644 index a72bc3a2688..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalNotification.java +++ /dev/null @@ -1,82 +0,0 @@ -package us.ihmc.communication.crdt; - -import us.ihmc.communication.ros2.ROS2ActorDesignation; -import us.ihmc.log.LogTools; - -/** - * Represents a notification that can be set by one actor and polled by the other. - */ -public class CRDTUnidirectionalNotification -{ - private final ROS2ActorDesignation sideThatCanSet; - private final CRDTInfo crdtInfo; - private final RequestConfirmFreezable requestConfirmFreezable; - - private boolean isSet = false; - - public CRDTUnidirectionalNotification(ROS2ActorDesignation sideThatCanSet, - RequestConfirmFreezable requestConfirmFreezable) - { - this.sideThatCanSet = sideThatCanSet; - this.requestConfirmFreezable = requestConfirmFreezable; - - crdtInfo = requestConfirmFreezable.getCRDTInfo(); - } - - public boolean poll() - { - if (sideThatCanSet == crdtInfo.getActorDesignation()) - throw new RuntimeException("%s is not allowed to poll this notification.".formatted(crdtInfo.getActorDesignation())); - - boolean wasSet = isSet; - - if (wasSet) - { - LogTools.debug("%s Update #%d: Poll.".formatted(crdtInfo.getActorDesignation(), crdtInfo.getUpdateNumber())); - requestConfirmFreezable.freeze(); - isSet = false; - } - - return wasSet; - } - - public boolean peek() - { - return isSet; - } - - public void set() - { - if (sideThatCanSet != crdtInfo.getActorDesignation()) - throw new RuntimeException("%s is not allowed to set this notification.".formatted(crdtInfo.getActorDesignation())); - - if (!isSet) - { - LogTools.debug("%s Update #%d: Set.".formatted(crdtInfo.getActorDesignation(), crdtInfo.getUpdateNumber())); - requestConfirmFreezable.freeze(); - isSet = true; - } - } - - public boolean toMessage() - { - return isSet; - } - - public void fromMessage(boolean isSet) - { - if (isSet != this.isSet && !requestConfirmFreezable.isFrozen()) - { - boolean pollConfirmed = !isSet && sideThatCanSet == crdtInfo.getActorDesignation(); - boolean setRequested = isSet && sideThatCanSet != crdtInfo.getActorDesignation(); - if (pollConfirmed || setRequested) - { - LogTools.debug("%s Update #%d: %b -> %b".formatted(crdtInfo.getActorDesignation(), - crdtInfo.getUpdateNumber(), - this.isSet, - isSet)); - this.isSet = isSet; - } - } - } -} \ No newline at end of file diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalOneDoFJointTrajectoryList.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalOneDoFJointTrajectoryList.java deleted file mode 100644 index d511408e34e..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalOneDoFJointTrajectoryList.java +++ /dev/null @@ -1,113 +0,0 @@ -package us.ihmc.communication.crdt; - -import controller_msgs.msg.dds.OneDoFJointTrajectoryMessage; -import ihmc_common_msgs.msg.dds.TrajectoryPoint1DMessage; -import us.ihmc.commons.lists.RecyclingArrayList; -import us.ihmc.communication.packets.MessageTools; -import us.ihmc.communication.ros2.ROS2ActorDesignation; -import us.ihmc.idl.IDLSequence; -import us.ihmc.robotics.math.trajectories.trajectorypoints.OneDoFTrajectoryPoint; -import us.ihmc.robotics.math.trajectories.trajectorypoints.interfaces.OneDoFTrajectoryPointReadOnly; - -public class CRDTUnidirectionalOneDoFJointTrajectoryList extends CRDTUnidirectionalMutableField>> -{ - public CRDTUnidirectionalOneDoFJointTrajectoryList(ROS2ActorDesignation sideThatCanModify, RequestConfirmFreezable requestConfirmFreezable) - { - super(sideThatCanModify, requestConfirmFreezable, () -> new RecyclingArrayList<>(() -> new RecyclingArrayList<>(OneDoFTrajectoryPoint::new))); - } - - public OneDoFTrajectoryPointReadOnly getValueReadOnly(int jointIndex, int trajectoryPointIndex) - { - return getValueInternal().get(jointIndex).get(trajectoryPointIndex); - } - - public OneDoFTrajectoryPointReadOnly getFirstValueReadOnly(int jointIndex) - { - return getValueInternal().get(jointIndex).get(0); - } - - public OneDoFTrajectoryPointReadOnly getLastValueReadOnly(int jointIndex) - { - return getValueInternal().get(jointIndex).get(getNumberOfJoints() - 1); - } - - public boolean isEmpty() - { - return getValueInternal().isEmpty(); - } - - public int getNumberOfJoints() - { - return getValueInternal().size(); - } - - public int getNumberOfPoints(int jointIndex) - { - return getValueInternal().get(jointIndex).size(); - } - - public void toMessage(IDLSequence.Object trajectoryMessage) - { - trajectoryMessage.clear(); - - for (RecyclingArrayList oneDoFTrajectory : getValueInternal()) - { - OneDoFJointTrajectoryMessage oneDoFJointTrajectoryMessage = trajectoryMessage.add(); - oneDoFJointTrajectoryMessage.getTrajectoryPoints().clear(); - - for (OneDoFTrajectoryPoint oneDoFTrajectoryPoint : oneDoFTrajectory) - { - MessageTools.toMessage(oneDoFTrajectoryPoint, oneDoFJointTrajectoryMessage.getTrajectoryPoints().add()); - } - } - } - - public void fromMessage(IDLSequence.Object trajectoryMessage) - { - if (isNotFrozen()) - { - getValueInternal().clear(); - - for (OneDoFJointTrajectoryMessage oneDoFJointTrajectoryMessage : trajectoryMessage) - { - RecyclingArrayList trajectoryPointList = getValueInternal().add(); - trajectoryPointList.clear(); - - for (TrajectoryPoint1DMessage trajectoryPointMessage : oneDoFJointTrajectoryMessage.getTrajectoryPoints()) - { - MessageTools.fromMessage(trajectoryPointMessage, trajectoryPointList.add()); - } - } - } - } - - public void clear(int numberOfJoints) - { - for (RecyclingArrayList oneDoFTrajectoryPoints : getValueAndFreeze()) - { - oneDoFTrajectoryPoints.clear(); - } - - getValueAndFreeze().clear(); - for (int i = 0; i < numberOfJoints; i++) - getValueAndFreeze().add(); - } - - public void addTrajectoryPoint(int jointIndex, double position, double time) - { - OneDoFTrajectoryPoint point = getValueAndFreeze().get(jointIndex).add(); - point.setTime(time); - point.setPosition(position); - } - - public void setSingleSegmentTrajectory(int jointIndex, double startPosition, double endPosition, double trajectoryDuration) - { - getValueAndFreeze().clear(); - OneDoFTrajectoryPoint start = getValueAndFreeze().get(jointIndex).add(); - start.setTime(0.0); - start.setPosition(startPosition); - OneDoFTrajectoryPoint end = getValueAndFreeze().get(jointIndex).add(); - end.setTime(trajectoryDuration); - end.setPosition(endPosition); - } -} \ No newline at end of file diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalPoint3D.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalPoint3D.java deleted file mode 100644 index 125da767e48..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalPoint3D.java +++ /dev/null @@ -1,36 +0,0 @@ -package us.ihmc.communication.crdt; - -import us.ihmc.communication.ros2.ROS2ActorDesignation; -import us.ihmc.euclid.tuple3D.Point3D; -import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; - -/** - * Represents a Point3D that should only be modified by one actor type - * and read-only for the others. The internal writeable instance is kept protected - * from unchecked modifications. - */ -public class CRDTUnidirectionalPoint3D extends CRDTUnidirectionalMutableField -{ - public CRDTUnidirectionalPoint3D(ROS2ActorDesignation sideThatCanModify, RequestConfirmFreezable requestConfirmFreezable) - { - super(sideThatCanModify, requestConfirmFreezable, Point3D::new); - } - - public Point3DReadOnly getValueReadOnly() - { - return getValueInternal(); - } - - public void toMessage(Point3D message) - { - message.set(getValueReadOnly()); - } - - public void fromMessage(Point3D message) - { - if (isNotFrozen()) - { - getValueInternal().set(message); - } - } -} \ No newline at end of file diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalPose3D.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalPose3D.java deleted file mode 100644 index 11d316540a6..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalPose3D.java +++ /dev/null @@ -1,36 +0,0 @@ -package us.ihmc.communication.crdt; - -import us.ihmc.communication.ros2.ROS2ActorDesignation; -import us.ihmc.euclid.geometry.Pose3D; -import us.ihmc.euclid.geometry.interfaces.Pose3DReadOnly; - -/** - * Represents a Pose3D that should only be modified by one actor type - * and read-only for the others. The internal writeable instance is kept protected - * from unchecked modifications. - */ -public class CRDTUnidirectionalPose3D extends CRDTUnidirectionalMutableField -{ - public CRDTUnidirectionalPose3D(ROS2ActorDesignation sideThatCanModify, RequestConfirmFreezable requestConfirmFreezable) - { - super(sideThatCanModify, requestConfirmFreezable, Pose3D::new); - } - - public Pose3DReadOnly getValueReadOnly() - { - return getValueInternal(); - } - - public void toMessage(Pose3D poseMessage) - { - poseMessage.set(getValueReadOnly()); - } - - public void fromMessage(Pose3D poseMessage) - { - if (isNotFrozen()) - { - getValueInternal().set(poseMessage); - } - } -} \ No newline at end of file diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalPoseList.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalPoseList.java deleted file mode 100644 index 47468f347fc..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalPoseList.java +++ /dev/null @@ -1,53 +0,0 @@ -package us.ihmc.communication.crdt; - -import us.ihmc.commons.lists.RecyclingArrayList; -import us.ihmc.communication.ros2.ROS2ActorDesignation; -import us.ihmc.euclid.geometry.Pose3D; -import us.ihmc.euclid.geometry.interfaces.Pose3DReadOnly; -import us.ihmc.idl.IDLSequence; - -/** - * Represents a list of that should only be modified by one actor type - * and read-only for the others. The internal writeable instance is kept protected - * from unchecked modifications. - */ -public class CRDTUnidirectionalPoseList extends CRDTUnidirectionalMutableField> -{ - public CRDTUnidirectionalPoseList(ROS2ActorDesignation sideThatCanModify, RequestConfirmFreezable requestConfirmFreezable) - { - super(sideThatCanModify, requestConfirmFreezable, () -> new RecyclingArrayList<>(Pose3D::new)); - } - - public Pose3DReadOnly getValueReadOnly(int index) - { - return getValueInternal().get(index); - } - - public int getSize() - { - return getValueInternal().size(); - } - - public void toMessage(IDLSequence.Object trajectoryMessage) - { - trajectoryMessage.clear(); - - for (Pose3D pose3D : getValueInternal()) - { - trajectoryMessage.add().set(pose3D); - } - } - - public void fromMessage(IDLSequence.Object trajectoryMessage) - { - if (isNotFrozen()) - { - getValueInternal().clear(); - - for (Pose3D pose3D : trajectoryMessage) - { - getValueInternal().add().set(pose3D); - } - } - } -} diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalRecyclingArrayList.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalRecyclingArrayList.java deleted file mode 100644 index 0e238b987d6..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalRecyclingArrayList.java +++ /dev/null @@ -1,48 +0,0 @@ -package us.ihmc.communication.crdt; - -import us.ihmc.commons.lists.RecyclingArrayList; -import us.ihmc.communication.ros2.ROS2ActorDesignation; - -import java.util.function.Consumer; -import java.util.function.Supplier; - -/** - * Represents a list that should only be modified by one actor type - * and read-only for the others. The internal writeable instance is kept protected - * from unchecked modifications. - */ -public class CRDTUnidirectionalRecyclingArrayList extends CRDTUnidirectionalMutableField> -{ - public CRDTUnidirectionalRecyclingArrayList(ROS2ActorDesignation sideThatCanModify, - RequestConfirmFreezable requestConfirmFreezable, - Supplier> valueSupplier) - { - super(sideThatCanModify, requestConfirmFreezable, valueSupplier); - } - - public T getValueReadOnly(int index) - { - return getValueInternal().get(index); - } - - public int getSize() - { - return getValueInternal().size(); - } - - /** - * Used only for preallocating using {@link us.ihmc.commons.lists.RecyclingArrayListTools#getUnsafe}. - */ - public RecyclingArrayList getValueUnsafe() - { - return getValueInternal(); - } - - public void fromMessage(Consumer> valueConsumer) - { - if (isModificationDisallowed()) - { - valueConsumer.accept(getValueInternal()); - } - } -} \ No newline at end of file diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalRigidBodyTransform.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalRigidBodyTransform.java deleted file mode 100644 index 9255a6e4b4f..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalRigidBodyTransform.java +++ /dev/null @@ -1,52 +0,0 @@ -package us.ihmc.communication.crdt; - -import controller_msgs.msg.dds.RigidBodyTransformMessage; -import us.ihmc.communication.packets.MessageTools; -import us.ihmc.communication.ros2.ROS2ActorDesignation; -import us.ihmc.euclid.geometry.Pose3D; -import us.ihmc.euclid.transform.RigidBodyTransform; -import us.ihmc.euclid.transform.interfaces.RigidBodyTransformReadOnly; - -/** - * Represents a RigidBodyTransform that should only be modified by one actor type - * and read-only for the others. The internal writeable instance is kept protected - * from unchecked modifications. - */ -public class CRDTUnidirectionalRigidBodyTransform extends CRDTUnidirectionalMutableField -{ - public CRDTUnidirectionalRigidBodyTransform(ROS2ActorDesignation sideThatCanModify, RequestConfirmFreezable requestConfirmFreezable) - { - super(sideThatCanModify, requestConfirmFreezable, RigidBodyTransform::new); - } - - public RigidBodyTransformReadOnly getValueReadOnly() - { - return getValueInternal(); - } - - public void toMessage(Pose3D poseMessage) - { - poseMessage.set(getValueReadOnly()); - } - - public void toMessage(RigidBodyTransformMessage rigidBodyTransformMessage) - { - MessageTools.toMessage(getValueInternal(), rigidBodyTransformMessage); - } - - public void fromMessage(RigidBodyTransformMessage rigidBodyTransformMessage) - { - if (isNotFrozen()) - { - MessageTools.toEuclid(rigidBodyTransformMessage, getValueInternal()); - } - } - - public void fromMessage(Pose3D poseMessage) - { - if (isNotFrozen()) - { - getValueInternal().set(poseMessage); - } - } -} diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalSE3Trajectory.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalSE3Trajectory.java deleted file mode 100644 index 6bff8e8b563..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalSE3Trajectory.java +++ /dev/null @@ -1,87 +0,0 @@ -package us.ihmc.communication.crdt; - -import ihmc_common_msgs.msg.dds.SE3TrajectoryPointMessage; -import us.ihmc.commons.lists.RecyclingArrayList; -import us.ihmc.communication.packets.MessageTools; -import us.ihmc.communication.ros2.ROS2ActorDesignation; -import us.ihmc.euclid.transform.interfaces.RigidBodyTransformReadOnly; -import us.ihmc.idl.IDLSequence; -import us.ihmc.robotics.math.trajectories.trajectorypoints.SE3TrajectoryPoint; -import us.ihmc.robotics.math.trajectories.trajectorypoints.interfaces.SE3TrajectoryPointReadOnly; - -public class CRDTUnidirectionalSE3Trajectory extends CRDTUnidirectionalMutableField> -{ - public CRDTUnidirectionalSE3Trajectory(ROS2ActorDesignation sideThatCanModify, RequestConfirmFreezable requestConfirmFreezable) - { - super(sideThatCanModify, requestConfirmFreezable, () -> new RecyclingArrayList<>(SE3TrajectoryPoint::new)); - } - - public SE3TrajectoryPointReadOnly getValueReadOnly(int index) - { - return getValueInternal().get(index); - } - - public SE3TrajectoryPointReadOnly getFirstValueReadOnly() - { - return getValueInternal().get(0); - } - - public SE3TrajectoryPointReadOnly getLastValueReadOnly() - { - return getValueInternal().get(getSize() - 1); - } - - public boolean isEmpty() - { - return getValueInternal().isEmpty(); - } - - public int getSize() - { - return getValueInternal().size(); - } - - public void toMessage(IDLSequence.Object trajectoryMessage) - { - trajectoryMessage.clear(); - - for (SE3TrajectoryPointReadOnly trajectoryPoint : getValueInternal()) - { - MessageTools.toMessage(trajectoryPoint, trajectoryMessage.add()); - } - } - - public void fromMessage(IDLSequence.Object trajectoryMessage) - { - if (isNotFrozen()) - { - getValueInternal().clear(); - - for (SE3TrajectoryPointMessage trajectoryPointMessage : trajectoryMessage) - { - MessageTools.fromMessage(trajectoryPointMessage, getValueInternal().add()); - } - } - } - - public void addTrajectoryPoint(RigidBodyTransformReadOnly pose, double time) - { - SE3TrajectoryPoint point = getValueAndFreeze().add(); - point.setTime(time); - point.getPosition().set(pose.getTranslation()); - point.getOrientation().set(pose.getRotation()); - } - - public void setSingleSegmentTrajectory(RigidBodyTransformReadOnly startPose, RigidBodyTransformReadOnly endPose, double trajectoryDuration) - { - getValueAndFreeze().clear(); - SE3TrajectoryPoint start = getValueAndFreeze().add(); - start.setTime(0.0); - start.getPosition().set(startPose.getTranslation()); - start.getOrientation().set(startPose.getRotation()); - SE3TrajectoryPoint end = getValueAndFreeze().add(); - end.setTime(trajectoryDuration); - end.getPosition().set(endPose.getTranslation()); - end.getOrientation().set(endPose.getRotation()); - } -} \ No newline at end of file diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalStoredPropertySet.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalStoredPropertySet.java deleted file mode 100644 index 3040b5ccf04..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalStoredPropertySet.java +++ /dev/null @@ -1,39 +0,0 @@ -package us.ihmc.communication.crdt; - -import ihmc_common_msgs.msg.dds.PrimitiveDataVectorMessage; -import us.ihmc.communication.property.StoredPropertySetMessageTools; -import us.ihmc.communication.ros2.ROS2ActorDesignation; -import us.ihmc.tools.property.StoredPropertySetBasics; -import us.ihmc.tools.property.StoredPropertySetReadOnly; - -public class CRDTUnidirectionalStoredPropertySet extends CRDTUnidirectionalMutableField -{ - private final StoredPropertySetBasics storedPropertySet; - - public CRDTUnidirectionalStoredPropertySet(ROS2ActorDesignation sideThatCanModify, - RequestConfirmFreezable requestConfirmFreezable, - StoredPropertySetBasics storedPropertySet) - { - super(sideThatCanModify, requestConfirmFreezable, () -> storedPropertySet); - - this.storedPropertySet = storedPropertySet; - } - - public StoredPropertySetReadOnly getValueReadOnly() - { - return storedPropertySet; - } - - public void toMessage(PrimitiveDataVectorMessage message) - { - StoredPropertySetMessageTools.toMessage(message, storedPropertySet); - } - - public void fromMessage(PrimitiveDataVectorMessage message) - { - if (isNotFrozen()) - { - StoredPropertySetMessageTools.fromMessage(message, storedPropertySet); - } - } -} diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalString.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalString.java deleted file mode 100644 index 359abe33992..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalString.java +++ /dev/null @@ -1,14 +0,0 @@ -package us.ihmc.communication.crdt; - -import us.ihmc.communication.ros2.ROS2ActorDesignation; - -/** - * An unidirectional String field. See {@link CRDTUnidirectionalImmutableField}. - */ -public class CRDTUnidirectionalString extends CRDTUnidirectionalImmutableField -{ - public CRDTUnidirectionalString(ROS2ActorDesignation sideThatCanModify, RequestConfirmFreezable requestConfirmFreezable, String initialValue) - { - super(sideThatCanModify, requestConfirmFreezable, initialValue); - } -} diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalVector3D.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalVector3D.java deleted file mode 100644 index 2495104890a..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/CRDTUnidirectionalVector3D.java +++ /dev/null @@ -1,36 +0,0 @@ -package us.ihmc.communication.crdt; - -import us.ihmc.communication.ros2.ROS2ActorDesignation; -import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; - -/** - * Represents a Vector3D that should only be modified by one actor type - * and read-only for the others. The internal writeable instance is kept protected - * from unchecked modifications. - */ -public class CRDTUnidirectionalVector3D extends CRDTUnidirectionalMutableField -{ - public CRDTUnidirectionalVector3D(ROS2ActorDesignation sideThatCanModify, RequestConfirmFreezable requestConfirmFreezable) - { - super(sideThatCanModify, requestConfirmFreezable, Vector3D::new); - } - - public Vector3DReadOnly getValueReadOnly() - { - return getValueInternal(); - } - - public void toMessage(Vector3D message) - { - message.set(getValueReadOnly()); - } - - public void fromMessage(Vector3D message) - { - if (isNotFrozen()) - { - getValueInternal().set(message); - } - } -} \ No newline at end of file diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/DurationFreezable.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/DurationFreezable.java deleted file mode 100644 index ba2142af913..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/DurationFreezable.java +++ /dev/null @@ -1,45 +0,0 @@ -package us.ihmc.communication.crdt; - -import us.ihmc.commons.Conversions; - -/** - * Implementation of {@link Freezable} that stays frozen for a predefined - * period of time and then unfreezes. - */ -public class DurationFreezable implements Freezable -{ - /** - * Certain changes to this node will cause a freeze of that data - * from being modified from incoming messages. - * Things are usually being synced at 30 Hz or faster, so 1 second - * should allow plenty of time for changes to propagate. - */ - public static final double FREEZE_DURATION_ON_MODIFICATION = 1.0; - /** - * This time is used as a time of modification of this node's - * data so it won't accept updates from other sources for a short period of time. - * This is to allow the changes to propagate elsewhere. - * - * Setting it to MIN_VALUE effectively unfreezes the node. - */ - private double freezeTime = Double.MIN_VALUE; - - @Override - public void freeze() - { - freezeTime = Conversions.nanosecondsToSeconds(System.nanoTime()); - } - - @Override - public boolean isFrozen() - { - double now = Conversions.nanosecondsToSeconds(System.nanoTime()); - - return now - freezeTime < FREEZE_DURATION_ON_MODIFICATION; - } - - public void unfreeze() - { - freezeTime = Double.MIN_VALUE; - } -} diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/Freezable.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/Freezable.java deleted file mode 100644 index 84defe2745c..00000000000 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/Freezable.java +++ /dev/null @@ -1,13 +0,0 @@ -package us.ihmc.communication.crdt; - -/** - * Part of a simple CRDT algorithm where something is frozen when modified - * to allow the changes to propagate without being immediately overritten - * with out of date data. - */ -public interface Freezable -{ - void freeze(); - - boolean isFrozen(); -} diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/RequestConfirmFreezable.java b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/RequestConfirmFreezable.java index 548243141e4..d3eb0ea27c3 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/crdt/RequestConfirmFreezable.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/crdt/RequestConfirmFreezable.java @@ -46,7 +46,7 @@ * in the CRDT graph. *

*/ -public class RequestConfirmFreezable implements Freezable +public class RequestConfirmFreezable { private final CRDTInfo crdtInfo; private final MutableLong nextRequestID = new MutableLong(); @@ -66,7 +66,6 @@ public RequestConfirmFreezable(CRDTInfo crdtInfo) this.crdtInfo = crdtInfo; } - @Override public void freeze() { updateNumberToUnfreeze = crdtInfo.getUpdateNumber() + crdtInfo.getMaxFreezeDuration(); @@ -97,7 +96,6 @@ public void freeze() this.getClass().getSimpleName())); } - @Override public boolean isFrozen() { boolean isFrozen = crdtInfo.getUpdateNumber() < updateNumberToUnfreeze; diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeNodeState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeNodeState.java index 572d16d9c17..6383f8b6d3d 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeNodeState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/BehaviorTreeNodeState.java @@ -43,7 +43,7 @@ public BehaviorTreeNodeState(long id, D definition, CRDTInfo crdtInfo) this.id = id; this.definition = definition; - logger = new BehaviorTreeNodeMessageLogger(definition); + logger = new BehaviorTreeNodeMessageLogger(crdtInfo); } /** Used to determine if the node's full data needs to be sent. */ diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/log/BehaviorTreeNodeMessageLogger.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/log/BehaviorTreeNodeMessageLogger.java index e7231e5908e..51bf43a7929 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/log/BehaviorTreeNodeMessageLogger.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/log/BehaviorTreeNodeMessageLogger.java @@ -3,8 +3,8 @@ import behavior_msgs.msg.dds.BehaviorTreeLogMessage; import org.apache.logging.log4j.Level; import org.apache.logging.log4j.message.ParameterizedMessage; -import us.ihmc.communication.crdt.CRDTUnidirectionalField; -import us.ihmc.communication.crdt.RequestConfirmFreezable; +import us.ihmc.communication.crdt.CRDTInfo; +import us.ihmc.communication.crdt.CRDTStatusField; import us.ihmc.communication.packets.MessageTools; import us.ihmc.communication.ros2.ROS2ActorDesignation; import us.ihmc.log.LogTools; @@ -18,20 +18,20 @@ /** * An unidirectional CRDT for log messages. */ -public class BehaviorTreeNodeMessageLogger extends CRDTUnidirectionalField implements LogToolsWriteOnly +public class BehaviorTreeNodeMessageLogger extends CRDTStatusField implements LogToolsWriteOnly { public record LogMessage(Instant instant, Level level, String message) { } private final LinkedList recentMessages = new LinkedList<>(); private Instant lastPrintedTimestamp = Instant.now(); - public BehaviorTreeNodeMessageLogger(RequestConfirmFreezable freezable) + public BehaviorTreeNodeMessageLogger(CRDTInfo crdtInfo) { - super(ROS2ActorDesignation.ROBOT, freezable); + super(ROS2ActorDesignation.ROBOT, crdtInfo); } private void queueLogMessage(Level level, String message) { - checkActorCanModifyAndFreeze(); + checkActorCanModifyAndMarkHasStatus(); Instant now = Instant.now(); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/topology/BehaviorTreeNodeInsertionDefinition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/topology/BehaviorTreeNodeInsertionDefinition.java index 7a99ac4008b..0591021bda5 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/topology/BehaviorTreeNodeInsertionDefinition.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/topology/BehaviorTreeNodeInsertionDefinition.java @@ -1,7 +1,7 @@ package us.ihmc.behaviors.behaviorTree.topology; import us.ihmc.behaviors.behaviorTree.BehaviorTreeNodeLayer; -import us.ihmc.communication.crdt.Freezable; +import us.ihmc.communication.crdt.RequestConfirmFreezable; import java.util.function.Consumer; @@ -14,14 +14,14 @@ public class BehaviorTreeNodeInsertionDefinition> rootNodeSetter; private BehaviorTreeNodeInsertionType insertionType; private int insertionIndex; public static > BehaviorTreeNodeInsertionDefinition build(R nodeToInsert, - Freezable freezableRootNodeHolder, + RequestConfirmFreezable freezableRootNodeHolder, Consumer> rootNodeSetter, R relativeNode, BehaviorTreeNodeInsertionType insertionType) @@ -55,7 +55,7 @@ private BehaviorTreeNodeInsertionDefinition() } private void setupInsertRoot(T newRoot, - Freezable freezableRootNodeHolder, + RequestConfirmFreezable freezableRootNodeHolder, Consumer> rootNodeSetter) { this.nodeToInsert = newRoot; @@ -126,7 +126,7 @@ public T getParent() return rootNodeSetter; } - public Freezable getFreezableRootNodeHolder() + public RequestConfirmFreezable getFreezableRootNodeHolder() { return freezableRootNodeHolder; } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/topology/BehaviorTreeTopologyOperationQueue.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/topology/BehaviorTreeTopologyOperationQueue.java index 63adbfd3205..c404e07443f 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/topology/BehaviorTreeTopologyOperationQueue.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/topology/BehaviorTreeTopologyOperationQueue.java @@ -1,7 +1,7 @@ package us.ihmc.behaviors.behaviorTree.topology; import us.ihmc.behaviors.behaviorTree.BehaviorTreeNodeLayer; -import us.ihmc.communication.crdt.Freezable; +import us.ihmc.communication.crdt.RequestConfirmFreezable; import java.util.LinkedList; import java.util.Queue; @@ -54,7 +54,7 @@ public boolean performAllQueuedOperations() public > void queueSetAndFreezeRootNode(BehaviorTreeNodeLayer node, Consumer> setter, - Freezable freezableRootHolder) + RequestConfirmFreezable freezableRootHolder) { topologyOperationQueue.add(() -> { diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/topology/BehaviorTreeTopologyOperations.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/topology/BehaviorTreeTopologyOperations.java index 124dd8ea116..415bb3e5f88 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/topology/BehaviorTreeTopologyOperations.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/topology/BehaviorTreeTopologyOperations.java @@ -2,7 +2,7 @@ import us.ihmc.behaviors.behaviorTree.BehaviorTreeNode; import us.ihmc.behaviors.behaviorTree.BehaviorTreeNodeLayer; -import us.ihmc.communication.crdt.Freezable; +import us.ihmc.communication.crdt.RequestConfirmFreezable; import us.ihmc.tools.Destroyable; /** @@ -164,7 +164,7 @@ public static > void insertBasic(T nodeToAdd, T pa public static void attemptFreeze(Object thingToFreeze) { - if (thingToFreeze instanceof Freezable freezable) + if (thingToFreeze instanceof RequestConfirmFreezable freezable) freezable.freeze(); } diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeMessage_.idl deleted file mode 100644 index f833be23578..00000000000 --- a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeMessage_.idl +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef __behavior_msgs__msg__BehaviorTreeMessage__idl__ -#define __behavior_msgs__msg__BehaviorTreeMessage__idl__ - -#include "behavior_msgs/msg/./BehaviorTreeNodeMessage_.idl" -module behavior_msgs -{ - module msg - { - module dds - { - - @TypeCode(type="behavior_msgs::msg::dds_::BehaviorTreeMessage_") - struct BehaviorTreeMessage - { - /** - * DEPRECATED: This is an old message replaced by BehaviorTreeStateMessage - * Nodes in the tree in "depth first" order. - * We must reconstruct the tree on the subscription end. - * This is because messages cannot be recursive. - */ - sequence nodes; - }; - }; - }; -}; - -#endif diff --git a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeNodeMessage_.idl b/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeNodeMessage_.idl deleted file mode 100644 index 43b8cb295b6..00000000000 --- a/ihmc-interfaces/src/main/generated-idl/behavior_msgs/msg/BehaviorTreeNodeMessage_.idl +++ /dev/null @@ -1,51 +0,0 @@ -#ifndef __behavior_msgs__msg__BehaviorTreeNodeMessage__idl__ -#define __behavior_msgs__msg__BehaviorTreeNodeMessage__idl__ - -#include "ihmc_common_msgs/msg/./InstantMessage_.idl" -module behavior_msgs -{ - module msg - { - module dds - { - - /** - * DEPRECATED - See BehaviorTreeNodeDefinitionMessage for the new version - * We will remove this when the implementation has been migrated to the - * new structure. - */ - @TypeCode(type="behavior_msgs::msg::dds_::BehaviorTreeNodeMessage_") - struct BehaviorTreeNodeMessage - { - /** - * The number of children - * This is used for a stack data structure to re-assemble the tree. - */ - unsigned long number_of_children; - /** - * Last tick instant - */ - ihmc_common_msgs::msg::dds::InstantMessage last_tick_instant; - /** - * Name of the node - */ - string node_name; - /** - * The type of the node, as a string - */ - string node_type; - /** - * Previous node status - */ - octet previous_status; - /** - * Whether this node has been clocked - * This field is only for control flow nodes. - */ - boolean has_been_clocked; - }; - }; - }; -}; - -#endif diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeMessage.java deleted file mode 100644 index d628389b36a..00000000000 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeMessage.java +++ /dev/null @@ -1,101 +0,0 @@ -package behavior_msgs.msg.dds; - -import us.ihmc.communication.packets.Packet; -import us.ihmc.euclid.interfaces.Settable; -import us.ihmc.euclid.interfaces.EpsilonComparable; -import java.util.function.Supplier; -import us.ihmc.pubsub.TopicDataType; - -public class BehaviorTreeMessage extends Packet implements Settable, EpsilonComparable -{ - /** - * DEPRECATED: This is an old message replaced by BehaviorTreeStateMessage - * Nodes in the tree in "depth first" order. - * We must reconstruct the tree on the subscription end. - * This is because messages cannot be recursive. - */ - public us.ihmc.idl.IDLSequence.Object nodes_; - - public BehaviorTreeMessage() - { - nodes_ = new us.ihmc.idl.IDLSequence.Object (2048, new behavior_msgs.msg.dds.BehaviorTreeNodeMessagePubSubType()); - - } - - public BehaviorTreeMessage(BehaviorTreeMessage other) - { - this(); - set(other); - } - - public void set(BehaviorTreeMessage other) - { - nodes_.set(other.nodes_); - } - - - /** - * DEPRECATED: This is an old message replaced by BehaviorTreeStateMessage - * Nodes in the tree in "depth first" order. - * We must reconstruct the tree on the subscription end. - * This is because messages cannot be recursive. - */ - public us.ihmc.idl.IDLSequence.Object getNodes() - { - return nodes_; - } - - - public static Supplier getPubSubType() - { - return BehaviorTreeMessagePubSubType::new; - } - - @Override - public Supplier getPubSubTypePacket() - { - return BehaviorTreeMessagePubSubType::new; - } - - @Override - public boolean epsilonEquals(BehaviorTreeMessage other, double epsilon) - { - if(other == null) return false; - if(other == this) return true; - - if (this.nodes_.size() != other.nodes_.size()) { return false; } - else - { - for (int i = 0; i < this.nodes_.size(); i++) - { if (!this.nodes_.get(i).epsilonEquals(other.nodes_.get(i), epsilon)) return false; } - } - - return true; - } - - @Override - public boolean equals(Object other) - { - if(other == null) return false; - if(other == this) return true; - if(!(other instanceof BehaviorTreeMessage)) return false; - - BehaviorTreeMessage otherMyClass = (BehaviorTreeMessage) other; - - if (!this.nodes_.equals(otherMyClass.nodes_)) return false; - - return true; - } - - @Override - public java.lang.String toString() - { - StringBuilder builder = new StringBuilder(); - - builder.append("BehaviorTreeMessage {"); - builder.append("nodes="); - builder.append(this.nodes_); - builder.append("}"); - return builder.toString(); - } -} diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeMessagePubSubType.java deleted file mode 100644 index faf13f5e457..00000000000 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeMessagePubSubType.java +++ /dev/null @@ -1,146 +0,0 @@ -package behavior_msgs.msg.dds; - -/** -* -* Topic data type of the struct "BehaviorTreeMessage" defined in "BehaviorTreeMessage_.idl". Use this class to provide the TopicDataType to a Participant. -* -* This file was automatically generated from BehaviorTreeMessage_.idl by us.ihmc.idl.generator.IDLGenerator. -* Do not update this file directly, edit BehaviorTreeMessage_.idl instead. -* -*/ -public class BehaviorTreeMessagePubSubType implements us.ihmc.pubsub.TopicDataType -{ - public static final java.lang.String name = "behavior_msgs::msg::dds_::BehaviorTreeMessage_"; - - @Override - public final java.lang.String getDefinitionChecksum() - { - return "b72f0625f45ae2a5a81152eb30b804ef377555294879ce354a37a47d1f9705af"; - } - - @Override - public final java.lang.String getDefinitionVersion() - { - return "local"; - } - - private final us.ihmc.idl.CDR serializeCDR = new us.ihmc.idl.CDR(); - private final us.ihmc.idl.CDR deserializeCDR = new us.ihmc.idl.CDR(); - - @Override - public void serialize(behavior_msgs.msg.dds.BehaviorTreeMessage data, us.ihmc.pubsub.common.SerializedPayload serializedPayload) throws java.io.IOException - { - serializeCDR.serialize(serializedPayload); - write(data, serializeCDR); - serializeCDR.finishSerialize(); - } - - @Override - public void deserialize(us.ihmc.pubsub.common.SerializedPayload serializedPayload, behavior_msgs.msg.dds.BehaviorTreeMessage data) throws java.io.IOException - { - deserializeCDR.deserialize(serializedPayload); - read(data, deserializeCDR); - deserializeCDR.finishDeserialize(); - } - - public static int getMaxCdrSerializedSize() - { - return getMaxCdrSerializedSize(0); - } - - public static int getMaxCdrSerializedSize(int current_alignment) - { - int initial_alignment = current_alignment; - - current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4);for(int i0 = 0; i0 < 2048; ++i0) - { - current_alignment += behavior_msgs.msg.dds.BehaviorTreeNodeMessagePubSubType.getMaxCdrSerializedSize(current_alignment);} - return current_alignment - initial_alignment; - } - - public final static int getCdrSerializedSize(behavior_msgs.msg.dds.BehaviorTreeMessage data) - { - return getCdrSerializedSize(data, 0); - } - - public final static int getCdrSerializedSize(behavior_msgs.msg.dds.BehaviorTreeMessage data, int current_alignment) - { - int initial_alignment = current_alignment; - - current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); - for(int i0 = 0; i0 < data.getNodes().size(); ++i0) - { - current_alignment += behavior_msgs.msg.dds.BehaviorTreeNodeMessagePubSubType.getCdrSerializedSize(data.getNodes().get(i0), current_alignment);} - - return current_alignment - initial_alignment; - } - - public static void write(behavior_msgs.msg.dds.BehaviorTreeMessage data, us.ihmc.idl.CDR cdr) - { - if(data.getNodes().size() <= 2048) - cdr.write_type_e(data.getNodes());else - throw new RuntimeException("nodes field exceeds the maximum length"); - - } - - public static void read(behavior_msgs.msg.dds.BehaviorTreeMessage data, us.ihmc.idl.CDR cdr) - { - cdr.read_type_e(data.getNodes()); - - } - - @Override - public final void serialize(behavior_msgs.msg.dds.BehaviorTreeMessage data, us.ihmc.idl.InterchangeSerializer ser) - { - ser.write_type_e("nodes", data.getNodes()); - } - - @Override - public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_msgs.msg.dds.BehaviorTreeMessage data) - { - ser.read_type_e("nodes", data.getNodes()); - } - - public static void staticCopy(behavior_msgs.msg.dds.BehaviorTreeMessage src, behavior_msgs.msg.dds.BehaviorTreeMessage dest) - { - dest.set(src); - } - - @Override - public behavior_msgs.msg.dds.BehaviorTreeMessage createData() - { - return new behavior_msgs.msg.dds.BehaviorTreeMessage(); - } - @Override - public int getTypeSize() - { - return us.ihmc.idl.CDR.getTypeSize(getMaxCdrSerializedSize()); - } - - @Override - public java.lang.String getName() - { - return name; - } - - public void serialize(behavior_msgs.msg.dds.BehaviorTreeMessage data, us.ihmc.idl.CDR cdr) - { - write(data, cdr); - } - - public void deserialize(behavior_msgs.msg.dds.BehaviorTreeMessage data, us.ihmc.idl.CDR cdr) - { - read(data, cdr); - } - - public void copy(behavior_msgs.msg.dds.BehaviorTreeMessage src, behavior_msgs.msg.dds.BehaviorTreeMessage dest) - { - staticCopy(src, dest); - } - - @Override - public BehaviorTreeMessagePubSubType newInstance() - { - return new BehaviorTreeMessagePubSubType(); - } -} diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeNodeMessage.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeNodeMessage.java deleted file mode 100644 index c967a87c1c9..00000000000 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeNodeMessage.java +++ /dev/null @@ -1,257 +0,0 @@ -package behavior_msgs.msg.dds; - -import us.ihmc.communication.packets.Packet; -import us.ihmc.euclid.interfaces.Settable; -import us.ihmc.euclid.interfaces.EpsilonComparable; -import java.util.function.Supplier; -import us.ihmc.pubsub.TopicDataType; - -/** - * DEPRECATED - See BehaviorTreeNodeDefinitionMessage for the new version - * We will remove this when the implementation has been migrated to the - * new structure. - */ -public class BehaviorTreeNodeMessage extends Packet implements Settable, EpsilonComparable -{ - /** - * The number of children - * This is used for a stack data structure to re-assemble the tree. - */ - public long number_of_children_; - /** - * Last tick instant - */ - public ihmc_common_msgs.msg.dds.InstantMessage last_tick_instant_; - /** - * Name of the node - */ - public java.lang.StringBuilder node_name_; - /** - * The type of the node, as a string - */ - public java.lang.StringBuilder node_type_; - /** - * Previous node status - */ - public byte previous_status_; - /** - * Whether this node has been clocked - * This field is only for control flow nodes. - */ - public boolean has_been_clocked_; - - public BehaviorTreeNodeMessage() - { - last_tick_instant_ = new ihmc_common_msgs.msg.dds.InstantMessage(); - node_name_ = new java.lang.StringBuilder(255); - node_type_ = new java.lang.StringBuilder(255); - } - - public BehaviorTreeNodeMessage(BehaviorTreeNodeMessage other) - { - this(); - set(other); - } - - public void set(BehaviorTreeNodeMessage other) - { - number_of_children_ = other.number_of_children_; - - ihmc_common_msgs.msg.dds.InstantMessagePubSubType.staticCopy(other.last_tick_instant_, last_tick_instant_); - node_name_.setLength(0); - node_name_.append(other.node_name_); - - node_type_.setLength(0); - node_type_.append(other.node_type_); - - previous_status_ = other.previous_status_; - - has_been_clocked_ = other.has_been_clocked_; - - } - - /** - * The number of children - * This is used for a stack data structure to re-assemble the tree. - */ - public void setNumberOfChildren(long number_of_children) - { - number_of_children_ = number_of_children; - } - /** - * The number of children - * This is used for a stack data structure to re-assemble the tree. - */ - public long getNumberOfChildren() - { - return number_of_children_; - } - - - /** - * Last tick instant - */ - public ihmc_common_msgs.msg.dds.InstantMessage getLastTickInstant() - { - return last_tick_instant_; - } - - /** - * Name of the node - */ - public void setNodeName(java.lang.String node_name) - { - node_name_.setLength(0); - node_name_.append(node_name); - } - - /** - * Name of the node - */ - public java.lang.String getNodeNameAsString() - { - return getNodeName().toString(); - } - /** - * Name of the node - */ - public java.lang.StringBuilder getNodeName() - { - return node_name_; - } - - /** - * The type of the node, as a string - */ - public void setNodeType(java.lang.String node_type) - { - node_type_.setLength(0); - node_type_.append(node_type); - } - - /** - * The type of the node, as a string - */ - public java.lang.String getNodeTypeAsString() - { - return getNodeType().toString(); - } - /** - * The type of the node, as a string - */ - public java.lang.StringBuilder getNodeType() - { - return node_type_; - } - - /** - * Previous node status - */ - public void setPreviousStatus(byte previous_status) - { - previous_status_ = previous_status; - } - /** - * Previous node status - */ - public byte getPreviousStatus() - { - return previous_status_; - } - - /** - * Whether this node has been clocked - * This field is only for control flow nodes. - */ - public void setHasBeenClocked(boolean has_been_clocked) - { - has_been_clocked_ = has_been_clocked; - } - /** - * Whether this node has been clocked - * This field is only for control flow nodes. - */ - public boolean getHasBeenClocked() - { - return has_been_clocked_; - } - - - public static Supplier getPubSubType() - { - return BehaviorTreeNodeMessagePubSubType::new; - } - - @Override - public Supplier getPubSubTypePacket() - { - return BehaviorTreeNodeMessagePubSubType::new; - } - - @Override - public boolean epsilonEquals(BehaviorTreeNodeMessage other, double epsilon) - { - if(other == null) return false; - if(other == this) return true; - - if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.number_of_children_, other.number_of_children_, epsilon)) return false; - - if (!this.last_tick_instant_.epsilonEquals(other.last_tick_instant_, epsilon)) return false; - if (!us.ihmc.idl.IDLTools.epsilonEqualsStringBuilder(this.node_name_, other.node_name_, epsilon)) return false; - - if (!us.ihmc.idl.IDLTools.epsilonEqualsStringBuilder(this.node_type_, other.node_type_, epsilon)) return false; - - if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.previous_status_, other.previous_status_, epsilon)) return false; - - if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.has_been_clocked_, other.has_been_clocked_, epsilon)) return false; - - - return true; - } - - @Override - public boolean equals(Object other) - { - if(other == null) return false; - if(other == this) return true; - if(!(other instanceof BehaviorTreeNodeMessage)) return false; - - BehaviorTreeNodeMessage otherMyClass = (BehaviorTreeNodeMessage) other; - - if(this.number_of_children_ != otherMyClass.number_of_children_) return false; - - if (!this.last_tick_instant_.equals(otherMyClass.last_tick_instant_)) return false; - if (!us.ihmc.idl.IDLTools.equals(this.node_name_, otherMyClass.node_name_)) return false; - - if (!us.ihmc.idl.IDLTools.equals(this.node_type_, otherMyClass.node_type_)) return false; - - if(this.previous_status_ != otherMyClass.previous_status_) return false; - - if(this.has_been_clocked_ != otherMyClass.has_been_clocked_) return false; - - - return true; - } - - @Override - public java.lang.String toString() - { - StringBuilder builder = new StringBuilder(); - - builder.append("BehaviorTreeNodeMessage {"); - builder.append("number_of_children="); - builder.append(this.number_of_children_); builder.append(", "); - builder.append("last_tick_instant="); - builder.append(this.last_tick_instant_); builder.append(", "); - builder.append("node_name="); - builder.append(this.node_name_); builder.append(", "); - builder.append("node_type="); - builder.append(this.node_type_); builder.append(", "); - builder.append("previous_status="); - builder.append(this.previous_status_); builder.append(", "); - builder.append("has_been_clocked="); - builder.append(this.has_been_clocked_); - builder.append("}"); - return builder.toString(); - } -} diff --git a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeNodeMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeNodeMessagePubSubType.java deleted file mode 100644 index f053f0364cf..00000000000 --- a/ihmc-interfaces/src/main/generated-java/behavior_msgs/msg/dds/BehaviorTreeNodeMessagePubSubType.java +++ /dev/null @@ -1,196 +0,0 @@ -package behavior_msgs.msg.dds; - -/** -* -* Topic data type of the struct "BehaviorTreeNodeMessage" defined in "BehaviorTreeNodeMessage_.idl". Use this class to provide the TopicDataType to a Participant. -* -* This file was automatically generated from BehaviorTreeNodeMessage_.idl by us.ihmc.idl.generator.IDLGenerator. -* Do not update this file directly, edit BehaviorTreeNodeMessage_.idl instead. -* -*/ -public class BehaviorTreeNodeMessagePubSubType implements us.ihmc.pubsub.TopicDataType -{ - public static final java.lang.String name = "behavior_msgs::msg::dds_::BehaviorTreeNodeMessage_"; - - @Override - public final java.lang.String getDefinitionChecksum() - { - return "4d1adcc4031b33de6b581967e1755596b4356127ec05de8d0890d51c31783239"; - } - - @Override - public final java.lang.String getDefinitionVersion() - { - return "local"; - } - - private final us.ihmc.idl.CDR serializeCDR = new us.ihmc.idl.CDR(); - private final us.ihmc.idl.CDR deserializeCDR = new us.ihmc.idl.CDR(); - - @Override - public void serialize(behavior_msgs.msg.dds.BehaviorTreeNodeMessage data, us.ihmc.pubsub.common.SerializedPayload serializedPayload) throws java.io.IOException - { - serializeCDR.serialize(serializedPayload); - write(data, serializeCDR); - serializeCDR.finishSerialize(); - } - - @Override - public void deserialize(us.ihmc.pubsub.common.SerializedPayload serializedPayload, behavior_msgs.msg.dds.BehaviorTreeNodeMessage data) throws java.io.IOException - { - deserializeCDR.deserialize(serializedPayload); - read(data, deserializeCDR); - deserializeCDR.finishDeserialize(); - } - - public static int getMaxCdrSerializedSize() - { - return getMaxCdrSerializedSize(0); - } - - public static int getMaxCdrSerializedSize(int current_alignment) - { - int initial_alignment = current_alignment; - - current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); - - current_alignment += ihmc_common_msgs.msg.dds.InstantMessagePubSubType.getMaxCdrSerializedSize(current_alignment); - - current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + 255 + 1; - current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + 255 + 1; - current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); - - current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); - - - return current_alignment - initial_alignment; - } - - public final static int getCdrSerializedSize(behavior_msgs.msg.dds.BehaviorTreeNodeMessage data) - { - return getCdrSerializedSize(data, 0); - } - - public final static int getCdrSerializedSize(behavior_msgs.msg.dds.BehaviorTreeNodeMessage data, int current_alignment) - { - int initial_alignment = current_alignment; - - current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4); - - - current_alignment += ihmc_common_msgs.msg.dds.InstantMessagePubSubType.getCdrSerializedSize(data.getLastTickInstant(), current_alignment); - - current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + data.getNodeName().length() + 1; - - current_alignment += 4 + us.ihmc.idl.CDR.alignment(current_alignment, 4) + data.getNodeType().length() + 1; - - current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); - - - current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); - - - - return current_alignment - initial_alignment; - } - - public static void write(behavior_msgs.msg.dds.BehaviorTreeNodeMessage data, us.ihmc.idl.CDR cdr) - { - cdr.write_type_4(data.getNumberOfChildren()); - - ihmc_common_msgs.msg.dds.InstantMessagePubSubType.write(data.getLastTickInstant(), cdr); - if(data.getNodeName().length() <= 255) - cdr.write_type_d(data.getNodeName());else - throw new RuntimeException("node_name field exceeds the maximum length"); - - if(data.getNodeType().length() <= 255) - cdr.write_type_d(data.getNodeType());else - throw new RuntimeException("node_type field exceeds the maximum length"); - - cdr.write_type_9(data.getPreviousStatus()); - - cdr.write_type_7(data.getHasBeenClocked()); - - } - - public static void read(behavior_msgs.msg.dds.BehaviorTreeNodeMessage data, us.ihmc.idl.CDR cdr) - { - data.setNumberOfChildren(cdr.read_type_4()); - - ihmc_common_msgs.msg.dds.InstantMessagePubSubType.read(data.getLastTickInstant(), cdr); - cdr.read_type_d(data.getNodeName()); - cdr.read_type_d(data.getNodeType()); - data.setPreviousStatus(cdr.read_type_9()); - - data.setHasBeenClocked(cdr.read_type_7()); - - - } - - @Override - public final void serialize(behavior_msgs.msg.dds.BehaviorTreeNodeMessage data, us.ihmc.idl.InterchangeSerializer ser) - { - ser.write_type_4("number_of_children", data.getNumberOfChildren()); - ser.write_type_a("last_tick_instant", new ihmc_common_msgs.msg.dds.InstantMessagePubSubType(), data.getLastTickInstant()); - - ser.write_type_d("node_name", data.getNodeName()); - ser.write_type_d("node_type", data.getNodeType()); - ser.write_type_9("previous_status", data.getPreviousStatus()); - ser.write_type_7("has_been_clocked", data.getHasBeenClocked()); - } - - @Override - public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, behavior_msgs.msg.dds.BehaviorTreeNodeMessage data) - { - data.setNumberOfChildren(ser.read_type_4("number_of_children")); - ser.read_type_a("last_tick_instant", new ihmc_common_msgs.msg.dds.InstantMessagePubSubType(), data.getLastTickInstant()); - - ser.read_type_d("node_name", data.getNodeName()); - ser.read_type_d("node_type", data.getNodeType()); - data.setPreviousStatus(ser.read_type_9("previous_status")); - data.setHasBeenClocked(ser.read_type_7("has_been_clocked")); - } - - public static void staticCopy(behavior_msgs.msg.dds.BehaviorTreeNodeMessage src, behavior_msgs.msg.dds.BehaviorTreeNodeMessage dest) - { - dest.set(src); - } - - @Override - public behavior_msgs.msg.dds.BehaviorTreeNodeMessage createData() - { - return new behavior_msgs.msg.dds.BehaviorTreeNodeMessage(); - } - @Override - public int getTypeSize() - { - return us.ihmc.idl.CDR.getTypeSize(getMaxCdrSerializedSize()); - } - - @Override - public java.lang.String getName() - { - return name; - } - - public void serialize(behavior_msgs.msg.dds.BehaviorTreeNodeMessage data, us.ihmc.idl.CDR cdr) - { - write(data, cdr); - } - - public void deserialize(behavior_msgs.msg.dds.BehaviorTreeNodeMessage data, us.ihmc.idl.CDR cdr) - { - read(data, cdr); - } - - public void copy(behavior_msgs.msg.dds.BehaviorTreeNodeMessage src, behavior_msgs.msg.dds.BehaviorTreeNodeMessage dest) - { - staticCopy(src, dest); - } - - @Override - public BehaviorTreeNodeMessagePubSubType newInstance() - { - return new BehaviorTreeNodeMessagePubSubType(); - } -} diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/BehaviorTreeMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/BehaviorTreeMessage.msg deleted file mode 100644 index 7308ff7e3cf..00000000000 --- a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/BehaviorTreeMessage.msg +++ /dev/null @@ -1,5 +0,0 @@ -# DEPRECATED: This is an old message replaced by BehaviorTreeStateMessage -# Nodes in the tree in "depth first" order. -# We must reconstruct the tree on the subscription end. -# This is because messages cannot be recursive. -behavior_msgs/BehaviorTreeNodeMessage[<=2048] nodes \ No newline at end of file diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/BehaviorTreeNodeMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/BehaviorTreeNodeMessage.msg deleted file mode 100644 index 527b964b1cc..00000000000 --- a/ihmc-interfaces/src/main/messages/ihmc_interfaces/behavior_msgs/msg/BehaviorTreeNodeMessage.msg +++ /dev/null @@ -1,23 +0,0 @@ -# DEPRECATED - See BehaviorTreeNodeDefinitionMessage for the new version -# We will remove this when the implementation has been migrated to the -# new structure. - -# The number of children -# This is used for a stack data structure to re-assemble the tree. -uint32 number_of_children - -# Last tick instant -ihmc_common_msgs/InstantMessage last_tick_instant - -# Name of the node -string node_name - -# The type of the node, as a string -string node_type - -# Previous node status -int8 previous_status - -# Whether this node has been clocked -# This field is only for control flow nodes. -bool has_been_clocked \ No newline at end of file diff --git a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/BehaviorTreeMessage.msg b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/BehaviorTreeMessage.msg deleted file mode 100644 index 7d9ea72733f..00000000000 --- a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/BehaviorTreeMessage.msg +++ /dev/null @@ -1,8 +0,0 @@ - -# DEPRECATED: This is an old message replaced by BehaviorTreeStateMessage -# Nodes in the tree in "depth first" order. -# We must reconstruct the tree on the subscription end. -# This is because messages cannot be recursive. -behavior_msgs/BehaviorTreeNodeMessage[] nodes - - diff --git a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/BehaviorTreeNodeMessage.msg b/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/BehaviorTreeNodeMessage.msg deleted file mode 100644 index 86872265876..00000000000 --- a/ihmc-interfaces/src/main/messages/ros1/behavior_msgs/msg/BehaviorTreeNodeMessage.msg +++ /dev/null @@ -1,25 +0,0 @@ -# DEPRECATED - See BehaviorTreeNodeDefinitionMessage for the new version -# We will remove this when the implementation has been migrated to the -# new structure. - -# The number of children -# This is used for a stack data structure to re-assemble the tree. -uint32 number_of_children - -# Last tick instant -ihmc_common_msgs/InstantMessage last_tick_instant - -# Name of the node -string node_name - -# The type of the node, as a string -string node_type - -# Previous node status -int8 previous_status - -# Whether this node has been clocked -# This field is only for control flow nodes. -bool has_been_clocked - - From 18bf430b0b6cad60efd783f2a83dbb52e53fac48 Mon Sep 17 00:00:00 2001 From: ds58 <30220598+ds58@users.noreply.github.com> Date: Tue, 17 Dec 2024 16:36:09 -0600 Subject: [PATCH 08/16] :arrow_up: ihmc-realtime 1.7.0 (#539) --- ihmc-communication/build.gradle.kts | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ihmc-communication/build.gradle.kts b/ihmc-communication/build.gradle.kts index 225ae686858..d4f9044bef7 100644 --- a/ihmc-communication/build.gradle.kts +++ b/ihmc-communication/build.gradle.kts @@ -11,7 +11,7 @@ ihmc { } mainDependencies { - api("us.ihmc:ihmc-realtime:1.6.0") + api("us.ihmc:ihmc-realtime:1.7.0") api("us.ihmc:ihmc-video-codecs:2.1.6") api("us.ihmc:ros2-library:1.1.4") api("commons-net:commons-net:3.6") From 4d3ad600de93c49ea14892ab71a685c960bbaaaf Mon Sep 17 00:00:00 2001 From: Nick Kitchel <59104880+PotatoPeeler3000@users.noreply.github.com> Date: Tue, 17 Dec 2024 19:02:00 -0600 Subject: [PATCH 09/16] Update the way we publish and subscribe to the Continuous Hiking Process. Upgrade to ContinuousHikingAPI (#523) * Switched Continuous Hiking to use the ContinuousHikingAPI. This allows for changing how we send and receive the messages. Removed the use of the planning mode as its no longer desired behavior. * Fixed high level UI to publish and stop correctly. * Add some comments explaining the logic of starting the CH state machine --- .../communication/ContinuousHikingAPI.java | 3 +- .../perception/RDXContinuousHikingPanel.java | 190 ++++++++++++------ .../RDXStancePoseSelectionPanel.java | 4 +- .../ContinuousHikingParameters.java | 1 - .../ContinuousHikingParametersBasics.java | 5 - .../ContinuousHikingParametersReadOnly.java | 5 - .../ActivePlanarMappingRemoteTask.java | 25 +-- .../ReadyToPlanState.java | 185 ++++++++--------- ...rtContinuousHikingTransitionCondition.java | 14 +- ...opContinuousHikingTransitionCondition.java | 13 +- .../activeMapping/ContinuousPlanner.java | 28 +-- .../ContinuousPlannerSchedulingTask.java | 18 +- .../TerrainPlanningDebugger.java | 12 +- .../ContinuousHikingParameters.json | 1 - 14 files changed, 250 insertions(+), 254 deletions(-) diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousHikingAPI.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousHikingAPI.java index e0179b27008..5ae047b78ad 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousHikingAPI.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousHikingAPI.java @@ -4,7 +4,6 @@ import behavior_msgs.msg.dds.ContinuousWalkingStatusMessage; import controller_msgs.msg.dds.FootstepDataListMessage; import ihmc_common_msgs.msg.dds.PoseListMessage; -import std_msgs.Empty; import us.ihmc.communication.property.StoredPropertySetROS2TopicPair; import us.ihmc.ros2.ROS2Topic; @@ -17,7 +16,7 @@ public class ContinuousHikingAPI // Commands supported for the Continuous Hiking Process public static final ROS2Topic CONTINUOUS_HIKING_COMMAND = IHMC_ROOT.withModule(moduleName).withType(ContinuousHikingCommandMessage.class).withSuffix("command"); - public static final ROS2Topic CLEAR_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(Empty.class).withSuffix("clear_goal_footsteps"); + public static final ROS2Topic CLEAR_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(std_msgs.msg.dds.Empty.class).withSuffix("clear_goal_footsteps"); public static final ROS2Topic PLACED_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(PoseListMessage.class).withSuffix("placed_goal_footsteps"); // Statuses supported from the Continuous Hiking Process diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java index 59c10d78696..a45324759d3 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java @@ -1,6 +1,6 @@ package us.ihmc.rdx.perception; -import behavior_msgs.msg.dds.ContinuousWalkingCommandMessage; +import behavior_msgs.msg.dds.ContinuousHikingCommandMessage; import com.badlogic.gdx.controllers.Controller; import com.badlogic.gdx.controllers.Controllers; import com.badlogic.gdx.graphics.g3d.Renderable; @@ -32,7 +32,7 @@ import us.ihmc.footstepPlanning.FootstepDataMessageConverter; import us.ihmc.footstepPlanning.FootstepPlan; import us.ihmc.footstepPlanning.MonteCarloFootstepPlannerParameters; -import us.ihmc.footstepPlanning.communication.ContinuousWalkingAPI; +import us.ihmc.footstepPlanning.communication.ContinuousHikingAPI; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersBasics; import us.ihmc.footstepPlanning.swing.SwingPlannerParametersBasics; import us.ihmc.footstepPlanning.tools.SwingPlannerTools; @@ -40,6 +40,7 @@ import us.ihmc.perception.comms.PerceptionComms; import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; import us.ihmc.perception.heightMap.TerrainMapData; +import us.ihmc.rdx.imgui.ImGuiSliderDouble; import us.ihmc.rdx.imgui.RDXPanel; import us.ihmc.rdx.input.ImGui3DViewInput; import us.ihmc.rdx.ui.ImGuiRemoteROS2StoredPropertySetGroup; @@ -63,22 +64,22 @@ public class RDXContinuousHikingPanel extends RDXPanel implements RenderableProv private static final int numberOfKnotPoints = 12; private static final int maxIterationsOptimization = 100; private final ROS2Node ros2Node; + private final ROS2Helper ros2Helper; private final DRCRobotModel robotModel; private final ROS2SyncedRobotModel syncedRobotModel; - private final ROS2Publisher commandPublisher; - private final ContinuousWalkingCommandMessage commandMessage = new ContinuousWalkingCommandMessage(); + private final ROS2Publisher commandPublisher; + private final ContinuousHikingCommandMessage commandMessage = new ContinuousHikingCommandMessage(); private final RDXStancePoseSelectionPanel stancePoseSelectionPanel; private final PositionOptimizedTrajectoryGenerator positionTrajectoryGenerator = new PositionOptimizedTrajectoryGenerator(numberOfKnotPoints, maxIterationsOptimization); private final RDXTerrainPlanningDebugger terrainPlanningDebugger; - private final ContinuousHikingParameters continuousHikingParameters = new ContinuousHikingParameters(); private final SwingTrajectoryParameters swingTrajectoryParameters; private final RDXStoredPropertySetTuner continuousHikingParametersPanel = new RDXStoredPropertySetTuner("Continuous Hiking Parameters (CH)"); private final ImGuiRemoteROS2StoredPropertySetGroup hostStoredPropertySets; - private final ImBoolean localRenderMode = new ImBoolean(false); - private final ImBoolean useMonteCarloReference = new ImBoolean(false); - private final ImBoolean useHybridPlanner = new ImBoolean(false); + private final ImGuiSliderDouble stepsBeforeSafetyStop = new ImGuiSliderDouble("Steps Before Safety Stop", "%.2f"); + private final ImBoolean squareUpToGoal = new ImBoolean(false); private final ImBoolean useAStarFootstepPlanner = new ImBoolean(true); + private final ImBoolean useMonteCarloReference = new ImBoolean(false); private final ImBoolean useMonteCarloFootstepPlanner = new ImBoolean(false); private SideDependentList startStancePose = new SideDependentList<>(new FramePose3D(), new FramePose3D()); private FootstepPlan latestFootstepPlan; @@ -92,17 +93,18 @@ public class RDXContinuousHikingPanel extends RDXPanel implements RenderableProv public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, ROS2Helper ros2Helper, DRCRobotModel robotModel, ROS2SyncedRobotModel syncedRobotModel) { super("Continuous Hiking"); + this.ros2Helper = ros2Helper; setRenderMethod(this::renderImGuiWidgets); this.ros2Node = ros2Node; this.robotModel = robotModel; this.syncedRobotModel = syncedRobotModel; - ros2Helper.subscribeViaCallback(ContinuousWalkingAPI.START_AND_GOAL_FOOTSTEPS, this::onStartAndGoalPosesReceived); - ros2Helper.subscribeViaCallback(ContinuousWalkingAPI.PLANNED_FOOTSTEPS, this::onPlannedFootstepsReceived); - ros2Helper.subscribeViaCallback(ContinuousWalkingAPI.MONTE_CARLO_FOOTSTEP_PLAN, this::onMonteCarloPlanReceived); + ros2Helper.subscribeViaCallback(ContinuousHikingAPI.START_AND_GOAL_FOOTSTEPS, this::onStartAndGoalPosesReceived); + ros2Helper.subscribeViaCallback(ContinuousHikingAPI.PLANNED_FOOTSTEPS, this::onPlannedFootstepsReceived); + ros2Helper.subscribeViaCallback(ContinuousHikingAPI.MONTE_CARLO_FOOTSTEP_PLAN, this::onMonteCarloPlanReceived); - commandPublisher = ros2Helper.getROS2Node().createPublisher(ContinuousWalkingAPI.CONTINUOUS_WALKING_COMMAND); + commandPublisher = ros2Helper.getROS2Node().createPublisher(ContinuousHikingAPI.CONTINUOUS_HIKING_COMMAND); SegmentDependentList> groundContactPoints = robotModel.getContactPointParameters().getControllerFootGroundContactPoints(); SideDependentList defaultContactPoints = new SideDependentList<>(); @@ -131,22 +133,23 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, ROS2Helper message -> terrainPlanningDebugger.reset()); hostStoredPropertySets = new ImGuiRemoteROS2StoredPropertySetGroup(ros2Helper); + ContinuousHikingParameters continuousHikingParameters = new ContinuousHikingParameters(); createParametersPanel(continuousHikingParameters, continuousHikingParametersPanel, hostStoredPropertySets, - ContinuousWalkingAPI.CONTINUOUS_HIKING_PARAMETERS); + ContinuousHikingAPI.CONTINUOUS_HIKING_PARAMETERS); RDXStoredPropertySetTuner monteCarloPlannerParametersPanel = new RDXStoredPropertySetTuner("Monte Carlo Footstep Planner Parameters (CH)"); createParametersPanel(monteCarloPlannerParameters, monteCarloPlannerParametersPanel, hostStoredPropertySets, - ContinuousWalkingAPI.MONTE_CARLO_PLANNER_PARAMETERS); + ContinuousHikingAPI.MONTE_CARLO_PLANNER_PARAMETERS); RDXStoredPropertySetTuner footstepPlanningParametersPanel = new RDXStoredPropertySetTuner("Footstep Planner Parameters (CH)"); createParametersPanel(footstepPlannerParameters, footstepPlanningParametersPanel, hostStoredPropertySets, - ContinuousWalkingAPI.FOOTSTEP_PLANNING_PARAMETERS); + ContinuousHikingAPI.FOOTSTEP_PLANNING_PARAMETERS); RDXStoredPropertySetTuner swingPlannerParametersPanel = new RDXStoredPropertySetTuner("Swing Planner Parameters (CH)"); - createParametersPanel(swingPlannerParameters, swingPlannerParametersPanel, hostStoredPropertySets, ContinuousWalkingAPI.SWING_PLANNING_PARAMETERS); + createParametersPanel(swingPlannerParameters, swingPlannerParametersPanel, hostStoredPropertySets, ContinuousHikingAPI.SWING_PLANNING_PARAMETERS); RDXStoredPropertySetTuner heightMapParametersPanel = new RDXStoredPropertySetTuner("Height Map Parameters (CH)"); createParametersPanel(RapidHeightMapExtractor.getHeightMapParameters(), heightMapParametersPanel, @@ -181,19 +184,19 @@ public void startContinuousPlannerSchedulingTask(boolean publishAndSubscribe) // Add Continuous Hiking Parameters to be between the UI and this process ContinuousHikingParameters continuousHikingParameters = new ContinuousHikingParameters(); - clientStoredPropertySets.registerStoredPropertySet(ContinuousWalkingAPI.CONTINUOUS_HIKING_PARAMETERS, continuousHikingParameters); + clientStoredPropertySets.registerStoredPropertySet(ContinuousHikingAPI.CONTINUOUS_HIKING_PARAMETERS, continuousHikingParameters); // Add Monte Carlo Footstep Planner Parameters to be between the UI and this process MonteCarloFootstepPlannerParameters monteCarloPlannerParameters = new MonteCarloFootstepPlannerParameters(); - clientStoredPropertySets.registerStoredPropertySet(ContinuousWalkingAPI.MONTE_CARLO_PLANNER_PARAMETERS, monteCarloPlannerParameters); + clientStoredPropertySets.registerStoredPropertySet(ContinuousHikingAPI.MONTE_CARLO_PLANNER_PARAMETERS, monteCarloPlannerParameters); // Add A* Footstep Planner Parameters to be between the UI and this process DefaultFootstepPlannerParametersBasics footstepPlannerParameters = robotModel.getFootstepPlannerParameters("ForContinuousWalking"); - clientStoredPropertySets.registerStoredPropertySet(ContinuousWalkingAPI.FOOTSTEP_PLANNING_PARAMETERS, footstepPlannerParameters); + clientStoredPropertySets.registerStoredPropertySet(ContinuousHikingAPI.FOOTSTEP_PLANNING_PARAMETERS, footstepPlannerParameters); // Add Swing Planner Parameters to be synced between the UI and this process SwingPlannerParametersBasics swingPlannerParameters = robotModel.getSwingPlannerParameters(); - clientStoredPropertySets.registerStoredPropertySet(ContinuousWalkingAPI.SWING_PLANNING_PARAMETERS, swingPlannerParameters); + clientStoredPropertySets.registerStoredPropertySet(ContinuousHikingAPI.SWING_PLANNING_PARAMETERS, swingPlannerParameters); continuousPlannerSchedulingTask = new ContinuousPlannerSchedulingTask(robotModel, ros2Node, @@ -220,7 +223,7 @@ public void update(TerrainMapData terrainMapData, HeightMapData heightMapData) /** * This method handles updating the stored property sets used in Continuous Hiking. * These are all the parameters that are getting synced back and forth between the remote process and the local process. - * There are three situations that can occur when tyring to use Continuous Hiking. + * There are three situations that can occur when trying to use Continuous Hiking. *
    *
  • Case 1: The situation where we are simulating the process running on a remote machine but in reality its running locally. * This is where we only want to update the property sets running on that process. Represented by {@link #clientStoredPropertySets}. @@ -229,7 +232,7 @@ public void update(TerrainMapData terrainMapData, HeightMapData heightMapData) * Here we want to publish, and subscribe in one place as everything is being run on the same machine. * So we update {@link #clientStoredPropertySets} and {@link #hostStoredPropertySets}
    • *
  • Case 3: Then the situation where we only want to publish the property sets to be sent to the remote process. - * This is when we don't want to subscribe but we publish and changes to {@link #hostStoredPropertySets} so the remote process can receive these chagnes
    • + * This is when we don't want to subscribe but we publish and changes to {@link #hostStoredPropertySets} so the remote process can receive these changes * *
*/ @@ -258,13 +261,52 @@ public void renderImGuiWidgets() ImGui.separator(); continuousHikingParametersPanel.renderImGuiWidgets(); - ImGui.checkbox("Local Render Mode", localRenderMode); + ImGui.separator(); + ImGui.text("Options for Continuous Hiking Message"); + ImGui.indent(); + ImGui.checkbox("Square Up To Goal", squareUpToGoal); + if (ImGui.button("Clear Planned footsteps")) + { + clearPlannedFootsteps(); + } + ImGui.sameLine(); + stepsBeforeSafetyStop.render(0.0, 50.0); ImGui.checkbox("Use A* Footstep Planner", useAStarFootstepPlanner); ImGui.checkbox("Use Monte-Carlo Footstep Planner", useMonteCarloFootstepPlanner); ImGui.checkbox("Use Monte-Carlo Reference", useMonteCarloReference); + ImGui.unindent(); ImGui.separator(); terrainPlanningDebugger.renderImGuiWidgets(); - publishInputCommandMessage(); + + // Check to see if a controller is plugged into the computer + Controller joystickController = Controllers.getCurrent(); + // Here we check against null rather then .isConnected() because if the controller is unplugged, that method won't work + boolean controllerConnected = joystickController != null; + + // The following logic determines how the Continuous Hiking State Machine will be started. + // This can be with buttons pressed on the keyboard, or with an XBox One Controller + if (ImGui.getIO().getKeyCtrl() && ImGui.getIO().getKeyShift()) + { + publishContinuousHikingCommandWithEnabled(); + } + else if (controllerConnected) + { + if (joystickController.getButton(joystickController.getMapping().buttonA)) + { + publishJoystickStatus(joystickController); + } + + if (joystickController.getButton(joystickController.getMapping().buttonX)) + { + publishStopContinuousHiking(); + } + } + + // Pressing this key will stop Continuous Hiking + if (ImGui.getIO().getKeyAlt()) + { + publishStopContinuousHiking(); + } } public void processImGui3DViewInput(ImGui3DViewInput input) @@ -279,6 +321,11 @@ public void getRenderables(Array renderables, Pool pool) terrainPlanningDebugger.getRenderables(renderables, pool); } + public void clearPlannedFootsteps() + { + ros2Helper.publish(ContinuousHikingAPI.CLEAR_GOAL_FOOTSTEPS); + } + /** * We have received the start and goal pose from the process, lets unpack this message and visualize the start and goal on the UI. */ @@ -327,49 +374,62 @@ public void onMonteCarloPlanReceived(FootstepDataListMessage message) terrainPlanningDebugger.generateMonteCarloPlanGraphic(message); } - private void publishInputCommandMessage() + /** + * Stop Continuous Hiking. Tells the state machine that we want to stop walking + */ + private void publishStopContinuousHiking() { - // Check to see if a controller is plugged into the computer - Controller joystickController = Controllers.getCurrent(); - // Here we check against null rather then .isConnected() because if the controller is unplugged that method won't work - boolean controllerConnected = joystickController != null; - - // Setup a bunch of variables to be published in the message - boolean walkWithKeyboard = ImGui.getIO().getKeyCtrl(); - boolean walkWithController = false; - boolean walkBackwards = false; - double lateralJoystickValue = 0.0; - double forwardJoystickValue = 0.0; - double turningJoystickValue = 0.0; + commandMessage.setEnableContinuousHiking(false); + commandPublisher.publish(commandMessage); + } - if (controllerConnected) - { - walkBackwards = joystickController.getButton(joystickController.getMapping().buttonB); - walkWithController = joystickController.getButton(joystickController.getMapping().buttonA); - walkWithController |= walkBackwards; - forwardJoystickValue = -joystickController.getAxis(joystickController.getMapping().axisLeftY); - lateralJoystickValue = -joystickController.getAxis(joystickController.getMapping().axisLeftX); - turningJoystickValue = -joystickController.getAxis(joystickController.getMapping().axisRightX); - } + /** + * Publish the status of the joystick controller. We define different buttons to perform different actions which get sent with the message. + */ + private void publishJoystickStatus(Controller joystickController) + { + // Setup variables to be published in the message + boolean walkBackwards; + double forwardJoystickValue; + double lateralJoystickValue; + double turningJoystickValue; + + walkBackwards = joystickController.getButton(joystickController.getMapping().buttonB); + forwardJoystickValue = -joystickController.getAxis(joystickController.getMapping().axisLeftY); + lateralJoystickValue = -joystickController.getAxis(joystickController.getMapping().axisLeftX); + turningJoystickValue = -joystickController.getAxis(joystickController.getMapping().axisRightX); + + commandMessage.setEnableContinuousHiking(true); + commandMessage.setUseJoystickController(true); + commandMessage.setForwardValue(forwardJoystickValue); + commandMessage.setWalkBackwards(walkBackwards); + commandMessage.setLateralValue(lateralJoystickValue); + commandMessage.setTurningValue(turningJoystickValue); + + commandPublisher.publish(commandMessage); + } - // Only allow Continuous Walking if the CTRL key is held and the checkbox is checked - // We publish this all the time to prevent any of the values from staying true all the time - if (continuousHikingParameters.getEnableContinuousHiking()) - { - commandMessage.setEnableContinuousHikingWithKeyboard(walkWithKeyboard); - commandMessage.setEnableContinuousHikingWithJoystickController(walkWithController); - commandMessage.setWalkBackwards(walkBackwards); - commandMessage.setForwardValue(forwardJoystickValue); - commandMessage.setLateralValue(lateralJoystickValue); - commandMessage.setTurningValue(turningJoystickValue); - commandMessage.setUseMonteCarloFootstepPlanner(useMonteCarloFootstepPlanner.get()); - commandMessage.setUseAstarFootstepPlanner(useAStarFootstepPlanner.get()); - commandMessage.setUseMonteCarloPlanAsReference(useMonteCarloReference.get()); - commandMessage.setUsePreviousPlanAsReference(!useMonteCarloReference.get()); - commandMessage.setUseHybridPlanner(useHybridPlanner.get()); - - commandPublisher.publish(commandMessage); - } + /** + * This publishes and tells the state machine that we want to start walking. Setting the enable Continuous Hiking to true + */ + private void publishContinuousHikingCommandWithEnabled() + { + commandMessage.setEnableContinuousHiking(true); + commandMessage.setStepsBeforeSafetyStop((int) stepsBeforeSafetyStop.getDoubleValue()); + commandMessage.setWalkForwards(true); + commandMessage.setSquareUpToGoal(squareUpToGoal.get()); + commandMessage.setUseAstarFootstepPlanner(useAStarFootstepPlanner.get()); + commandMessage.setUseMonteCarloFootstepPlanner(useMonteCarloFootstepPlanner.get()); + commandMessage.setUseMonteCarloPlanAsReference(useMonteCarloReference.get()); + commandMessage.setUsePreviousPlanAsReference(!useMonteCarloReference.get()); + + commandMessage.setUseJoystickController(false); + commandMessage.setForwardValue(0.0); + commandMessage.setWalkBackwards(false); + commandMessage.setLateralValue(0.0); + commandMessage.setTurningValue(0.0); + + commandPublisher.publish(commandMessage); } public void destroy() @@ -392,4 +452,4 @@ public RDXStancePoseSelectionPanel getStancePoseSelectionPanel() { return stancePoseSelectionPanel; } -} +} \ No newline at end of file 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 1131bf4565e..7ddd28c719f 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.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple2D.Point2D; import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; -import us.ihmc.footstepPlanning.communication.ContinuousWalkingAPI; +import us.ihmc.footstepPlanning.communication.ContinuousHikingAPI; import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; import us.ihmc.footstepPlanning.tools.PlannerTools; import us.ihmc.perception.heightMap.TerrainMapData; @@ -263,7 +263,7 @@ private void setGoalFootsteps() PoseListMessage poseListMessage = new PoseListMessage(); MessageTools.packPoseListMessage(poses, poseListMessage); - ros2Helper.publish(ContinuousWalkingAPI.PLACED_GOAL_FOOTSTEPS, poseListMessage); + ros2Helper.publish(ContinuousHikingAPI.PLACED_GOAL_FOOTSTEPS, poseListMessage); } public boolean isSelectionActive() diff --git a/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.java b/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.java index 8dfc012ad5f..c057eee1fe6 100644 --- a/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.java +++ b/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.java @@ -18,7 +18,6 @@ public class ContinuousHikingParameters extends StoredPropertySet implements Con { public static final StoredPropertyKeyList keys = new StoredPropertyKeyList(); - public static final BooleanStoredPropertyKey enableContinuousHiking = keys.addBooleanKey("Enable continuous hiking"); public static final BooleanStoredPropertyKey stepPublisherEnabled = keys.addBooleanKey("Step publisher enabled"); public static final BooleanStoredPropertyKey overrideEntireQueueEachStep = keys.addBooleanKey("Override entire queue each step"); public static final IntegerStoredPropertyKey numberOfStepsToSend = keys.addIntegerKey("Number of steps to send"); diff --git a/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersBasics.java b/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersBasics.java index 1fba1aef983..4860c31305d 100644 --- a/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersBasics.java +++ b/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersBasics.java @@ -8,11 +8,6 @@ */ public interface ContinuousHikingParametersBasics extends ContinuousHikingParametersReadOnly, StoredPropertySetBasics { - default void setEnableContinuousHiking(boolean enableContinuousHiking) - { - set(ContinuousHikingParameters.enableContinuousHiking, enableContinuousHiking); - } - default void setStepPublisherEnabled(boolean stepPublisherEnabled) { set(ContinuousHikingParameters.stepPublisherEnabled, stepPublisherEnabled); diff --git a/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersReadOnly.java b/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersReadOnly.java index ed3127d2de4..ecf1c13533c 100644 --- a/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersReadOnly.java +++ b/ihmc-high-level-behaviors/src/main/generated-java/us/ihmc/behaviors/activeMapping/ContinuousHikingParametersReadOnly.java @@ -10,11 +10,6 @@ */ public interface ContinuousHikingParametersReadOnly extends StoredPropertySetReadOnly { - default boolean getEnableContinuousHiking() - { - return get(enableContinuousHiking); - } - default boolean getStepPublisherEnabled() { return get(stepPublisherEnabled); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ActivePlanarMappingRemoteTask.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ActivePlanarMappingRemoteTask.java index 5fae724fba1..0a8cff9a628 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ActivePlanarMappingRemoteTask.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ActivePlanarMappingRemoteTask.java @@ -93,26 +93,23 @@ private void generalUpdate() */ private void updateActiveMappingPlan() { - if (continuousPlanningParameters.getEnableContinuousHiking()) + if (walkingStatusMessage.get() != null) { - if (walkingStatusMessage.get() != null) + if (walkingStatusMessage.get().getWalkingStatus() == WalkingStatusMessage.COMPLETED && !continuousPlanner.isPlanAvailable()) { - if (walkingStatusMessage.get().getWalkingStatus() == WalkingStatusMessage.COMPLETED && !continuousPlanner.isPlanAvailable()) - { - continuousPlanner.planBodyPathWithPlanarRegionMap(planarRegionMap); - } + continuousPlanner.planBodyPathWithPlanarRegionMap(planarRegionMap); } + } - if (continuousPlanner.isPlanAvailable()) - { - // Publishing Plan Result - FootstepDataListMessage footstepDataList = continuousPlanner.getFootstepDataListMessage(); - publisherMap.publish(controllerFootstepDataTopic, footstepDataList); + if (continuousPlanner.isPlanAvailable()) + { + // Publishing Plan Result + FootstepDataListMessage footstepDataList = continuousPlanner.getFootstepDataListMessage(); + publisherMap.publish(controllerFootstepDataTopic, footstepDataList); - continuousPlanner.setPlanAvailable(false); - } -// configurationParameters.setActiveMapping(false); + continuousPlanner.setPlanAvailable(false); } + // configurationParameters.setActiveMapping(false); } public ContinuousPlannerForPlanarRegions getContinuousPlanner() 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 1baf018836c..df2b6b6fadf 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 @@ -1,14 +1,12 @@ package us.ihmc.behaviors.activeMapping.ContinuousHikingStateMachine; -import behavior_msgs.msg.dds.ContinuousWalkingCommandMessage; +import behavior_msgs.msg.dds.ContinuousHikingCommandMessage; import controller_msgs.msg.dds.FootstepDataListMessage; import ihmc_common_msgs.msg.dds.PoseListMessage; import org.apache.commons.lang3.time.StopWatch; -import std_msgs.msg.dds.Empty; import us.ihmc.behaviors.activeMapping.ContinuousHikingLogger; import us.ihmc.behaviors.activeMapping.ContinuousHikingParameters; import us.ihmc.behaviors.activeMapping.ContinuousPlanner; -import us.ihmc.behaviors.activeMapping.ContinuousPlannerSchedulingTask.PlanningMode; import us.ihmc.behaviors.activeMapping.ContinuousPlannerTools; import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; import us.ihmc.communication.packets.MessageTools; @@ -18,7 +16,7 @@ import us.ihmc.euclid.tuple3D.Point3D; import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics; import us.ihmc.footstepPlanning.FootstepDataMessageConverter; -import us.ihmc.footstepPlanning.communication.ContinuousWalkingAPI; +import us.ihmc.footstepPlanning.communication.ContinuousHikingAPI; import us.ihmc.behaviors.activeMapping.TerrainPlanningDebugger; import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames; import us.ihmc.log.LogTools; @@ -38,7 +36,7 @@ public class ReadyToPlanState implements State private static final float NOMINAL_STANCE_WIDTH = 0.22f; private final HumanoidReferenceFrames referenceFrames; - private final AtomicReference commandMessage; + private final AtomicReference commandMessage; private final ContinuousPlanner continuousPlanner; private final ControllerFootstepQueueMonitor controllerFootstepQueueMonitor; private final ContinuousHikingParameters continuousHikingParameters; @@ -49,8 +47,7 @@ public class ReadyToPlanState implements State private final Point3D robotLocation = new Point3D(); private final StopWatch stopWatch = new StopWatch(); double timeInSwingToStopPlanningAndWaitTillNextAttempt = 0; - - private PlanningMode planningMode; + private boolean wasWalkingTowardsGoal = false; /** * This state exists to plan footsteps based on the conditions of the {@link ContinuousHikingParameters}. This state publishes visuals the UI but doesn't @@ -61,14 +58,13 @@ public class ReadyToPlanState implements State */ public ReadyToPlanState(ROS2Helper ros2Helper, HumanoidReferenceFrames referenceFrames, - AtomicReference commandMessage, + AtomicReference commandMessage, ContinuousPlanner continuousPlanner, ControllerFootstepQueueMonitor controllerFootstepQueueMonitor, ContinuousHikingParameters continuousHikingParameters, TerrainMapData terrainMap, TerrainPlanningDebugger debugger, - ContinuousHikingLogger continuousHikingLogger, - PlanningMode planningMode) + ContinuousHikingLogger continuousHikingLogger) { this.referenceFrames = referenceFrames; this.commandMessage = commandMessage; @@ -78,10 +74,9 @@ public ReadyToPlanState(ROS2Helper ros2Helper, this.terrainMap = terrainMap; this.debugger = debugger; this.continuousHikingLogger = continuousHikingLogger; - this.planningMode = planningMode; - ros2Helper.subscribeViaCallback(ContinuousWalkingAPI.PLACED_GOAL_FOOTSTEPS, this::addWayPointPoseToList); - ros2Helper.subscribeViaCallback(ContinuousWalkingAPI.CLEAR_GOAL_FOOTSTEPS, this::clearWayPointList); + ros2Helper.subscribeViaCallback(ContinuousHikingAPI.PLACED_GOAL_FOOTSTEPS, this::addWayPointPoseToList); + ros2Helper.subscribeViaCallback(ContinuousHikingAPI.CLEAR_GOAL_FOOTSTEPS, this::clearWayPointList); } @Override @@ -110,11 +105,10 @@ public void doAction(double timeInState) debugger.publishStartAndGoalForVisualization(continuousPlanner.getStartStancePose(), goalPoses); // Plan to the goal and log the plan - continuousPlanner.planToGoal(commandMessage.get(), goalPoses); + continuousPlanner.planToGoal(goalPoses); continuousPlanner.logFootStePlan(); - if (commandMessage.get().getUseHybridPlanner() || commandMessage.get().getUseMonteCarloFootstepPlanner() || commandMessage.get() - .getUseMonteCarloPlanAsReference()) + if (commandMessage.get().getUseMonteCarloFootstepPlanner() || commandMessage.get().getUseMonteCarloPlanAsReference()) { debugger.publishMonteCarloPlan(continuousPlanner.getMonteCarloFootstepDataListMessage()); debugger.publishMonteCarloNodesForVisualization(continuousPlanner.getMonteCarloFootstepPlanner().getRoot(), terrainMap); @@ -148,100 +142,81 @@ public boolean isDone(double timeInState) public SideDependentList getGoalPosesBasedOnPlanningMode() { String message = commandMessage.get().toString(); - continuousHikingLogger.appendString("Command Message that was published: \n" + message); - SideDependentList goalPoses = new SideDependentList<>(); + continuousHikingLogger.appendString("Continuous Hiking Command Being Used: \n" + message); + + SideDependentList goalPoses = new SideDependentList<>(new FramePose3D(), new FramePose3D()); - switch (this.planningMode) + if (wasWalkingTowardsGoal && walkToGoalWayPointPoses.isEmpty()) { - case FAST_HIKING -> - { - if (commandMessage.get().getEnableContinuousHikingWithKeyboard()) - { - goalPoses = ContinuousPlannerTools.setStraightForwardGoalPoses(continuousPlanner.getWalkingStartMidPose(), - continuousPlanner.getStartStancePose(), - (float) continuousHikingParameters.getGoalPoseForwardDistance(), - (float) continuousHikingParameters.getGoalPoseUpDistance(), - X_RANDOM_MARGIN, - NOMINAL_STANCE_WIDTH); - } - else if (commandMessage.get().getEnableContinuousHikingWithJoystickController()) - { - if (commandMessage.get().getWalkBackwards()) - { - goalPoses = ContinuousPlannerTools.setStraightBackwardGoalPoses(continuousPlanner.getWalkingStartMidPose(), - continuousPlanner.getStartStancePose(), - (float) continuousHikingParameters.getGoalPoseBackwardDistance(), - (float) continuousHikingParameters.getGoalPoseUpDistance(), - X_RANDOM_MARGIN, - NOMINAL_STANCE_WIDTH); - } - // Here we assume the joystick isn't being turned at all, so we give a direction of straight forward - else if (Math.abs(commandMessage.get().getLateralValue()) < 0.1) - { - goalPoses = ContinuousPlannerTools.setStraightForwardGoalPoses(continuousPlanner.getWalkingStartMidPose(), - continuousPlanner.getStartStancePose(), - (float) continuousHikingParameters.getGoalPoseForwardDistance(), - (float) continuousHikingParameters.getGoalPoseUpDistance(), - X_RANDOM_MARGIN, - NOMINAL_STANCE_WIDTH); - } - else - { - goalPoses = ContinuousPlannerTools.setGoalPoseBasedOnLateralJoystickValue(referenceFrames.getPelvisZUpFrame(), - referenceFrames.getMidFeetZUpFrame(), - commandMessage.get().getLateralValue(), - (float) continuousHikingParameters.getGoalPoseForwardDistance(), - (float) continuousHikingParameters.getGoalPoseUpDistance(), - NOMINAL_STANCE_WIDTH); + commandMessage.get().setEnableContinuousHiking(false); + wasWalkingTowardsGoal = false; + } + else if (!walkToGoalWayPointPoses.isEmpty()) + { + wasWalkingTowardsGoal = true; + // Allow for more planning time with this one, just plan for one-step length + continuousHikingParameters.setPlanningWithoutReferenceTimeout(1.0); - // We update this pose because when we start walking straight forward again, it's from the point where we are currently - // And not the point from which we were at before we started turning - FramePose3D stanceMidPose = new FramePose3D(); - stanceMidPose.interpolate(continuousPlanner.getStartStancePose().get(RobotSide.LEFT), - continuousPlanner.getStartStancePose().get(RobotSide.RIGHT), - 0.5); - continuousPlanner.setWalkingStartMidPose(stanceMidPose); - } - } + // Set the goalPoses here so that we return a good value regardless of what happens next + goalPoses = walkToGoalWayPointPoses.get(0); - return goalPoses; - } + // Update the current robot location + Vector3DBasics robotLocationVector = referenceFrames.getMidFeetZUpFrame().getTransformToWorldFrame().getTranslation(); + robotLocation.set(robotLocationVector); + double distanceToGoalPose = ContinuousPlannerTools.getDistanceFromRobotToGoalPoseOnXYPlane(robotLocation, goalPoses); - // This allows for walking to a goal that isn't straight forward; its assumed that if there is no goal we will just resume walking straight forward - case WALK_TO_GOAL -> + if (distanceToGoalPose < continuousHikingParameters.getNextWaypointDistanceMargin()) { - // Allow for more planning time with this one, just plan for one-step length - continuousHikingParameters.setPlanningWithoutReferenceTimeout(1.0); - - // Set the goalPoses here so that we return a good value regardless of what happens next - goalPoses = walkToGoalWayPointPoses.get(0); - - // Update the current robot location - Vector3DBasics robotLocationVector = referenceFrames.getMidFeetZUpFrame().getTransformToWorldFrame().getTranslation(); - robotLocation.set(robotLocationVector); - - double distanceToGoalPose = ContinuousPlannerTools.getDistanceFromRobotToGoalPoseOnXYPlane(robotLocation, goalPoses); - - if (distanceToGoalPose < continuousHikingParameters.getNextWaypointDistanceMargin()) - { - walkToGoalWayPointPoses.remove(0); - - if (!walkToGoalWayPointPoses.isEmpty()) - { - goalPoses = walkToGoalWayPointPoses.get(0); - } - else - { - // We do this here because as soon as continuous hiking gets set to false, we exit this state - planningMode = PlanningMode.FAST_HIKING; - debugger.setPlanningMode(planningMode); - debugger.resetVisualizationForUIPublisher(); - continuousHikingParameters.setEnableContinuousHiking(false); - } - } - - return goalPoses; + walkToGoalWayPointPoses.remove(0); + } + } + else if (commandMessage.get().getUseJoystickController()) + { + if (commandMessage.get().getWalkBackwards()) + { + goalPoses = ContinuousPlannerTools.setStraightBackwardGoalPoses(continuousPlanner.getWalkingStartMidPose(), + continuousPlanner.getStartStancePose(), + (float) continuousHikingParameters.getGoalPoseBackwardDistance(), + (float) continuousHikingParameters.getGoalPoseUpDistance(), + X_RANDOM_MARGIN, + NOMINAL_STANCE_WIDTH); } + // Here we assume the joystick isn't being turned at all, so we give a direction of straight forward + else if (Math.abs(commandMessage.get().getLateralValue()) < 0.1) + { + goalPoses = ContinuousPlannerTools.setStraightForwardGoalPoses(continuousPlanner.getWalkingStartMidPose(), + continuousPlanner.getStartStancePose(), + (float) continuousHikingParameters.getGoalPoseForwardDistance(), + (float) continuousHikingParameters.getGoalPoseUpDistance(), + X_RANDOM_MARGIN, + NOMINAL_STANCE_WIDTH); + } + else + { + goalPoses = ContinuousPlannerTools.setGoalPoseBasedOnLateralJoystickValue(referenceFrames.getPelvisZUpFrame(), + referenceFrames.getMidFeetZUpFrame(), + commandMessage.get().getLateralValue(), + (float) continuousHikingParameters.getGoalPoseForwardDistance(), + (float) continuousHikingParameters.getGoalPoseUpDistance(), + NOMINAL_STANCE_WIDTH); + + // We update this pose because when we start walking straight forward again, it's from the point where we are currently at + // And not the point from which we were at before we started turning + FramePose3D stanceMidPose = new FramePose3D(); + stanceMidPose.interpolate(continuousPlanner.getStartStancePose().get(RobotSide.LEFT), + continuousPlanner.getStartStancePose().get(RobotSide.RIGHT), + 0.5); + continuousPlanner.setWalkingStartMidPose(stanceMidPose); + } + } + else + { + goalPoses = ContinuousPlannerTools.setStraightForwardGoalPoses(continuousPlanner.getWalkingStartMidPose(), + continuousPlanner.getStartStancePose(), + (float) continuousHikingParameters.getGoalPoseForwardDistance(), + (float) continuousHikingParameters.getGoalPoseUpDistance(), + X_RANDOM_MARGIN, + NOMINAL_STANCE_WIDTH); } return goalPoses; @@ -256,7 +231,6 @@ public void addWayPointPoseToList(PoseListMessage poseListMessage) leftFootPose.set(poses.get(0)); rightFootPose.set(poses.get(1)); - planningMode = PlanningMode.WALK_TO_GOAL; SideDependentList latestWayPoint = new SideDependentList<>(); latestWayPoint.put(RobotSide.LEFT, leftFootPose); latestWayPoint.put(RobotSide.RIGHT, rightFootPose); @@ -270,10 +244,9 @@ public void addWayPointPoseToList(PoseListMessage poseListMessage) * This allows the {@link ReadyToPlanState#walkToGoalWayPointPoses} to be cleared. * This empties the list so the user can place a fresh goal that Continuous Hiking will use. */ - public void clearWayPointList(Empty emptyMessage) + public void clearWayPointList() { LogTools.info("Clearing waypoint list for WALK_TO_GOAL"); walkToGoalWayPointPoses.clear(); - continuousHikingParameters.setEnableContinuousHiking(false); } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/StartContinuousHikingTransitionCondition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/StartContinuousHikingTransitionCondition.java index 3bab3bc87cc..93128699f48 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/StartContinuousHikingTransitionCondition.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/StartContinuousHikingTransitionCondition.java @@ -1,32 +1,26 @@ package us.ihmc.behaviors.activeMapping.ContinuousHikingStateMachine; -import behavior_msgs.msg.dds.ContinuousWalkingCommandMessage; -import us.ihmc.behaviors.activeMapping.ContinuousHikingParameters; +import behavior_msgs.msg.dds.ContinuousHikingCommandMessage; import us.ihmc.robotics.stateMachine.core.StateTransitionCondition; import java.util.concurrent.atomic.AtomicReference; public class StartContinuousHikingTransitionCondition implements StateTransitionCondition { - private final AtomicReference commandMessage; - private final ContinuousHikingParameters continuousHikingParameters; + private final AtomicReference commandMessage; /** * This transition is used in the {@link us.ihmc.behaviors.activeMapping.ContinuousPlannerSchedulingTask} to determine whether the Continuous Hiking state * machine should be started. */ - public StartContinuousHikingTransitionCondition(AtomicReference commandMessage, - ContinuousHikingParameters continuousHikingParameters) + public StartContinuousHikingTransitionCondition(AtomicReference commandMessage) { this.commandMessage = commandMessage; - this.continuousHikingParameters = continuousHikingParameters; } @Override public boolean testCondition(double timeInCurrentState) { - // Both conditions have to be true in order for this to work. The makes things a bit safer to use and can prevent accidentally starting things and having the robot walk - return continuousHikingParameters.getEnableContinuousHiking() && (commandMessage.get().getEnableContinuousHikingWithKeyboard() || commandMessage.get() - .getEnableContinuousHikingWithJoystickController()); + return commandMessage.get().getEnableContinuousHiking(); } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/StopContinuousHikingTransitionCondition.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/StopContinuousHikingTransitionCondition.java index bc4c012c8ee..8baf4dff83c 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/StopContinuousHikingTransitionCondition.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/StopContinuousHikingTransitionCondition.java @@ -1,31 +1,26 @@ package us.ihmc.behaviors.activeMapping.ContinuousHikingStateMachine; -import behavior_msgs.msg.dds.ContinuousWalkingCommandMessage; -import us.ihmc.behaviors.activeMapping.ContinuousHikingParameters; +import behavior_msgs.msg.dds.ContinuousHikingCommandMessage; import us.ihmc.robotics.stateMachine.core.StateTransitionCondition; import java.util.concurrent.atomic.AtomicReference; public class StopContinuousHikingTransitionCondition implements StateTransitionCondition { - private final AtomicReference commandMessage; - private final ContinuousHikingParameters continuousHikingParameters; + private final AtomicReference commandMessage; /** * This transition is used in the {@link us.ihmc.behaviors.activeMapping.ContinuousPlannerSchedulingTask} to determine whether the Continuous Hiking state * machine should be stopped. We want to be able to stop the state machine from whatever state we are in. */ - public StopContinuousHikingTransitionCondition(AtomicReference commandMessage, - ContinuousHikingParameters continuousHikingParameters) + public StopContinuousHikingTransitionCondition(AtomicReference commandMessage) { this.commandMessage = commandMessage; - this.continuousHikingParameters = continuousHikingParameters; } @Override public boolean testCondition(double timeInCurrentState) { - return !continuousHikingParameters.getEnableContinuousHiking() || !(commandMessage.get().getEnableContinuousHikingWithKeyboard() || commandMessage.get() - .getEnableContinuousHikingWithJoystickController()); + return !commandMessage.get().getEnableContinuousHiking(); } } 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 77a8674da5a..21397c510ff 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 @@ -1,6 +1,6 @@ package us.ihmc.behaviors.activeMapping; -import behavior_msgs.msg.dds.ContinuousWalkingCommandMessage; +import behavior_msgs.msg.dds.ContinuousHikingCommandMessage; import controller_msgs.msg.dds.FootstepDataListMessage; import controller_msgs.msg.dds.FootstepStatusMessage; import controller_msgs.msg.dds.QueuedFootstepStatusMessage; @@ -60,7 +60,7 @@ public class ContinuousPlanner private final FramePose3D imminentFootstepPose = new FramePose3D(); private RobotSide imminentFootstepSide = RobotSide.LEFT; - private ContinuousWalkingCommandMessage command; + private final AtomicReference commandMessage; private AtomicReference latestFootstepStatusMessage = new AtomicReference<>(new FootstepStatusMessage()); private List controllerQueue = new ArrayList<>(); @@ -73,6 +73,7 @@ public class ContinuousPlanner public ContinuousPlanner(DRCRobotModel robotModel, HumanoidReferenceFrames referenceFrames, + AtomicReference commandMessage, ContinuousHikingParameters continuousHikingParameters, MonteCarloFootstepPlannerParameters monteCarloPlannerParameters, DefaultFootstepPlannerParametersBasics footstepPlannerParameters, @@ -81,6 +82,7 @@ public ContinuousPlanner(DRCRobotModel robotModel, ContinuousHikingLogger continuousHikingLogger) { this.referenceFrames = referenceFrames; + this.commandMessage = commandMessage; this.continuousHikingParameters = continuousHikingParameters; this.footstepPlannerParameters = footstepPlannerParameters; this.swingPlannerParameters = swingPlannerParameters; @@ -127,27 +129,17 @@ public void setWalkingStartMidPose(FramePose3D startingPose) walkingStartMidPose.getOrientation().setToYawOrientation(startingPose.getRotation().getYaw()); } - public void planToGoal(ContinuousWalkingCommandMessage command, SideDependentList goalPoses) + public void planToGoal(SideDependentList goalPoses) { - this.command = command; - if (command.getUseAstarFootstepPlanner()) + if (commandMessage.get().getUseAstarFootstepPlanner()) { - latestFootstepPlan = generateAStarFootstepPlan(latestHeightMapData, latestTerrainMapData, command.getUsePreviousPlanAsReference(), false, goalPoses); + latestFootstepPlan = generateAStarFootstepPlan(latestHeightMapData, latestTerrainMapData, commandMessage.get().getUsePreviousPlanAsReference(), false, goalPoses); } - else if (command.getUseMonteCarloFootstepPlanner()) + else if (commandMessage.get().getUseMonteCarloFootstepPlanner()) { latestFootstepPlan = generateMonteCarloFootstepPlan(goalPoses); } - else if (command.getUseHybridPlanner()) - { - generateMonteCarloFootstepPlan(goalPoses); - latestFootstepPlan = generateAStarFootstepPlan(latestHeightMapData, - latestTerrainMapData, - command.getUsePreviousPlanAsReference(), - command.getUseMonteCarloPlanAsReference(), - goalPoses); - } else { latestFootstepPlan = generateAStarFootstepPlan(latestHeightMapData, latestTerrainMapData, true, false, goalPoses); @@ -180,7 +172,7 @@ public FootstepPlan generateAStarFootstepPlan(HeightMapData heightMapData, request.setAbortIfGoalStepSnappingFails(true); // When walking backwards, we want to keep the body facing in the same direction, otherwise the robot will turn as it tries to step backwards - if (command.getWalkBackwards()) + if (commandMessage.get().getWalkBackwards()) { FramePose3D bodyMidGoalPose = new FramePose3D(); bodyMidGoalPose.interpolate(goalPoses.get(RobotSide.LEFT), goalPoses.get(RobotSide.RIGHT), 0.5); @@ -473,7 +465,7 @@ public void transitionCallback() continuousHikingLogger.appendString("[TRANSITION]: Resetting Previous Plan Reference"); this.previousFootstepPlan = new FootstepPlan(latestFootstepPlan); - if (command.getUseMonteCarloFootstepPlanner()) + if (commandMessage.get().getUseMonteCarloFootstepPlanner()) { monteCarloFootstepPlanner.transitionToOptimal(); } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlannerSchedulingTask.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlannerSchedulingTask.java index e4bf5aa1013..869f5b124dd 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlannerSchedulingTask.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlannerSchedulingTask.java @@ -1,11 +1,11 @@ package us.ihmc.behaviors.activeMapping; -import behavior_msgs.msg.dds.ContinuousWalkingCommandMessage; +import behavior_msgs.msg.dds.ContinuousHikingCommandMessage; import us.ihmc.avatar.drcRobot.DRCRobotModel; import us.ihmc.behaviors.activeMapping.ContinuousHikingStateMachine.*; import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.footstepPlanning.MonteCarloFootstepPlannerParameters; -import us.ihmc.footstepPlanning.communication.ContinuousWalkingAPI; +import us.ihmc.footstepPlanning.communication.ContinuousHikingAPI; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersBasics; import us.ihmc.footstepPlanning.swing.SwingPlannerParametersBasics; import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames; @@ -61,13 +61,14 @@ public ContinuousPlannerSchedulingTask(DRCRobotModel robotModel, String simpleRobotName = robotModel.getSimpleRobotName(); ROS2Helper ros2Helper = new ROS2Helper(ros2Node); - AtomicReference commandMessage = new AtomicReference<>(new ContinuousWalkingCommandMessage()); - ros2Helper.subscribeViaCallback(ContinuousWalkingAPI.CONTINUOUS_WALKING_COMMAND, commandMessage::set); + AtomicReference commandMessage = new AtomicReference<>(new ContinuousHikingCommandMessage()); + ros2Helper.subscribeViaCallback(ContinuousHikingAPI.CONTINUOUS_HIKING_COMMAND, commandMessage::set); TerrainPlanningDebugger debugger = new TerrainPlanningDebugger(ros2Node, monteCarloFootstepPlannerParameters, planningMode); ContinuousHikingLogger continuousHikingLogger = new ContinuousHikingLogger(); continuousPlanner = new ContinuousPlanner(robotModel, referenceFrames, + commandMessage, continuousHikingParameters, monteCarloFootstepPlannerParameters, footstepPlannerParameters, @@ -96,8 +97,7 @@ public ContinuousPlannerSchedulingTask(DRCRobotModel robotModel, continuousHikingParameters, terrainMap, debugger, - continuousHikingLogger, - planningMode); + continuousHikingLogger); State waitingtoLandState = new WaitingToLandState(ros2Helper, simpleRobotName, continuousPlanner, @@ -111,10 +111,8 @@ public ContinuousPlannerSchedulingTask(DRCRobotModel robotModel, stateMachineFactory.addState(ContinuousHikingState.WAITING_TO_LAND, waitingtoLandState); // Create different conditions - StartContinuousHikingTransitionCondition startContinuousHikingTransitionCondition = new StartContinuousHikingTransitionCondition(commandMessage, - continuousHikingParameters); - StopContinuousHikingTransitionCondition stopContinuousHikingTransitionCondition = new StopContinuousHikingTransitionCondition(commandMessage, - continuousHikingParameters); + StartContinuousHikingTransitionCondition startContinuousHikingTransitionCondition = new StartContinuousHikingTransitionCondition(commandMessage); + StopContinuousHikingTransitionCondition stopContinuousHikingTransitionCondition = new StopContinuousHikingTransitionCondition(commandMessage); PlanAgainTransitionCondition planAgainTransitionCondition = new PlanAgainTransitionCondition(continuousPlanner, continuousHikingParameters); //NOTE: The transitions for the state machine are checked in the order they are added. 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 57dc2d2cfeb..c99d6e36235 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 @@ -15,7 +15,7 @@ import us.ihmc.euclid.tuple3D.Point3D; import us.ihmc.footstepPlanning.FootstepPlan; import us.ihmc.footstepPlanning.MonteCarloFootstepPlannerParameters; -import us.ihmc.footstepPlanning.communication.ContinuousWalkingAPI; +import us.ihmc.footstepPlanning.communication.ContinuousHikingAPI; import us.ihmc.footstepPlanning.monteCarloPlanning.MonteCarloFootstepNode; import us.ihmc.footstepPlanning.monteCarloPlanning.MonteCarloFootstepPlannerRequest; import us.ihmc.footstepPlanning.monteCarloPlanning.MonteCarloPlannerTools; @@ -74,11 +74,11 @@ public TerrainPlanningDebugger(ROS2Node ros2Node, MonteCarloFootstepPlannerParam this.planningMode = planningMode; if (ros2Node != null) { - plannedFootstesPublisherForUI = ros2Node.createPublisher(ContinuousWalkingAPI.PLANNED_FOOTSTEPS); - statusPublisher = ros2Node.createPublisher(ContinuousWalkingAPI.CONTINUOUS_WALKING_STATUS); - monteCarloPlanPublisherForUI = ros2Node.createPublisher(ContinuousWalkingAPI.MONTE_CARLO_FOOTSTEP_PLAN); - startAndGoalPublisherForUI = ros2Node.createPublisher(ContinuousWalkingAPI.START_AND_GOAL_FOOTSTEPS); - monteCarloNodesPublisherForUI = ros2Node.createPublisher(ContinuousWalkingAPI.MONTE_CARLO_TREE_NODES); + plannedFootstesPublisherForUI = ros2Node.createPublisher(ContinuousHikingAPI.PLANNED_FOOTSTEPS); + statusPublisher = ros2Node.createPublisher(ContinuousHikingAPI.CONTINUOUS_WALKING_STATUS); + monteCarloPlanPublisherForUI = ros2Node.createPublisher(ContinuousHikingAPI.MONTE_CARLO_FOOTSTEP_PLAN); + startAndGoalPublisherForUI = ros2Node.createPublisher(ContinuousHikingAPI.START_AND_GOAL_FOOTSTEPS); + monteCarloNodesPublisherForUI = ros2Node.createPublisher(ContinuousHikingAPI.MONTE_CARLO_TREE_NODES); } } diff --git a/ihmc-high-level-behaviors/src/main/resources/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.json b/ihmc-high-level-behaviors/src/main/resources/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.json index 86bddf8ebe5..86aa0543ae0 100644 --- a/ihmc-high-level-behaviors/src/main/resources/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.json +++ b/ihmc-high-level-behaviors/src/main/resources/us/ihmc/behaviors/activeMapping/ContinuousHikingParameters.json @@ -1,6 +1,5 @@ { "title" : "Continuous Hiking parameters", - "Enable continuous hiking" : false, "Step publisher enabled" : false, "Override entire queue each step" : true, "Number of steps to send" : 3, From 1e533de205800556a182b447f4797ea737452ce0 Mon Sep 17 00:00:00 2001 From: Tomasz Bialek Date: Mon, 16 Dec 2024 12:13:41 -0600 Subject: [PATCH 10/16] Only instantiating ImageMessages once --- .../RealsenseColorDepthImagePublisher.java | 4 ++-- .../sensors/ZEDColorDepthImagePublisher.java | 20 +++++++++---------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/ihmc-perception/src/main/java/us/ihmc/sensors/RealsenseColorDepthImagePublisher.java b/ihmc-perception/src/main/java/us/ihmc/sensors/RealsenseColorDepthImagePublisher.java index 77a709f36b5..3243320cff9 100644 --- a/ihmc-perception/src/main/java/us/ihmc/sensors/RealsenseColorDepthImagePublisher.java +++ b/ihmc-perception/src/main/java/us/ihmc/sensors/RealsenseColorDepthImagePublisher.java @@ -21,7 +21,9 @@ public class RealsenseColorDepthImagePublisher { private final ROS2Node ros2Node; private final ROS2Publisher ros2DepthImagePublisher; + private final ImageMessage depthImageMessage = new ImageMessage(); private final ROS2Publisher ros2ColorImagePublisher; + private final ImageMessage colorImageMessage = new ImageMessage(); private CUDAJPEGProcessor imageEncoder; private CUDACompressionTools compressionTools; @@ -88,7 +90,6 @@ private void publishDepthImage(RawImage depthImageToPublish) BytePointer depthPNGPointer = compressionTools.compressDepth(depthImageToPublish.getGpuImageMat()); // Publish image - ImageMessage depthImageMessage = new ImageMessage(); PerceptionMessageTools.packImageMessage(depthImageToPublish, depthPNGPointer, CompressionType.ZSTD_NVJPEG_HYBRID, depthImageMessage); ros2DepthImagePublisher.publish(depthImageMessage); @@ -136,7 +137,6 @@ private void publishColorImage(RawImage colorImageToPublish) imageEncoder.encodeBGR(colorImageToPublish.getGpuImageMat(), colorJPEGPointer); // Publish compressed image - ImageMessage colorImageMessage = new ImageMessage(); PerceptionMessageTools.packImageMessage(colorImageToPublish, colorJPEGPointer, CompressionType.NVJPEG, colorImageMessage); ros2ColorImagePublisher.publish(colorImageMessage); diff --git a/ihmc-perception/src/main/java/us/ihmc/sensors/ZEDColorDepthImagePublisher.java b/ihmc-perception/src/main/java/us/ihmc/sensors/ZEDColorDepthImagePublisher.java index 04b5fe5f3c3..f350e6a4df3 100644 --- a/ihmc-perception/src/main/java/us/ihmc/sensors/ZEDColorDepthImagePublisher.java +++ b/ihmc-perception/src/main/java/us/ihmc/sensors/ZEDColorDepthImagePublisher.java @@ -23,7 +23,10 @@ public class ZEDColorDepthImagePublisher { private final ROS2Node ros2Node; private final SideDependentList> ros2ColorImagePublishers; + private final SideDependentList colorImageMessages = new SideDependentList<>(side -> new ImageMessage()); private final ROS2Publisher ros2DepthImagePublisher; + private final ImageMessage depthImageMessage = new ImageMessage(); + private final ROS2Publisher ros2CutOutDepthImagePublisher; private final SideDependentList imageEncoders = new SideDependentList<>(); @@ -84,7 +87,8 @@ private void publishDepthThreadFunction() if (nextGpuDepthImage != null) { - ros2DepthImagePublisher.publish(createDepthImageMessage(nextGpuDepthImage)); + packDepthImageMessage(nextGpuDepthImage); + ros2DepthImagePublisher.publish(depthImageMessage); lastDepthSequenceNumber = nextGpuDepthImage.getSequenceNumber(); } } @@ -111,7 +115,8 @@ private void publishCutOutDepth() if (nextCutOutDepthImage != null) { - ros2CutOutDepthImagePublisher.publish(createDepthImageMessage(nextCutOutDepthImage)); + packDepthImageMessage(nextCutOutDepthImage); + ros2CutOutDepthImagePublisher.publish(depthImageMessage); lastCutOutDepthSequenceNumber = nextCutOutDepthImage.getSequenceNumber(); } } @@ -125,10 +130,8 @@ private void publishCutOutDepth() } } - private ImageMessage createDepthImageMessage(RawImage depthImageToPublish) + private void packDepthImageMessage(RawImage depthImageToPublish) { - ImageMessage depthImageMessage = new ImageMessage(); - if (depthImageToPublish != null && depthImageToPublish.isAvailable()) { depthImageToPublish.get(); @@ -144,8 +147,6 @@ private ImageMessage createDepthImageMessage(RawImage depthImageToPublish) depthImageToPublish.release(); } - - return depthImageMessage; } private void publishLeftColorThreadFunction() @@ -209,9 +210,8 @@ private void publishColorImage(RawImage colorImageToPublish, RobotSide side) imageEncoders.get(side).encodeBGR(colorImageToPublish.getGpuImageMat(), colorJPEGPointer); // Publish compressed image - ImageMessage colorImageMessage = new ImageMessage(); - PerceptionMessageTools.packImageMessage(colorImageToPublish, colorJPEGPointer, CompressionType.NVJPEG, colorImageMessage); - ros2ColorImagePublishers.get(side).publish(colorImageMessage); + PerceptionMessageTools.packImageMessage(colorImageToPublish, colorJPEGPointer, CompressionType.NVJPEG, colorImageMessages.get(side)); + ros2ColorImagePublishers.get(side).publish(colorImageMessages.get(side)); lastColorSequenceNumbers.put(side, colorImageToPublish.getSequenceNumber()); From 0a37708d1ef7f3841a402ee3770d206c543711c2 Mon Sep 17 00:00:00 2001 From: Tomasz Bialek <103042423+TomaszTB@users.noreply.github.com> Date: Wed, 18 Dec 2024 09:19:08 -0600 Subject: [PATCH 11/16] [Feature] Faster Point Cloud Visualizers (#534) * WIP - Basic visualizer working * Working delay compensation + demo * Minor improvements * Improved visualizer * Added ROS2 Point Cloud Renderer * Replaced old point cloud visualizer with new one * Removed old point cloud visualizer * Added ability to disable color image rendering * Fixed transform issue * colorToDepth -> depthToColor * decodeMessage -> decodeMessageCPU * RDXROS2RawImagePointCloudVisualizer -> RDXROS2ColoredPointCloudVisualizer --- .../rdx/AbstractRDXPointCloudRenderer.java | 5 + .../perception/RDXZEDSVORecordingDemo.java | 2 +- .../RDXRawImagePointCloudVisualizerDemo.java | 121 ++++++ .../RDXRawImagePointCloudRenderer.java | 4 +- .../RDXRawImagePointCloudVisualizer.java | 279 ++++++++++++++ ...PinholePinholeColoredPointCloudKernel.java | 98 ----- .../RDXROS2ColoredPointCloudVisualizer.java | 344 ++++-------------- ...OS2ColoredPointCloudVisualizerChannel.java | 265 -------------- ...loredPointCloudVisualizerColorChannel.java | 67 ---- ...loredPointCloudVisualizerDepthChannel.java | 65 ---- .../ImGuiPanels.json | 7 + .../ImGuiSettings.ini | 27 ++ .../openCL/PinholePinholeColoredPointCloud.cl | 83 ----- .../imageMessage/ImageMessageDecoder.java | 32 ++ .../java/us/ihmc/robotics/time/TimeTools.java | 31 ++ .../us/ihmc/robotics/time/TimeToolsTest.java | 48 +++ 16 files changed, 630 insertions(+), 848 deletions(-) create mode 100644 ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/RDXRawImagePointCloudVisualizerDemo.java create mode 100644 ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXRawImagePointCloudVisualizer.java delete mode 100644 ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXPinholePinholeColoredPointCloudKernel.java delete mode 100644 ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizerChannel.java delete mode 100644 ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizerColorChannel.java delete mode 100644 ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizerDepthChannel.java create mode 100644 ihmc-high-level-behaviors/src/libgdx/resources/configurations/RDXRawImagePointCloudVisualizerDemo/ImGuiPanels.json create mode 100644 ihmc-high-level-behaviors/src/libgdx/resources/configurations/RDXRawImagePointCloudVisualizerDemo/ImGuiSettings.ini delete mode 100644 ihmc-high-level-behaviors/src/libgdx/resources/openCL/PinholePinholeColoredPointCloud.cl diff --git a/ihmc-graphics/src/libgdx/java/us/ihmc/rdx/AbstractRDXPointCloudRenderer.java b/ihmc-graphics/src/libgdx/java/us/ihmc/rdx/AbstractRDXPointCloudRenderer.java index fdc06c6d44b..fc93ff2d310 100644 --- a/ihmc-graphics/src/libgdx/java/us/ihmc/rdx/AbstractRDXPointCloudRenderer.java +++ b/ihmc-graphics/src/libgdx/java/us/ihmc/rdx/AbstractRDXPointCloudRenderer.java @@ -80,6 +80,11 @@ public void setPointScale(float scale) pointScale = scale; } + public void setDefaultPointColor(float r, float g, float b, float a) + { + defaultPointColor.set(r, g, b, a); + } + public void setDefaultPointColor(Color color) { defaultPointColor.set(color); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXZEDSVORecordingDemo.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXZEDSVORecordingDemo.java index a45a6ffe5bd..9f098aa58fe 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXZEDSVORecordingDemo.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXZEDSVORecordingDemo.java @@ -29,7 +29,7 @@ public class RDXZEDSVORecordingDemo // Demo file on Google Drive https://drive.google.com/file/d/1wF5tFVqEJM21uK12g_O5GpvACPJhXKZ1/view // Uncomment below lines to play it back private static final RecordMode RECORD_MODE = RecordMode.PLAYBACK; - private static final String SVO_FILE_NAME = IHMCCommonPaths.PERCEPTION_LOGS_DIRECTORY.toAbsolutePath() + "/20240625_154000_ZEDRecording_Demo.svo2"; + private static final String SVO_FILE_NAME = IHMCCommonPaths.PERCEPTION_LOGS_DIRECTORY.toAbsolutePath() + "/20240715_103234_ZEDRecording_NewONRCourseWalk.svo2"; private final RDXBaseUI baseUI = new RDXBaseUI(); private final ROS2Node ros2Node; diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/RDXRawImagePointCloudVisualizerDemo.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/RDXRawImagePointCloudVisualizerDemo.java new file mode 100644 index 00000000000..ca0f38512cb --- /dev/null +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/RDXRawImagePointCloudVisualizerDemo.java @@ -0,0 +1,121 @@ +package us.ihmc.rdx.ui; + +import imgui.ImGui; +import imgui.type.ImFloat; +import us.ihmc.commons.thread.RepeatingTaskThread; +import us.ihmc.perception.RawImage; +import us.ihmc.rdx.Lwjgl3ApplicationAdapter; +import us.ihmc.rdx.ui.gizmo.RDXPose3DGizmo; +import us.ihmc.rdx.ui.graphics.RDXRawImagePointCloudVisualizer; +import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.time.TimeTools; +import us.ihmc.sensors.ZEDColorDepthImageRetriever; + +import java.time.Instant; +import java.util.LinkedList; +import java.util.Queue; + +public class RDXRawImagePointCloudVisualizerDemo +{ + private RDXRawImagePointCloudVisualizer pointCloudVisualizer; + + private ZEDColorDepthImageRetriever zed; + private final RepeatingTaskThread zedGrabThread = new RepeatingTaskThread("ZEDGrabThread", this::zedGrabThread); + + private final Queue depthImageQueue = new LinkedList<>(); + private final ImFloat depthImageDelay = new ImFloat(0.0f); + private final Queue colorImageQueue = new LinkedList<>(); + private final ImFloat colorImageDelay = new ImFloat(0.0f); + + private RDXPose3DGizmo sensorPoseGizmo; + + private RDXRawImagePointCloudVisualizerDemo() + { + RDXBaseUI baseUI = new RDXBaseUI(); + baseUI.launchRDXApplication(new Lwjgl3ApplicationAdapter() + { + @Override + public void create() + { + baseUI.create(); + baseUI.getPrimaryScene().addCoordinateFrame(0.3); + + sensorPoseGizmo = new RDXPose3DGizmo(); + sensorPoseGizmo.createAndSetupDefault(baseUI.getPrimary3DPanel()); + baseUI.getPrimaryScene().addRenderableProvider(sensorPoseGizmo); + baseUI.getPrimary3DPanel().addImGui3DViewPickCalculator(sensorPoseGizmo::calculate3DViewPick); + + zed = new ZEDColorDepthImageRetriever(0, sensorPoseGizmo::getGizmoFrame, () -> true, () -> true); + zed.start(); + + pointCloudVisualizer = new RDXRawImagePointCloudVisualizer("ZED Point Cloud"); + baseUI.getPrimaryScene().addRenderableProvider(pointCloudVisualizer); + + baseUI.getImGuiPanelManager().addPanel("Options", this::renderOptions); + zedGrabThread.startRepeating(); + } + + @Override + public void render() + { + pointCloudVisualizer.update(); + + baseUI.renderBeforeOnScreenUI(); + baseUI.renderEnd(); + } + + private void renderOptions() + { + pointCloudVisualizer.renderImGuiWidgets(); + + ImGui.separator(); + + ImGui.sliderFloat("Depth Delay", depthImageDelay.getData(), 0.0f, 10.0f); + ImGui.sliderFloat("Color Delay", colorImageDelay.getData(), 0.0f, 10.0f); + } + + @Override + public void dispose() + { + zedGrabThread.blockingKill(); + zed.destroy(); + pointCloudVisualizer.destroy(); + colorImageQueue.forEach(RawImage::release); + depthImageQueue.forEach(RawImage::release); + baseUI.dispose(); + } + }); + } + + private void zedGrabThread() + { + RawImage colorImage = zed.getLatestRawColorImage(RobotSide.LEFT); + RawImage depthImage = zed.getLatestRawDepthImage(); + + colorImageQueue.add(colorImage); + depthImageQueue.add(depthImage); + + Instant now = Instant.now(); + + RawImage oldestColorImage = colorImageQueue.peek(); + while (oldestColorImage != null && TimeTools.secondsBetween(oldestColorImage.getAcquisitionTime(), now) > colorImageDelay.get()) + { + pointCloudVisualizer.setColorImage(oldestColorImage); + colorImageQueue.remove().release(); + oldestColorImage = colorImageQueue.peek(); + } + + RawImage oldestDepthImage = depthImageQueue.peek(); + while (oldestDepthImage != null && TimeTools.secondsBetween(oldestDepthImage.getAcquisitionTime(), now) > depthImageDelay.get()) + { + pointCloudVisualizer.setDepthImage(oldestDepthImage); + depthImageQueue.remove().release(); + oldestDepthImage = depthImageQueue.peek(); + } + } + + public static void main(String[] args) + { + new RDXRawImagePointCloudVisualizerDemo(); + } +} diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXRawImagePointCloudRenderer.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXRawImagePointCloudRenderer.java index e4a5e7c8c5c..a3838874be1 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXRawImagePointCloudRenderer.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXRawImagePointCloudRenderer.java @@ -109,7 +109,8 @@ public void updateMesh(RawImage depthImage, RawImage colorImage) // Update color image uniforms colorIntrinsics = colorImage.getIntrinsicsCopy(); - depthPose.getReferenceFrame().getTransformToDesiredFrame(depthToColorTransform, colorImage.getPose().getReferenceFrame()); + depthToColorTransform.setAndInvert(colorImage.getPose()); + depthToColorTransform.multiply(depthPose); LibGDXTools.toLibGDX(depthToColorTransform, tempColorTransform, libGDXColorTransform); // Reallocate pixmap and data pointer if image size changed @@ -225,7 +226,6 @@ private void registerColorImageUniforms(RDXShader rdxShader) RDXUniform depthTransformUniform = RDXUniform.createGlobalUniform("u_depthToColorTransform", (shader, inputID, renderable, combinedAttributes) -> { - LibGDXTools.toLibGDX(depthToColorTransform, tempColorTransform, libGDXColorTransform); shader.set(inputID, libGDXColorTransform); }); rdxShader.registerUniform(depthTransformUniform); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXRawImagePointCloudVisualizer.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXRawImagePointCloudVisualizer.java new file mode 100644 index 00000000000..9ed66dd4bcd --- /dev/null +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXRawImagePointCloudVisualizer.java @@ -0,0 +1,279 @@ +package us.ihmc.rdx.ui.graphics; + +import com.badlogic.gdx.graphics.g3d.Renderable; +import com.badlogic.gdx.utils.Array; +import com.badlogic.gdx.utils.Pool; +import imgui.ImGui; +import imgui.flag.ImGuiCol; +import imgui.type.ImFloat; +import imgui.type.ImInt; +import us.ihmc.commons.thread.Notification; +import us.ihmc.perception.RawImage; +import us.ihmc.rdx.AbstractRDXPointCloudRenderer.ColoringMethod; +import us.ihmc.rdx.imgui.ImGuiExpandCollapseRenderer; +import us.ihmc.rdx.imgui.ImGuiPlot; +import us.ihmc.rdx.imgui.ImGuiTools; +import us.ihmc.rdx.sceneManager.RDXSceneLevel; +import us.ihmc.robotics.time.TimeTools; + +import java.time.Instant; +import java.util.Arrays; +import java.util.Deque; +import java.util.LinkedList; +import java.util.List; +import java.util.Set; + +/** + * Visualizer for visualizing point cloud from depth and color images. + * Attempts to compensate for de-synchronization of color and depth + * using the image acquisition times. + */ +public class RDXRawImagePointCloudVisualizer extends RDXVisualizer +{ + // The first element in the history is the newest, and the last is the oldest + private final LinkedList depthImageHistory = new LinkedList<>(); + private final LinkedList colorImageHistory = new LinkedList<>(); + private final Notification newImageNotification = new Notification(); + + private RDXRawImagePointCloudRenderer pointCloudRenderer; + private int maxPoints = 0; + private String[] availableColoringMethods = new String[0]; + + private final ImInt coloringMethod = new ImInt(ColoringMethod.COLOR_IMAGE.ordinal()); + private final float[] defaultColor = new float[] {1.0f, 1.0f, 1.0f}; + private final ImFloat pointScale = new ImFloat(1.5f); + + private final ImGuiPlot deSyncPlot = new ImGuiPlot("Depth Color De-Sync", 100, 230, 22); + private double colorDepthDeSync = Double.NaN; + + private final ImGuiExpandCollapseRenderer expandCollapseRenderer = new ImGuiExpandCollapseRenderer(); + private boolean showExtraOptions = false; + + /** + * The duration of history kept in the image histories. + * Affects how much delay difference the visualizer can compensate for. + * For example, if the color images have one second of delay compared to depth, + * at least one second of history is required to sync the color and depth. + */ + private final ImFloat historyLength = new ImFloat(1.0f); + + /** + * Maximum de-synchronization allowed between color and depth images, + * after attempting to compensate using the image history. + * If the newest depth image and the oldest color image are + * de-synchronized by more than {@code maxDeSync} seconds, + * the visualizer will default to rendering depth with gradient colors. + * This value should not be more than half the history length. + */ + private final ImFloat maxDeSync = new ImFloat(0.1f); + + private final boolean enableColorImageRendering; + private boolean switchBackToColor = false; + + public RDXRawImagePointCloudVisualizer(String title) + { + this(title, true); + } + + public RDXRawImagePointCloudVisualizer(String title, boolean enableColorImageRendering) + { + super(title); + this.enableColorImageRendering = enableColorImageRendering; + } + + public void setDepthImage(RawImage depthImage) + { + RawImage image = depthImage.get(); + + synchronized (depthImageHistory) + { + if (image != null) + depthImageHistory.addFirst(depthImage); + } + + newImageNotification.set(); + } + + public void setColorImage(RawImage colorImage) + { + RawImage image = colorImage.get(); + + synchronized (colorImageHistory) + { + if (image != null) + colorImageHistory.addFirst(colorImage); + } + + newImageNotification.set(); + } + + @Override + public void update() + { + super.update(); + + // Ensure we got something new to render + if (!newImageNotification.poll()) + return; + + // Ensure we got at least one depth image + if (depthImageHistory.isEmpty()) + return; + + RawImage depthImage = null; + RawImage colorImage = null; + boolean foundMatchingColorImage = false; + + // We try to match the color and depth images according to acquisition time + if (enableColorImageRendering && !colorImageHistory.isEmpty() && (coloringMethod.get() == ColoringMethod.COLOR_IMAGE.ordinal() || switchBackToColor)) + { + depthImage = depthImageHistory.getFirst(); + colorImage = colorImageHistory.getFirst(); + colorDepthDeSync = Math.abs(TimeTools.secondsBetween(colorImage.getAcquisitionTime(), depthImage.getAcquisitionTime())); + + if (depthImage.getAcquisitionTime().isBefore(colorImage.getAcquisitionTime())) + { + synchronized (colorImageHistory) + { // Newest depth image is older than the newest color image, so we find an older color image + colorImage = findClosest(depthImage, colorImageHistory); + } + } + else + { + synchronized (depthImageHistory) + { // Newest depth image is newer than the newest color image, so we find an older depth image + depthImage = findClosest(colorImage, depthImageHistory); + } + } + + // If the closest images are too far apart, we default to rendering the newest depth image without color + double secondsBetweenImages = Math.abs(TimeTools.secondsBetween(depthImage.getAcquisitionTime(), colorImage.getAcquisitionTime())); + foundMatchingColorImage = secondsBetweenImages < maxDeSync.get(); + } + + // If no matching color image was found, use the newest depth image and set coloring method to gradient + if (!foundMatchingColorImage) + { + depthImage = depthImageHistory.getFirst(); + colorImage = null; + + if (!switchBackToColor && coloringMethod.get() == ColoringMethod.COLOR_IMAGE.ordinal()) + { + coloringMethod.set(ColoringMethod.GRADIENT_WORLD_Z.ordinal()); + switchBackToColor = true; + } + } + else if (switchBackToColor) + { + coloringMethod.set(ColoringMethod.COLOR_IMAGE.ordinal()); + switchBackToColor = false; + } + + // Ensure the renderer is initialized with a large enough max size + int size = depthImage.getWidth() * depthImage.getHeight(); + if (maxPoints < size) + { + maxPoints = size; + + if (pointCloudRenderer != null) + pointCloudRenderer.dispose(); + pointCloudRenderer = new RDXRawImagePointCloudRenderer(enableColorImageRendering); + pointCloudRenderer.create(maxPoints); + availableColoringMethods = Arrays.stream(pointCloudRenderer.getAvailableColoringMethods()).map(Enum::name).toArray(String[]::new); + } + + // Update the render settings + pointCloudRenderer.setPointScale(pointScale.get() / depthImage.getFocalLengthX()); + pointCloudRenderer.setDefaultPointColor(defaultColor[0], defaultColor[1], defaultColor[2], 1.0f); + pointCloudRenderer.setColoringMethod(ColoringMethod.values()[coloringMethod.get()]); + + // Update the mesh + if (colorImage == null) + pointCloudRenderer.updateMesh(depthImage); + else + pointCloudRenderer.updateMesh(depthImage, colorImage); + + // Remove images that are too old from history + synchronized (depthImageHistory) + { + clearExpiredHistory(depthImageHistory, historyLength.get()); + } + synchronized (colorImageHistory) + { + clearExpiredHistory(colorImageHistory, historyLength.get()); + } + } + + private RawImage findClosest(RawImage targetImage, List imageHistory) + { + List imageAcquisitionTimes = imageHistory.stream().map(RawImage::getAcquisitionTime).toList(); + int closestColorImageIndex = TimeTools.findClosestTimeIndex(targetImage.getAcquisitionTime(), imageAcquisitionTimes); + return imageHistory.get(closestColorImageIndex); + } + + private static void clearExpiredHistory(Deque imageHistory, double maxDuration) + { + while (!imageHistory.isEmpty() + && TimeTools.secondsBetween(imageHistory.getLast().getAcquisitionTime(), imageHistory.getFirst().getAcquisitionTime()) > maxDuration) + { + imageHistory.removeLast().release(); + } + } + + @Override + public void renderImGuiWidgets() + { + if (enableColorImageRendering) + { + // Render the de-sync plot + deSyncPlot.setYScale(0, historyLength.get()); + deSyncPlot.setWidth((int) (0.65f * ImGui.getWindowWidth())); + ImGui.pushStyleColor(ImGuiCol.PlotLines, ImGuiTools.greenRedGradientColor(colorDepthDeSync, 0.0f, historyLength.get())); + deSyncPlot.render(colorDepthDeSync); + ImGui.popStyleColor(); + } + + // Render the renderer options + if (ImGui.combo("Coloring Method", coloringMethod, availableColoringMethods)) + switchBackToColor = false; + ImGui.colorEdit3("Default Color", defaultColor); + ImGui.sliderFloat("Point Scale", pointScale.getData(), 0.0f, 2.0f); + + // Render extra options (if expanded) + if (enableColorImageRendering && expandCollapseRenderer.render(showExtraOptions)) + showExtraOptions = !showExtraOptions; + ImGui.sameLine(); + ImGui.text((showExtraOptions ? "Hide" : "Show") + " Extra Options"); + if (showExtraOptions) + { + if (ImGui.sliderFloat("History Length (S)", historyLength.getData(), 0.0f, 3.0f)) + { + if (maxDeSync.get() > 0.5f * historyLength.get()) + maxDeSync.set(0.5f * historyLength.get()); + } + ImGuiTools.previousWidgetTooltip("Affects how much de-sync can be corrected."); + + ImGui.sliderFloat("Max De-Sync (S)", maxDeSync.getData(), 0.0f, 0.5f * historyLength.get()); + ImGuiTools.previousWidgetTooltip("Amount of de-sync allowed after correction."); + } + } + + @Override + public void getRenderables(Array renderables, Pool pool, Set sceneLevels) + { + if (pointCloudRenderer != null) + pointCloudRenderer.getRenderables(renderables, pool); + } + + @Override + public void destroy() + { + super.destroy(); + + depthImageHistory.forEach(RawImage::release); + colorImageHistory.forEach(RawImage::release); + + if (pointCloudRenderer != null) + pointCloudRenderer.dispose(); + } +} diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXPinholePinholeColoredPointCloudKernel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXPinholePinholeColoredPointCloudKernel.java deleted file mode 100644 index b0b5bea9e9e..00000000000 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXPinholePinholeColoredPointCloudKernel.java +++ /dev/null @@ -1,98 +0,0 @@ -package us.ihmc.rdx.ui.graphics.ros2.pointCloud; - -import org.bytedeco.opencl._cl_kernel; -import org.bytedeco.opencl._cl_program; -import org.bytedeco.opencl.global.OpenCL; -import org.bytedeco.opencv.global.opencv_core; -import us.ihmc.euclid.transform.RigidBodyTransform; -import us.ihmc.perception.BytedecoImage; -import us.ihmc.perception.opencl.OpenCLFloatBuffer; -import us.ihmc.perception.opencl.OpenCLFloatParameters; -import us.ihmc.perception.opencl.OpenCLManager; -import us.ihmc.perception.opencl.OpenCLRigidBodyTransformParameter; -import us.ihmc.robotics.referenceFrames.MutableReferenceFrame; - -/** - * This class is for rendering colored point clouds when the depth camera - * and the color camera are both pinhole model. - */ -public class RDXPinholePinholeColoredPointCloudKernel -{ - private final OpenCLManager openCLManager; - private final _cl_program openCLProgram; - private final _cl_kernel kernel; - private final OpenCLFloatParameters parametersBuffer = new OpenCLFloatParameters(); - private final OpenCLRigidBodyTransformParameter depthToWorldTransformParameter = new OpenCLRigidBodyTransformParameter(); - private final OpenCLRigidBodyTransformParameter depthToColorTransformParameter = new OpenCLRigidBodyTransformParameter(); - private final BytedecoImage placeholderColorImage; - private final MutableReferenceFrame depthFrame = new MutableReferenceFrame(); - private final MutableReferenceFrame colorFrame = new MutableReferenceFrame(); - private final RigidBodyTransform depthToColorTransform = new RigidBodyTransform(); - - - public RDXPinholePinholeColoredPointCloudKernel(OpenCLManager openCLManager) - { - this.openCLManager = openCLManager; - openCLProgram = openCLManager.loadProgram("PinholePinholeColoredPointCloud", "PerceptionCommon.cl"); - kernel = openCLManager.createKernel(openCLProgram, "computeVertexBuffer"); - placeholderColorImage = new BytedecoImage(1, 1, opencv_core.CV_8UC4); - placeholderColorImage.createOpenCLImage(openCLManager, OpenCL.CL_MEM_READ_ONLY); - } - - public void computeVertexBuffer(RDXROS2ColoredPointCloudVisualizerColorChannel colorChannel, - RDXROS2ColoredPointCloudVisualizerDepthChannel depthChannel, - boolean useSensorColor, - int gradientMode, - boolean useSinusoidalGradientPattern, - float pointSize, - OpenCLFloatBuffer pointCloudVertexBuffer) - { - depthFrame.update(transformToWorld -> transformToWorld.set(depthChannel.getRotationMatrixToWorld(), depthChannel.getTranslationToWorld())); - colorFrame.update(transformToWorld -> transformToWorld.set(colorChannel.getRotationMatrixToWorld(), colorChannel.getTranslationToWorld())); - depthFrame.getReferenceFrame().getTransformToDesiredFrame(depthToColorTransform, colorFrame.getReferenceFrame()); - - parametersBuffer.setParameter(colorChannel.getFx()); - parametersBuffer.setParameter(colorChannel.getFy()); - parametersBuffer.setParameter(colorChannel.getCx()); - parametersBuffer.setParameter(colorChannel.getCy()); - parametersBuffer.setParameter(depthChannel.getFx()); - parametersBuffer.setParameter(depthChannel.getFy()); - parametersBuffer.setParameter(depthChannel.getCx()); - parametersBuffer.setParameter(depthChannel.getCy()); - parametersBuffer.setParameter((float) depthChannel.getImageWidth()); - parametersBuffer.setParameter((float) depthChannel.getImageHeight()); - parametersBuffer.setParameter((float) colorChannel.getImageWidth()); - parametersBuffer.setParameter((float) colorChannel.getImageHeight()); - parametersBuffer.setParameter(depthChannel.getDepthDiscretization()); - parametersBuffer.setParameter(useSensorColor); - parametersBuffer.setParameter(gradientMode); - parametersBuffer.setParameter(useSinusoidalGradientPattern); - parametersBuffer.setParameter(pointSize); - depthToWorldTransformParameter.setParameter(depthChannel.getTranslationToWorld(), depthChannel.getRotationMatrixToWorld()); - depthToColorTransformParameter.setParameter(depthToColorTransform.getTranslation(), depthToColorTransform.getRotation()); - - // Upload the buffers to the OpenCL device (GPU) - depthChannel.getDepth16UC1Image().writeOpenCLImage(openCLManager); - // It appears you've got to write something to the OpenCL argument even if you don't use it, - // so we write a placeholder image. - BytedecoImage colorImage = useSensorColor ? colorChannel.getColor8UC4Image() : placeholderColorImage; - colorImage.writeOpenCLImage(openCLManager); - parametersBuffer.writeOpenCLBufferObject(openCLManager); - depthToWorldTransformParameter.writeOpenCLBufferObject(openCLManager); - depthToColorTransformParameter.writeOpenCLBufferObject(openCLManager); - - // Set the OpenCL kernel arguments - openCLManager.setKernelArgument(kernel, 0, depthChannel.getDepth16UC1Image().getOpenCLImageObject()); - openCLManager.setKernelArgument(kernel, 1, colorImage.getOpenCLImageObject()); - openCLManager.setKernelArgument(kernel, 2, pointCloudVertexBuffer.getOpenCLBufferObject()); - openCLManager.setKernelArgument(kernel, 3, parametersBuffer.getOpenCLBufferObject()); - openCLManager.setKernelArgument(kernel, 4, depthToWorldTransformParameter.getOpenCLBufferObject()); - openCLManager.setKernelArgument(kernel, 5, depthToColorTransformParameter.getOpenCLBufferObject()); - - // Run the OpenCL kernel - openCLManager.execute2D(kernel, depthChannel.getImageWidth(), depthChannel.getImageHeight()); - - // Read the OpenCL buffer back to the CPU - pointCloudVertexBuffer.readOpenCLBufferObject(openCLManager); - } -} diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizer.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizer.java index 69a44fdee67..87c8f40a6e0 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizer.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizer.java @@ -3,79 +3,49 @@ import com.badlogic.gdx.graphics.g3d.Renderable; import com.badlogic.gdx.utils.Array; import com.badlogic.gdx.utils.Pool; -import imgui.ImGui; -import imgui.type.ImBoolean; -import imgui.type.ImFloat; -import imgui.type.ImInt; import perception_msgs.msg.dds.ImageMessage; -import us.ihmc.euclid.transform.RigidBodyTransform; -import us.ihmc.log.LogTools; -import us.ihmc.perception.CameraModel; -import us.ihmc.perception.opencl.OpenCLFloatBuffer; -import us.ihmc.perception.opencl.OpenCLManager; -import us.ihmc.rdx.RDXPointCloudRendererOld; -import us.ihmc.rdx.imgui.ImGuiAveragedFrequencyText; -import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap; +import us.ihmc.perception.RawImage; +import us.ihmc.perception.imageMessage.ImageMessageDecoder; import us.ihmc.rdx.sceneManager.RDXSceneLevel; -import us.ihmc.rdx.ui.graphics.RDXColorGradientMode; -import us.ihmc.rdx.ui.graphics.RDXOusterFisheyeColoredPointCloudKernel; +import us.ihmc.rdx.ui.graphics.RDXMessageSizeReadout; +import us.ihmc.rdx.ui.graphics.RDXRawImagePointCloudVisualizer; +import us.ihmc.rdx.ui.graphics.RDXSequenceDiscontinuityPlot; import us.ihmc.rdx.ui.graphics.ros2.RDXROS2MultiTopicVisualizer; -import us.ihmc.robotics.referenceFrames.MutableReferenceFrame; import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2Subscription; import us.ihmc.ros2.ROS2Topic; -import us.ihmc.tools.Timer; import java.util.List; import java.util.Set; -/** - * Supports Realsenses and Ouster and Fisheye colored point clouds. - * This visualizer is designed to automatically resize and adapt to changing - * parameters, such as focal length, principal points, etc. - * It provides our full set of analytics as plots, such as message size, - * delay, sequence discontinuities, etc. and coloring options. - */ public class RDXROS2ColoredPointCloudVisualizer extends RDXROS2MultiTopicVisualizer { - private final String titleBeforeAdditions; - private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass()); - private final ImBoolean useSensorColor = new ImBoolean(true); - private RDXColorGradientMode gradientMode = RDXColorGradientMode.WORLD_Z; - private final ImBoolean useSinusoidalGradientPattern = new ImBoolean(true); - private final ImFloat pointSizeScale = new ImFloat(1.0f); - private final ImInt levelOfColorDetail = new ImInt(0); - private boolean subscribed = false; - private final ROS2Node ros2Node; - private final RDXROS2ColoredPointCloudVisualizerDepthChannel depthChannel; - private final RDXROS2ColoredPointCloudVisualizerColorChannel colorChannel; - private final Timer colorReceptionTimer = new Timer(); - - private final Object imageMessagesSyncObject = new Object(); + private ROS2Topic depthTopic; + private ROS2Subscription depthSubscription; + private final RDXMessageSizeReadout depthMessageSizeReadout = new RDXMessageSizeReadout(); + private final RDXSequenceDiscontinuityPlot depthDiscontinuityPlot = new RDXSequenceDiscontinuityPlot(); - private RDXPointCloudRendererOld pointCloudRenderer; - private OpenCLManager openCLManager; - private OpenCLFloatBuffer pointCloudVertexBuffer; - private RDXPinholePinholeColoredPointCloudKernel pinholePinholeKernel; - private RDXOusterFisheyeColoredPointCloudKernel ousterFisheyeKernel; - private boolean usingColor; + private ROS2Topic colorTopic; + private ROS2Subscription colorSubscription; + private final RDXMessageSizeReadout colorMessageSizeReadout = new RDXMessageSizeReadout(); + private final RDXSequenceDiscontinuityPlot colorDiscontinuityPlot = new RDXSequenceDiscontinuityPlot(); - private final MutableReferenceFrame depthFrame = new MutableReferenceFrame(); - private final MutableReferenceFrame colorFrame = new MutableReferenceFrame(); - private final RigidBodyTransform depthToColorTransform = new RigidBodyTransform(); + private final ImageMessageDecoder depthMessageDecoder = new ImageMessageDecoder(); + private final ImageMessageDecoder colorMessageDecoder = new ImageMessageDecoder(); + private final RDXRawImagePointCloudVisualizer visualizer; - public RDXROS2ColoredPointCloudVisualizer(String title, - ROS2Node ros2Node, - ROS2Topic depthTopic, - ROS2Topic colorTopic) + public RDXROS2ColoredPointCloudVisualizer(String title, ROS2Node ros2Node, ROS2Topic depthTopic, ROS2Topic colorTopic) { super(title); - titleBeforeAdditions = title; + this.ros2Node = ros2Node; - depthChannel = new RDXROS2ColoredPointCloudVisualizerDepthChannel(depthTopic); - colorChannel = new RDXROS2ColoredPointCloudVisualizerColorChannel(colorTopic); + this.depthTopic = depthTopic; + this.colorTopic = colorTopic; + + visualizer = new RDXRawImagePointCloudVisualizer(title); setActivenessChangeCallback(isActive -> { @@ -86,265 +56,105 @@ public RDXROS2ColoredPointCloudVisualizer(String title, }); } - private void subscribe() + public void changeTopics(ROS2Topic depthTopic, ROS2Topic colorTopic) { - subscribed = true; - depthChannel.subscribe(ros2Node, imageMessagesSyncObject); - colorChannel.subscribe(ros2Node, imageMessagesSyncObject); - } + boolean wasSubscribed = depthSubscription != null; + unsubscribe(); - @Override - public void create() - { - super.create(); + this.depthTopic = depthTopic; + this.colorTopic = colorTopic; - openCLManager = new OpenCLManager(); - pinholePinholeKernel = new RDXPinholePinholeColoredPointCloudKernel(openCLManager); - ousterFisheyeKernel = new RDXOusterFisheyeColoredPointCloudKernel(openCLManager); + if (wasSubscribed) + subscribe(); } @Override public void update() { super.update(); - - if (subscribed && isActive()) - { - synchronized (imageMessagesSyncObject) - { - depthChannel.update(openCLManager); - colorChannel.update(openCLManager); - } - - if (depthChannel.getDecompressedImageReady().poll()) - { - // Stop coloring points if there's been no data in a few seconds - if (colorChannel.getDecompressedImageReady().poll()) - colorReceptionTimer.reset(); - usingColor = colorReceptionTimer.isRunning(2.0); - - int totalNumberOfPoints = depthChannel.getTotalNumberOfPixels(); - if (depthChannel.getCameraModel() == CameraModel.OUSTER) - { - int sanitizedLevelOfColorDetail = usingColor && colorChannel.getCameraModel() == CameraModel.EQUIDISTANT_FISHEYE ? levelOfColorDetail.get() : 0; - totalNumberOfPoints = ousterFisheyeKernel.calculateNumberOfPointsForLevelOfColorDetail(depthChannel.getImageWidth(), - depthChannel.getImageHeight(), - sanitizedLevelOfColorDetail); - } - - if (pointCloudVertexBuffer == null - || pointCloudVertexBuffer.getBackingDirectFloatBuffer().capacity() / RDXPointCloudRendererOld.FLOATS_PER_VERTEX != totalNumberOfPoints) - { - LogTools.info("Allocating new buffers. {} total points", totalNumberOfPoints); - - if (pointCloudRenderer != null) - pointCloudRenderer.dispose(); - pointCloudRenderer = new RDXPointCloudRendererOld(); - pointCloudRenderer.create(totalNumberOfPoints); - - pointCloudVertexBuffer = new OpenCLFloatBuffer(totalNumberOfPoints * RDXPointCloudRendererOld.FLOATS_PER_VERTEX, pointCloudRenderer.getVertexBuffer()); - pointCloudVertexBuffer.createOpenCLBufferObject(openCLManager); - } - - pointCloudRenderer.updateMeshFastestBeforeKernel(); - pointCloudVertexBuffer.syncWithBackingBuffer(); // TODO: Is this necessary? - - synchronized (depthChannel.getDecompressionAccessSyncObject()) - { - if (usingColor) - { - synchronized (colorChannel.getDecompressionAccessSyncObject()) - { - runKernels(); - } - } - else - { - runKernels(); - } - } - - pointCloudRenderer.updateMeshFastestAfterKernel(); - } - } + visualizer.update(); } - private void runKernels() + @Override + public List> getTopics() { - float pointSize = pointSizeScale.get() / depthChannel.getFx(); - if (depthChannel.getCameraModel() == CameraModel.PINHOLE) // Assuming color camera is also pinhole if using it - { - pinholePinholeKernel.computeVertexBuffer(colorChannel, - depthChannel, - usingColor && useSensorColor.get(), - gradientMode.ordinal(), - useSinusoidalGradientPattern.get(), - pointSize, - pointCloudVertexBuffer); - } - else if (depthChannel.getCameraModel() == CameraModel.OUSTER) // Assuming color is equidistant fisheye if using it - { - depthFrame.update(transformToWorld -> transformToWorld.set(depthChannel.getRotationMatrixToWorld(), depthChannel.getTranslationToWorld())); - colorFrame.update(transformToWorld -> transformToWorld.set(colorChannel.getRotationMatrixToWorld(), colorChannel.getTranslationToWorld())); - depthFrame.getReferenceFrame().getTransformToDesiredFrame(depthToColorTransform, colorFrame.getReferenceFrame()); - - ousterFisheyeKernel.getOusterToWorldTransformToPack().set(depthChannel.getRotationMatrixToWorld(), depthChannel.getTranslationToWorld()); - ousterFisheyeKernel.getOusterToFisheyeTransformToPack().set(depthToColorTransform.getRotation(), depthToColorTransform.getTranslation()); - ousterFisheyeKernel.setInstrinsicParameters(depthChannel.getOusterBeamAltitudeAnglesBuffer(), depthChannel.getOusterBeamAzimuthAnglesBuffer()); - ousterFisheyeKernel.runKernel(0.0f, - pointSize, - usingColor && useSensorColor.get(), - gradientMode.ordinal(), - useSinusoidalGradientPattern.get(), - depthChannel.getDepth16UC1Image(), - colorChannel.getFx(), - colorChannel.getFy(), - colorChannel.getCx(), - colorChannel.getCy(), - usingColor ? colorChannel.getColor8UC4Image() : null, - pointCloudVertexBuffer); - } + return List.of(depthTopic, colorTopic); } @Override public void renderImGuiWidgets() { - renderStatistics(); + depthMessageSizeReadout.renderImGuiWidgets(); + colorMessageSizeReadout.renderImGuiWidgets(); - ImGui.checkbox(labels.get("Use sensor color"), useSensorColor); - ImGui.text("Gradient mode:"); - ImGui.sameLine(); - if (ImGui.radioButton(labels.get("World Z"), gradientMode == RDXColorGradientMode.WORLD_Z)) - gradientMode = RDXColorGradientMode.WORLD_Z; - ImGui.sameLine(); - if (ImGui.radioButton(labels.get("Sensor X"), gradientMode == RDXColorGradientMode.SENSOR_X)) - gradientMode = RDXColorGradientMode.SENSOR_X; - ImGui.checkbox(labels.get("Sinusoidal gradient"), useSinusoidalGradientPattern); - ImGui.sliderFloat(labels.get("Point scale"), pointSizeScale.getData(), 0.0f, 2.0f); - if (depthChannel.getCameraModel() == CameraModel.OUSTER && colorChannel.getCameraModel() == CameraModel.EQUIDISTANT_FISHEYE) - { - ImGui.sliderInt(labels.get("Level of color detail"), levelOfColorDetail.getData(), 0, 3); - } - } + depthDiscontinuityPlot.renderImGuiWidgets(); + colorDiscontinuityPlot.renderImGuiWidgets(); - public void renderStatistics() - { - if (colorChannel.getReceivedOne()) - colorChannel.getMessageSizeReadout().renderImGuiWidgets(); - if (depthChannel.getReceivedOne()) - depthChannel.getMessageSizeReadout().renderImGuiWidgets(); -// if (colorChannel.getReceivedOne()) -// colorChannel.getDelayPlot().renderImGuiWidgets(); -// if (depthChannel.getReceivedOne()) -// depthChannel.getDelayPlot().renderImGuiWidgets(); - if (colorChannel.getReceivedOne()) - colorChannel.getSequenceDiscontinuityPlot().renderImGuiWidgets(); - if (depthChannel.getReceivedOne()) - depthChannel.getSequenceDiscontinuityPlot().renderImGuiWidgets(); + visualizer.renderImGuiWidgets(); } + @Override public void getRenderables(Array renderables, Pool pool, Set sceneLevels) { - if (isActive() && pointCloudRenderer != null && sceneLevelCheck(sceneLevels)) - pointCloudRenderer.getRenderables(renderables, pool); + visualizer.getRenderables(renderables, pool, sceneLevels); } @Override public void destroy() { unsubscribe(); - depthChannel.destroy(); - colorChannel.destroy(); - super.destroy(); - } - - private void unsubscribe() - { - colorChannel.unsubscribe(); - depthChannel.unsubscribe(); - subscribed = false; - } - - public void changeTopics(ROS2Topic colorTopic, ROS2Topic depthTopic) - { - boolean wasSubscribed = isSubscribed(); - unsubscribe(); - - colorChannel.setTopic(colorTopic); - depthChannel.setTopic(depthTopic); - - if (wasSubscribed) - subscribe(); + visualizer.destroy(); + depthMessageDecoder.destroy(); + colorMessageDecoder.destroy(); } - public boolean isSubscribed() - { - return subscribed; - } - - public void setPointSizeScale(float pointSizeScale) - { - this.pointSizeScale.set(pointSizeScale); - } - - public void setLevelOfColorDetail(int levelOfColorDetail) - { - this.levelOfColorDetail.set(levelOfColorDetail); - } - - public ImBoolean useSensorColor() + private void subscribe() { - return useSensorColor; + depthSubscription = ros2Node.createSubscription2(depthTopic, this::onDepthMessageReceived); + colorSubscription = ros2Node.createSubscription2(colorTopic, this::onColorMessageReceived); } - public ImBoolean useSinusoidalGradientPattern() + private void onDepthMessageReceived(ImageMessage message) { - return useSinusoidalGradientPattern; - } + if (message.getData().isEmpty()) + return; - public RDXColorGradientMode getGradientMode() - { - return gradientMode; - } + RawImage depthImage = depthMessageDecoder.decodeMessageCPU(message); + visualizer.setDepthImage(depthImage); - public void setGradientMode(RDXColorGradientMode mode) - { - gradientMode = mode; - } + getFrequency(depthTopic).ping(); + depthMessageSizeReadout.update(message.getData().size()); + depthDiscontinuityPlot.update(message.getSequenceNumber()); - public ImFloat getPointSizeScale() - { - return pointSizeScale; + depthImage.release(); } - public CameraModel getColorChannelCamera() + private void onColorMessageReceived(ImageMessage message) { - return colorChannel.getCameraModel(); - } + if (message.getData().isEmpty()) + return; - public CameraModel getDepthChannelCamera() - { - return depthChannel.getCameraModel(); - } + RawImage colorImage = colorMessageDecoder.decodeMessageCPU(message); + visualizer.setColorImage(colorImage); - public ImInt getLevelOfColorDetail() - { - return levelOfColorDetail; - } + getFrequency(colorTopic).ping(); + colorMessageSizeReadout.update(message.getData().size()); + colorDiscontinuityPlot.update(message.getSequenceNumber()); - @Override - public List> getTopics() - { - return List.of(colorChannel.getTopic(), depthChannel.getTopic()); + colorImage.release(); } - @Override - public ImGuiAveragedFrequencyText getFrequency(ROS2Topic topic) + private void unsubscribe() { - if (topic == colorChannel.getTopic()) - return colorChannel.getFrequencyText(); - else if (topic == depthChannel.getTopic()) - return depthChannel.getFrequencyText(); - return null; + if (depthSubscription != null) + { + depthSubscription.remove(); + depthSubscription = null; + } + if (colorSubscription != null) + { + colorSubscription.remove(); + colorSubscription = null; + } } -} \ No newline at end of file +} diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizerChannel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizerChannel.java deleted file mode 100644 index 9fcc01ca695..00000000000 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizerChannel.java +++ /dev/null @@ -1,265 +0,0 @@ -package us.ihmc.rdx.ui.graphics.ros2.pointCloud; - -import perception_msgs.msg.dds.ImageMessage; -import us.ihmc.commons.thread.Notification; -import us.ihmc.communication.packets.MessageTools; -import us.ihmc.euclid.matrix.RotationMatrix; -import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.perception.CameraModel; -import us.ihmc.perception.imageMessage.ImageMessageDecoder; -import us.ihmc.perception.opencl.OpenCLManager; -import us.ihmc.perception.ouster.OusterNetServer; -import us.ihmc.perception.tools.NativeMemoryTools; -import us.ihmc.pubsub.common.SampleInfo; -import us.ihmc.rdx.imgui.ImGuiAveragedFrequencyText; -import us.ihmc.rdx.ui.graphics.RDXMessageSizeReadout; -import us.ihmc.rdx.ui.graphics.RDXSequenceDiscontinuityPlot; -import us.ihmc.ros2.ROS2Node; -import us.ihmc.ros2.ROS2Subscription; -import us.ihmc.ros2.ROS2Topic; -import us.ihmc.tools.thread.MissingThreadTools; -import us.ihmc.tools.thread.ResettableExceptionHandlingExecutorService; -import us.ihmc.tools.thread.SwapReference; - -import java.nio.ByteBuffer; - -/** - * The common part of the depth and color channels. - * A channel is image data coming in from ROS 2 and making it's - * way into a kernel for point cloud rendering. - */ -public abstract class RDXROS2ColoredPointCloudVisualizerChannel -{ - protected final ImageMessageDecoder imageMessageDecoder = new ImageMessageDecoder(); - protected final ImageMessage imageMessage = new ImageMessage(); - private final SampleInfo sampleInfo = new SampleInfo(); - private ROS2Topic topic; - private ROS2Subscription imageMessageSubscription; - private final RDXMessageSizeReadout messageSizeReadout = new RDXMessageSizeReadout(); - private final RDXSequenceDiscontinuityPlot sequenceDiscontinuityPlot = new RDXSequenceDiscontinuityPlot(); - protected final SwapReference imageMessageSwapReference = new SwapReference<>(ImageMessage::new); - private final ResettableExceptionHandlingExecutorService channelDecompressionThreadExecutor; - private final Runnable decompressionAsynchronousThread = this::decompressionAsynchronousThread; - private boolean initialized = false; - private final Notification subscribedImageAvailable = new Notification(); - private final Notification decompressedImageReady = new Notification(); - private volatile boolean receivedOne = false; - protected int imageWidth; - protected int imageHeight; - private int totalNumberOfPixels = 0; - private float fx; - private float fy; - private float cx; - private float cy; - private final RotationMatrix rotationMatrixToWorld = new RotationMatrix(); - private final Vector3D translationToWorld = new Vector3D(); - private float depthDiscretization; - private CameraModel cameraModel; - private final ByteBuffer ousterBeamAltitudeAnglesBuffer = NativeMemoryTools.allocate(Float.BYTES * OusterNetServer.MAX_POINTS_PER_COLUMN); - private final ByteBuffer ousterBeamAzimuthAnglesBuffer = NativeMemoryTools.allocate(Float.BYTES * OusterNetServer.MAX_POINTS_PER_COLUMN); - - private ImGuiAveragedFrequencyText frequencyText = new ImGuiAveragedFrequencyText(); - - public RDXROS2ColoredPointCloudVisualizerChannel(String name, ROS2Topic topic) - { - this.topic = topic; - -// delayPlot = new ImPlotDoublePlot(name + " Delay", 30); - - boolean daemon = true; - int queueSize = 1; - channelDecompressionThreadExecutor = MissingThreadTools.newSingleThreadExecutor("ChannelDecompressionThread", daemon, queueSize); - } - - public void subscribe(ROS2Node ros2Node, Object imageMessagesSyncObject) - { - if (imageMessageSubscription != null) - unsubscribe(); - - imageMessageSubscription = ros2Node.createSubscription(topic, subscriber -> { - synchronized (imageMessagesSyncObject) - { - imageMessage.getData().resetQuick(); - subscriber.takeNextData(imageMessage, sampleInfo); - subscribedImageAvailable.set(); - receivedOne = true; - frequencyText.ping(); - } - }); - } - - public void unsubscribe() - { - if (imageMessageSubscription != null) - { - imageMessageSubscription.remove(); - imageMessageSubscription = null; - } - } - - /** - * This method is synchronized so that the ROS 2 callback is not updating the ImageMessage - * while this update is running. We store everything as fields and the data in a swap - * reference so we can do the decompression and run the GPU kernels without needing to - * synchronize that part. - */ - public void update(OpenCLManager openCLManager) - { - if (subscribedImageAvailable.poll()) - { - if (!initialized) - { - imageWidth = imageMessage.getImageWidth(); - imageHeight = imageMessage.getImageHeight(); - totalNumberOfPixels = imageHeight * imageWidth; - imageMessageSwapReference.initializeBoth(message -> message.set(imageMessage)); - - initialize(openCLManager); - initialized = true; - } - - fx = imageMessage.getFocalLengthXPixels(); - fy = imageMessage.getFocalLengthYPixels(); - cx = imageMessage.getPrincipalPointXPixels(); - cy = imageMessage.getPrincipalPointYPixels(); - depthDiscretization = imageMessage.getDepthDiscretization(); - translationToWorld.set(imageMessage.getPosition()); - rotationMatrixToWorld.set(imageMessage.getOrientation()); - - // We decompress outside of the incoming message synchronization block. - // Using an internal swap reference to the unpacked data, we can allow ROS 2 - // to not have to wait for compression to finish and also not have to copy - // the unpacked result to a decompression input buffer. - imageMessageSwapReference.getForThreadOne().set(imageMessage); - imageMessageSwapReference.swap(); - // FIXME: This call prints "no afterExecute handlers" warnings whe the Ouster Fisheye point cloud's refresh rate is too fast - channelDecompressionThreadExecutor.clearQueueAndExecute(decompressionAsynchronousThread); - - messageSizeReadout.update(imageMessage.getData().size()); - sequenceDiscontinuityPlot.update(imageMessage.getSequenceNumber()); -// delayPlot.addValue(MessageTools.calculateDelay(imageMessage)); - - cameraModel = CameraModel.getCameraModel(imageMessage); - MessageTools.extractIDLSequence(imageMessage.getOusterBeamAltitudeAngles(), ousterBeamAltitudeAnglesBuffer); - MessageTools.extractIDLSequence(imageMessage.getOusterBeamAzimuthAngles(), ousterBeamAzimuthAnglesBuffer); - } - } - - private void decompressionAsynchronousThread() - { - decompress(); - decompressedImageReady.set(); - } - - protected abstract void initialize(OpenCLManager openCLManager); - - protected abstract void decompress(); - - protected abstract Object getDecompressionAccessSyncObject(); - - public void destroy() - { - channelDecompressionThreadExecutor.destroy(); - imageMessageDecoder.destroy(); - } - - public ROS2Topic getTopic() - { - return topic; - } - - public Notification getDecompressedImageReady() - { - return decompressedImageReady; - } - - public int getImageWidth() - { - return imageWidth; - } - - public int getImageHeight() - { - return imageHeight; - } - - public int getTotalNumberOfPixels() - { - return totalNumberOfPixels; - } - - public float getFx() - { - return fx; - } - - public float getFy() - { - return fy; - } - - public float getCx() - { - return cx; - } - - public float getCy() - { - return cy; - } - - public Vector3D getTranslationToWorld() - { - return translationToWorld; - } - - public RotationMatrix getRotationMatrixToWorld() - { - return rotationMatrixToWorld; - } - - public boolean getReceivedOne() - { - return receivedOne; - } - - public RDXMessageSizeReadout getMessageSizeReadout() - { - return messageSizeReadout; - } - - public RDXSequenceDiscontinuityPlot getSequenceDiscontinuityPlot() - { - return sequenceDiscontinuityPlot; - } - - public float getDepthDiscretization() - { - return depthDiscretization; - } - - public CameraModel getCameraModel() - { - return cameraModel; - } - - public ByteBuffer getOusterBeamAltitudeAnglesBuffer() - { - return ousterBeamAltitudeAnglesBuffer; - } - - public ByteBuffer getOusterBeamAzimuthAnglesBuffer() - { - return ousterBeamAzimuthAnglesBuffer; - } - - public void setTopic(ROS2Topic topic) - { - this.topic = topic; - } - - public ImGuiAveragedFrequencyText getFrequencyText() - { - return frequencyText; - } -} diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizerColorChannel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizerColorChannel.java deleted file mode 100644 index 4eb2e1104f4..00000000000 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizerColorChannel.java +++ /dev/null @@ -1,67 +0,0 @@ -package us.ihmc.rdx.ui.graphics.ros2.pointCloud; - -import org.bytedeco.opencl.global.OpenCL; -import org.bytedeco.opencv.global.opencv_core; -import org.bytedeco.opencv.opencv_core.Mat; -import perception_msgs.msg.dds.ImageMessage; -import us.ihmc.perception.BytedecoImage; -import us.ihmc.perception.opencl.OpenCLManager; -import us.ihmc.ros2.ROS2Topic; -import us.ihmc.tools.thread.SwapReference; - -/** - * Channel used for the color part. Handles decompression and conversion. - */ -public class RDXROS2ColoredPointCloudVisualizerColorChannel extends RDXROS2ColoredPointCloudVisualizerChannel -{ - private final Mat imageFromMessage = new Mat(); - private SwapReference color8UC4ImageSwapReference; - - public RDXROS2ColoredPointCloudVisualizerColorChannel(ROS2Topic topic) - { - super("Color ", topic); - } - - @Override - protected void initialize(OpenCLManager openCLManager) - { - color8UC4ImageSwapReference = new SwapReference<>(() -> - { - BytedecoImage color8UC4Image = new BytedecoImage(imageWidth, imageHeight, opencv_core.CV_8UC4); - color8UC4Image.createOpenCLImage(openCLManager, OpenCL.CL_MEM_READ_ONLY); - getFrequencyText().ping(); - return color8UC4Image; - }); - } - - @Override - protected void decompress() - { - synchronized (imageMessageSwapReference) - { - imageMessageDecoder.decodeMessageToRGBA(imageMessageSwapReference.getForThreadTwo(), - imageFromMessage); - } - - imageFromMessage.copyTo(color8UC4ImageSwapReference.getForThreadOne().getBytedecoOpenCVMat()); - color8UC4ImageSwapReference.swap(); - } - - @Override - protected Object getDecompressionAccessSyncObject() - { - return color8UC4ImageSwapReference; - } - - public BytedecoImage getColor8UC4Image() - { - return color8UC4ImageSwapReference.getForThreadTwo(); - } - - @Override - public void destroy() - { - super.destroy(); - imageFromMessage.close(); - } -} diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizerDepthChannel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizerDepthChannel.java deleted file mode 100644 index fb4fb8e8631..00000000000 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizerDepthChannel.java +++ /dev/null @@ -1,65 +0,0 @@ -package us.ihmc.rdx.ui.graphics.ros2.pointCloud; - -import org.bytedeco.opencl.global.OpenCL; -import org.bytedeco.opencv.global.opencv_core; -import org.bytedeco.opencv.opencv_core.Mat; -import perception_msgs.msg.dds.ImageMessage; -import us.ihmc.perception.BytedecoImage; -import us.ihmc.perception.opencl.OpenCLManager; -import us.ihmc.ros2.ROS2Topic; -import us.ihmc.tools.thread.SwapReference; - -/** - * Channel used for the depth part. Handles decompression. - */ -public class RDXROS2ColoredPointCloudVisualizerDepthChannel extends RDXROS2ColoredPointCloudVisualizerChannel -{ - private final Mat decompressedDepthImage = new Mat(); - private SwapReference depth16UC1ImageSwapReference; - - public RDXROS2ColoredPointCloudVisualizerDepthChannel(ROS2Topic topic) - { - super("Depth ", topic); - } - - @Override - protected void initialize(OpenCLManager openCLManager) - { - depth16UC1ImageSwapReference = new SwapReference<>(() -> - { - BytedecoImage depth16UC1Image = new BytedecoImage(imageWidth, imageHeight, opencv_core.CV_16UC1); - depth16UC1Image.createOpenCLImage(openCLManager, OpenCL.CL_MEM_READ_ONLY); - getFrequencyText().ping(); - return depth16UC1Image; - }); - } - - @Override - protected void decompress() - { - synchronized (imageMessageSwapReference) - { - imageMessageDecoder.decodeMessage(imageMessageSwapReference.getForThreadTwo(), decompressedDepthImage); - decompressedDepthImage.copyTo(depth16UC1ImageSwapReference.getForThreadOne().getBytedecoOpenCVMat()); - } - depth16UC1ImageSwapReference.swap(); - } - - @Override - protected Object getDecompressionAccessSyncObject() - { - return depth16UC1ImageSwapReference; - } - - @Override - public void destroy() - { - super.destroy(); - decompressedDepthImage.close(); - } - - public BytedecoImage getDepth16UC1Image() - { - return depth16UC1ImageSwapReference.getForThreadTwo(); - } -} diff --git a/ihmc-high-level-behaviors/src/libgdx/resources/configurations/RDXRawImagePointCloudVisualizerDemo/ImGuiPanels.json b/ihmc-high-level-behaviors/src/libgdx/resources/configurations/RDXRawImagePointCloudVisualizerDemo/ImGuiPanels.json new file mode 100644 index 00000000000..f033cbf8963 --- /dev/null +++ b/ihmc-high-level-behaviors/src/libgdx/resources/configurations/RDXRawImagePointCloudVisualizerDemo/ImGuiPanels.json @@ -0,0 +1,7 @@ +{ + "dockspacePanels" : { }, + "windows" : { + "3D View" : true, + "Options" : true + } +} \ No newline at end of file diff --git a/ihmc-high-level-behaviors/src/libgdx/resources/configurations/RDXRawImagePointCloudVisualizerDemo/ImGuiSettings.ini b/ihmc-high-level-behaviors/src/libgdx/resources/configurations/RDXRawImagePointCloudVisualizerDemo/ImGuiSettings.ini new file mode 100644 index 00000000000..f216285e4b9 --- /dev/null +++ b/ihmc-high-level-behaviors/src/libgdx/resources/configurations/RDXRawImagePointCloudVisualizerDemo/ImGuiSettings.ini @@ -0,0 +1,27 @@ +[Window][DockSpaceViewport_11111111] +Pos=0,22 +Size=909,1085 +Collapsed=0 + +[Window][Options] +Pos=0,22 +Size=299,1085 +Collapsed=0 +DockId=0x00000001,0 + +[Window][3D View] +Pos=301,22 +Size=608,1085 +Collapsed=0 +DockId=0x00000002,0 + +[Window][Debug##Default] +Pos=60,60 +Size=400,400 +Collapsed=0 + +[Docking][Data] +DockSpace ID=0x8B93E3BD Window=0xA787BDB4 Pos=999,103 Size=909,1085 Split=X Selected=0x2E64DC63 + DockNode ID=0x00000001 Parent=0x8B93E3BD SizeRef=299,1085 Selected=0x1F88C31B + DockNode ID=0x00000002 Parent=0x8B93E3BD SizeRef=608,1085 CentralNode=1 Selected=0x2E64DC63 + diff --git a/ihmc-high-level-behaviors/src/libgdx/resources/openCL/PinholePinholeColoredPointCloud.cl b/ihmc-high-level-behaviors/src/libgdx/resources/openCL/PinholePinholeColoredPointCloud.cl deleted file mode 100644 index 8cf43f71daa..00000000000 --- a/ihmc-high-level-behaviors/src/libgdx/resources/openCL/PinholePinholeColoredPointCloud.cl +++ /dev/null @@ -1,83 +0,0 @@ -#define COLOR_FOCAL_LENGTH_X_PIXELS 0 -#define COLOR_FOCAL_LENGTH_Y_PIXELS 1 -#define COLOR_PRINCIPAL_POINT_X_PIXELS 2 -#define COLOR_PRINCIPAL_POINT_Y_PIXELS 3 -#define FOCAL_LENGTH_PIXELS_X 4 -#define FOCAL_LENGTH_PIXELS_Y 5 -#define DEPTH_PRINCIPAL_POINT_X_PIXELS 6 -#define DEPTH_PRINCIPAL_POINT_Y_PIXELS 7 -#define DEPTH_IMAGE_WIDTH 8 -#define DEPTH_IMAGE_HEIGHT 9 -#define COLOR_IMAGE_WIDTH 10 -#define COLOR_IMAGE_HEIGHT 11 -#define DISCRETE_RESOLUTION 12 -#define USE_SENSOR_COLOR 13 -#define GRADIENT_MODE 14 -#define SINUSOIDAL 15 -#define POINT_SIZE 16 - -#define GRADIENT_MODE_WORLD_Z 0 -#define GRADIENT_MODE_SENSOR_X 1 - -kernel void computeVertexBuffer(read_only image2d_t depthImageDiscretized, - read_only image2d_t colorRGBAImage, - global float* pointCloudVertexBuffer, - global float* parameters, - global float* depthToWorldTransform, - global float* depthToColorTransform) -{ - int x = get_global_id(0); - int y = get_global_id(1); - - float eyeDepthInMeters = read_imageui(depthImageDiscretized, (int2) (x, y)).x * parameters[DISCRETE_RESOLUTION]; - - float3 depthFramePoint = (float3) (eyeDepthInMeters, - -(x - parameters[DEPTH_PRINCIPAL_POINT_X_PIXELS]) / parameters[FOCAL_LENGTH_PIXELS_X] * eyeDepthInMeters, - -(y - parameters[DEPTH_PRINCIPAL_POINT_Y_PIXELS]) / parameters[FOCAL_LENGTH_PIXELS_Y] * eyeDepthInMeters); - - float3 worldFramePoint = transformPoint3D32(depthFramePoint, depthToWorldTransform); - - float4 pointColor; - bool appliedColorFromSensor = false; - if (parameters[USE_SENSOR_COLOR]) - { - float3 colorFramePoint = transformPoint3D32(depthFramePoint, depthToColorTransform); - - // Flip because positive yaw is to the left, but image coordinates go to the right - float yaw = -angle(1.0f, 0.0f, colorFramePoint.x, colorFramePoint.y); - int pixelCol = round(parameters[COLOR_PRINCIPAL_POINT_X_PIXELS] + parameters[COLOR_FOCAL_LENGTH_X_PIXELS] * tan(yaw)); - - float pitch = -angle(1.0f, 0.0f, colorFramePoint.x, colorFramePoint.z); - int pixelRow = round(parameters[COLOR_PRINCIPAL_POINT_Y_PIXELS] + parameters[COLOR_FOCAL_LENGTH_Y_PIXELS] * tan(pitch)); - - bool pixelInBounds = intervalContains(pixelCol, 0, parameters[COLOR_IMAGE_WIDTH]) && intervalContains(pixelRow, 0, parameters[COLOR_IMAGE_HEIGHT]); - - if (pixelInBounds) - { - uint4 rgba8888Color = read_imageui(colorRGBAImage, (int2) (pixelCol, pixelRow)); - pointColor = convert_float4(rgba8888Color) / 255.0f; - appliedColorFromSensor = true; - } - } - if (!appliedColorFromSensor) - { - if (parameters[GRADIENT_MODE] == GRADIENT_MODE_WORLD_Z) - { - pointColor = calculateGradientColorOptionFloat4(worldFramePoint.z, parameters[SINUSOIDAL]); - } - else // GRADIENT_MODE_SENSOR_X - { - pointColor = calculateGradientColorOptionFloat4(eyeDepthInMeters, parameters[SINUSOIDAL]); - } - } - - int pointStartIndex = (parameters[DEPTH_IMAGE_WIDTH] * y + x) * 8; - pointCloudVertexBuffer[pointStartIndex] = worldFramePoint.x; - pointCloudVertexBuffer[pointStartIndex + 1] = worldFramePoint.y; - pointCloudVertexBuffer[pointStartIndex + 2] = worldFramePoint.z; - pointCloudVertexBuffer[pointStartIndex + 3] = pointColor.x; - pointCloudVertexBuffer[pointStartIndex + 4] = pointColor.y; - pointCloudVertexBuffer[pointStartIndex + 5] = pointColor.z; - pointCloudVertexBuffer[pointStartIndex + 6] = pointColor.w; - pointCloudVertexBuffer[pointStartIndex + 7] = parameters[POINT_SIZE] * eyeDepthInMeters; -} \ No newline at end of file diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/imageMessage/ImageMessageDecoder.java b/ihmc-perception/src/main/java/us/ihmc/perception/imageMessage/ImageMessageDecoder.java index e236b441437..9c63e8a19c2 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/imageMessage/ImageMessageDecoder.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/imageMessage/ImageMessageDecoder.java @@ -7,11 +7,19 @@ import org.bytedeco.opencv.opencv_core.GpuMat; import org.bytedeco.opencv.opencv_core.Mat; import perception_msgs.msg.dds.ImageMessage; +import us.ihmc.communication.packets.MessageTools; +import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.log.LogTools; +import us.ihmc.perception.CameraModel; +import us.ihmc.perception.RawImage; +import us.ihmc.perception.camera.CameraIntrinsics; import us.ihmc.perception.cuda.CUDACompressionTools; import us.ihmc.perception.cuda.CUDAJPEGProcessor; import us.ihmc.perception.cuda.CUDATools; +import java.time.Instant; + public class ImageMessageDecoder { private final ImageMessageDecompressionInput messageDataExtractor = new ImageMessageDecompressionInput(); @@ -31,6 +39,30 @@ public ImageMessageDecoder() } } + public RawImage decodeMessageCPU(ImageMessage messageToDecode) + { + Mat image = new Mat(); + decodeMessage(messageToDecode, image); + + PixelFormat pixelFormat = PixelFormat.fromByte(messageToDecode.getPixelFormat()); + + CameraIntrinsics intrinsics = new CameraIntrinsics(); + intrinsics.setWidth(messageToDecode.getImageWidth()); + intrinsics.setHeight(messageToDecode.getImageHeight()); + intrinsics.setFx(messageToDecode.getFocalLengthXPixels()); + intrinsics.setFy(messageToDecode.getFocalLengthYPixels()); + intrinsics.setCx(messageToDecode.getPrincipalPointXPixels()); + intrinsics.setCy(messageToDecode.getPrincipalPointYPixels()); + + CameraModel cameraModel = CameraModel.fromByte(messageToDecode.getCameraModel()); + FramePose3D sensorPose = new FramePose3D(ReferenceFrame.getWorldFrame(), messageToDecode.getPosition(), messageToDecode.getOrientation()); + Instant acquisitionTime = MessageTools.toInstant(messageToDecode.getAcquisitionTime()); + long sequenceNumber = messageToDecode.getSequenceNumber(); + float depthDiscretization = messageToDecode.getDepthDiscretization(); + + return new RawImage(image, null, pixelFormat, intrinsics, cameraModel, sensorPose, acquisitionTime, sequenceNumber, depthDiscretization); + } + /** * Decode the image contained within the {@code messageToDecode}, and pack it into the {@code imageToPack}. * Use {@link #getDecodedImagePixelFormat()} to get the pixel format of the decoded image. diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/time/TimeTools.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/time/TimeTools.java index dfb4616d72f..1ffefb9cbeb 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/time/TimeTools.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/time/TimeTools.java @@ -4,6 +4,9 @@ import java.time.Duration; import java.time.Instant; +import java.time.temporal.ChronoUnit; +import java.time.temporal.Temporal; +import java.util.List; /** * This class is to assist with calulating delays and timings @@ -42,6 +45,11 @@ public static double calculateDelay(long eventSecondsSinceEpoch, long eventAddit return seconds + Conversions.nanosecondsToSeconds(nano); } + public static double secondsBetween(Temporal startInclusive, Temporal endExclusive) + { + return toDoubleSeconds(Duration.between(startInclusive, endExclusive)); + } + public static double toDoubleSeconds(Duration duration) { return secondsNanosToDoubleSeconds(duration.getSeconds(), duration.getNano()); @@ -64,4 +72,27 @@ public static Duration durationOfSeconds(double seconds) long nanos = (long) ((seconds - longSeconds) * 1E9); return Duration.ofSeconds(longSeconds, nanos); } + + public static int findClosestTimeIndex(Temporal targetTime, List times) + { + long closestNanosFromTargetTime = Long.MAX_VALUE; + int closestTimeIndex = -1; + + for (int i = 0; i < times.size(); ++i) + { + Temporal time = times.get(i); + long nanosFromTargetTime = Math.abs(time.until(targetTime, ChronoUnit.NANOS)); + + if (nanosFromTargetTime == 0L) + return i; + + if (nanosFromTargetTime < closestNanosFromTargetTime) + { + closestNanosFromTargetTime = nanosFromTargetTime; + closestTimeIndex = i; + } + } + + return closestTimeIndex; + } } diff --git a/ihmc-robotics-toolkit/src/test/java/us/ihmc/robotics/time/TimeToolsTest.java b/ihmc-robotics-toolkit/src/test/java/us/ihmc/robotics/time/TimeToolsTest.java index 69f26607269..442855752d5 100644 --- a/ihmc-robotics-toolkit/src/test/java/us/ihmc/robotics/time/TimeToolsTest.java +++ b/ihmc-robotics-toolkit/src/test/java/us/ihmc/robotics/time/TimeToolsTest.java @@ -4,6 +4,9 @@ import us.ihmc.commons.Conversions; import java.time.Duration; +import java.time.Instant; +import java.time.temporal.Temporal; +import java.util.List; import static org.junit.jupiter.api.Assertions.*; @@ -58,4 +61,49 @@ public void testUnitConversions() assertEquals(trueNanos, durationNanos); } } + + @Test + public void testSecondsBetween() + { + Instant start = Instant.now(); + + for (int i = 0; i < 100; ++i) + { + Instant secondsAfter = start.plusSeconds(i); + + double startToAfter = TimeTools.secondsBetween(start, secondsAfter); + assertEquals(i, startToAfter, 1E-6); + + double afterToStart = TimeTools.secondsBetween(secondsAfter, start); + assertEquals(-i, afterToStart, 1E-6); + } + } + + @Test + public void testFindClosestTime() + { + Instant target = Instant.now(); + Instant milliAfter = target.plusMillis(1L); + Instant twoMillisAfter = target.plusMillis(2L); + Instant secondAfter = target.plusSeconds(1L); + Instant twoSecondsAfter = target.plusSeconds(2L); + + Instant milliBefore = target.minusMillis(1L); + Instant twoMillisBefore = target.minusMillis(2L); + Instant secondBefore = target.minusSeconds(1L); + Instant twoSecondsBefore = target.minusSeconds(2L); + + testFindClosestTime(target, List.of(), -1); + testFindClosestTime(target, List.of(twoSecondsBefore, milliBefore, twoMillisAfter, secondAfter), 1); + testFindClosestTime(target, List.of(milliAfter, twoMillisBefore, secondBefore), 0); + testFindClosestTime(target, List.of(milliBefore, milliAfter, target, twoSecondsAfter), 2); + testFindClosestTime(target, List.of(twoSecondsBefore, twoSecondsAfter), 0); + testFindClosestTime(target, List.of(secondAfter, secondBefore), 0); + } + + private void testFindClosestTime(Temporal target, List times, int expectedResult) + { + int closestIndex = TimeTools.findClosestTimeIndex(target, times); + assertEquals(expectedResult, closestIndex); + } } From fa4de94578f13fa99b8edd00dff7638d53b18317 Mon Sep 17 00:00:00 2001 From: Tomasz Bialek <103042423+TomaszTB@users.noreply.github.com> Date: Wed, 18 Dec 2024 10:21:38 -0600 Subject: [PATCH 12/16] Perception Autonomy Cleanup (#450) * Added basic Perception Autonomy Process with ZED * WIP - added ImageSensor + YOLO * Added RealsenseImageSensor * Reset ROS2DemandGraphNode to develop * Created ImageSensorPublishThread * Minor ImageSensor cleanup * Use PausableLoopingThread, add YOLO * Cleanup * Correct YOLO input to BGR * Add scene graph update thread, fix YOLO null pointer issue * WIP - Adding planar regions * WIP - adding ZEDSVOPlayback * Fixes * Improve planar regions extraction * Working planar regions * Added ArUco detection thread * Added ArUco to SceneGraph update * Added Height Map * Added ICP * Added Behavior Tree Update Thread * Cleanup * Remove old perception autonomy process * Allow specifying ZED input type * Polishing --- .../us/ihmc/communication/PerceptionAPI.java | 8 +- .../perception/RDXZEDSVORecordingDemo.java | 8 +- .../sceneGraph/RDXSceneGraphDemo.java | 6 +- .../us/ihmc/PerceptionAndAutonomyProcess.java | 712 ------------------ .../ros2/ROS2BehaviorTreeUpdateThread.java | 51 ++ .../IterativeClosestPointManager.java | 49 +- .../RapidHeightMapUpdateThread.java | 110 +++ .../StandAloneRealsenseProcess.java | 2 +- .../rdx/perception/RDXBallTrackingDemo.java | 8 +- .../RDXIterativeClosestPointWorkerDemo.java | 8 +- .../rdx/perception/RDXOpenCVTrackerDemo.java | 6 +- .../RDXYOLOv8PointCloudSegmentationDemo.java | 8 +- .../perception/ImageSensorPublishThread.java | 55 ++ .../java/us/ihmc/perception/RawImage.java | 46 +- .../us/ihmc/perception/RawImagePublisher.java | 108 +++ .../yolo/YOLOv8DetectionExecutor.java | 96 +-- .../yolo/YOLOv8DetectionResults.java | 30 +- .../yolo/YOLOv8DetectionThread.java | 70 ++ .../ihmc/perception/ffmpeg/FFmpegEncoder.java | 11 +- .../OpenCVArUcoMarkerDetectionThread.java | 98 +++ .../RapidPlanarRegionsExtractionThread.java | 112 +++ .../realsense/RealsenseDeviceManager.java | 9 +- .../ros2/ROS2SceneGraphUpdateThread.java | 83 ++ .../streaming/ROS2SRTSensorStreamer.java | 14 +- .../streaming/SRTVideoStreamer.java | 9 +- .../tools/PerceptionMessageTools.java | 4 +- .../java/us/ihmc/sensors/ImageSensor.java | 131 ++++ .../us/ihmc/sensors/RealsenseImageSensor.java | 152 ++++ .../java/us/ihmc/sensors/ZEDImageSensor.java | 380 ++++++++++ .../us/ihmc/sensors/ZEDSVOPlaybackSensor.java | 96 +++ 30 files changed, 1638 insertions(+), 842 deletions(-) delete mode 100644 ihmc-high-level-behaviors/src/main/java/us/ihmc/PerceptionAndAutonomyProcess.java create mode 100644 ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeUpdateThread.java create mode 100644 ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java create mode 100644 ihmc-perception/src/main/java/us/ihmc/perception/ImageSensorPublishThread.java create mode 100644 ihmc-perception/src/main/java/us/ihmc/perception/RawImagePublisher.java create mode 100644 ihmc-perception/src/main/java/us/ihmc/perception/detections/yolo/YOLOv8DetectionThread.java create mode 100644 ihmc-perception/src/main/java/us/ihmc/perception/opencv/OpenCVArUcoMarkerDetectionThread.java create mode 100644 ihmc-perception/src/main/java/us/ihmc/perception/rapidRegions/RapidPlanarRegionsExtractionThread.java create mode 100644 ihmc-perception/src/main/java/us/ihmc/perception/sceneGraph/ros2/ROS2SceneGraphUpdateThread.java create mode 100644 ihmc-perception/src/main/java/us/ihmc/sensors/ImageSensor.java create mode 100644 ihmc-perception/src/main/java/us/ihmc/sensors/RealsenseImageSensor.java create mode 100644 ihmc-perception/src/main/java/us/ihmc/sensors/ZEDImageSensor.java create mode 100644 ihmc-perception/src/main/java/us/ihmc/sensors/ZEDSVOPlaybackSensor.java diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/PerceptionAPI.java b/ihmc-communication/src/main/java/us/ihmc/communication/PerceptionAPI.java index 5501f88a4c8..97b64051d76 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/PerceptionAPI.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/PerceptionAPI.java @@ -108,7 +108,8 @@ public final class PerceptionAPI public static final ROS2Topic L515_DEBUG_EXTRACTION = BEST_EFFORT.withModule(L515_NAME) .withType(BigVideoPacket.class) .withSuffix("debug_extraction"); - public static final ROS2Topic REQUEST_REALSENSE_POINT_CLOUD = PERCEPTION_MODULE.withSuffix("request_realsense_point_cloud").withType(Empty.class); + public static final ROS2Topic REQUEST_REALSENSE = PERCEPTION_MODULE.withSuffix("request_realsense").withType(Empty.class); + public static final ROS2Topic REQUEST_REALSENSE_PUBLICATION = PERCEPTION_MODULE.withSuffix("request_realsense_publication").withType(Empty.class); public static final SideDependentList> BLACKFLY_VIDEO = new SideDependentList<>(BEST_EFFORT.withModule(BLACKFLY_NAME + "left") .withType(BigVideoPacket.class) .withSuffix("video"), @@ -139,9 +140,8 @@ public final class PerceptionAPI public static final ROS2Topic ZED_SVO_SET_POSITION = PERCEPTION_MODULE.withSuffix("zed_svo_set_position").withType(Int64.class); public static final ROS2Topic ZED_SVO_PAUSE = PERCEPTION_MODULE.withSuffix("zed_svo_pause").withType(Empty.class); public static final ROS2Topic ZED_SVO_PLAY = PERCEPTION_MODULE.withSuffix("zed_svo_play").withType(Empty.class); - public static final ROS2Topic REQUEST_ZED_COLOR = PERCEPTION_MODULE.withSuffix("request_zed_color").withType(Empty.class); - public static final ROS2Topic REQUEST_ZED_DEPTH = PERCEPTION_MODULE.withSuffix("request_zed_depth").withType(Empty.class); - public static final ROS2Topic REQUEST_ZED_POINT_CLOUD = PERCEPTION_MODULE.withSuffix("request_zed_point_cloud").withType(Empty.class); + public static final ROS2Topic REQUEST_ZED = PERCEPTION_MODULE.withSuffix("request_zed").withType(Empty.class); + public static final ROS2Topic REQUEST_ZED_PUBLICATION = PERCEPTION_MODULE.withSuffix("request_zed_publication").withType(Empty.class); public static final ROS2Topic REQUEST_CENTERPOSE = PERCEPTION_MODULE.withSuffix("request_centerpose").withType(Empty.class); public static final ROS2Topic CENTERPOSE_DETECTED_OBJECT = IHMC_ROOT.withModule("centerpose").withType(DetectedObjectPacket.class); public static final ROS2Topic REQUEST_YOLO_ZED = PERCEPTION_MODULE.withSuffix("request_yolo_zed").withType(Empty.class); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXZEDSVORecordingDemo.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXZEDSVORecordingDemo.java index 9f098aa58fe..3288ad544d1 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXZEDSVORecordingDemo.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXZEDSVORecordingDemo.java @@ -136,7 +136,7 @@ public void renderImGuiWidgets() } } }; - zed2ColoredPointCloudVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_POINT_CLOUD); + zed2ColoredPointCloudVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION); zed2ColoredPointCloudVisualizer.setActive(true); perceptionVisualizerPanel.addVisualizer(zed2ColoredPointCloudVisualizer); } @@ -146,7 +146,7 @@ public void renderImGuiWidgets() RDXROS2ImageMessageVisualizer zedLeftColorImageVisualizer = new RDXROS2ImageMessageVisualizer("ZED 2 Color Left", ros2Node, PerceptionAPI.ZED2_COLOR_IMAGES.get(RobotSide.LEFT)); - zedLeftColorImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_COLOR); + zedLeftColorImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION); perceptionVisualizerPanel.addVisualizer(zedLeftColorImageVisualizer); } @@ -155,7 +155,7 @@ public void renderImGuiWidgets() RDXROS2ImageMessageVisualizer zedRightColorImageVisualizer = new RDXROS2ImageMessageVisualizer("ZED 2 Color Right", ros2Node, PerceptionAPI.ZED2_COLOR_IMAGES.get(RobotSide.RIGHT)); - zedRightColorImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_COLOR); + zedRightColorImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION); perceptionVisualizerPanel.addVisualizer(zedRightColorImageVisualizer); } @@ -164,7 +164,7 @@ public void renderImGuiWidgets() RDXROS2ImageMessageVisualizer zed2DepthImageVisualizer = new RDXROS2ImageMessageVisualizer("ZED 2 Depth Image", ros2Node, PerceptionAPI.ZED2_DEPTH); - zed2DepthImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_DEPTH); + zed2DepthImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION); perceptionVisualizerPanel.addVisualizer(zed2DepthImageVisualizer); } } diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphDemo.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphDemo.java index 3e30090373b..3ec800c1520 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphDemo.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphDemo.java @@ -104,14 +104,14 @@ public void create() = new RDXROS2ImageMessageVisualizer("ZED 2 Color %s".formatted(side.getPascalCaseName()), ros2Node, PerceptionAPI.ZED2_COLOR_IMAGES.get(side)); - zedColorImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_COLOR); + zedColorImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION); perceptionVisualizerPanel.addVisualizer(zedColorImageVisualizer); } RDXROS2ImageMessageVisualizer zed2DepthImageVisualizer = new RDXROS2ImageMessageVisualizer("ZED 2 Depth Image", ros2Node, PerceptionAPI.ZED2_DEPTH); - zed2DepthImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_DEPTH); + zed2DepthImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION); perceptionVisualizerPanel.addVisualizer(zed2DepthImageVisualizer); RDXROS2ColoredPointCloudVisualizer zed2ColoredPointCloudVisualizer @@ -119,7 +119,7 @@ public void create() ros2Node, PerceptionAPI.ZED2_DEPTH, PerceptionAPI.ZED2_COLOR_IMAGES.get(RobotSide.LEFT)); - zed2ColoredPointCloudVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_POINT_CLOUD); + zed2ColoredPointCloudVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION); zed2ColoredPointCloudVisualizer.setActive(true); perceptionVisualizerPanel.addVisualizer(zed2ColoredPointCloudVisualizer); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/PerceptionAndAutonomyProcess.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/PerceptionAndAutonomyProcess.java deleted file mode 100644 index b7f9e45619b..00000000000 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/PerceptionAndAutonomyProcess.java +++ /dev/null @@ -1,712 +0,0 @@ -package us.ihmc; - -import org.bytedeco.opencl.global.OpenCL; -import perception_msgs.msg.dds.ImageMessage; -import us.ihmc.avatar.colorVision.BlackflyImagePublisher; -import us.ihmc.avatar.colorVision.BlackflyImageRetriever; -import us.ihmc.avatar.drcRobot.DRCRobotModel; -import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; -import us.ihmc.avatar.ros2.ROS2ControllerHelper; -import us.ihmc.behaviors.behaviorTree.ros2.ROS2BehaviorTreeExecutor; -import us.ihmc.commons.thread.ThreadTools; -import us.ihmc.commons.thread.TypedNotification; -import us.ihmc.communication.PerceptionAPI; -import us.ihmc.communication.property.ROS2StoredPropertySet; -import us.ihmc.communication.ros2.ROS2DemandGraphNode; -import us.ihmc.communication.ros2.ROS2Heartbeat; -import us.ihmc.communication.ros2.ROS2Helper; -import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.log.LogTools; -import us.ihmc.perception.BallDetectionManager; -import us.ihmc.perception.BytedecoImage; -import us.ihmc.perception.IterativeClosestPointManager; -import us.ihmc.perception.RapidHeightMapManager; -import us.ihmc.perception.RawImage; -import us.ihmc.perception.comms.PerceptionComms; -import us.ihmc.perception.detections.DetectionManager; -import us.ihmc.perception.detections.centerPose.CenterPoseDetectionSubscriber; -import us.ihmc.perception.detections.yolo.YOLOv8DetectionExecutor; -import us.ihmc.perception.opencl.OpenCLManager; -import us.ihmc.perception.opencv.OpenCVArUcoMarkerDetectionResults; -import us.ihmc.perception.ouster.OusterDepthImagePublisher; -import us.ihmc.perception.ouster.OusterDepthImageRetriever; -import us.ihmc.perception.ouster.OusterNetServer; -import us.ihmc.perception.rapidRegions.RapidPlanarRegionsExtractor; -import us.ihmc.perception.rapidRegions.RapidRegionsExtractorParameters; -import us.ihmc.perception.realsense.RealsenseConfiguration; -import us.ihmc.perception.realsense.RealsenseDeviceManager; -import us.ihmc.perception.sceneGraph.SceneGraph; -import us.ihmc.perception.sceneGraph.SceneNode; -import us.ihmc.perception.sceneGraph.arUco.ArUcoDetectionUpdater; -import us.ihmc.perception.sceneGraph.arUco.ArUcoSceneTools; -import us.ihmc.perception.sceneGraph.rigidBody.doors.DoorNode; -import us.ihmc.perception.sceneGraph.ros2.ROS2SceneGraph; -import us.ihmc.perception.sensorHead.BlackflyLensProperties; -import us.ihmc.perception.streaming.ROS2SRTSensorStreamer; -import us.ihmc.perception.tools.PerceptionMessageTools; -import us.ihmc.robotics.geometry.FramePlanarRegionsList; -import us.ihmc.robotics.geometry.PlanarRegionsList; -import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; -import us.ihmc.robotics.robotSide.RobotSide; -import us.ihmc.robotics.robotSide.SideDependentList; -import us.ihmc.ros2.ROS2Node; -import us.ihmc.ros2.ROS2NodeBuilder; -import us.ihmc.ros2.ROS2Topic; -import us.ihmc.sensors.RealsenseColorDepthImagePublisher; -import us.ihmc.sensors.RealsenseColorDepthImageRetriever; -import us.ihmc.sensors.ZEDColorDepthImagePublisher; -import us.ihmc.sensors.ZEDColorDepthImageRetriever; -import us.ihmc.tools.thread.RestartableThread; -import us.ihmc.tools.thread.RestartableThrottledThread; -import us.ihmc.tools.thread.SwapReference; - -import javax.annotation.Nullable; -import java.util.Collections; -import java.util.function.Supplier; - -/** - *

- * This class holds all sensor drivers and publishers, as well as some algorithms which consume the images - * produced by the sensors. In general, each sensor has "Retriever" and "Publisher" classes. The retriever - * is responsible for grabbing images from the sensor and providing them to this class. Then, any kind of - * processing can be performed on the images, which are then given to the publisher or any algorithm which - * requires the images. - *

- *

- * In general, the retrievers have a thread in which images are grabbed from the sensors. - * The publishers have threads which wait for new images to arrive, and once given an image they publish it - * and wait again. - * Within this class, each sensor has a thread which takes the most recent image from a retriever, performs - * any processing, and passes the image on to the publisher. - *

- *

- * To launch the Nadia specific sensor configuration, see: {@code NadiaPerceptionAndAutonomyProcess} - * To launch the process with no specific configuration, uncomment the call to {@code forceEnableAllSensors} - * in the {@code main} method. When launching only one sensor, comment out the other sensor heartbeats in the - * {@code forceEnableAllSensors} method. - *

- */ -public class PerceptionAndAutonomyProcess -{ - private static final int ZED_CAMERA_ID = 0; - private static final SideDependentList> ZED_COLOR_TOPICS = PerceptionAPI.ZED2_COLOR_IMAGES; - private static final ROS2Topic ZED_DEPTH_TOPIC = PerceptionAPI.ZED2_DEPTH; - private static final ROS2Topic ZED_CUT_OUT_DEPTH = PerceptionAPI.ZED2_CUT_OUT_DEPTH; - - private static final ROS2Topic REALSENSE_COLOR_TOPIC = PerceptionAPI.D455_COLOR_IMAGE; - private static final ROS2Topic REALSENSE_DEPTH_TOPIC = PerceptionAPI.D455_DEPTH_IMAGE; - - private static final ROS2Topic OUSTER_DEPTH_TOPIC = PerceptionAPI.OUSTER_DEPTH_IMAGE; - - private static final String LEFT_BLACKFLY_SERIAL_NUMBER = System.getProperty("blackfly.left.serial.number", "17403057"); - private static final String RIGHT_BLACKFLY_SERIAL_NUMBER = System.getProperty("blackfly.right.serial.number", "00000000"); - private static final BlackflyLensProperties BLACKFLY_LENS = BlackflyLensProperties.BFS_U3_27S5C_FE185C086HA_1; - private static final ROS2Topic BLACKFLY_IMAGE_TOPIC = PerceptionAPI.BLACKFLY_FISHEYE_COLOR_IMAGE.get(RobotSide.RIGHT); - - private final ROS2Helper ros2Helper; - private final @Nullable ROS2SyncedRobotModel syncedRobot; - private Supplier zedFrameSupplier = ReferenceFrame::getWorldFrame; - private Supplier realsenseFrameSupplier = ReferenceFrame::getWorldFrame; - private Supplier realsenseZUpFrameSupplier = ReferenceFrame::getWorldFrame; - private Supplier ousterFrameSupplier = ReferenceFrame::getWorldFrame; - private final SideDependentList> blackflyFrameSuppliers = new SideDependentList<>(ReferenceFrame::getWorldFrame, - ReferenceFrame::getWorldFrame); - private Supplier robotPelvisFrameSupplier = ReferenceFrame::getWorldFrame; - private final SideDependentList> soleFrameSuppliers = new SideDependentList<>(ReferenceFrame::getWorldFrame, - ReferenceFrame::getWorldFrame); - - private final DepthImageOverlapRemover overlapRemover = new DepthImageOverlapRemover(); - - private ROS2DemandGraphNode depthOverlapRemovalDemandNode; - private ROS2DemandGraphNode zedPointCloudDemandNode; - private ROS2DemandGraphNode zedColorDemandNode; - private ROS2DemandGraphNode zedDepthDemandNode; - private RawImage zedDepthImage; - private final SideDependentList zedColorImages = new SideDependentList<>(); - private final ZEDColorDepthImageRetriever zedImageRetriever; - private final ZEDColorDepthImagePublisher zedImagePublisher; - private final RestartableThread zedProcessAndPublishThread; - - private ROS2DemandGraphNode realsenseDemandNode; - private RawImage realsenseDepthImage; - private RawImage realsenseColorImage; - private final RealsenseColorDepthImageRetriever realsenseImageRetriever; - private final RealsenseColorDepthImagePublisher realsenseImagePublisher; - private final RestartableThread realsenseProcessAndPublishThread; - - private ROS2DemandGraphNode ousterDepthDemandNode; - private ROS2DemandGraphNode ousterLidarScanDemandNode; - private ROS2DemandGraphNode ousterHeightMapDemandNode; - private final OusterNetServer ouster; - private RawImage ousterDepthImage; - private final OusterDepthImageRetriever ousterDepthImageRetriever; - private final OusterDepthImagePublisher ousterDepthImagePublisher; - private final RestartableThread ousterProcessAndPublishThread; - - private final SideDependentList blackflyImageDemandNodes = new SideDependentList<>(); - private final SideDependentList blackflyImages = new SideDependentList<>(); - private final SideDependentList blackflyImageRetrievers = new SideDependentList<>(); - private final SideDependentList blackflyImagePublishers = new SideDependentList<>(); - private final RestartableThread blackflyProcessAndPublishThread; - - private final DetectionManager detectionManager; - - private final RestartableThread arUcoMarkerDetectionThread; - private final ArUcoDetectionUpdater arUcoUpdater; - private final SwapReference sharedArUcoDetectionResults = new SwapReference<>(OpenCVArUcoMarkerDetectionResults::new); - - private final ROS2SceneGraph sceneGraph; - private final RestartableThrottledThread sceneGraphUpdateThread; - private ROS2DemandGraphNode arUcoDetectionDemandNode; - private long sceneGraphUpdateIndex = 0; - - private final CenterPoseDetectionSubscriber centerPoseDetectionSubscriber; - private ROS2DemandGraphNode centerPoseDemandNode; - - private final YOLOv8DetectionExecutor yolov8DetectionExecutor; - private ROS2DemandGraphNode yoloAnnotatedImageDemandNode; - private ROS2DemandGraphNode yoloZEDDemandNode; - private ROS2DemandGraphNode yoloRealsenseDemandNode; - - private final BallDetectionManager ballDetectionManager; - private ROS2DemandGraphNode ballDetectionDemandNode; - - @Nullable - private RapidPlanarRegionsExtractor planarRegionsExtractor; - private ROS2StoredPropertySet planarRegionsExtractorParameterSync; - private final RestartableThrottledThread planarRegionsExtractorThread; - private final TypedNotification newPlanarRegions = new TypedNotification<>(); - private ROS2DemandGraphNode planarRegionsDemandNode; - - private RapidHeightMapManager heightMapManager; - private final RestartableThrottledThread heightMapExtractorThread; - private ROS2DemandGraphNode heightMapDemandNode; - - private final IterativeClosestPointManager icpManager; - - private final OpenCLManager openCLManager = new OpenCLManager(); - private final ROS2SRTSensorStreamer sensorStreamer = new ROS2SRTSensorStreamer(); - - private ROS2SyncedRobotModel behaviorTreeSyncedRobot; - private ReferenceFrameLibrary behaviorTreeReferenceFrameLibrary; - private ROS2BehaviorTreeExecutor behaviorTreeExecutor; - - // Sensor heartbeats to run main method without UI - private ROS2Heartbeat zedHeartbeat; - private ROS2Heartbeat realsenseHeartbeat; - private ROS2Heartbeat ousterHeartbeat; - private ROS2Heartbeat leftBlackflyHeartbeat; - private ROS2Heartbeat rightBlackflyHeartbeat; - - public PerceptionAndAutonomyProcess(ROS2Helper ros2Helper, ROS2SyncedRobotModel syncedRobot) - { - this.ros2Helper = ros2Helper; - this.syncedRobot = syncedRobot; - if (syncedRobot != null) - { - zedFrameSupplier = syncedRobot.getReferenceFrames()::getExperimentalCameraFrame; - realsenseFrameSupplier = syncedRobot.getReferenceFrames()::getSteppingCameraFrame; - realsenseZUpFrameSupplier = syncedRobot.getReferenceFrames()::getSteppingCameraZUpFrame; - ousterFrameSupplier = syncedRobot.getReferenceFrames()::getOusterLidarFrame; - robotPelvisFrameSupplier = syncedRobot.getReferenceFrames()::getPelvisZUpFrame; - for (RobotSide side : RobotSide.values) - { - blackflyFrameSuppliers.put(side, () -> syncedRobot.getReferenceFrames().getSituationalAwarenessCameraFrame(side)); - soleFrameSuppliers.put(side, () -> syncedRobot.getReferenceFrames().getSoleFrame(side)); - } - } - - initializeDependencyGraph(ros2Helper); - - detectionManager = new DetectionManager(ros2Helper); - - zedImageRetriever = new ZEDColorDepthImageRetriever(ZED_CAMERA_ID, zedFrameSupplier, zedDepthDemandNode::isDemanded, zedColorDemandNode::isDemanded); - zedImageRetriever.start(); - zedImagePublisher = new ZEDColorDepthImagePublisher(ZED_COLOR_TOPICS, ZED_DEPTH_TOPIC, ZED_CUT_OUT_DEPTH); - zedProcessAndPublishThread = new RestartableThread("ZEDImageProcessAndPublish", this::processAndPublishZED); - zedProcessAndPublishThread.start(); - - realsenseImageRetriever = new RealsenseColorDepthImageRetriever(new RealsenseDeviceManager(), - RealsenseConfiguration.D455_COLOR_720P_DEPTH_720P_30HZ, - realsenseFrameSupplier, realsenseDemandNode::isDemanded); - realsenseImagePublisher = new RealsenseColorDepthImagePublisher(REALSENSE_DEPTH_TOPIC, REALSENSE_COLOR_TOPIC); - realsenseProcessAndPublishThread = new RestartableThread("RealsenseProcessAndPublish", this::processAndPublishRealsense); - realsenseProcessAndPublishThread.start(); - - ouster = new OusterNetServer(); -// ouster.start(); - ousterDepthImageRetriever = new OusterDepthImageRetriever(ouster, - ousterFrameSupplier, - ousterLidarScanDemandNode::isDemanded, - ousterHeightMapDemandNode::isDemanded, ousterDepthDemandNode); - ousterDepthImagePublisher = new OusterDepthImagePublisher(ouster, OUSTER_DEPTH_TOPIC); - ousterProcessAndPublishThread = new RestartableThread("OusterProcessAndPublish", this::processAndPublishOuster); - ousterProcessAndPublishThread.start(); - - blackflyProcessAndPublishThread = new RestartableThread("BlackflyProcessAndPublish", this::processAndPublishBlackfly); - blackflyProcessAndPublishThread.start(); - - arUcoUpdater = new ArUcoDetectionUpdater(ros2Helper, BLACKFLY_LENS, blackflyFrameSuppliers.get(RobotSide.RIGHT)); - arUcoMarkerDetectionThread = new RestartableThread("ArUcoMarkerDetection", this::detectAndPublishArUcoMarkers); - arUcoMarkerDetectionThread.start(); - - sceneGraph = new ROS2SceneGraph(ros2Helper); - sceneGraphUpdateThread = new RestartableThrottledThread("SceneGraphUpdater", SceneGraph.UPDATE_FREQUENCY, this::updateSceneGraph); - - centerPoseDetectionSubscriber = new CenterPoseDetectionSubscriber(detectionManager); - - yolov8DetectionExecutor = new YOLOv8DetectionExecutor(ros2Helper, yoloAnnotatedImageDemandNode::isDemanded); - yolov8DetectionExecutor.addDetectionConsumerCallback(detectionManager::addDetections); - - icpManager = new IterativeClosestPointManager(ros2Helper, sceneGraph); - icpManager.startWorkers(); - - ballDetectionManager = new BallDetectionManager(ros2Helper); - - planarRegionsExtractorThread = new RestartableThrottledThread("PlanarRegionsExtractor", 10.0, this::updatePlanarRegions); - planarRegionsExtractorThread.start(); - - heightMapExtractorThread = new RestartableThrottledThread("HeightMapExtractor", 30.0, this::updateHeightMap); - heightMapExtractorThread.start(); - } - - /** Needs to be a separate method to allow constructing test bench version. */ - public void addBehaviorTree(ROS2Node ros2Node, DRCRobotModel robotModel) - { - ROS2ControllerHelper ros2ControllerHelper = new ROS2ControllerHelper(ros2Node, robotModel); - behaviorTreeSyncedRobot = new ROS2SyncedRobotModel(robotModel, ros2ControllerHelper.getROS2Node()); - - behaviorTreeReferenceFrameLibrary = new ReferenceFrameLibrary(); - behaviorTreeReferenceFrameLibrary.addAll(Collections.singleton(ReferenceFrame.getWorldFrame())); - behaviorTreeReferenceFrameLibrary.addAll(behaviorTreeSyncedRobot.getReferenceFrames().getCommonReferenceFrames()); - behaviorTreeReferenceFrameLibrary.addDynamicCollection(sceneGraph.asNewDynamicReferenceFrameCollection()); - - behaviorTreeExecutor = new ROS2BehaviorTreeExecutor(ros2ControllerHelper, - robotModel, - behaviorTreeSyncedRobot, - behaviorTreeReferenceFrameLibrary, - sceneGraph, - detectionManager); - } - - public void startAutonomyThread() - { - sceneGraphUpdateThread.start(); // scene graph runs at all times - } - - public void destroy() - { - LogTools.info("Destroying {}", getClass().getSimpleName()); - icpManager.destroy(); - yolov8DetectionExecutor.destroy(); - - if (zedImageRetriever != null) - { - zedProcessAndPublishThread.stop(); - zedImagePublisher.destroy(); - zedImageRetriever.destroy(); - } - - if (realsenseImageRetriever != null) - { - realsenseProcessAndPublishThread.stop(); - realsenseImagePublisher.destroy(); - realsenseImageRetriever.destroy(); - } - - if (ouster != null) - { - System.out.println("Destroying OusterNetServer..."); - ouster.destroy(); - System.out.println("Destroyed OusterNetServer..."); - ousterProcessAndPublishThread.stop(); - ousterDepthImagePublisher.destroy(); - ousterDepthImageRetriever.destroy(); - } - - sceneGraphUpdateThread.stop(); - - if (behaviorTreeExecutor != null) - { - behaviorTreeExecutor.destroy(); - behaviorTreeSyncedRobot.destroy(); - } - - if (blackflyProcessAndPublishThread != null) - blackflyProcessAndPublishThread.stop(); - for (RobotSide side : RobotSide.values) - { - if (blackflyImagePublishers.get(side) != null) - blackflyImagePublishers.get(side).destroy(); - if (blackflyImageRetrievers.get(side) != null) - blackflyImageRetrievers.get(side).destroy(); - } - - planarRegionsExtractorThread.stop(); - if (planarRegionsExtractor != null) - planarRegionsExtractor.destroy(); - - heightMapExtractorThread.stop(); - if (heightMapManager != null) - heightMapManager.destroy(); - - ballDetectionManager.destroy(); - - // TODO: Why does this result in a native crash? -// openCLManager.destroy(); - - overlapRemover.destroy(); - sensorStreamer.destroy(); - - depthOverlapRemovalDemandNode.destroy(); - zedPointCloudDemandNode.destroy(); - zedColorDemandNode.destroy(); - zedDepthDemandNode.destroy(); - realsenseDemandNode.destroy(); - ousterDepthDemandNode.destroy(); - ousterLidarScanDemandNode.destroy(); - blackflyImageDemandNodes.forEach(ROS2DemandGraphNode::destroy); - arUcoDetectionDemandNode.destroy(); - centerPoseDemandNode.destroy(); - yoloAnnotatedImageDemandNode.destroy(); - yoloZEDDemandNode.destroy(); - yoloRealsenseDemandNode.destroy(); - ballDetectionDemandNode.destroy(); - planarRegionsDemandNode.destroy(); - - if (zedHeartbeat != null) - zedHeartbeat.destroy(); - if (realsenseHeartbeat != null) - realsenseHeartbeat.destroy(); - if (ousterHeartbeat != null) - ousterHeartbeat.destroy(); - if (leftBlackflyHeartbeat != null) - leftBlackflyHeartbeat.destroy(); - if (rightBlackflyHeartbeat != null) - rightBlackflyHeartbeat.destroy(); - - LogTools.info("Destroyed {}", getClass().getSimpleName()); - } - - private void processAndPublishZED() - { - if (zedDepthDemandNode.isDemanded() || zedColorDemandNode.isDemanded()) - { - zedDepthImage = zedImageRetriever.getLatestRawDepthImage(); - for (RobotSide side : RobotSide.values) - { - zedColorImages.put(side, zedImageRetriever.getLatestRawColorImage(side)); - } - - if (zedDepthImage != null && !zedDepthImage.isEmpty() && icpManager.isDemanded()) - icpManager.setEnvironmentPointCloud(zedDepthImage); - - if (yoloZEDDemandNode.isDemanded()) - yolov8DetectionExecutor.runYOLODetectionOnAllModels(zedColorImages.get(RobotSide.LEFT), zedDepthImage); - - if (ballDetectionDemandNode.isDemanded()) - ballDetectionManager.run(zedColorImages.get(RobotSide.LEFT)); - - if (depthOverlapRemovalDemandNode.isDemanded() && realsenseDemandNode.isDemanded() && realsenseDepthImage != null) - { - RawImage zedCutOutDepthImage = overlapRemover.removeOverlap(zedDepthImage, 20); - zedImagePublisher.setNextCutOutDepthImage(zedCutOutDepthImage.get()); - zedCutOutDepthImage.release(); - } - - zedImagePublisher.setNextGpuDepthImage(zedDepthImage.get()); - sensorStreamer.sendFrame(PerceptionAPI.SRT_ZED_LEFT_COLOR_STREAM_STATUS, zedColorImages.get(RobotSide.LEFT)); - sensorStreamer.sendFrame(PerceptionAPI.SRT_ZED_RIGHT_COLOR_STREAM_STATUS, zedColorImages.get(RobotSide.RIGHT)); - - zedDepthImage.release(); - zedColorImages.forEach(RawImage::release); - } - else - ThreadTools.sleep(500); - } - - private void processAndPublishRealsense() - { - if (realsenseDemandNode.isDemanded()) - { - realsenseDepthImage = realsenseImageRetriever.getLatestRawDepthImage(); - realsenseColorImage = realsenseImageRetriever.getLatestRawColorImage(); - - if (yoloRealsenseDemandNode.isDemanded()) - yolov8DetectionExecutor.runYOLODetectionOnAllModels(realsenseColorImage, realsenseDepthImage); - - overlapRemover.setHighQualityImage(realsenseDepthImage.get()); - - realsenseImagePublisher.setNextDepthImage(realsenseDepthImage.get()); - sensorStreamer.sendFrame(PerceptionAPI.SRT_REALSENSE_COLOR_STREAM_STATUS, realsenseColorImage); - - realsenseDepthImage.release(); - realsenseColorImage.release(); - } - else - ThreadTools.sleep(500); - } - - private void processAndPublishOuster() - { - if (ousterDepthDemandNode.isDemanded()) - { -// ouster.start(); - ousterDepthImage = ousterDepthImageRetriever.getLatestRawDepthImage(); - if (ousterDepthImage == null) - return; - - ousterDepthImagePublisher.setNextDepthImage(ousterDepthImage.get()); - - ousterDepthImage.release(); - } - else - { - ouster.stop(); - ThreadTools.sleep(500); - } - } - - private void processAndPublishBlackfly() - { - boolean atLeastOneDemanded = false; - for (RobotSide side : RobotSide.values) - { - if (blackflyImageDemandNodes.get(side).isDemanded()) - { - atLeastOneDemanded = true; - if (blackflyImageRetrievers.get(side) != null && blackflyImagePublishers.get(side) != null) - { - blackflyImages.put(side, blackflyImageRetrievers.get(side).getLatestRawImage()); - - blackflyImagePublishers.get(side).setNextDistortedImage(blackflyImages.get(side).get()); - arUcoUpdater.setNextDistortedImage(blackflyImages.get(side).get()); - - blackflyImages.get(side).release(); - } - else - { - initializeBlackfly(side); - } - } - } - - if (!atLeastOneDemanded) - ThreadTools.sleep(500); - } - - private void detectAndPublishArUcoMarkers() - { - if (arUcoDetectionDemandNode.isDemanded()) - { - boolean performedDetection = arUcoUpdater.undistortAndUpdateArUco(); - if (performedDetection) - { - sharedArUcoDetectionResults.getForThreadOne().copyOutputData(arUcoUpdater.getArUcoMarkerDetector()); - sharedArUcoDetectionResults.swap(); - } - } - else - { - ThreadTools.sleep(500); - } - } - - private void updateSceneGraph() - { - sceneGraph.updateSubscription(); - synchronized (sharedArUcoDetectionResults) - { - ArUcoSceneTools.updateSceneGraph(sharedArUcoDetectionResults.getForThreadTwo(), blackflyFrameSuppliers.get(RobotSide.RIGHT).get(), sceneGraph); - } - - // Update CenterPose stuff - if (centerPoseDemandNode.isDemanded()) - centerPoseDetectionSubscriber.subscribe(); - else - centerPoseDetectionSubscriber.unsubscribe(); - - sceneGraph.updateDetections(detectionManager); - - ReferenceFrame robotPelvisFrame = robotPelvisFrameSupplier.get(); - - if (newPlanarRegions.poll()) - for (SceneNode sceneNode : sceneGraph.getSceneNodesByID()) - if (sceneNode instanceof DoorNode doorNode) - doorNode.getDoorPanel().filterAndSetPlanarRegionFromPlanarRegionsList(newPlanarRegions.read()); - - // Update general stuff - sceneGraph.updateOnRobotOnly(robotPelvisFrame); - sceneGraph.updatePublication(); - - ++sceneGraphUpdateIndex; - - if (behaviorTreeExecutor != null && sceneGraphUpdateIndex % 2 == 1) - { - behaviorTreeSyncedRobot.update(); - behaviorTreeExecutor.update(); - } - } - - private void updatePlanarRegions() - { - if (zedDepthImage != null && zedDepthImage.isAvailable() && planarRegionsDemandNode.isDemanded()) - { - RawImage latestZEDDepthImage = zedDepthImage.get(); - - if (planarRegionsExtractor == null) - { - int imageHeight = latestZEDDepthImage.getHeight(); - int imageWidth = latestZEDDepthImage.getWidth(); - double fx = latestZEDDepthImage.getFocalLengthX(); - double fy = latestZEDDepthImage.getFocalLengthY(); - double cx = latestZEDDepthImage.getPrincipalPointX(); - double cy = latestZEDDepthImage.getPrincipalPointY(); - planarRegionsExtractor = new RapidPlanarRegionsExtractor(openCLManager, - imageHeight, - imageWidth, - fx, - fy, - cx, - cy); - planarRegionsExtractor.getDebugger().setEnabled(false); - - planarRegionsExtractorParameterSync = new ROS2StoredPropertySet<>(ros2Helper, PerceptionComms.PERSPECTIVE_RAPID_REGION_PARAMETERS, planarRegionsExtractor.getParameters()); - } - - planarRegionsExtractorParameterSync.updateAndPublishThrottledStatus(); - - FramePlanarRegionsList framePlanarRegionsList = new FramePlanarRegionsList(); - - // TODO: Get rid of BytedecoImage, RapidPlanarRegionsExtractor requires it - BytedecoImage bytedecoImage = new BytedecoImage(latestZEDDepthImage.getCpuImageMat().clone()); - bytedecoImage.createOpenCLImage(openCLManager, OpenCL.CL_MEM_READ_WRITE); - planarRegionsExtractor.update(bytedecoImage, zedFrameSupplier.get(), framePlanarRegionsList); - planarRegionsExtractor.setProcessing(false); - bytedecoImage.destroy(openCLManager); - - PlanarRegionsList planarRegionsInWorldFrame = framePlanarRegionsList.getPlanarRegionsList().copy(); - planarRegionsInWorldFrame.applyTransform(zedFrameSupplier.get().getTransformToWorldFrame()); - - newPlanarRegions.set(planarRegionsInWorldFrame); - - PerceptionMessageTools.publishFramePlanarRegionsList(framePlanarRegionsList, PerceptionAPI.PERSPECTIVE_RAPID_REGIONS, ros2Helper); - - latestZEDDepthImage.release(); - } - } - - private void updateHeightMap() - { - if (realsenseDepthImage != null && realsenseDepthImage.isAvailable() && heightMapDemandNode.isDemanded()) - { - RawImage latestRealsenseDepthImage = realsenseDepthImage.get(); - - if (heightMapManager == null) - { - heightMapManager = new RapidHeightMapManager(openCLManager, - syncedRobot == null ? null : syncedRobot.getRobotModel(), - soleFrameSuppliers.get(RobotSide.LEFT).get(), - soleFrameSuppliers.get(RobotSide.RIGHT).get(), - latestRealsenseDepthImage.getIntrinsicsCopy(), - ros2Helper); - } - - heightMapManager.update(latestRealsenseDepthImage.getCpuImageMat(), - latestRealsenseDepthImage.getAcquisitionTime(), - realsenseFrameSupplier.get(), - realsenseZUpFrameSupplier.get(), - ros2Helper); - - latestRealsenseDepthImage.release(); - } - } - - private void initializeBlackfly(RobotSide side) - { - String serialNumber = side == RobotSide.LEFT ? LEFT_BLACKFLY_SERIAL_NUMBER : RIGHT_BLACKFLY_SERIAL_NUMBER; - if (serialNumber.equals("00000000")) - { - LogTools.error("{} blackfly with serial number was not provided", side.getPascalCaseName()); - ThreadTools.sleep(3000); - return; - } - - blackflyImageRetrievers.put(side, - new BlackflyImageRetriever(serialNumber, - BLACKFLY_LENS, - RobotSide.RIGHT, - blackflyFrameSuppliers.get(side), - blackflyImageDemandNodes.get(side))); - blackflyImagePublishers.put(side, new BlackflyImagePublisher(BLACKFLY_LENS, BLACKFLY_IMAGE_TOPIC, 0.5f)); - } - - private void initializeDependencyGraph(ROS2PublishSubscribeAPI ros2) - { - // Initialize all nodes - depthOverlapRemovalDemandNode = new ROS2DemandGraphNode(ros2, PerceptionAPI.REQUEST_OVERLAP_REMOVAL); - zedPointCloudDemandNode = new ROS2DemandGraphNode(ros2, PerceptionAPI.REQUEST_ZED_POINT_CLOUD); - zedDepthDemandNode = new ROS2DemandGraphNode(ros2, PerceptionAPI.REQUEST_ZED_DEPTH); - zedColorDemandNode = new ROS2DemandGraphNode(ros2, PerceptionAPI.REQUEST_ZED_COLOR); - realsenseDemandNode = new ROS2DemandGraphNode(ros2, PerceptionAPI.REQUEST_REALSENSE_POINT_CLOUD); - ousterDepthDemandNode = new ROS2DemandGraphNode(ros2, PerceptionAPI.REQUEST_OUSTER_DEPTH); - ousterHeightMapDemandNode = new ROS2DemandGraphNode(ros2, PerceptionAPI.REQUEST_HEIGHT_MAP); - ousterLidarScanDemandNode = new ROS2DemandGraphNode(ros2, PerceptionAPI.REQUEST_LIDAR_SCAN); - for (RobotSide side : RobotSide.values) - blackflyImageDemandNodes.put(side, new ROS2DemandGraphNode(ros2, PerceptionAPI.REQUEST_BLACKFLY_COLOR_IMAGE.get(side))); - arUcoDetectionDemandNode = new ROS2DemandGraphNode(ros2, PerceptionAPI.REQUEST_ARUCO); - centerPoseDemandNode = new ROS2DemandGraphNode(ros2, PerceptionAPI.REQUEST_CENTERPOSE); - yoloAnnotatedImageDemandNode = new ROS2DemandGraphNode(ros2, PerceptionAPI.REQUEST_YOLO_ANNOTATED_IMAGE); - yoloZEDDemandNode = new ROS2DemandGraphNode(ros2, PerceptionAPI.REQUEST_YOLO_ZED); - yoloRealsenseDemandNode = new ROS2DemandGraphNode(ros2, PerceptionAPI.REQUEST_YOLO_REALSENSE); - ballDetectionDemandNode = new ROS2DemandGraphNode(ros2, PerceptionAPI.REQUEST_BALL_TRACKING); - planarRegionsDemandNode = new ROS2DemandGraphNode(ros2, PerceptionAPI.REQUEST_PLANAR_REGIONS); - heightMapDemandNode = new ROS2DemandGraphNode(ros2, PerceptionAPI.REQUEST_HEIGHT_MAP); - - // build the graph - blackflyImageDemandNodes.get(RobotSide.RIGHT).addDependents(ousterDepthDemandNode); // For point cloud coloring - zedDepthDemandNode.addDependents(planarRegionsDemandNode); // Using ZED for planar regions - zedDepthDemandNode.addDependents(zedPointCloudDemandNode); // Used by global visualizer to demand color & depth - zedColorDemandNode.addDependents(zedPointCloudDemandNode, centerPoseDemandNode, ballDetectionDemandNode); - planarRegionsDemandNode.addDependents(yoloZEDDemandNode); // Planar region used for door detection - realsenseDemandNode.addDependents(yoloRealsenseDemandNode); - realsenseDemandNode.addDependents(heightMapDemandNode); - blackflyImageDemandNodes.get(RobotSide.RIGHT).addDependents(arUcoDetectionDemandNode); // ArUco set to use Blackfly images - } - - /* - * This method is used to enable sensors without needing to run the UI. - * Unneeded sensors can be commented out. - */ - private void forceEnableAllSensors(ROS2PublishSubscribeAPI ros2) - { - zedHeartbeat = new ROS2Heartbeat(ros2, PerceptionAPI.REQUEST_ZED_POINT_CLOUD); - zedHeartbeat.setAlive(true); - - realsenseHeartbeat = new ROS2Heartbeat(ros2, PerceptionAPI.REQUEST_REALSENSE_POINT_CLOUD); - realsenseHeartbeat.setAlive(true); - - ousterHeartbeat = new ROS2Heartbeat(ros2, PerceptionAPI.REQUEST_OUSTER_DEPTH); - ousterHeartbeat.setAlive(true); - - leftBlackflyHeartbeat = new ROS2Heartbeat(ros2, PerceptionAPI.REQUEST_BLACKFLY_COLOR_IMAGE.get(RobotSide.LEFT)); - leftBlackflyHeartbeat.setAlive(true); - - rightBlackflyHeartbeat = new ROS2Heartbeat(ros2, PerceptionAPI.REQUEST_BLACKFLY_COLOR_IMAGE.get(RobotSide.RIGHT)); - rightBlackflyHeartbeat.setAlive(true); - } - - public static void main(String[] args) - { - ROS2Node ros2Node = new ROS2NodeBuilder().build("perception_autonomy_process"); - ROS2Helper ros2Helper = new ROS2Helper(ros2Node); - - PerceptionAndAutonomyProcess perceptionAndAutonomyProcess = new PerceptionAndAutonomyProcess(ros2Helper, null); - perceptionAndAutonomyProcess.startAutonomyThread(); - - // To run a sensor without the UI, uncomment the below line. - // perceptionAndAutonomyProcess.forceEnableAllSensors(ros2Helper); - Runtime.getRuntime().addShutdownHook(new Thread(perceptionAndAutonomyProcess::destroy, "PerceptionAutonomyProcessShutdown")); - } -} diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeUpdateThread.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeUpdateThread.java new file mode 100644 index 00000000000..adeab1142de --- /dev/null +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/behaviorTree/ros2/ROS2BehaviorTreeUpdateThread.java @@ -0,0 +1,51 @@ +package us.ihmc.behaviors.behaviorTree.ros2; + +import us.ihmc.avatar.drcRobot.DRCRobotModel; +import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; +import us.ihmc.avatar.ros2.ROS2ControllerHelper; +import us.ihmc.commons.thread.RepeatingTaskThread; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.perception.detections.DetectionManager; +import us.ihmc.perception.sceneGraph.SceneGraph; +import us.ihmc.robotics.referenceFrames.ReferenceFrameLibrary; +import us.ihmc.ros2.ROS2Node; + +import java.util.Collections; + +public class ROS2BehaviorTreeUpdateThread extends RepeatingTaskThread +{ + private final ROS2SyncedRobotModel syncedRobot; + private final ROS2BehaviorTreeExecutor executor; + + public ROS2BehaviorTreeUpdateThread(ROS2Node ros2Node, DRCRobotModel robotModel, SceneGraph sceneGraph, DetectionManager detectionManager) + { + super(ROS2BehaviorTreeUpdateThread.class.getSimpleName()); + setFrequencyLimit(ROS2BehaviorTreeState.SYNC_FREQUENCY); + + ROS2ControllerHelper ros2ControllerHelper = new ROS2ControllerHelper(ros2Node, robotModel); + syncedRobot = new ROS2SyncedRobotModel(robotModel, ros2ControllerHelper.getROS2Node()); + + ReferenceFrameLibrary referenceFrameLibrary = new ReferenceFrameLibrary(); + referenceFrameLibrary.addAll(Collections.singleton(ReferenceFrame.getWorldFrame())); + referenceFrameLibrary.addAll(syncedRobot.getReferenceFrames().getCommonReferenceFrames()); + referenceFrameLibrary.addDynamicCollection(sceneGraph.asNewDynamicReferenceFrameCollection()); + + executor = new ROS2BehaviorTreeExecutor(ros2ControllerHelper, robotModel, syncedRobot, referenceFrameLibrary, sceneGraph, detectionManager); + } + + @Override + protected synchronized void runTask() + { + syncedRobot.update(); + executor.update(); + } + + @Override + public synchronized void kill() + { + super.kill(); + + syncedRobot.destroy(); + executor.destroy(); + } +} diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/IterativeClosestPointManager.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/IterativeClosestPointManager.java index 417d73acdf2..ad15a1bfa36 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/IterativeClosestPointManager.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/IterativeClosestPointManager.java @@ -1,7 +1,7 @@ package us.ihmc.perception; import perception_msgs.msg.dds.IterativeClosestPointRequest; -import us.ihmc.ros2.ROS2Input; +import us.ihmc.commons.thread.RepeatingTaskThread; import us.ihmc.communication.PerceptionAPI; import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.euclid.referenceFrame.FramePose3D; @@ -13,7 +13,8 @@ import us.ihmc.perception.sceneGraph.SceneGraph; import us.ihmc.perception.sceneGraph.SceneNode; import us.ihmc.perception.sceneGraph.rigidBody.primitive.PrimitiveRigidBodyShape; -import us.ihmc.tools.thread.RestartableThrottledThread; +import us.ihmc.ros2.ROS2Input; +import us.ihmc.sensors.ImageSensor; import java.util.List; import java.util.Map; @@ -22,7 +23,7 @@ public class IterativeClosestPointManager { - private static final double ICP_WORK_FREQUENCY = 20.0; + private static final double ICP_WORK_FREQUENCY = 10.0; private static final double EPSILON = 0.001; private final ROS2Helper ros2Helper; @@ -34,7 +35,10 @@ public class IterativeClosestPointManager private final ConcurrentHashMap nodeIDToWorkerMap = new ConcurrentHashMap<>(); private final ConcurrentHashMap workerToIterationsMap = new ConcurrentHashMap<>(); private final ROS2Input requestMessageSubscription; - private final RestartableThrottledThread workerThread; + private final RepeatingTaskThread workerThread; + + private ImageSensor imageSensor; + private int depthImageKey; private List environmentPointCloud; @@ -46,7 +50,6 @@ public IterativeClosestPointManager(ROS2Helper ros2Helper, SceneGraph sceneGraph requestMessageSubscription = ros2Helper.subscribe(PerceptionAPI.ICP_REQUEST); // Each time a request message is received, we update the corresponding worker accordingly - // TODO: Find out whether messages can be skipped, resulting in updates being missed requestMessageSubscription.addCallback(requestMessage -> { long nodeID = requestMessage.getNodeId(); @@ -65,7 +68,6 @@ else if (!requestMessage.getRunIcp()) // ICP Worker no longer needed ( IterativeClosestPointWorker worker = nodeIDToWorkerMap.get(nodeID); // Check if size changed - PrimitiveRigidBodyShape shape = PrimitiveRigidBodyShape.fromByte(requestMessage.getShape()); Vector3D lengths = requestMessage.getLengths(); Vector3D radii = requestMessage.getRadii(); int numberOfShapeSamples = requestMessage.getNumberOfShapeSamples(); @@ -90,7 +92,20 @@ else if (!requestMessage.getRunIcp()) // ICP Worker no longer needed ( } }); - workerThread = new RestartableThrottledThread("ICPWorkers", ICP_WORK_FREQUENCY, this::runWorkers); + workerThread = new RepeatingTaskThread("ICPWorkers", this::runWorkers).setFrequencyLimit(ICP_WORK_FREQUENCY); + } + + /** + * Optionally, you may provide an image sensor to retrieve depth images from. + * This removes the need to call {@link #setEnvironmentPointCloud(RawImage)}. + * + * @param imageSensor The image sensor from which to retrieve depth images. + * @param depthImageKey The key of the depth image. + */ + public void setImageSensor(ImageSensor imageSensor, int depthImageKey) + { + this.imageSensor = imageSensor; + this.depthImageKey = depthImageKey; } /** @@ -109,12 +124,12 @@ public void setEnvironmentPointCloud(RawImage depthImage) public void startWorkers() { - workerThread.start(); + workerThread.startRepeating(); } public void stopWorkers() { - workerThread.stop(); + workerThread.stopRepeating(); } public boolean isDemanded() @@ -124,7 +139,8 @@ public boolean isDemanded() public void destroy() { - workerThread.blockingStop(); + nodeIDToWorkerMap.clear(); + workerThread.blockingKill(); } /** @@ -133,6 +149,19 @@ public void destroy() */ private void runWorkers() { + if (!isDemanded()) + return; + + if (imageSensor != null) + { + RawImage depthImage = imageSensor.getImage(depthImageKey); + if (depthImage == null) + return; + + setEnvironmentPointCloud(imageSensor.getImage(depthImageKey)); + depthImage.release(); + } + for (Map.Entry entry : nodeIDToWorkerMap.entrySet()) { long nodeID = entry.getKey(); 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/RapidHeightMapUpdateThread.java new file mode 100644 index 00000000000..99598c3ecd3 --- /dev/null +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java @@ -0,0 +1,110 @@ +package us.ihmc.perception; + +import us.ihmc.avatar.drcRobot.DRCRobotModel; +import us.ihmc.commons.thread.RepeatingTaskThread; +import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.perception.heightMap.TerrainMapData; +import us.ihmc.perception.opencl.OpenCLManager; +import us.ihmc.robotics.referenceFrames.MutableReferenceFrame; +import us.ihmc.sensorProcessing.heightMap.HeightMapData; +import us.ihmc.sensors.ImageSensor; + +public class RapidHeightMapUpdateThread extends RepeatingTaskThread +{ + private final ROS2PublishSubscribeAPI ros2; + private final DRCRobotModel robotModel; + private final ReferenceFrame leftFootFrame; + private final ReferenceFrame rightFootFrame; + private final OpenCLManager openCLManager; + + private RapidHeightMapManager heightMapManager; + private final Object heightMapLock = new Object(); + + private final ImageSensor imageSensor; + private final MutableReferenceFrame sensorFrame; + private final MutableReferenceFrame zUpSensorFrame; + private final int depthImageKey; + + public RapidHeightMapUpdateThread(ROS2PublishSubscribeAPI ros2, + DRCRobotModel robotModel, + ReferenceFrame leftFootFrame, + ReferenceFrame rightFootFrame, + OpenCLManager openCLManager, + ImageSensor imageSensor, + int depthImageKey) + { + super(imageSensor.getSensorName() + RapidHeightMapUpdateThread.class.getSimpleName()); + + this.ros2 = ros2; + this.robotModel = robotModel; + this.leftFootFrame = leftFootFrame; + this.rightFootFrame = rightFootFrame; + this.openCLManager = openCLManager; + this.imageSensor = imageSensor; + this.depthImageKey = depthImageKey; + + sensorFrame = new MutableReferenceFrame(); + zUpSensorFrame = new MutableReferenceFrame(sensorFrame.getReferenceFrame()); + } + + @Override + protected void runTask() + { + try + { + imageSensor.waitForGrab(); + RawImage depthImage = imageSensor.getImage(depthImageKey); + + // Initialize + if (heightMapManager == null) + heightMapManager = new RapidHeightMapManager(openCLManager, robotModel, leftFootFrame, rightFootFrame, depthImage.getIntrinsicsCopy(), ros2); + + // Update sensor frames + sensorFrame.update(transformToWorld -> transformToWorld.set(depthImage.getPose())); + zUpSensorFrame.update(transformToWorld -> + { + transformToWorld.setRotationToZero(); + transformToWorld.appendYawRotation(sensorFrame.getTransformToParent().getRotation().getYaw()); + }); + + // Update height map + synchronized (heightMapLock) + { + heightMapManager.update(depthImage.getCpuImageMat(), + depthImage.getAcquisitionTime(), + sensorFrame.getReferenceFrame(), + zUpSensorFrame.getReferenceFrame(), + ros2); + } + + depthImage.release(); + } catch (InterruptedException ignored) {} + } + + public HeightMapData getLatestHeightMapData() + { + synchronized (heightMapLock) + { + return heightMapManager.getLatestHeightMapData(); + } + } + + public TerrainMapData getTerrainMapData() + { + synchronized (heightMapLock) + { + return heightMapManager.getTerrainMapData(); + } + } + + @Override + public void kill() + { + super.kill(); + interrupt(); + + if (heightMapManager != null) + 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 8342287ab1c..ade67b240b3 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 @@ -61,7 +61,7 @@ public StandAloneRealsenseProcess(ROS2Helper ros2Helper, ROS2SyncedRobotModel sy soleFrameSuppliers.put(side, () -> syncedRobot.getReferenceFrames().getSoleFrame(side)); } } - realsenseDemandNode = new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_REALSENSE_POINT_CLOUD); + realsenseDemandNode = new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_REALSENSE_PUBLICATION); realsenseImageRetriever = new RealsenseColorDepthImageRetriever(new RealsenseDeviceManager(), RealsenseConfiguration.D455_COLOR_720P_DEPTH_720P_30HZ, diff --git a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXBallTrackingDemo.java b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXBallTrackingDemo.java index 32a9731fb25..89d7ac8b29d 100644 --- a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXBallTrackingDemo.java +++ b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXBallTrackingDemo.java @@ -62,8 +62,8 @@ public RDXBallTrackingDemo() imageRetriever = new ZEDColorDepthImageRetriever(0, ReferenceFrame::getWorldFrame, - new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_DEPTH)::isDemanded, - new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_COLOR)::isDemanded); + new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_PUBLICATION)::isDemanded, + new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_PUBLICATION)::isDemanded); imageRetriever.start(); imagePublisher = new ZEDColorDepthImagePublisher(PerceptionAPI.ZED2_COLOR_IMAGES, PerceptionAPI.ZED2_DEPTH, PerceptionAPI.ZED2_CUT_OUT_DEPTH); @@ -128,14 +128,14 @@ public void create() RDXROS2ImageMessageVisualizer colorImageVisualizer = new RDXROS2ImageMessageVisualizer("Color Image", ros2Node, PerceptionAPI.ZED2_COLOR_IMAGES.get(RobotSide.LEFT)); - colorImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_COLOR); + colorImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION); perceptionVisualizerPanel.addVisualizer(colorImageVisualizer); RDXROS2ColoredPointCloudVisualizer pointCloudVisualizer = new RDXROS2ColoredPointCloudVisualizer("Point Cloud", ros2Node, PerceptionAPI.ZED2_DEPTH, PerceptionAPI.ZED2_COLOR_IMAGES.get(RobotSide.LEFT)); - pointCloudVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_POINT_CLOUD); + pointCloudVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION); perceptionVisualizerPanel.addVisualizer(pointCloudVisualizer); RDXROS2BallTrackingVisualizer ballTrajectoryVisualizer = new RDXROS2BallTrackingVisualizer("Ball Trajectory", PerceptionAPI.BALL_TRAJECTORY, ros2Helper); diff --git a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXIterativeClosestPointWorkerDemo.java b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXIterativeClosestPointWorkerDemo.java index 7f5b3820c7d..e5e77843aca 100644 --- a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXIterativeClosestPointWorkerDemo.java +++ b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXIterativeClosestPointWorkerDemo.java @@ -101,8 +101,8 @@ public RDXIterativeClosestPointWorkerDemo() { zedImageRetriever = new ZEDColorDepthImageRetriever(0, ReferenceFrame::getWorldFrame, - new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_DEPTH)::isDemanded, - new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_COLOR)::isDemanded); + new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_PUBLICATION)::isDemanded, + new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_PUBLICATION)::isDemanded); zedImageRetriever.start(); zedImagePublisher = new ZEDColorDepthImagePublisher(PerceptionAPI.ZED2_COLOR_IMAGES, PerceptionAPI.ZED2_DEPTH, PerceptionAPI.ZED2_CUT_OUT_DEPTH); @@ -220,14 +220,14 @@ public void create() RDXROS2ImageMessageVisualizer zedDepthImageVisualizer = new RDXROS2ImageMessageVisualizer("ZED2 Depth Image", node, PerceptionAPI.ZED2_DEPTH); - zedDepthImageVisualizer.createRequestHeartbeat(node, PerceptionAPI.REQUEST_ZED_COLOR); + zedDepthImageVisualizer.createRequestHeartbeat(node, PerceptionAPI.REQUEST_ZED_PUBLICATION); perceptionVisualizerPanel.addVisualizer(zedDepthImageVisualizer); RDXROS2ColoredPointCloudVisualizer zedPointCloudVisualizer = new RDXROS2ColoredPointCloudVisualizer("ZED2 Colored Point Cloud", node, PerceptionAPI.ZED2_DEPTH, PerceptionAPI.ZED2_COLOR_IMAGES.get(RobotSide.LEFT)); - zedPointCloudVisualizer.createRequestHeartbeat(node, PerceptionAPI.REQUEST_ZED_POINT_CLOUD); + zedPointCloudVisualizer.createRequestHeartbeat(node, PerceptionAPI.REQUEST_ZED_PUBLICATION); perceptionVisualizerPanel.addVisualizer(zedPointCloudVisualizer); baseUI.getImGuiPanelManager().addPanel("Settings", this::renderSettings); diff --git a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXOpenCVTrackerDemo.java b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXOpenCVTrackerDemo.java index 11f461b1a56..6b75d69e6dc 100644 --- a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXOpenCVTrackerDemo.java +++ b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXOpenCVTrackerDemo.java @@ -65,8 +65,8 @@ public RDXOpenCVTrackerDemo() imageRetriever = new ZEDColorDepthImageRetriever(0, ReferenceFrame::getWorldFrame, - new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_DEPTH)::isDemanded, - new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_COLOR)::isDemanded); + new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_PUBLICATION)::isDemanded, + new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_PUBLICATION)::isDemanded); imageRetriever.start(); imagePublisher = new ZEDColorDepthImagePublisher(PerceptionAPI.ZED2_COLOR_IMAGES, PerceptionAPI.ZED2_DEPTH, PerceptionAPI.ZED2_CUT_OUT_DEPTH); @@ -171,7 +171,7 @@ public void create() RDXROS2ImageMessageVisualizer colorImageVisualizer = new RDXROS2ImageMessageVisualizer("Color Image", ros2Node, PerceptionAPI.ZED2_COLOR_IMAGES.get(RobotSide.LEFT)); - colorImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_COLOR); + colorImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION); perceptionVisualizerPanel.addVisualizer(colorImageVisualizer); perceptionVisualizerPanel.create(baseUI); diff --git a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXYOLOv8PointCloudSegmentationDemo.java b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXYOLOv8PointCloudSegmentationDemo.java index fb646d09ef4..c332711f2db 100644 --- a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXYOLOv8PointCloudSegmentationDemo.java +++ b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXYOLOv8PointCloudSegmentationDemo.java @@ -56,8 +56,8 @@ public RDXYOLOv8PointCloudSegmentationDemo() { zedImageRetriever = new ZEDColorDepthImageRetriever(0, ReferenceFrame::getWorldFrame, - new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_DEPTH)::isDemanded, - new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_COLOR)::isDemanded); + new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_PUBLICATION)::isDemanded, + new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_PUBLICATION)::isDemanded); zedImageRetriever.start(); zedImagePublisher = new ZEDColorDepthImagePublisher(PerceptionAPI.ZED2_COLOR_IMAGES, PerceptionAPI.ZED2_DEPTH, PerceptionAPI.ZED2_CUT_OUT_DEPTH); @@ -117,13 +117,13 @@ public void create() RDXROS2ImageMessageVisualizer zedColorImageVisualizer = new RDXROS2ImageMessageVisualizer("ZED2 Color Image", node, PerceptionAPI.ZED2_COLOR_IMAGES.get(RobotSide.LEFT)); - zedColorImageVisualizer.createRequestHeartbeat(node, PerceptionAPI.REQUEST_ZED_COLOR); + zedColorImageVisualizer.createRequestHeartbeat(node, PerceptionAPI.REQUEST_ZED_PUBLICATION); perceptionVisualizerPanel.addVisualizer(zedColorImageVisualizer); RDXROS2ColoredPointCloudVisualizer zedPointCloudVisualizer = new RDXROS2ColoredPointCloudVisualizer("ZED 2 Colored Point Cloud", node, PerceptionAPI.ZED2_DEPTH, PerceptionAPI.ZED2_COLOR_IMAGES.get(RobotSide.LEFT)); - zedPointCloudVisualizer.createRequestHeartbeat(node, PerceptionAPI.REQUEST_ZED_POINT_CLOUD); + zedPointCloudVisualizer.createRequestHeartbeat(node, PerceptionAPI.REQUEST_ZED_PUBLICATION); perceptionVisualizerPanel.addVisualizer(zedPointCloudVisualizer); baseUI.create(); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/ImageSensorPublishThread.java b/ihmc-perception/src/main/java/us/ihmc/perception/ImageSensorPublishThread.java new file mode 100644 index 00000000000..83743fb2487 --- /dev/null +++ b/ihmc-perception/src/main/java/us/ihmc/perception/ImageSensorPublishThread.java @@ -0,0 +1,55 @@ +package us.ihmc.perception; + +import us.ihmc.commons.thread.RepeatingTaskThread; +import us.ihmc.communication.packets.Packet; +import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2Topic; +import us.ihmc.sensors.ImageSensor; + +import java.util.Map; +import java.util.Map.Entry; + +public class ImageSensorPublishThread extends RepeatingTaskThread +{ + private final Map>> imageKeyToTopicMap; + private final RawImagePublisher publisher; + private final ImageSensor imageSensor; + + public ImageSensorPublishThread(ROS2Node ros2Node, ImageSensor sensorToPublish, Map>> imageKeyToTopicMap) + { + super(sensorToPublish.getSensorName() + "PublishThread"); + + imageSensor = sensorToPublish; + this.imageKeyToTopicMap = imageKeyToTopicMap; + publisher = new RawImagePublisher(ros2Node); + } + + @Override + protected void runTask() + { + try + { // Wait for images to be grabbed + imageSensor.waitForGrab(); + + for (Entry>> imageEntry : imageKeyToTopicMap.entrySet()) + { + int imageKey = imageEntry.getKey(); + ROS2Topic> imageTopic = imageEntry.getValue(); + + RawImage imageToPublish = imageSensor.getImage(imageKey); + publisher.publishImage(imageTopic, imageToPublish); + imageToPublish.release(); + } + } + catch (InterruptedException ignored) {} + } + + @Override + public void kill() + { + super.kill(); + interrupt(); + + publisher.close(); + } +} diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/RawImage.java b/ihmc-perception/src/main/java/us/ihmc/perception/RawImage.java index 0399fdd95b7..a6a7311a597 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/RawImage.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/RawImage.java @@ -167,19 +167,31 @@ else if (matPointer instanceof GpuMat gpuImage) } /** - * Provides a new {@link RawImage} with the same intrinsics and metadata as this one, but with a different image. + * Provides a new {@link RawImage} with the same pixel format, intrinsics, and metadata as this one, but with a different image. * Useful when applying changes to Mats and wishing to keep the same intrinsics & metadata in the {@link RawImage}. * @param newCpuImageMat new CPU image mat to replace the current image. Must have the same dimensions. * @return A new {@link RawImage} with the same intrinsics & metadata, but with a different image. */ public RawImage replaceImage(Mat newCpuImageMat) + { + return replaceImage(newCpuImageMat, this.pixelFormat); + } + + /** + * Provides a new {@link RawImage} with the same intrinsics and metadata as this one, but with a different image. + * Useful when applying changes to Mats and wishing to keep the same intrinsics & metadata in the {@link RawImage}. + * @param newCpuImageMat new CPU image mat to replace the current image. Must have the same dimensions. + * @param newPixelFormat the PixelFormat of the new image. + * @return A new {@link RawImage} with the same intrinsics & metadata, but with a different image. + */ + public RawImage replaceImage(Mat newCpuImageMat, PixelFormat newPixelFormat) { if (getWidth() != newCpuImageMat.cols() || getHeight() != newCpuImageMat.rows()) throw new IllegalArgumentException("New image must have the same dimensions as the current image"); return new RawImage(newCpuImageMat, null, - this.pixelFormat, + newPixelFormat, this.cameraIntrinsics, this.cameraModel, this.sensorPose, @@ -189,19 +201,31 @@ public RawImage replaceImage(Mat newCpuImageMat) } /** - * Provides a new {@link RawImage} with the same intrinsics and metadata as this one, but with a different image. + * Provides a new {@link RawImage} with the same pixel format, intrinsics, and metadata as this one, but with a different image. * Useful when applying changes to Mats and wishing to keep the same intrinsics & metadata in the {@link RawImage}. * @param newGpuImageMat new GPU image mat to replace the current image. Must have the same dimensions. * @return A new {@link RawImage} with the same intrinsics & metadata, but with a different image. */ public RawImage replaceImage(GpuMat newGpuImageMat) + { + return replaceImage(newGpuImageMat, this.pixelFormat); + } + + /** + * Provides a new {@link RawImage} with the same intrinsics and metadata as this one, but with a different image. + * Useful when applying changes to Mats and wishing to keep the same intrinsics & metadata in the {@link RawImage}. + * @param newGpuImageMat new GPU image mat to replace the current image. Must have the same dimensions. + * @param newPixelFormat the PixelFormat of the new image. + * @return A new {@link RawImage} with the same intrinsics & metadata, but with a different image. + */ + public RawImage replaceImage(GpuMat newGpuImageMat, PixelFormat newPixelFormat) { if (getWidth() != newGpuImageMat.cols() || getHeight() != newGpuImageMat.rows()) throw new IllegalArgumentException("New image must have the same dimensions as the current image"); return new RawImage(null, newGpuImageMat, - this.pixelFormat, + newPixelFormat, this.cameraIntrinsics, this.cameraModel, this.sensorPose, @@ -222,22 +246,12 @@ public Instant getAcquisitionTime() public int getWidth() { - if (hasCpuImage()) - return cpuImageMat.cols(); - else if (hasGpuImage()) - return gpuImageMat.cols(); - - throw new NullPointerException("Neither CPU nor GPU Mats were initialized"); + return cameraIntrinsics.getWidth(); } public int getHeight() { - if (hasCpuImage()) - return cpuImageMat.rows(); - else if (hasGpuImage()) - return gpuImageMat.rows(); - - throw new NullPointerException("Neither CPU nor GPU Mats were initialized"); + return cameraIntrinsics.getHeight(); } public float getDepthDiscretization() diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/RawImagePublisher.java b/ihmc-perception/src/main/java/us/ihmc/perception/RawImagePublisher.java new file mode 100644 index 00000000000..ce1ca04c260 --- /dev/null +++ b/ihmc-perception/src/main/java/us/ihmc/perception/RawImagePublisher.java @@ -0,0 +1,108 @@ +package us.ihmc.perception; + +import org.apache.commons.lang3.NotImplementedException; +import org.bytedeco.javacpp.BytePointer; +import org.bytedeco.opencv.global.opencv_cudaimgproc; +import org.bytedeco.opencv.global.opencv_imgproc; +import org.bytedeco.opencv.opencv_core.GpuMat; +import perception_msgs.msg.dds.ImageMessage; +import perception_msgs.msg.dds.SRTStreamStatus; +import us.ihmc.communication.packets.Packet; +import us.ihmc.communication.ros2.ROS2Helper; +import us.ihmc.perception.cuda.CUDACompressionTools; +import us.ihmc.perception.cuda.CUDAJPEGProcessor; +import us.ihmc.perception.imageMessage.CompressionType; +import us.ihmc.perception.streaming.ROS2SRTSensorStreamer; +import us.ihmc.perception.tools.PerceptionMessageTools; +import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2Topic; + +import static us.ihmc.perception.imageMessage.CompressionType.NVJPEG; +import static us.ihmc.perception.imageMessage.CompressionType.ZSTD_NVJPEG_HYBRID; + +public class RawImagePublisher implements AutoCloseable +{ + private final CUDACompressionTools compressionTools; + private final CUDAJPEGProcessor jpegProcessor; + private final ROS2SRTSensorStreamer sensorStreamer; + + private final ROS2Helper ros2Helper; + private final ImageMessage imageMessage; + + public RawImagePublisher(ROS2Node ros2Node) + { + compressionTools = new CUDACompressionTools(); + jpegProcessor = new CUDAJPEGProcessor(); + sensorStreamer = new ROS2SRTSensorStreamer(ros2Node); + + ros2Helper = new ROS2Helper(ros2Node); + imageMessage = new ImageMessage(); + } + + @SuppressWarnings("unchecked") // Trust me bro, I know what I'm doing + public synchronized void publishImage(ROS2Topic> imageTopic, RawImage imageToPublish) + { + if (imageTopic.getType().equals(ImageMessage.class)) + { // Topic is an ImageMessage topic -> publish as image message + publishAsImageMessage((ROS2Topic) imageTopic, imageToPublish); + } + else if (imageTopic.getType().equals(SRTStreamStatus.class)) + { // Topic is an SRT stream topic -> stream video over SRT + sensorStreamer.sendFrame((ROS2Topic) imageTopic, imageToPublish); + } + } + + private void publishAsImageMessage(ROS2Topic imageTopic, RawImage imageToPublish) + { + GpuMat imageToCompress = imageToPublish.getGpuImageMat(); + BytePointer compressedImage; + CompressionType compressionType; + + switch (imageToPublish.getPixelFormat()) + { + case GRAY16: // Depth image -> compress using ZSTD nvJPEG hybrid compression + compressedImage = compressionTools.compressDepth(imageToCompress); + compressionType = ZSTD_NVJPEG_HYBRID; + break; + case BGRA8: // BGRA image -> convert to BGR, then compress using nvJPEG + GpuMat bgr8Image = new GpuMat(); + opencv_cudaimgproc.cvtColor(imageToCompress, bgr8Image, opencv_imgproc.COLOR_BGRA2BGR); + imageToCompress = bgr8Image; + case BGR8: // BGR image -> compress using nvJPEG + compressedImage = new BytePointer(imageToCompress.limit()); + jpegProcessor.encodeBGR(imageToCompress, compressedImage); + compressionType = NVJPEG; + break; + case RGBA8: // RGBA image -> convert to RGB, then compress using nvJPEG + GpuMat rgb8Image = new GpuMat(); + opencv_cudaimgproc.cvtColor(imageToCompress, rgb8Image, opencv_imgproc.COLOR_RGBA2RGB); + imageToCompress = rgb8Image; + case RGB8: // RGB image -> compress using nvJPEG + compressedImage = new BytePointer(imageToCompress.limit()); + jpegProcessor.encodeRGB(imageToCompress, compressedImage); + compressionType = NVJPEG; + break; + case GRAY8: // Black and white image -> compress using nvJPEG + compressedImage = new BytePointer(imageToCompress.limit()); + jpegProcessor.encodeGray(imageToCompress, compressedImage); + compressionType = NVJPEG; + break; + default: + throw new NotImplementedException("Tomasz has not implemented the compression method for this pixel format yet."); + } + + // Pack the message and send it off + PerceptionMessageTools.packImageMessage(imageToPublish, compressedImage, compressionType, imageMessage); + ros2Helper.publish(imageTopic, imageMessage); + } + + @Override + public synchronized void close() + { + System.out.println("Closing " + getClass().getSimpleName()); + compressionTools.destroy(); + jpegProcessor.destroy(); + sensorStreamer.destroy(); + System.out.println("Closed " + getClass().getSimpleName()); + } +} diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/detections/yolo/YOLOv8DetectionExecutor.java b/ihmc-perception/src/main/java/us/ihmc/perception/detections/yolo/YOLOv8DetectionExecutor.java index c299744cb6a..8472712607c 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/detections/yolo/YOLOv8DetectionExecutor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/detections/yolo/YOLOv8DetectionExecutor.java @@ -151,21 +151,22 @@ public void runYOLODetection(YOLOv8ObjectDetector yoloDetector, RawImage colorIm { if (yoloDetector.isReady() && !yoloExecutorService.isShutdown()) { + // Acquire the images + if (colorImage.get() == null || depthImage.get() == null) + return; + yoloExecutorService.submit(() -> { - // Acquire the images - if (!colorImage.isAvailable() || !depthImage.isAvailable()) - return; - colorImage.get(); - depthImage.get(); - // Run YOLO to get results YOLOv8DetectionResults yoloResults = yoloDetector.runOnImage(colorImage, yoloConfidenceThreshold, yoloNMSThreshold, yoloMaskThreshold); // TODO: temp hack - if (yoloDetectionResults.containsKey(lastRunDetectorIndex)) - yoloDetectionResults.remove(lastRunDetectorIndex).destroy(); - yoloDetectionResults.put(lastRunDetectorIndex, yoloResults); + synchronized (yoloDetectionResults) + { + if (yoloDetectionResults.containsKey(lastRunDetectorIndex)) + yoloDetectionResults.remove(lastRunDetectorIndex).destroy(); + yoloDetectionResults.put(lastRunDetectorIndex, yoloResults); + } newestColorImage = colorImage; // Get the object masks from the results @@ -277,55 +278,60 @@ public void annotateAndPublishImage() detectionMasks.putAll(value.getSegmentationImages()); } - detectionMasks.entrySet().stream().filter(entry -> entry.getKey().confidence() >= yoloConfidenceThreshold).forEach(entry -> + synchronized (yoloDetectionResults) { - YOLOv8DetectionOutput detection = entry.getKey(); - RawImage maskImage = entry.getValue(); + detectionMasks.entrySet().stream().filter(entry -> entry.getKey().confidence() >= yoloConfidenceThreshold).forEach(entry -> + { + YOLOv8DetectionOutput detection = entry.getKey(); + RawImage maskImage = entry.getValue().get(); + if (maskImage == null || maskImage.isEmpty()) + return; - String text = String.format("%s: %.2f", detection.objectClass().toString(), detection.confidence()); + String text = String.format("%s: %.2f", detection.objectClass().toString(), detection.confidence()); - // Draw the bounding box - Rect boundingBox = new Rect(detection.x(), detection.y(), detection.width(), detection.height()); - opencv_imgproc.rectangle(resultMat, boundingBox, BOUNDING_BOX_COLOR, 5, LINE_TYPE, 0); + // Draw the bounding box + Rect boundingBox = new Rect(detection.x(), detection.y(), detection.width(), detection.height()); + opencv_imgproc.rectangle(resultMat, boundingBox, BOUNDING_BOX_COLOR, 5, LINE_TYPE, 0); - // Draw text background - Size textSize = opencv_imgproc.getTextSize(text, FONT, FONT_SCALE, FONT_THICKNESS, new IntPointer()); + // Draw text background + Size textSize = opencv_imgproc.getTextSize(text, FONT, FONT_SCALE, FONT_THICKNESS, new IntPointer()); - int textBoxClampedX = MathTools.clamp(detection.x(), 0, colorImage.getWidth() - textSize.width()); - int textBoxClampedY = MathTools.clamp(detection.y() - textSize.height(), 0, colorImage.getHeight() - textSize.height()); + int textBoxClampedX = MathTools.clamp(detection.x(), 0, colorImage.getWidth() - textSize.width()); + int textBoxClampedY = MathTools.clamp(detection.y() - textSize.height(), 0, colorImage.getHeight() - textSize.height()); - Rect textBox = new Rect(textBoxClampedX, textBoxClampedY, textSize.width(), textSize.height()); + Rect textBox = new Rect(textBoxClampedX, textBoxClampedY, textSize.width(), textSize.height()); - opencv_imgproc.rectangle(resultMat, textBox, BOUNDING_BOX_COLOR, opencv_imgproc.FILLED, LINE_TYPE, 0); + opencv_imgproc.rectangle(resultMat, textBox, BOUNDING_BOX_COLOR, opencv_imgproc.FILLED, LINE_TYPE, 0); - opencv_imgproc.putText(resultMat, - text, - new Point(textBoxClampedX, textBoxClampedY + textSize.height()), - opencv_imgproc.CV_FONT_HERSHEY_DUPLEX, - FONT_SCALE, - new Scalar(255.0, 255.0, 255.0, 255.0), - FONT_THICKNESS, - LINE_TYPE, - false); + opencv_imgproc.putText(resultMat, + text, + new Point(textBoxClampedX, textBoxClampedY + textSize.height()), + opencv_imgproc.CV_FONT_HERSHEY_DUPLEX, + FONT_SCALE, + new Scalar(255.0, 255.0, 255.0, 255.0), + FONT_THICKNESS, + LINE_TYPE, + false); - // Add green tint to show mask - // first convert 32F mask to 8U - Mat maskMat = new Mat(maskImage.getHeight(), maskImage.getWidth(), opencv_core.CV_8U); - maskImage.getCpuImageMat().convertTo(maskMat, opencv_core.CV_8U, 255.0, 0.0); + // Add green tint to show mask + // first convert 32F mask to 8U + Mat maskMat = new Mat(maskImage.getHeight(), maskImage.getWidth(), opencv_core.CV_8U); + maskImage.getCpuImageMat().convertTo(maskMat, opencv_core.CV_8U, 255.0, 0.0); - // resize the mask to fit the result image - opencv_imgproc.resize(maskMat, maskMat, resultMat.size(), 0.0, 0.0, opencv_imgproc.INTER_NEAREST); + // resize the mask to fit the result image + opencv_imgproc.resize(maskMat, maskMat, resultMat.size(), 0.0, 0.0, opencv_imgproc.INTER_NEAREST); - // ensure the green Mat is same size as image - if (resultMat.cols() != GREEN_MAT.cols() || resultMat.rows() != GREEN_MAT.rows()) - opencv_imgproc.resize(GREEN_MAT, GREEN_MAT, resultMat.size()); + // ensure the green Mat is same size as image + if (resultMat.cols() != GREEN_MAT.cols() || resultMat.rows() != GREEN_MAT.rows()) + opencv_imgproc.resize(GREEN_MAT, GREEN_MAT, resultMat.size()); - // add a green tint where mask = 255 - opencv_core.add(resultMat, GREEN_MAT, resultMat, maskMat, -1); + // add a green tint where mask = 255 + opencv_core.add(resultMat, GREEN_MAT, resultMat, maskMat, -1); - maskImage.release(); - maskMat.release(); - }); + maskImage.release(); + maskMat.release(); + }); + } BytePointer annotatedImagePointer = new BytePointer(); opencv_imgcodecs.imencode(".jpg", resultMat, annotatedImagePointer); // for some reason using CUDAImageEncoder broke YOLO's CUDNN diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/detections/yolo/YOLOv8DetectionResults.java b/ihmc-perception/src/main/java/us/ihmc/perception/detections/yolo/YOLOv8DetectionResults.java index 164b0683913..192f6bae16c 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/detections/yolo/YOLOv8DetectionResults.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/detections/yolo/YOLOv8DetectionResults.java @@ -23,7 +23,7 @@ public class YOLOv8DetectionResults private final MatVector outputBlobs; private final RawImage detectionImage; private final FloatIndexer outputMasksIndexer; - private final Map objectMasks = new HashMap<>(); + private final Map objectMasks = new HashMap<>(); private final float maskThreshold; @@ -66,10 +66,7 @@ public Map getSegmentationImages() { for (YOLOv8DetectionOutput detection : detections) { - Mat detectionMask = getDetectionMask(detection); - - if (detectionMask != null && !detectionMask.isNull()) // FIXME: This can be NULL - segmentationImages.put(detection, createMaskRawImage(detectionMask)); + segmentationImages.put(detection, getDetectionMask(detection)); } } @@ -86,9 +83,7 @@ public Map getTargetSegmentationImages(Set getDetections() return detections; } - private Mat getDetectionMask(YOLOv8DetectionOutput detection) + private RawImage getDetectionMask(YOLOv8DetectionOutput detection) { - if (objectMasks.containsKey(detection)) - return objectMasks.get(detection); + RawImage existingMask = objectMasks.get(detection); + if (existingMask != null) + return existingMask; Mat floatMaskMat = getFloatMaskMat(detection); Mat booleanMaskMat = getBooleanMaskMat(detection, floatMaskMat); - objectMasks.put(detection, booleanMaskMat); + RawImage maskRawImage = createMaskRawImage(booleanMaskMat); + objectMasks.put(detection, maskRawImage); floatMaskMat.close(); - return booleanMaskMat; + return maskRawImage; } private Mat getFloatMaskMat(YOLOv8DetectionOutput detection) @@ -172,9 +169,8 @@ private RawImage createMaskRawImage(Mat mask) public synchronized void destroy() { - for (Mat mat : objectMasks.values()) - if (mat != null && !mat.isNull()) - mat.close(); + for (RawImage maskImage : objectMasks.values()) + maskImage.release(); detectionImage.release(); outputMasksIndexer.close(); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/detections/yolo/YOLOv8DetectionThread.java b/ihmc-perception/src/main/java/us/ihmc/perception/detections/yolo/YOLOv8DetectionThread.java new file mode 100644 index 00000000000..829a6f62b98 --- /dev/null +++ b/ihmc-perception/src/main/java/us/ihmc/perception/detections/yolo/YOLOv8DetectionThread.java @@ -0,0 +1,70 @@ +package us.ihmc.perception.detections.yolo; + +import org.bytedeco.opencv.opencv_core.GpuMat; +import us.ihmc.commons.thread.RepeatingTaskThread; +import us.ihmc.communication.ros2.ROS2Helper; +import us.ihmc.perception.RawImage; +import us.ihmc.perception.detections.DetectionManager; +import us.ihmc.perception.imageMessage.PixelFormat; +import us.ihmc.sensors.ImageSensor; + +import java.util.function.BooleanSupplier; + +public class YOLOv8DetectionThread extends RepeatingTaskThread +{ + private final YOLOv8DetectionExecutor yoloExecutor; + + private ImageSensor imageSensor; + private int colorImageKey; + private int depthImageKey; + + public YOLOv8DetectionThread(ROS2Helper ros2Helper, DetectionManager detectionManager, BooleanSupplier annotatedImageDemandSupplier) + { + super(YOLOv8DetectionThread.class.getSimpleName()); + + yoloExecutor = new YOLOv8DetectionExecutor(ros2Helper, annotatedImageDemandSupplier); + yoloExecutor.addDetectionConsumerCallback(detectionManager::addDetections); + } + + // synchronized along with runInLoop() to not change sensors in middle of execution + public synchronized void setImageSensor(ImageSensor imageSensor, int colorImageKey, int depthImageKey) + { + this.imageSensor = imageSensor; + this.colorImageKey = colorImageKey; + this.depthImageKey = depthImageKey; + } + + @Override + protected synchronized void runTask() + { + try + { + imageSensor.waitForGrab(); + + RawImage colorImage = imageSensor.getImage(colorImageKey); + RawImage depthImage = imageSensor.getImage(depthImageKey); + + // Ensure color image is in BGR8 + if (colorImage.getPixelFormat() != PixelFormat.BGR8) + { + GpuMat bgrMat = new GpuMat(); + colorImage.getPixelFormat().convertToPixelFormat(colorImage.getGpuImageMat(), bgrMat, PixelFormat.BGR8); + colorImage.release(); + colorImage = colorImage.replaceImage(bgrMat, PixelFormat.BGR8); + } + + yoloExecutor.runYOLODetectionOnAllModels(colorImage, depthImage); + + colorImage.release(); + depthImage.release(); + } catch (InterruptedException ignored) {} + } + + @Override + public void kill() + { + super.kill(); + interrupt(); + yoloExecutor.destroy(); + } +} diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/ffmpeg/FFmpegEncoder.java b/ihmc-perception/src/main/java/us/ihmc/perception/ffmpeg/FFmpegEncoder.java index 3b8bf2e335e..25452605d0f 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/ffmpeg/FFmpegEncoder.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/ffmpeg/FFmpegEncoder.java @@ -127,6 +127,13 @@ public boolean encodeNextFrame(Consumer packetConsumer) return error != AVERROR_EOF(); } + public boolean flush(Consumer packetConsumer) + { + av_frame_free(frameToEncode); + frameToEncode.close(); + return encodeNextFrame(packetConsumer); + } + public AVRational getTimeBase() { return encoderContext.time_base(); @@ -134,12 +141,8 @@ public AVRational getTimeBase() public void destroy() { - frameToEncode.close(); - encodeNextFrame(null); - avcodec_close(encoderContext); avcodec_free_context(encoderContext); - av_frame_free(frameToEncode); av_packet_free(encodedPacket); encoder.close(); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/opencv/OpenCVArUcoMarkerDetectionThread.java b/ihmc-perception/src/main/java/us/ihmc/perception/opencv/OpenCVArUcoMarkerDetectionThread.java new file mode 100644 index 00000000000..8459a47415a --- /dev/null +++ b/ihmc-perception/src/main/java/us/ihmc/perception/opencv/OpenCVArUcoMarkerDetectionThread.java @@ -0,0 +1,98 @@ +package us.ihmc.perception.opencv; + +import us.ihmc.commons.thread.RepeatingTaskThread; +import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.perception.RawImage; +import us.ihmc.robotics.referenceFrames.MutableReferenceFrame; +import us.ihmc.sensors.ImageSensor; +import us.ihmc.tools.thread.SwapReference; + +import java.util.function.Function; + +public class OpenCVArUcoMarkerDetectionThread extends RepeatingTaskThread +{ + private final OpenCVArUcoMarkerDetector detector = new OpenCVArUcoMarkerDetector(); + private final SwapReference resultSwapReference = new SwapReference<>(OpenCVArUcoMarkerDetectionResults::new); + + private OpenCVArUcoMarkerROS2Publisher publisher; + private OpenCVArUcoMarkerDetectionResults resultsToPublish; + + private final ImageSensor imageSensor; + private final MutableReferenceFrame sensorFrame = new MutableReferenceFrame(); + private final int colorImageKey; + + private boolean firstRun = true; + + public OpenCVArUcoMarkerDetectionThread(ImageSensor imageSensor, int colorImageKey) + { + super(imageSensor.getSensorName() + OpenCVArUcoMarkerDetectionThread.class.getSimpleName()); + this.imageSensor = imageSensor; + this.colorImageKey = colorImageKey; + } + + public OpenCVArUcoMarkerDetectionThread enablePublishing(ROS2PublishSubscribeAPI ros2, Function markerSizeProvider) + { + resultsToPublish = new OpenCVArUcoMarkerDetectionResults(); + publisher = new OpenCVArUcoMarkerROS2Publisher(resultsToPublish, ros2, markerSizeProvider, sensorFrame.getReferenceFrame()); + return this; + } + + @Override + protected void runTask() + { + try + { // Get an image + imageSensor.waitForGrab(); + RawImage colorImage = imageSensor.getImage(colorImageKey); + + if (firstRun) + { // Set the camera intrinsics on first run + detector.setCameraIntrinsics(colorImage.getIntrinsicsCopy()); + firstRun = false; + } + + // Update the sensor frame + sensorFrame.update(transformToWorld -> transformToWorld.set(colorImage.getPose())); + + // Detect markers + detector.update(colorImage.getCpuImageMat()); + resultSwapReference.getForThreadOne().copyOutputData(detector); + resultSwapReference.swap(); + + if (publisher != null) + { // Publish results if enabled + resultsToPublish.copyOutputData(detector); + publisher.update(); + } + + colorImage.release(); + } catch (InterruptedException ignored) {} + } + + /** + * Get the result swap reference. + *

+ * This is thread one, and you are thread two. + * Synchronize over the swap reference while using the results. + * Do not call {@link SwapReference#swap()} (this thread takes care of that for you). + * + * @return A swap reference to the latest results. + */ + public SwapReference getResultSwapReference() + { + return resultSwapReference; + } + + public ReferenceFrame getSensorFrame() + { + return sensorFrame.getReferenceFrame(); + } + + @Override + public void kill() + { + super.kill(); + interrupt(); + } +} diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/rapidRegions/RapidPlanarRegionsExtractionThread.java b/ihmc-perception/src/main/java/us/ihmc/perception/rapidRegions/RapidPlanarRegionsExtractionThread.java new file mode 100644 index 00000000000..3bf4230ff42 --- /dev/null +++ b/ihmc-perception/src/main/java/us/ihmc/perception/rapidRegions/RapidPlanarRegionsExtractionThread.java @@ -0,0 +1,112 @@ +package us.ihmc.perception.rapidRegions; + +import org.bytedeco.opencl.global.OpenCL; +import us.ihmc.commons.thread.RepeatingTaskThread; +import us.ihmc.commons.thread.TypedNotification; +import us.ihmc.communication.PerceptionAPI; +import us.ihmc.communication.property.ROS2StoredPropertySet; +import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.perception.BytedecoImage; +import us.ihmc.perception.RawImage; +import us.ihmc.perception.comms.PerceptionComms; +import us.ihmc.perception.opencl.OpenCLManager; +import us.ihmc.perception.tools.PerceptionMessageTools; +import us.ihmc.robotics.geometry.FramePlanarRegionsList; +import us.ihmc.robotics.referenceFrames.MutableReferenceFrame; +import us.ihmc.sensors.ImageSensor; + +import java.util.Collections; +import java.util.Set; +import java.util.concurrent.ConcurrentHashMap; + +public class RapidPlanarRegionsExtractionThread extends RepeatingTaskThread +{ + private static final double UPDATE_FREQUENCY = 10.0; + + private final ROS2PublishSubscribeAPI ros2; + + private final OpenCLManager openCLManager; + + private final ImageSensor imageSensor; + private final int depthImageKey; + private final MutableReferenceFrame sensorFrame; + + private RapidPlanarRegionsExtractor extractor; + private ROS2StoredPropertySet extractorParametersSync; + private final FramePlanarRegionsList framePlanarRegions = new FramePlanarRegionsList(); + private final Set> newPlanarRegionNotifications = Collections.newSetFromMap(new ConcurrentHashMap<>()); + + public RapidPlanarRegionsExtractionThread(ROS2PublishSubscribeAPI ros2, OpenCLManager openCLManager, ImageSensor imageSensor, int depthImageKey) + { + super(imageSensor.getSensorName() + RapidPlanarRegionsExtractionThread.class.getSimpleName()); + setFrequencyLimit(UPDATE_FREQUENCY); + + this.ros2 = ros2; + this.openCLManager = openCLManager; + this.imageSensor = imageSensor; + this.depthImageKey = depthImageKey; + + sensorFrame = new MutableReferenceFrame("PlanarRegionExtractionSensorFrame", ReferenceFrame.getWorldFrame()); + } + + @Override + protected void runTask() + { + // Get an image from the sensor + RawImage depthImage = imageSensor.getImage(depthImageKey); + if (depthImage == null) + return; + + // Initialize if not yet initialized + if (extractor == null) + initialize(depthImage); + + // Update parameters + extractorParametersSync.updateAndPublishThrottledStatus(); + + // Update the sensor frame using the depth image pose + sensorFrame.update(transformToWorld -> transformToWorld.set(depthImage.getOrientation(), depthImage.getPosition())); + + // Extract the planar regions + BytedecoImage bytedecoImage = new BytedecoImage(depthImage.getCpuImageMat().clone()); + bytedecoImage.createOpenCLImage(openCLManager, OpenCL.CL_MEM_READ_WRITE); + extractor.update(bytedecoImage, sensorFrame.getReferenceFrame(), framePlanarRegions); + extractor.setProcessing(false); + bytedecoImage.destroy(openCLManager); + + // Give a copy to notifications + FramePlanarRegionsList planarRegionsCopy = framePlanarRegions.copy(); + for (TypedNotification notification : newPlanarRegionNotifications) + notification.set(planarRegionsCopy); + + // Publish the frame planar regions + PerceptionMessageTools.publishFramePlanarRegionsList(framePlanarRegions, PerceptionAPI.PERSPECTIVE_RAPID_REGIONS, ros2); + + depthImage.release(); + } + + private void initialize(RawImage depthImage) + { + extractor = new RapidPlanarRegionsExtractor(openCLManager, depthImage.getIntrinsicsCopy()); + extractor.getDebugger().setEnabled(false); + + extractorParametersSync = new ROS2StoredPropertySet<>(ros2, PerceptionComms.PERSPECTIVE_RAPID_REGION_PARAMETERS, extractor.getParameters()); + } + + public TypedNotification getNewPlanarRegionsNotification() + { + TypedNotification notification = new TypedNotification<>(); + newPlanarRegionNotifications.add(notification); + return notification; + } + + @Override + public void kill() + { + super.kill(); + + if (extractor != null) + extractor.destroy(); + } +} diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/realsense/RealsenseDeviceManager.java b/ihmc-perception/src/main/java/us/ihmc/perception/realsense/RealsenseDeviceManager.java index 725ff5d9f77..c790ca9b6ba 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/realsense/RealsenseDeviceManager.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/realsense/RealsenseDeviceManager.java @@ -27,7 +27,7 @@ public class RealsenseDeviceManager private final YoDouble numberOfDevices = new YoDouble("numberOfDevices", registry); private final rs2_context context; - private final rs2_device_list devices; + private rs2_device_list devices; private final rs2_error error = new rs2_error(); public RealsenseDeviceManager() @@ -45,9 +45,6 @@ public RealsenseDeviceManager(YoRegistry parentRegistry, YoGraphicsListRegistry context = realsense2.rs2_create_context(realsense2.RS2_API_VERSION, error); checkError(); - devices = realsense2.rs2_query_devices(context, error); - checkError(); - updateDeviceCount(); if (parentRegistry != null) @@ -139,6 +136,10 @@ private String getDeviceInfo(rs2_device rs2Device, int deviceInfoEnum) private int updateDeviceCount() { + realsense2.rs2_delete_device_list(devices); + devices = realsense2.rs2_query_devices(context, error); + checkError(); + int rs2DeviceCount = realsense2.rs2_get_device_count(devices, error); checkError(); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/sceneGraph/ros2/ROS2SceneGraphUpdateThread.java b/ihmc-perception/src/main/java/us/ihmc/perception/sceneGraph/ros2/ROS2SceneGraphUpdateThread.java new file mode 100644 index 00000000000..b35a91539b3 --- /dev/null +++ b/ihmc-perception/src/main/java/us/ihmc/perception/sceneGraph/ros2/ROS2SceneGraphUpdateThread.java @@ -0,0 +1,83 @@ +package us.ihmc.perception.sceneGraph.ros2; + +import us.ihmc.commons.thread.RepeatingTaskThread; +import us.ihmc.commons.thread.TypedNotification; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.perception.detections.DetectionManager; +import us.ihmc.perception.opencv.OpenCVArUcoMarkerDetectionResults; +import us.ihmc.perception.opencv.OpenCVArUcoMarkerDetectionThread; +import us.ihmc.perception.sceneGraph.SceneGraph; +import us.ihmc.perception.sceneGraph.SceneNode; +import us.ihmc.perception.sceneGraph.arUco.ArUcoSceneTools; +import us.ihmc.perception.sceneGraph.rigidBody.doors.DoorNode; +import us.ihmc.robotics.geometry.FramePlanarRegionsList; +import us.ihmc.robotics.geometry.PlanarRegionsList; +import us.ihmc.tools.thread.SwapReference; + +import java.util.function.Supplier; + +public class ROS2SceneGraphUpdateThread extends RepeatingTaskThread +{ + private final ROS2SceneGraph sceneGraph; + private final DetectionManager detectionManager; + private final Supplier robotPelvisFrameSupplier; + + private TypedNotification planarRegionsNotification; + private OpenCVArUcoMarkerDetectionThread arUcoDetectionThread; + + public ROS2SceneGraphUpdateThread(ROS2SceneGraph sceneGraph, + DetectionManager detectionManager, + Supplier robotPelvisFrameSupplier) + { + super(ROS2SceneGraphUpdateThread.class.getSimpleName()); + setFrequencyLimit(SceneGraph.UPDATE_FREQUENCY); + this.sceneGraph = sceneGraph; + this.detectionManager = detectionManager; + this.robotPelvisFrameSupplier = robotPelvisFrameSupplier; + } + + public void setPlanarRegionsNotification(TypedNotification planarRegionsNotification) + { + this.planarRegionsNotification = planarRegionsNotification; + } + + public void setArUcoDetectionThread(OpenCVArUcoMarkerDetectionThread arUcoDetectionThread) + { + this.arUcoDetectionThread = arUcoDetectionThread; + } + + @Override + protected void runTask() + { + sceneGraph.updateSubscription(); + + if (arUcoDetectionThread != null) + { + SwapReference arUcoResultsReference = arUcoDetectionThread.getResultSwapReference(); + synchronized (arUcoResultsReference) + { + ArUcoSceneTools.updateSceneGraph(arUcoResultsReference.getForThreadTwo(), arUcoDetectionThread.getSensorFrame(), sceneGraph); + } + } + + sceneGraph.updateDetections(detectionManager); + + // Update planar regions for door nodes + if (planarRegionsNotification != null && planarRegionsNotification.poll()) + { + FramePlanarRegionsList framePlanarRegions = planarRegionsNotification.read(); + PlanarRegionsList planarRegionsInWorldFrame = framePlanarRegions.getPlanarRegionsList(); + planarRegionsInWorldFrame.applyTransform(framePlanarRegions.getSensorToWorldFrameTransform()); + + for (SceneNode sceneNode : sceneGraph.getSceneNodesByID()) + { + if (sceneNode instanceof DoorNode doorNode) + doorNode.getDoorPanel().filterAndSetPlanarRegionFromPlanarRegionsList(planarRegionsInWorldFrame); + } + } + + ReferenceFrame pelvisFrame = robotPelvisFrameSupplier.get(); + sceneGraph.updateOnRobotOnly(pelvisFrame); + sceneGraph.updatePublication(); + } +} diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/streaming/ROS2SRTSensorStreamer.java b/ihmc-perception/src/main/java/us/ihmc/perception/streaming/ROS2SRTSensorStreamer.java index 9e7063bb41d..e17e8b335e6 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/streaming/ROS2SRTSensorStreamer.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/streaming/ROS2SRTSensorStreamer.java @@ -21,13 +21,20 @@ public class ROS2SRTSensorStreamer { private final ROS2Node ros2Node; + private boolean destroyROS2Node = false; private final Map, ROS2SRTVideoStreamer> videoStreamers = new HashMap<>(); private final ExecutorService sendFrameExecutor = Executors.newCachedThreadPool(); public ROS2SRTSensorStreamer() { - ros2Node = new ROS2NodeBuilder().build(getClass().getSimpleName().toLowerCase() + "_node"); + this(new ROS2NodeBuilder().build(ROS2SRTSensorStreamer.class.getSimpleName().toLowerCase() + "_node")); + destroyROS2Node = true; + } + + public ROS2SRTSensorStreamer(ROS2Node ros2Node) + { + this.ros2Node = ros2Node; } public void addStream(ROS2Topic streamTopic, @@ -51,7 +58,7 @@ public boolean hasStream(ROS2Topic streamTopic) public void sendFrame(ROS2Topic streamTopic, RawImage frame) { - if (frame != null && frame.get() == null) + if (frame == null || frame.get() == null) return; if (!hasStream(streamTopic)) @@ -70,6 +77,9 @@ public void destroy() for (ROS2SRTVideoStreamer videoStreamer : videoStreamers.values()) videoStreamer.destroy(); + + if (destroyROS2Node) + ros2Node.destroy(); } private void addStreamWithGuessedParameters(ROS2Topic streamTopic, RawImage exampleImage) diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/streaming/SRTVideoStreamer.java b/ihmc-perception/src/main/java/us/ihmc/perception/streaming/SRTVideoStreamer.java index b201591235c..2d102058790 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/streaming/SRTVideoStreamer.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/streaming/SRTVideoStreamer.java @@ -217,6 +217,12 @@ public void destroy() interruptCallback.interrupt(); } + if (encoder != null) + { + encoder.flush(this::writeToCallers); + encoder.destroy(); + } + Iterator callerIterator = callers.iterator(); while (callerIterator.hasNext()) { @@ -224,9 +230,6 @@ public void destroy() callerIterator.remove(); } - if (encoder != null) - encoder.destroy(); - interruptCallback.close(); } 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 28c797f2762..8f143a89f74 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 @@ -106,9 +106,9 @@ public static void publishJPGCompressedColorImage(BytePointer compressedColorPoi public static void publishFramePlanarRegionsList(FramePlanarRegionsList framePlanarRegionsList, ROS2Topic topic, - ROS2Helper ros2Helper) + ROS2PublishSubscribeAPI ros2) { - ros2Helper.publish(topic, PlanarRegionMessageConverter.convertToFramePlanarRegionsListMessage(framePlanarRegionsList)); + ros2.publish(topic, PlanarRegionMessageConverter.convertToFramePlanarRegionsListMessage(framePlanarRegionsList)); } public static void publishPlanarRegionsList(PlanarRegionsList planarRegionsList, ROS2Topic topic, ROS2Helper ros2Helper) diff --git a/ihmc-perception/src/main/java/us/ihmc/sensors/ImageSensor.java b/ihmc-perception/src/main/java/us/ihmc/sensors/ImageSensor.java new file mode 100644 index 00000000000..4d3045d8537 --- /dev/null +++ b/ihmc-perception/src/main/java/us/ihmc/sensors/ImageSensor.java @@ -0,0 +1,131 @@ +package us.ihmc.sensors; + +import us.ihmc.commons.Conversions; +import us.ihmc.commons.thread.RepeatingTaskThread; +import us.ihmc.commons.thread.ThreadTools; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.perception.RawImage; + +import java.util.function.Supplier; + +public abstract class ImageSensor implements AutoCloseable +{ + private static final double SECONDS_BETWEEN_RETRIES = 1.0; // Wait 1 second between retries for starting sensors + + private final String sensorName; + /** Sensor will be in world frame by default, unless a sensor frame supplier is specified through {@link #setSensorFrameSupplier(Supplier)}. */ + protected volatile Supplier sensorFrameSupplier; + + private final RepeatingTaskThread grabThread; + private final Object grabNotification = new Object(); + + public ImageSensor(String sensorName) + { + this.sensorName = sensorName; + grabThread = new RepeatingTaskThread(sensorName + "Grabber", this::grabAndNotify); + } + + /** + * Initializes and starts the sensor. + * @return Whether the sensor was successfully initialized and started. + */ + protected abstract boolean startSensor(); + + /** + * Set the sensor frame supplier. + * @param sensorFrameSupplier Supplier of the sensor's reference frame. + */ + public void setSensorFrameSupplier(Supplier sensorFrameSupplier) + { + this.sensorFrameSupplier = sensorFrameSupplier; + } + + /** + * @return Whether the sensor is running. Not necessarily the same as whether the grab thread is running. + */ + public abstract boolean isSensorRunning(); + + /** + * Grab all images the sensor provides. Blocks until new images are available. + * @return Whether new images were successfully grabbed. + */ + protected abstract boolean grab(); + + /** + *

+ * Get the latest image grabbed by the sensor, specifying the image to get using its key. + *

+ *

+ * This method should either return a new {@link RawImage}, such that the reference count = 1, + * or increment the reference count of the returned image by calling {@link RawImage#get()}. + * It is the caller's responsibility to call {@link RawImage#release()} after calling this method. + *

+ * @param imageKey Key of the image to get. + * @return A {@link RawImage} with an incremented reference count. + * The caller must call {@link RawImage#release()}. + */ + public abstract RawImage getImage(int imageKey); + + public String getSensorName() + { + return sensorName; + } + + public synchronized void run(boolean run) + { + if (!grabThread.isAlive()) + grabThread.start(); + + grabThread.setRepeating(run); + } + + public void waitForGrab() throws InterruptedException + { + synchronized (grabNotification) + { + grabNotification.wait(); + } + } + + public void waitForGrab(double timeoutSeconds) throws InterruptedException + { + long millis = (long) Conversions.secondsToMilliseconds(timeoutSeconds); + long additionalNanos = Conversions.secondsToNanoseconds(timeoutSeconds) - Conversions.millisecondsToNanoseconds(millis); + + synchronized (grabNotification) + { + grabNotification.wait(millis, (int) additionalNanos); + } + } + + @Override + public void close() + { + grabThread.blockingKill(); + } + + public RepeatingTaskThread getGrabThread() + { + return grabThread; + } + + private void grabAndNotify() + { + // If the sensor is not running, try to start the sensor + if (!isSensorRunning() && !startSensor()) + { // if sensor failed to start, sleep a bit and retry + ThreadTools.park(SECONDS_BETWEEN_RETRIES); + return; + } + + // Grab the images + if (!grab()) + return; // Grab failed, return + + // Grab succeeded, notify threads waiting for new images + synchronized (grabNotification) + { + grabNotification.notifyAll(); + } + } +} diff --git a/ihmc-perception/src/main/java/us/ihmc/sensors/RealsenseImageSensor.java b/ihmc-perception/src/main/java/us/ihmc/sensors/RealsenseImageSensor.java new file mode 100644 index 00000000000..83eca7e2745 --- /dev/null +++ b/ihmc-perception/src/main/java/us/ihmc/sensors/RealsenseImageSensor.java @@ -0,0 +1,152 @@ +package us.ihmc.sensors; + +import org.bytedeco.opencv.global.opencv_core; +import org.bytedeco.opencv.opencv_core.Mat; +import us.ihmc.commons.thread.Throttler; +import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.log.LogTools; +import us.ihmc.perception.RawImage; +import us.ihmc.perception.realsense.RealsenseConfiguration; +import us.ihmc.perception.realsense.RealsenseDevice; +import us.ihmc.perception.realsense.RealsenseDeviceManager; + +import java.time.Instant; + +public class RealsenseImageSensor extends ImageSensor +{ + public static final int COLOR_IMAGE_KEY = 0; + public static final int DEPTH_IMAGE_KEY = 1; + public static final int OUTPUT_IMAGE_COUNT = 2; + + private static final double OUTPUT_FREQUENCY = 20.0; + + private final RealsenseConfiguration realsenseConfiguration; + private final RealsenseDeviceManager realsenseManager; + private RealsenseDevice realsense = null; + + private final RawImage[] grabbedImages = new RawImage[OUTPUT_IMAGE_COUNT]; + private long grabSequenceNumber = 0L; + private int grabFailureCount = 0; + + private final FramePose3D depthPose = new FramePose3D(); + private final FramePose3D colorPose = new FramePose3D(); + private final Throttler grabThrottler = new Throttler().setFrequency(OUTPUT_FREQUENCY); + + public RealsenseImageSensor(RealsenseDeviceManager realsenseManager, RealsenseConfiguration realsenseConfiguration) + { + super(realsenseConfiguration.name().split("_")[0]); + + this.realsenseManager = realsenseManager; + this.realsenseConfiguration = realsenseConfiguration; + } + + @Override + protected boolean startSensor() + { + if (realsense != null) + { + if (realsense.getDevice() != null) + realsense.deleteDevice(); + realsense = null; + } + + realsense = realsenseManager.createBytedecoRealsenseDevice(realsenseConfiguration); + + boolean success = realsense != null && realsense.getDevice() != null; + if (success) + { + LogTools.info("Initializing Realsense..."); + realsense.enableColor(realsenseConfiguration); + realsense.initialize(); + grabFailureCount = 0; + } + else + { + LogTools.error("Failed to initialize Realsense"); + } + + return success; + } + + @Override + public boolean isSensorRunning() + { + return realsense != null && realsense.getDevice() != null && grabFailureCount < OUTPUT_FREQUENCY; + } + + @Override + protected boolean grab() + { + grabThrottler.waitAndRun(); + + // Update the sensor poses + ReferenceFrame sensorFrame = sensorFrameSupplier.get(); + depthPose.setToZero(sensorFrame); + depthPose.changeFrame(ReferenceFrame.getWorldFrame()); + + colorPose.setIncludingFrame(sensorFrame, realsense.getDepthToColorTranslation(), realsense.getDepthToColorRotation()); + colorPose.invert(); + colorPose.changeFrame(ReferenceFrame.getWorldFrame()); + + // Read grabbed images + if (!realsense.readFrameData()) + { + grabFailureCount++; + return false; + } + Instant grabTime = Instant.now(); + grabSequenceNumber++; + + // Create mats with the images + realsense.updateDataBytePointers(); + Mat bgrImage = new Mat(realsense.getColorHeight(), realsense.getColorWidth(), opencv_core.CV_8UC3, realsense.getColorFrameData()); + Mat depthImage = new Mat(realsense.getDepthHeight(), realsense.getDepthWidth(), opencv_core.CV_16UC1, realsense.getDepthFrameData()); + + // Update grabbed images + synchronized (grabbedImages) + { + grabbedImages[COLOR_IMAGE_KEY] = RawImage.createWithBGRImage(bgrImage, + realsense.getColorCameraIntrinsics(), + new FramePose3D(colorPose), + grabTime, + grabSequenceNumber); + grabbedImages[DEPTH_IMAGE_KEY] = RawImage.createWith16BitDepth(depthImage, + realsense.getDepthCameraIntrinsics(), + new FramePose3D(depthPose), + grabTime, + grabSequenceNumber, + (float) realsense.getDepthDiscretization()); + } + + grabFailureCount = 0; + return true; + } + + @Override + public RawImage getImage(int imageKey) + { + synchronized (grabbedImages) + { + return new RawImage(grabbedImages[imageKey]); + } + } + + @Override + public void close() + { + System.out.println("Closing " + getClass().getSimpleName()); + super.close(); + + // Release the images + for (RawImage image : grabbedImages) + if (image != null) + image.release(); + + // Close the camera + if (realsense != null && realsense.getDevice() != null) + realsense.deleteDevice(); + + System.out.println("Closed " + getClass().getSimpleName()); + } +} diff --git a/ihmc-perception/src/main/java/us/ihmc/sensors/ZEDImageSensor.java b/ihmc-perception/src/main/java/us/ihmc/sensors/ZEDImageSensor.java new file mode 100644 index 00000000000..2e9dc903ccc --- /dev/null +++ b/ihmc-perception/src/main/java/us/ihmc/sensors/ZEDImageSensor.java @@ -0,0 +1,380 @@ +package us.ihmc.sensors; + +import org.bytedeco.javacpp.Pointer; +import org.bytedeco.opencv.opencv_core.GpuMat; +import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.euclid.tuple4D.Quaternion; +import us.ihmc.log.LogTools; +import us.ihmc.perception.CameraModel; +import us.ihmc.perception.RawImage; +import us.ihmc.perception.camera.CameraIntrinsics; +import us.ihmc.perception.imageMessage.PixelFormat; +import us.ihmc.robotics.referenceFrames.MutableReferenceFrame; +import us.ihmc.zed.SL_CalibrationParameters; +import us.ihmc.zed.SL_CameraInformation; +import us.ihmc.zed.SL_InitParameters; +import us.ihmc.zed.SL_PositionalTrackingParameters; +import us.ihmc.zed.SL_Quaternion; +import us.ihmc.zed.SL_RuntimeParameters; +import us.ihmc.zed.SL_Vector3; +import us.ihmc.zed.library.ZEDJavaAPINativeLibrary; + +import java.time.Instant; + +import static us.ihmc.zed.global.zed.*; + +public class ZEDImageSensor extends ImageSensor +{ + static + { + ZEDJavaAPINativeLibrary.load(); + } + + public static final int LEFT_COLOR_IMAGE_KEY = 0; + public static final int RIGHT_COLOR_IMAGE_KEY = 1; + public static final int DEPTH_IMAGE_KEY = 2; + + public static final int OUTPUT_IMAGE_COUNT = 3; + + protected static final int CAMERA_FPS = 30; + private static final float MILLIMETER_TO_METERS = 0.001f; + + private final int cameraID; + private final ZEDModelData zedModel; + private final int slInputType; + + private final RawImage[] grabbedImages = new RawImage[OUTPUT_IMAGE_COUNT]; + private final Pointer[] slMatPointers = new Pointer[OUTPUT_IMAGE_COUNT]; + private final CameraIntrinsics leftSensorIntrinsics = new CameraIntrinsics(); + private final CameraIntrinsics rightSensorIntrinsics = new CameraIntrinsics(); + private int imageWidth; + private int imageHeight; + + private float sensorCenterToCameraDistanceY = 0.0f; + private final FramePose3D leftSensorPose = new FramePose3D(); + private final FramePose3D rightSensorPose = new FramePose3D(); + + private long grabSequenceNumber = 0L; + private Instant lastGrabTime; + private boolean lastGrabFailed = false; + + protected final SL_InitParameters zedInitParameters = new SL_InitParameters(); + protected final SL_RuntimeParameters zedRuntimeParameters = new SL_RuntimeParameters(); + + private boolean positionalTrackingEnabled = false; + private final MutableReferenceFrame trackedSensorFrame = new MutableReferenceFrame(); + private final SL_Quaternion sensorRotation = new SL_Quaternion(); + private final SL_Vector3 sensorTranslation = new SL_Vector3(); + + public ZEDImageSensor(int cameraID, ZEDModelData zedModel, int slInputType) + { + super(zedModel.name()); + + this.cameraID = cameraID; + this.zedModel = zedModel; + this.slInputType = slInputType; + + // Set runtime parameters to default values + zedRuntimeParameters.reference_frame(SL_REFERENCE_FRAME_CAMERA); + zedRuntimeParameters.enable_depth(true); + zedRuntimeParameters.confidence_threshold(100); + zedRuntimeParameters.texture_confidence_threshold(100); + zedRuntimeParameters.remove_saturated_areas(true); + zedRuntimeParameters.enable_fill_mode(false); + } + + @Override + protected boolean startSensor() + { + try + { + if (sl_is_opened(cameraID)) + sl_close_camera(cameraID); + + sl_create_camera(cameraID); + + // Set the initialization parameters + setInitParameters(zedInitParameters); + + // Open the camera + int returnCode = openCamera(); + throwOnError(returnCode); + + if (positionalTrackingEnabled) + { + SL_PositionalTrackingParameters positionalTrackingParameters = new SL_PositionalTrackingParameters(); + sl_enable_positional_tracking(cameraID, positionalTrackingParameters, ""); + } + + // Get camera intrinsics + SL_CalibrationParameters sensorIntrinsics = sl_get_calibration_parameters(cameraID, false); + imageWidth = sl_get_width(cameraID); + imageHeight = sl_get_height(cameraID); + + leftSensorIntrinsics.setWidth(imageWidth); + leftSensorIntrinsics.setHeight(imageHeight); + leftSensorIntrinsics.setFx(sensorIntrinsics.left_cam().fx()); + leftSensorIntrinsics.setFy(sensorIntrinsics.left_cam().fy()); + leftSensorIntrinsics.setCx(sensorIntrinsics.left_cam().cx()); + leftSensorIntrinsics.setCy(sensorIntrinsics.left_cam().cy()); + + rightSensorIntrinsics.setWidth(imageWidth); + rightSensorIntrinsics.setHeight(imageHeight); + rightSensorIntrinsics.setFx(sensorIntrinsics.right_cam().fx()); + rightSensorIntrinsics.setFy(sensorIntrinsics.right_cam().fy()); + rightSensorIntrinsics.setCx(sensorIntrinsics.right_cam().cx()); + rightSensorIntrinsics.setCy(sensorIntrinsics.right_cam().cy()); + sensorIntrinsics.close(); + + // Get center to camera distance + SL_CameraInformation cameraInformation = sl_get_camera_information(cameraID, 0, 0); + sensorCenterToCameraDistanceY = 0.5f * cameraInformation.camera_configuration().calibration_parameters().translation().y(); + cameraInformation.close(); + + // Create image retrieval pointers + slMatPointers[LEFT_COLOR_IMAGE_KEY] = new Pointer(sl_mat_create_new(imageWidth, imageHeight, SL_MAT_TYPE_U8_C4, SL_MEM_GPU)); + slMatPointers[RIGHT_COLOR_IMAGE_KEY] = new Pointer(sl_mat_create_new(imageWidth, imageHeight, SL_MAT_TYPE_U8_C4, SL_MEM_GPU)); + slMatPointers[DEPTH_IMAGE_KEY] = new Pointer(sl_mat_create_new(imageWidth, imageHeight, SL_MAT_TYPE_U16_C1, SL_MEM_GPU)); + } + catch (ZEDException exception) + { + LogTools.error(exception); + return false; + } + + lastGrabFailed = false; + return true; + } + + protected void setInitParameters(SL_InitParameters parametersToSet) + { + parametersToSet.camera_fps(CAMERA_FPS); + parametersToSet.resolution(SL_RESOLUTION_HD720); + parametersToSet.input_type(slInputType); + parametersToSet.camera_device_id(cameraID); + parametersToSet.camera_image_flip(SL_FLIP_MODE_OFF); + parametersToSet.camera_disable_self_calib(false); + parametersToSet.enable_image_enhancement(true); + parametersToSet.depth_mode(SL_DEPTH_MODE_ULTRA); + parametersToSet.depth_stabilization(1); + parametersToSet.depth_maximum_distance(zedModel.getMaximumDepthDistance()); + parametersToSet.depth_minimum_distance(zedModel.getMinimumDepthDistance()); + parametersToSet.coordinate_unit(SL_UNIT_METER); + parametersToSet.coordinate_system(SL_COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD); + parametersToSet.sdk_gpu_id(-1); // Will find and use the best available GPU + parametersToSet.sdk_verbose(0); // false + parametersToSet.sensors_required(true); + parametersToSet.enable_right_side_measure(false); + parametersToSet.open_timeout_sec(5.0f); + parametersToSet.async_grab_camera_recovery(false); + } + + protected int openCamera() + { + return sl_open_camera(cameraID, zedInitParameters, 0, "", "", 0, "", "", ""); + } + + @Override + public boolean isSensorRunning() + { + return sl_is_opened(cameraID) && !lastGrabFailed; + } + + @Override + protected boolean grab() + { + // Update the sensor pose + ReferenceFrame sensorFrame = sensorFrameSupplier.get(); + + leftSensorPose.setToZero(sensorFrame); + leftSensorPose.getPosition().subY(sensorCenterToCameraDistanceY); + leftSensorPose.changeFrame(ReferenceFrame.getWorldFrame()); + + rightSensorPose.setToZero(sensorFrame); + rightSensorPose.getPosition().addY(sensorCenterToCameraDistanceY); + rightSensorPose.changeFrame(ReferenceFrame.getWorldFrame()); + + int returnCode; + try + { + // Grab images now + returnCode = sl_grab(cameraID, zedRuntimeParameters); + throwOnError(returnCode); + lastGrabTime = Instant.now(); + ++grabSequenceNumber; + + // Update tracked position if tracking enabled + if (positionalTrackingEnabled) + { + sl_get_position(cameraID, sensorRotation, sensorTranslation, SL_REFERENCE_FRAME_WORLD); + + Quaternion euclidRotation = new Quaternion(sensorRotation.x(), sensorRotation.y(), sensorRotation.z(), sensorRotation.w()); + Vector3D euclidTranslation = new Vector3D(sensorTranslation.x(), sensorTranslation.y(), sensorTranslation.z()); + if (!euclidRotation.containsNaN() && !euclidTranslation.containsNaN()) + trackedSensorFrame.update(transformToWorld -> transformToWorld.set(euclidRotation, euclidTranslation)); + } + + // Retrieve the grabbed depth image + Pointer depthImagePointer = slMatPointers[DEPTH_IMAGE_KEY]; + returnCode = sl_retrieve_measure(cameraID, depthImagePointer, SL_MEASURE_DEPTH_U16_MM, SL_MEM_GPU, imageWidth, imageHeight); + throwOnError(returnCode); + + // Retrieve the grabbed left color image + Pointer leftColorImagePointer = slMatPointers[LEFT_COLOR_IMAGE_KEY]; + returnCode = sl_retrieve_image(cameraID, leftColorImagePointer, SL_VIEW_LEFT, SL_MEM_GPU, imageWidth, imageHeight); + throwOnError(returnCode); + + // Retrieve the grabbed right color image + Pointer rightColorImagePointer = slMatPointers[RIGHT_COLOR_IMAGE_KEY]; + returnCode = sl_retrieve_image(cameraID, rightColorImagePointer, SL_VIEW_RIGHT, SL_MEM_GPU, imageWidth, imageHeight); + throwOnError(returnCode); + + synchronized (grabbedImages) + { // Create RawImages from the grabbed retrieved slMats + grabbedImages[LEFT_COLOR_IMAGE_KEY] = slMatToRawImage(leftColorImagePointer, PixelFormat.BGRA8, leftSensorIntrinsics, leftSensorPose); + grabbedImages[RIGHT_COLOR_IMAGE_KEY] = slMatToRawImage(rightColorImagePointer, PixelFormat.BGRA8, rightSensorIntrinsics, rightSensorPose); + grabbedImages[DEPTH_IMAGE_KEY] = slMatToRawImage(depthImagePointer, PixelFormat.GRAY16, leftSensorIntrinsics, leftSensorPose); + } + } + catch (ZEDException exception) + { + LogTools.error(exception); + lastGrabFailed = true; + return false; + } + + lastGrabFailed = false; + return true; + } + + private RawImage slMatToRawImage(Pointer slMatPointer, PixelFormat imagePixelFormat, CameraIntrinsics cameraIntrinsics, FramePose3D sensorPose) + { + GpuMat imageGpuMat = new GpuMat(imageHeight, + imageWidth, + imagePixelFormat.toOpenCVType(), + sl_mat_get_ptr(slMatPointer, SL_MEM_GPU), + sl_mat_get_step_bytes(slMatPointer, SL_MEM_GPU)); + return new RawImage(null, + imageGpuMat, + imagePixelFormat, + cameraIntrinsics, + CameraModel.PINHOLE, + new FramePose3D(sensorPose), + lastGrabTime, + grabSequenceNumber, + MILLIMETER_TO_METERS); + } + + @Override + public RawImage getImage(int imageKey) + { + synchronized (grabbedImages) + { + return grabbedImages[imageKey].get(); + } + } + + public ReferenceFrame getTrackedSensorFrame() + { + return trackedSensorFrame.getReferenceFrame(); + } + + public int getCameraID() + { + return cameraID; + } + + public void enablePositionalTracking(boolean enable) + { + positionalTrackingEnabled = enable; + } + + @Override + public void close() + { + System.out.println("Closing " + getClass().getSimpleName()); + super.close(); + + for (Pointer slMat : slMatPointers) + { + if (slMat != null && !slMat.isNull()) + { + sl_mat_free(slMat, SL_MEM_GPU); + slMat.close(); + } + } + + sl_close_camera(cameraID); + + System.out.println("Closed " + getClass().getSimpleName()); + } + + private void throwOnError(int errorCode) throws ZEDException + { + if (errorCode != SL_ERROR_CODE_SUCCESS) + throw new ZEDException(errorCode); + } + + private class ZEDException extends Exception + { + private final int zedErrorCode; + + public ZEDException(int zedErrorCode) + { + this.zedErrorCode = zedErrorCode; + } + + @Override + public String getMessage() + { + return "ZED Error (%d): %s".formatted(zedErrorCode, getZEDErrorName(zedErrorCode)); + } + } + + private String getZEDErrorName(int errorCode) + { + return switch (errorCode) + { + case SL_ERROR_CODE_CORRUPTED_FRAME -> "SL_ERROR_CODE_CORRUPTED_FRAME"; + case SL_ERROR_CODE_CAMERA_REBOOTING -> "SL_ERROR_CODE_CAMERA_REBOOTING"; + case SL_ERROR_CODE_SUCCESS -> "SL_ERROR_CODE_SUCCESS"; + case SL_ERROR_CODE_FAILURE -> "SL_ERROR_CODE_FAILURE"; + case SL_ERROR_CODE_NO_GPU_COMPATIBLE -> "SL_ERROR_CODE_NO_GPU_COMPATIBLE"; + case SL_ERROR_CODE_NOT_ENOUGH_GPU_MEMORY -> "SL_ERROR_CODE_NOT_ENOUGH_GPU_MEMORY"; + case SL_ERROR_CODE_CAMERA_NOT_DETECTED -> "SL_ERROR_CODE_CAMERA_NOT_DETECTED"; + case SL_ERROR_CODE_SENSORS_NOT_INITIALIZED -> "SL_ERROR_CODE_SENSORS_NOT_INITIALIZED"; + case SL_ERROR_CODE_SENSORS_NOT_AVAILABLE -> "SL_ERROR_CODE_SENSORS_NOT_AVAILABLE"; + case SL_ERROR_CODE_INVALID_RESOLUTION -> "SL_ERROR_CODE_INVALID_RESOLUTION"; + case SL_ERROR_CODE_LOW_USB_BANDWIDTH -> "SL_ERROR_CODE_LOW_USB_BANDWIDTH"; + case SL_ERROR_CODE_CALIBRATION_FILE_NOT_AVAILABLE -> "SL_ERROR_CODE_CALIBRATION_FILE_NOT_AVAILABLE"; + case SL_ERROR_CODE_INVALID_CALIBRATION_FILE -> "SL_ERROR_CODE_INVALID_CALIBRATION_FILE"; + case SL_ERROR_CODE_INVALID_SVO_FILE -> "SL_ERROR_CODE_INVALID_SVO_FILE"; + case SL_ERROR_CODE_SVO_RECORDING_ERROR -> "SL_ERROR_CODE_SVO_RECORDING_ERROR"; + case SL_ERROR_CODE_SVO_UNSUPPORTED_COMPRESSION -> "SL_ERROR_CODE_SVO_UNSUPPORTED_COMPRESSION"; + case SL_ERROR_CODE_END_OF_SVOFILE_REACHED -> "SL_ERROR_CODE_END_OF_SVOFILE_REACHED"; + case SL_ERROR_CODE_INVALID_COORDINATE_SYSTEM -> "SL_ERROR_CODE_INVALID_COORDINATE_SYSTEM"; + case SL_ERROR_CODE_INVALID_FIRMWARE -> "SL_ERROR_CODE_INVALID_FIRMWARE"; + case SL_ERROR_CODE_INVALID_FUNCTION_PARAMETERS -> "SL_ERROR_CODE_INVALID_FUNCTION_PARAMETERS"; + case SL_ERROR_CODE_CUDA_ERROR -> "SL_ERROR_CODE_CUDA_ERROR"; + case SL_ERROR_CODE_CAMERA_NOT_INITIALIZED -> "SL_ERROR_CODE_CAMERA_NOT_INITIALIZED"; + case SL_ERROR_CODE_NVIDIA_DRIVER_OUT_OF_DATE -> "SL_ERROR_CODE_NVIDIA_DRIVER_OUT_OF_DATE"; + case SL_ERROR_CODE_INVALID_FUNCTION_CALL -> "SL_ERROR_CODE_INVALID_FUNCTION_CALL"; + case SL_ERROR_CODE_CORRUPTED_SDK_INSTALLATION -> "SL_ERROR_CODE_CORRUPTED_SDK_INSTALLATION"; + case SL_ERROR_CODE_INCOMPATIBLE_SDK_VERSION -> "SL_ERROR_CODE_INCOMPATIBLE_SDK_VERSION"; + case SL_ERROR_CODE_INVALID_AREA_FILE -> "SL_ERROR_CODE_INVALID_AREA_FILE"; + case SL_ERROR_CODE_INCOMPATIBLE_AREA_FILE -> "SL_ERROR_CODE_INCOMPATIBLE_AREA_FILE"; + case SL_ERROR_CODE_CAMERA_FAILED_TO_SETUP -> "SL_ERROR_CODE_CAMERA_FAILED_TO_SETUP"; + case SL_ERROR_CODE_CAMERA_DETECTION_ISSUE -> "SL_ERROR_CODE_CAMERA_DETECTION_ISSUE"; + case SL_ERROR_CODE_CANNOT_START_CAMERA_STREAM -> "SL_ERROR_CODE_CANNOT_START_CAMERA_STREAM"; + case SL_ERROR_CODE_NO_GPU_DETECTED -> "SL_ERROR_CODE_NO_GPU_DETECTED"; + case SL_ERROR_CODE_PLANE_NOT_FOUND -> "SL_ERROR_CODE_PLANE_NOT_FOUND"; + case SL_ERROR_CODE_MODULE_NOT_COMPATIBLE_WITH_CAMERA -> "SL_ERROR_CODE_MODULE_NOT_COMPATIBLE_WITH_CAMERA"; + case SL_ERROR_CODE_MOTION_SENSORS_REQUIRED -> "SL_ERROR_CODE_MOTION_SENSORS_REQUIRED"; + case SL_ERROR_CODE_MODULE_NOT_COMPATIBLE_WITH_CUDA_VERSION -> "SL_ERROR_CODE_MODULE_NOT_COMPATIBLE_WITH_CUDA_VERSION"; + case SL_ERROR_CODE_SENSORS_DATA_REQUIRED -> "SL_ERROR_CODE_SENSORS_DATA_REQUIRED"; + default -> "UNKNOWN"; + }; + } +} diff --git a/ihmc-perception/src/main/java/us/ihmc/sensors/ZEDSVOPlaybackSensor.java b/ihmc-perception/src/main/java/us/ihmc/sensors/ZEDSVOPlaybackSensor.java new file mode 100644 index 00000000000..ee51ecfaafe --- /dev/null +++ b/ihmc-perception/src/main/java/us/ihmc/sensors/ZEDSVOPlaybackSensor.java @@ -0,0 +1,96 @@ +package us.ihmc.sensors; + +import perception_msgs.msg.dds.ZEDSVOCurrentFileMessage; +import us.ihmc.commons.thread.RepeatingTaskThread; +import us.ihmc.communication.PerceptionAPI; +import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; +import us.ihmc.zed.SL_InitParameters; + +import java.nio.file.Files; +import java.nio.file.Path; + +import static us.ihmc.zed.global.zed.*; + +public class ZEDSVOPlaybackSensor extends ZEDImageSensor +{ + private final String svoFileName; + private final ZEDSVOCurrentFileMessage svoStatusMessage = new ZEDSVOCurrentFileMessage(); + private final RepeatingTaskThread publishInfoThread; + + public ZEDSVOPlaybackSensor(ROS2PublishSubscribeAPI ros2, int cameraID, ZEDModelData zedModel, String svoFileName) + { + super(cameraID, zedModel, SL_INPUT_TYPE_SVO); + this.svoFileName = svoFileName; + + if (!Files.exists(Path.of(svoFileName))) + throw new RuntimeException("SVO file does not exist"); + + // Subscription to set position message + ros2.subscribeViaCallback(PerceptionAPI.ZED_SVO_SET_POSITION, position -> + { + setCurrentPosition((int) position.getData()); + if (getGrabThread().getScheduled() == 0) + getGrabThread().setScheduled(1); + }); + + // Subscribe for pause message + ros2.subscribeViaCallback(PerceptionAPI.ZED_SVO_PAUSE, () -> getGrabThread().stopRepeating()); + + // Subscribe to play message + ros2.subscribeViaCallback(PerceptionAPI.ZED_SVO_PLAY, () -> getGrabThread().startRepeating()); + + publishInfoThread = new RepeatingTaskThread("PublishSVOInfoThread", () -> + { + svoStatusMessage.setCurrentFileName(svoFileName); + svoStatusMessage.setRecordMode((byte) 1); // playback + svoStatusMessage.setCurrentPosition(getCurrentPosition()); + svoStatusMessage.setLength(getLength()); + + ros2.publish(PerceptionAPI.ZED_SVO_CURRENT_FILE, svoStatusMessage); + }).setFrequencyLimit(ZEDImageSensor.CAMERA_FPS); + publishInfoThread.start(); + } + + public void useTrackedPose(boolean useTrackedPose) + { + enablePositionalTracking(useTrackedPose); + if (useTrackedPose) + setSensorFrameSupplier(this::getTrackedSensorFrame); + } + + @Override + protected void setInitParameters(SL_InitParameters parametersToSet) + { + super.setInitParameters(parametersToSet); + parametersToSet.svo_real_time_mode(true); + } + + @Override + protected int openCamera() + { + return sl_open_camera(getCameraID(), zedInitParameters, 0, svoFileName, "", 0, "", "", ""); + } + + @Override + public void close() + { + publishInfoThread.blockingKill(); + + super.close(); + } + + public int getLength() + { + return sl_get_svo_number_of_frames(getCameraID()); + } + + public int getCurrentPosition() + { + return sl_get_svo_position(getCameraID()); + } + + public void setCurrentPosition(int position) + { + sl_set_svo_position(getCameraID(), position); + } +} From a45172c54e095da40a6cdf9f901aec72423f47eb Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Wed, 18 Dec 2024 13:21:15 -0600 Subject: [PATCH 13/16] Implement ROS 2 peer clock offset estimator (#532) --- ihmc-communication/build.gradle.kts | 1 + .../communication/packets/MessageTools.java | 23 +++ .../sync/ROS2PeerClockOffsetEstimator.java | 153 +++++++++++++++ .../ROS2PeerClockOffsetEstimatorPeer.java | 61 ++++++ .../ros2/sync/RDXROS2PeerClockTest.java | 117 +++++++++++ .../ROS2PeerClockOffsetEstimatorTest.java | 56 ++++++ .../RDXROS2PeerClockTest/ImGuiPanels.json | 7 + .../RDXROS2PeerClockTest/ImGuiSettings.ini | 24 +++ .../ihmc_common_msgs/msg/GuidMessage_.idl | 30 +++ .../PeerClockOffsetEstimatorPingMessage_.idl | 45 +++++ .../ihmc_common_msgs/msg/dds/GuidMessage.java | 139 +++++++++++++ .../msg/dds/GuidMessagePubSubType.java | 165 ++++++++++++++++ .../PeerClockOffsetEstimatorPingMessage.java | 177 +++++++++++++++++ ...kOffsetEstimatorPingMessagePubSubType.java | 184 ++++++++++++++++++ .../ihmc_common_msgs/msg/GuidMessage.msg | 7 + .../PeerClockOffsetEstimatorPingMessage.msg | 17 ++ .../ros1/ihmc_common_msgs/msg/GuidMessage.msg | 9 + .../PeerClockOffsetEstimatorPingMessage.msg | 19 ++ 18 files changed, 1234 insertions(+) create mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/ros2/sync/ROS2PeerClockOffsetEstimator.java create mode 100644 ihmc-communication/src/main/java/us/ihmc/communication/ros2/sync/ROS2PeerClockOffsetEstimatorPeer.java create mode 100644 ihmc-communication/src/test/java/us/ihmc/communication/ros2/sync/RDXROS2PeerClockTest.java create mode 100644 ihmc-communication/src/test/java/us/ihmc/communication/ros2/sync/ROS2PeerClockOffsetEstimatorTest.java create mode 100644 ihmc-communication/src/test/resources/configurations/RDXROS2PeerClockTest/ImGuiPanels.json create mode 100644 ihmc-communication/src/test/resources/configurations/RDXROS2PeerClockTest/ImGuiSettings.ini create mode 100644 ihmc-interfaces/src/main/generated-idl/ihmc_common_msgs/msg/GuidMessage_.idl create mode 100644 ihmc-interfaces/src/main/generated-idl/ihmc_common_msgs/msg/PeerClockOffsetEstimatorPingMessage_.idl create mode 100644 ihmc-interfaces/src/main/generated-java/ihmc_common_msgs/msg/dds/GuidMessage.java create mode 100644 ihmc-interfaces/src/main/generated-java/ihmc_common_msgs/msg/dds/GuidMessagePubSubType.java create mode 100644 ihmc-interfaces/src/main/generated-java/ihmc_common_msgs/msg/dds/PeerClockOffsetEstimatorPingMessage.java create mode 100644 ihmc-interfaces/src/main/generated-java/ihmc_common_msgs/msg/dds/PeerClockOffsetEstimatorPingMessagePubSubType.java create mode 100644 ihmc-interfaces/src/main/messages/ihmc_interfaces/ihmc_common_msgs/msg/GuidMessage.msg create mode 100644 ihmc-interfaces/src/main/messages/ihmc_interfaces/ihmc_common_msgs/msg/PeerClockOffsetEstimatorPingMessage.msg create mode 100644 ihmc-interfaces/src/main/messages/ros1/ihmc_common_msgs/msg/GuidMessage.msg create mode 100644 ihmc-interfaces/src/main/messages/ros1/ihmc_common_msgs/msg/PeerClockOffsetEstimatorPingMessage.msg diff --git a/ihmc-communication/build.gradle.kts b/ihmc-communication/build.gradle.kts index d4f9044bef7..96cbbf1e895 100644 --- a/ihmc-communication/build.gradle.kts +++ b/ihmc-communication/build.gradle.kts @@ -24,5 +24,6 @@ mainDependencies { testDependencies { api("us.ihmc:ihmc-robotics-toolkit-test:source") + api("us.ihmc:ihmc-graphics-libgdx:source") api("us.ihmc:ros2-library-test:1.1.4") } diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/packets/MessageTools.java b/ihmc-communication/src/main/java/us/ihmc/communication/packets/MessageTools.java index d09aa202bc9..7858649b003 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/packets/MessageTools.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/packets/MessageTools.java @@ -17,6 +17,7 @@ import ihmc_common_msgs.msg.dds.ConvexPolytope3DMessage; import ihmc_common_msgs.msg.dds.Cylinder3DMessage; import ihmc_common_msgs.msg.dds.Ellipsoid3DMessage; +import ihmc_common_msgs.msg.dds.GuidMessage; import ihmc_common_msgs.msg.dds.InstantMessage; import ihmc_common_msgs.msg.dds.PoseListMessage; import ihmc_common_msgs.msg.dds.Ramp3DMessage; @@ -82,6 +83,9 @@ import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyReadOnly; import us.ihmc.mecano.spatial.interfaces.TwistReadOnly; import us.ihmc.pubsub.TopicDataType; +import us.ihmc.pubsub.common.Guid; +import us.ihmc.pubsub.common.Guid.Entity; +import us.ihmc.pubsub.common.Guid.GuidPrefix; import us.ihmc.pubsub.common.SerializedPayload; import us.ihmc.robotics.lidar.LidarScanParameters; import us.ihmc.robotics.math.QuaternionCalculus; @@ -1422,6 +1426,25 @@ public static UUID toUUID(UUIDMessage uuidMessage) return new UUID(uuidMessage.getMostSignificantBits(), uuidMessage.getLeastSignificantBits()); } + public static void toMessage(Guid guid, GuidMessage guidMessage) + { + System.arraycopy(guid.getGuidPrefix().getValue(), 0, guidMessage.getPrefix(), 0, GuidPrefix.size); + System.arraycopy(guid.getEntity().getValue(), 0, guidMessage.getEntity(), 0, Entity.size); + } + + public static void fromMessage(GuidMessage guidMessage, Guid guid) + { + System.arraycopy(guidMessage.getPrefix(), 0, guid.getGuidPrefix().getValue(), 0, GuidPrefix.size); + System.arraycopy(guidMessage.getEntity(), 0, guid.getEntity().getValue(), 0, Entity.size); + } + + public static Guid toGuid(GuidMessage guidMessage) + { + Guid guid = new Guid(); + fromMessage(guidMessage, guid); + return guid; + } + public static double calculateDelay(ImageMessage imageMessage) { return TimeTools.calculateDelay(imageMessage.getAcquisitionTime().getSecondsSinceEpoch(), imageMessage.getAcquisitionTime().getAdditionalNanos()); diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/ros2/sync/ROS2PeerClockOffsetEstimator.java b/ihmc-communication/src/main/java/us/ihmc/communication/ros2/sync/ROS2PeerClockOffsetEstimator.java new file mode 100644 index 00000000000..bb9c76a6ceb --- /dev/null +++ b/ihmc-communication/src/main/java/us/ihmc/communication/ros2/sync/ROS2PeerClockOffsetEstimator.java @@ -0,0 +1,153 @@ +package us.ihmc.communication.ros2.sync; + +import ihmc_common_msgs.msg.dds.PeerClockOffsetEstimatorPingMessage; +import us.ihmc.commons.exception.DefaultExceptionHandler; +import us.ihmc.commons.exception.ExceptionTools; +import us.ihmc.commons.thread.RepeatingTaskThread; +import us.ihmc.commons.thread.ThreadTools; +import us.ihmc.communication.ROS2Tools; +import us.ihmc.communication.packets.MessageTools; +import us.ihmc.pubsub.common.Guid; +import us.ihmc.pubsub.common.SampleInfo; +import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2Publisher; +import us.ihmc.ros2.ROS2Subscription; +import us.ihmc.ros2.ROS2Topic; + +import java.time.Instant; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.concurrent.ExecutorService; +import java.util.concurrent.Executors; + +/** + * Sends out pings to other instances of this class on the network to estimate + * the clock offsets on those computers. This can be used to synchronize data + * modifications. We account for and assume a symmetric network delay. + */ +public class ROS2PeerClockOffsetEstimator +{ + private static final ROS2Topic TOPIC = ROS2Tools.IHMC_ROOT.withModule("peer_clock_offset_estimator") + .withType(PeerClockOffsetEstimatorPingMessage.class); + + private final HashMap peerMap = new HashMap<>(); + private final List peerList = new ArrayList<>(); + private int nextPeerToPing = 0; + private final ROS2Publisher publisher; + private final Guid ourGuid; + private final RepeatingTaskThread requestThread = new RepeatingTaskThread(getClass().getSimpleName(), + this::runRequestTask, + DefaultExceptionHandler.MESSAGE_AND_STACKTRACE); + private final ROS2Subscription subscription; + private final ExecutorService cachedThreadPool + = Executors.newCachedThreadPool(ThreadTools.createNamedThreadFactory(getClass().getSimpleName() + "PublishReply", true)); + private final PeerClockOffsetEstimatorPingMessage requestMessage = new PeerClockOffsetEstimatorPingMessage(); + private final PeerClockOffsetEstimatorPingMessage receivedMessage = new PeerClockOffsetEstimatorPingMessage(); + private final SampleInfo sampleInfo = new SampleInfo(); + private final Guid receivedRequestTarget = new Guid(); + private final Guid receivedReplyTarget = new Guid(); + + public ROS2PeerClockOffsetEstimator(ROS2Node ros2Node) + { + publisher = ros2Node.createPublisher(TOPIC); + ourGuid = publisher.getPublisher().getGuid(); + + subscription = ros2Node.createSubscription(TOPIC, subscriber -> + { + subscriber.takeNextData(receivedMessage, sampleInfo); + + MessageTools.fromMessage(receivedMessage.getRequestTarget(), receivedRequestTarget); + MessageTools.fromMessage(receivedMessage.getReplyTarget(), receivedReplyTarget); + + if (receivedMessage.getIsRequest() && receivedRequestTarget.equals(ourGuid)) // Reply + { + PeerClockOffsetEstimatorPingMessage replyMessage = new PeerClockOffsetEstimatorPingMessage(); + replyMessage.set(receivedMessage); + replyMessage.setIsRequest(false); + MessageTools.toMessage(Instant.now(), replyMessage.getReplySendTime()); + + cachedThreadPool.submit(() -> ExceptionTools.handle(() -> + { + synchronized (publisher) + { + publisher.publish(replyMessage); + } + }, DefaultExceptionHandler.MESSAGE_AND_STACKTRACE)); + } + else if (!receivedMessage.getIsRequest() && receivedReplyTarget.equals(ourGuid)) // Update clock offset estimate + { + ROS2PeerClockOffsetEstimatorPeer peer = peerMap.get(receivedRequestTarget); + if (peer != null) + { + peer.update(MessageTools.toInstant(receivedMessage.getRequestSendTime()), + Instant.now(), + MessageTools.toInstant(receivedMessage.getReplySendTime())); + } + } + }, (subscriber, info) -> + { + Guid guid = info.getGuid(); + if (!guid.equals(publisher.getPublisher().getGuid())) // Exclude our publisher + { + switch (info.getStatus()) + { + case MATCHED_MATCHING -> + { + if (!peerMap.containsKey(guid)) + { + Guid guidCopy = new Guid(); + guidCopy.set(guid); + + ROS2PeerClockOffsetEstimatorPeer peer = new ROS2PeerClockOffsetEstimatorPeer(guidCopy); + peerMap.put(guidCopy, peer); + peerList.add(peer); + } + } + case REMOVED_MATCHING -> + { + peerMap.remove(guid); + peerList.removeIf(peer -> peer.getGuid().equals(guid)); + } + } + } + }); + + requestThread.setFrequencyLimit(5.0); + requestThread.startRepeating(); + } + + private void runRequestTask() + { + if (!peerList.isEmpty()) + { + if (nextPeerToPing >= peerList.size()) + nextPeerToPing = 0; + + ROS2PeerClockOffsetEstimatorPeer peer = peerList.get(nextPeerToPing); + requestMessage.setIsRequest(true); + MessageTools.toMessage(peer.getGuid(), requestMessage.getRequestTarget()); + MessageTools.toMessage(ourGuid, requestMessage.getReplyTarget()); + MessageTools.toMessage(Instant.now(), requestMessage.getRequestSendTime()); + synchronized (publisher) + { + publisher.publish(requestMessage); + } + + ++nextPeerToPing; + } + } + + public void destroy() + { + subscription.remove(); + publisher.remove(); + cachedThreadPool.shutdownNow(); + requestThread.kill(); + } + + public List getPeerList() + { + return peerList; + } +} diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/ros2/sync/ROS2PeerClockOffsetEstimatorPeer.java b/ihmc-communication/src/main/java/us/ihmc/communication/ros2/sync/ROS2PeerClockOffsetEstimatorPeer.java new file mode 100644 index 00000000000..0bdbfa3774f --- /dev/null +++ b/ihmc-communication/src/main/java/us/ihmc/communication/ros2/sync/ROS2PeerClockOffsetEstimatorPeer.java @@ -0,0 +1,61 @@ +package us.ihmc.communication.ros2.sync; + +import us.ihmc.pubsub.common.Guid; + +import java.time.Duration; +import java.time.Instant; +import java.time.temporal.ChronoUnit; +import java.time.temporal.TemporalUnit; + +/** + * Keeps track of the clock offset of a peer. + * + * TODO: Add an alpha filter since the network latency and compute + * is noisy, but the clock rates are smooth. + */ +public class ROS2PeerClockOffsetEstimatorPeer +{ + private final Guid guid; + private Duration peerClockOffset; + private Instant replyReceiveTime; + + public ROS2PeerClockOffsetEstimatorPeer(Guid guid) + { + this.guid = guid; + } + + public void update(Instant requestSendTime, Instant replyReceiveTime, Instant peerClockTime) + { + this.replyReceiveTime = replyReceiveTime; + + Duration roundTripTime = Duration.between(requestSendTime, replyReceiveTime); + Duration halfRoundTripTime = roundTripTime.dividedBy(2); + Instant peerNow = peerClockTime.plus(halfRoundTripTime); + peerClockOffset = Duration.between(replyReceiveTime, peerNow); + } + + public Instant getPeerTimeInLocalFrame(Instant peerTime) + { + return peerTime.minus(peerClockOffset); + } + + public Instant getPeerTimeInPeerFrame(Instant ourTime) + { + return ourTime.plus(peerClockOffset); + } + + public boolean isAlive(Instant ourTime) + { + return replyReceiveTime != null && replyReceiveTime.isAfter(ourTime.minus(1, ChronoUnit.SECONDS)); + } + + public Duration getPeerClockOffset() + { + return peerClockOffset; + } + + public Guid getGuid() + { + return guid; + } +} diff --git a/ihmc-communication/src/test/java/us/ihmc/communication/ros2/sync/RDXROS2PeerClockTest.java b/ihmc-communication/src/test/java/us/ihmc/communication/ros2/sync/RDXROS2PeerClockTest.java new file mode 100644 index 00000000000..03db470ba5e --- /dev/null +++ b/ihmc-communication/src/test/java/us/ihmc/communication/ros2/sync/RDXROS2PeerClockTest.java @@ -0,0 +1,117 @@ +package us.ihmc.communication.ros2.sync; + +import imgui.ImGui; +import us.ihmc.commons.thread.Throttler; +import us.ihmc.pubsub.common.Guid; +import us.ihmc.rdx.Lwjgl3ApplicationAdapter; +import us.ihmc.rdx.imgui.ImGuiTools; +import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap; +import us.ihmc.rdx.ui.RDXBaseUI; +import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2NodeBuilder; + +import java.time.Instant; +import java.time.ZoneId; +import java.time.format.DateTimeFormatter; +import java.util.HashMap; +import java.util.Map; + +public class RDXROS2PeerClockTest +{ + public RDXROS2PeerClockTest() + { + RDXBaseUI baseUI = new RDXBaseUI(); + baseUI.launchRDXApplication(new Lwjgl3ApplicationAdapter() + { + private ROS2Node ros2Node; + private ROS2PeerClockOffsetEstimator clockEstimator; + private final DateTimeFormatter formatter = DateTimeFormatter.ofPattern("HH:mm:ss:SSS"); + private final Map plots = new HashMap<>(); + private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass()); + private final double history = 5.0; + private final int samples = 1000; + private final Throttler plotThottler = new Throttler().setPeriod(history / samples); + + @Override + public void create() + { + baseUI.create(); + baseUI.getImGuiPanelManager().addPanel("Main", this::renderImGuiWidgets); + + ros2Node = new ROS2NodeBuilder().build("peer_clock_test"); + clockEstimator = new ROS2PeerClockOffsetEstimator(ros2Node); + } + + @Override + public void render() + { + baseUI.renderBeforeOnScreenUI(); + baseUI.renderEnd(); + } + + @Override + public void dispose() + { + clockEstimator.destroy(); + ros2Node.destroy(); + baseUI.dispose(); + } + + private void renderImGuiWidgets() + { + Instant now = Instant.now(); + + ImGui.text("Current time:"); + ImGui.pushFont(ImGuiTools.getBigFont()); + ImGui.text(formatter.format(now.atZone(ZoneId.systemDefault()))); + ImGui.popFont(); + + boolean shiftPlots = plotThottler.run(); + + for (ROS2PeerClockOffsetEstimatorPeer peer : clockEstimator.getPeerList()) + { + if (peer.isAlive(now)) + { + ImGui.separator(); + ImGui.text("Peer %s time (peer frame):".formatted(peer.getGuid())); + ImGui.pushFont(ImGuiTools.getBigFont()); + ImGui.text(formatter.format(peer.getPeerTimeInPeerFrame(now).atZone(ZoneId.systemDefault()))); + ImGui.popFont(); + long offsetInMillis = peer.getPeerClockOffset().toMillis(); + ImGui.text("Offset (ms):"); + + float[] plot = plots.get(peer.getGuid()); + if (plot == null) + { + plot = new float[samples]; + plots.put(peer.getGuid(), plot); + } + + if (shiftPlots) + for (int i = 0; i < plot.length - 1; i++) + plot[i] = plot[i + 1]; + + plot[plot.length - 1] = offsetInMillis; + + ImGui.pushFont(ImGuiTools.getBigFont()); + ImGui.plotLines(labels.get("Offset (ms)", peer.getGuid().hashCode()), + plot, + samples, + 0, + "%d ms".formatted(offsetInMillis), + -30.0f, + 30.0f, + ImGui.getColumnWidth(), + 70.0f); + ImGui.popFont(); + } + } + } + }); + } + + public static void main(String[] args) + { + new RDXROS2PeerClockTest(); + } +} diff --git a/ihmc-communication/src/test/java/us/ihmc/communication/ros2/sync/ROS2PeerClockOffsetEstimatorTest.java b/ihmc-communication/src/test/java/us/ihmc/communication/ros2/sync/ROS2PeerClockOffsetEstimatorTest.java new file mode 100644 index 00000000000..6c8f49b421d --- /dev/null +++ b/ihmc-communication/src/test/java/us/ihmc/communication/ros2/sync/ROS2PeerClockOffsetEstimatorTest.java @@ -0,0 +1,56 @@ +package us.ihmc.communication.ros2.sync; + +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.Test; +import us.ihmc.commons.thread.ThreadTools; +import us.ihmc.log.LogTools; +import us.ihmc.robotics.time.TimeTools; +import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2NodeBuilder; +import us.ihmc.ros2.ROS2NodeBuilder.SpecialTransportMode; + +import java.time.Duration; +import java.time.Instant; + +public class ROS2PeerClockOffsetEstimatorTest +{ + @Test + public void test() + { + ROS2Node ros2Node0 = new ROS2NodeBuilder().specialTransportMode(SpecialTransportMode.INTRAPROCESS_ONLY).build("peer_clock_test0"); + ROS2Node ros2Node1 = new ROS2NodeBuilder().specialTransportMode(SpecialTransportMode.INTRAPROCESS_ONLY).build("peer_clock_test1"); + + ROS2PeerClockOffsetEstimator[] clocks = new ROS2PeerClockOffsetEstimator[3]; + clocks[0] = new ROS2PeerClockOffsetEstimator(ros2Node0); + clocks[1] = new ROS2PeerClockOffsetEstimator(ros2Node0); + clocks[2] = new ROS2PeerClockOffsetEstimator(ros2Node1); + + ThreadTools.park(0.5); + + for (int i = 0; i < 10; i++) + { + for (ROS2PeerClockOffsetEstimator clock : clocks) + Assertions.assertEquals(clocks.length - 1, clock.getPeerList().size()); + + for (ROS2PeerClockOffsetEstimator clock : clocks) + { + Duration offset = clock.getPeerList().get(0).getPeerClockOffset(); + LogTools.info("Clock offset: {}", offset); + Assertions.assertTrue(TimeTools.toDoubleSeconds(offset) < 0.1); + } + + for (ROS2PeerClockOffsetEstimator clock : clocks) + { + LogTools.info("Converted peer time: {}", clock.getPeerList().get(0).getPeerTimeInLocalFrame(Instant.now())); + } + + ThreadTools.park(0.3); + } + + for (ROS2PeerClockOffsetEstimator clock : clocks) + clock.destroy(); + + ros2Node0.destroy(); + ros2Node1.destroy(); + } +} diff --git a/ihmc-communication/src/test/resources/configurations/RDXROS2PeerClockTest/ImGuiPanels.json b/ihmc-communication/src/test/resources/configurations/RDXROS2PeerClockTest/ImGuiPanels.json new file mode 100644 index 00000000000..33a24b29810 --- /dev/null +++ b/ihmc-communication/src/test/resources/configurations/RDXROS2PeerClockTest/ImGuiPanels.json @@ -0,0 +1,7 @@ +{ + "dockspacePanels" : { }, + "windows" : { + "3D View" : false, + "Main" : true + } +} \ No newline at end of file diff --git a/ihmc-communication/src/test/resources/configurations/RDXROS2PeerClockTest/ImGuiSettings.ini b/ihmc-communication/src/test/resources/configurations/RDXROS2PeerClockTest/ImGuiSettings.ini new file mode 100644 index 00000000000..ef4c5f7f64e --- /dev/null +++ b/ihmc-communication/src/test/resources/configurations/RDXROS2PeerClockTest/ImGuiSettings.ini @@ -0,0 +1,24 @@ +[Window][DockSpaceViewport_11111111] +Pos=0,22 +Size=800,578 +Collapsed=0 + +[Window][Main] +Pos=0,22 +Size=800,578 +Collapsed=0 +DockId=0x8B93E3BD,0 + +[Window][Debug##Default] +Pos=60,60 +Size=400,400 +Collapsed=0 + +[Window][3D View] +Pos=132,100 +Size=300,200 +Collapsed=0 + +[Docking][Data] +DockSpace ID=0x8B93E3BD Window=0xA787BDB4 Pos=73,88 Size=800,578 CentralNode=1 Selected=0x1F1A625A + diff --git a/ihmc-interfaces/src/main/generated-idl/ihmc_common_msgs/msg/GuidMessage_.idl b/ihmc-interfaces/src/main/generated-idl/ihmc_common_msgs/msg/GuidMessage_.idl new file mode 100644 index 00000000000..1fe7bebf6d0 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-idl/ihmc_common_msgs/msg/GuidMessage_.idl @@ -0,0 +1,30 @@ +#ifndef __ihmc_common_msgs__msg__GuidMessage__idl__ +#define __ihmc_common_msgs__msg__GuidMessage__idl__ + +module ihmc_common_msgs +{ + module msg + { + module dds + { + + /** + * Represents a DDS-RTPS Guid + */ + @TypeCode(type="ihmc_common_msgs::msg::dds_::GuidMessage_") + struct GuidMessage + { + /** + * Prefix + */ + octet prefix[12]; + /** + * Entity + */ + octet entity[4]; + }; + }; + }; +}; + +#endif diff --git a/ihmc-interfaces/src/main/generated-idl/ihmc_common_msgs/msg/PeerClockOffsetEstimatorPingMessage_.idl b/ihmc-interfaces/src/main/generated-idl/ihmc_common_msgs/msg/PeerClockOffsetEstimatorPingMessage_.idl new file mode 100644 index 00000000000..60c0f1a61b3 --- /dev/null +++ b/ihmc-interfaces/src/main/generated-idl/ihmc_common_msgs/msg/PeerClockOffsetEstimatorPingMessage_.idl @@ -0,0 +1,45 @@ +#ifndef __ihmc_common_msgs__msg__PeerClockOffsetEstimatorPingMessage__idl__ +#define __ihmc_common_msgs__msg__PeerClockOffsetEstimatorPingMessage__idl__ + +#include "ihmc_common_msgs/msg/./GuidMessage_.idl" +#include "ihmc_common_msgs/msg/./InstantMessage_.idl" +module ihmc_common_msgs +{ + module msg + { + module dds + { + + /** + * This message is a request-reply ping used to estimate the clock offset + * of another node to be used by data synchronization algorithms. + */ + @TypeCode(type="ihmc_common_msgs::msg::dds_::PeerClockOffsetEstimatorPingMessage_") + struct PeerClockOffsetEstimatorPingMessage + { + /** + * If this is a request message, else it's a reply message + */ + boolean is_request; + /** + * The guid of the publisher that's expected to send back a reply + */ + ihmc_common_msgs::msg::dds::GuidMessage request_target; + /** + * The guid of the publisher associated with the requester + */ + ihmc_common_msgs::msg::dds::GuidMessage reply_target; + /** + * The time at which the request is sent + */ + ihmc_common_msgs::msg::dds::InstantMessage request_send_time; + /** + * The time at which the reply is sent + */ + ihmc_common_msgs::msg::dds::InstantMessage reply_send_time; + }; + }; + }; +}; + +#endif diff --git a/ihmc-interfaces/src/main/generated-java/ihmc_common_msgs/msg/dds/GuidMessage.java b/ihmc-interfaces/src/main/generated-java/ihmc_common_msgs/msg/dds/GuidMessage.java new file mode 100644 index 00000000000..93616a3a20c --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/ihmc_common_msgs/msg/dds/GuidMessage.java @@ -0,0 +1,139 @@ +package ihmc_common_msgs.msg.dds; + +import us.ihmc.communication.packets.Packet; +import us.ihmc.euclid.interfaces.Settable; +import us.ihmc.euclid.interfaces.EpsilonComparable; +import java.util.function.Supplier; +import us.ihmc.pubsub.TopicDataType; + +/** + * Represents a DDS-RTPS Guid + */ +public class GuidMessage extends Packet implements Settable, EpsilonComparable +{ + /** + * Prefix + */ + public byte[] prefix_; + /** + * Entity + */ + public byte[] entity_; + + public GuidMessage() + { + prefix_ = new byte[12]; + + entity_ = new byte[4]; + + } + + public GuidMessage(GuidMessage other) + { + this(); + set(other); + } + + public void set(GuidMessage other) + { + for(int i1 = 0; i1 < prefix_.length; ++i1) + { + prefix_[i1] = other.prefix_[i1]; + + } + + for(int i3 = 0; i3 < entity_.length; ++i3) + { + entity_[i3] = other.entity_[i3]; + + } + + } + + + /** + * Prefix + */ + public byte[] getPrefix() + { + return prefix_; + } + + + /** + * Entity + */ + public byte[] getEntity() + { + return entity_; + } + + + public static Supplier getPubSubType() + { + return GuidMessagePubSubType::new; + } + + @Override + public Supplier getPubSubTypePacket() + { + return GuidMessagePubSubType::new; + } + + @Override + public boolean epsilonEquals(GuidMessage other, double epsilon) + { + if(other == null) return false; + if(other == this) return true; + + for(int i5 = 0; i5 < prefix_.length; ++i5) + { + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.prefix_[i5], other.prefix_[i5], epsilon)) return false; + } + + for(int i7 = 0; i7 < entity_.length; ++i7) + { + if (!us.ihmc.idl.IDLTools.epsilonEqualsPrimitive(this.entity_[i7], other.entity_[i7], epsilon)) return false; + } + + + return true; + } + + @Override + public boolean equals(Object other) + { + if(other == null) return false; + if(other == this) return true; + if(!(other instanceof GuidMessage)) return false; + + GuidMessage otherMyClass = (GuidMessage) other; + + for(int i9 = 0; i9 < prefix_.length; ++i9) + { + if(this.prefix_[i9] != otherMyClass.prefix_[i9]) return false; + + } + for(int i11 = 0; i11 < entity_.length; ++i11) + { + if(this.entity_[i11] != otherMyClass.entity_[i11]) return false; + + } + + return true; + } + + @Override + public java.lang.String toString() + { + StringBuilder builder = new StringBuilder(); + + builder.append("GuidMessage {"); + builder.append("prefix="); + builder.append(java.util.Arrays.toString(this.prefix_)); builder.append(", "); + builder.append("entity="); + builder.append(java.util.Arrays.toString(this.entity_)); + builder.append("}"); + return builder.toString(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/ihmc_common_msgs/msg/dds/GuidMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/ihmc_common_msgs/msg/dds/GuidMessagePubSubType.java new file mode 100644 index 00000000000..4bdeea0bf5d --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/ihmc_common_msgs/msg/dds/GuidMessagePubSubType.java @@ -0,0 +1,165 @@ +package ihmc_common_msgs.msg.dds; + +/** +* +* Topic data type of the struct "GuidMessage" defined in "GuidMessage_.idl". Use this class to provide the TopicDataType to a Participant. +* +* This file was automatically generated from GuidMessage_.idl by us.ihmc.idl.generator.IDLGenerator. +* Do not update this file directly, edit GuidMessage_.idl instead. +* +*/ +public class GuidMessagePubSubType implements us.ihmc.pubsub.TopicDataType +{ + public static final java.lang.String name = "ihmc_common_msgs::msg::dds_::GuidMessage_"; + + @Override + public final java.lang.String getDefinitionChecksum() + { + return "5f61ddf5c243dbf8995931ed6448c975b957a0835684ac6064ec5e810f93607d"; + } + + @Override + public final java.lang.String getDefinitionVersion() + { + return "local"; + } + + private final us.ihmc.idl.CDR serializeCDR = new us.ihmc.idl.CDR(); + private final us.ihmc.idl.CDR deserializeCDR = new us.ihmc.idl.CDR(); + + @Override + public void serialize(ihmc_common_msgs.msg.dds.GuidMessage data, us.ihmc.pubsub.common.SerializedPayload serializedPayload) throws java.io.IOException + { + serializeCDR.serialize(serializedPayload); + write(data, serializeCDR); + serializeCDR.finishSerialize(); + } + + @Override + public void deserialize(us.ihmc.pubsub.common.SerializedPayload serializedPayload, ihmc_common_msgs.msg.dds.GuidMessage data) throws java.io.IOException + { + deserializeCDR.deserialize(serializedPayload); + read(data, deserializeCDR); + deserializeCDR.finishDeserialize(); + } + + public static int getMaxCdrSerializedSize() + { + return getMaxCdrSerializedSize(0); + } + + public static int getMaxCdrSerializedSize(int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += ((12) * 1) + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + current_alignment += ((4) * 1) + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + + return current_alignment - initial_alignment; + } + + public final static int getCdrSerializedSize(ihmc_common_msgs.msg.dds.GuidMessage data) + { + return getCdrSerializedSize(data, 0); + } + + public final static int getCdrSerializedSize(ihmc_common_msgs.msg.dds.GuidMessage data, int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += ((12) * 1) + us.ihmc.idl.CDR.alignment(current_alignment, 1); + current_alignment += ((4) * 1) + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + return current_alignment - initial_alignment; + } + + public static void write(ihmc_common_msgs.msg.dds.GuidMessage data, us.ihmc.idl.CDR cdr) + { + for(int i0 = 0; i0 < data.getPrefix().length; ++i0) + { + cdr.write_type_9(data.getPrefix()[i0]); + } + + for(int i0 = 0; i0 < data.getEntity().length; ++i0) + { + cdr.write_type_9(data.getEntity()[i0]); + } + + } + + public static void read(ihmc_common_msgs.msg.dds.GuidMessage data, us.ihmc.idl.CDR cdr) + { + for(int i0 = 0; i0 < data.getPrefix().length; ++i0) + { + data.getPrefix()[i0] = cdr.read_type_9(); + + } + + for(int i0 = 0; i0 < data.getEntity().length; ++i0) + { + data.getEntity()[i0] = cdr.read_type_9(); + + } + + + } + + @Override + public final void serialize(ihmc_common_msgs.msg.dds.GuidMessage data, us.ihmc.idl.InterchangeSerializer ser) + { + ser.write_type_f("prefix", data.getPrefix()); + ser.write_type_f("entity", data.getEntity()); + } + + @Override + public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, ihmc_common_msgs.msg.dds.GuidMessage data) + { + ser.read_type_f("prefix", data.getPrefix()); + ser.read_type_f("entity", data.getEntity()); + } + + public static void staticCopy(ihmc_common_msgs.msg.dds.GuidMessage src, ihmc_common_msgs.msg.dds.GuidMessage dest) + { + dest.set(src); + } + + @Override + public ihmc_common_msgs.msg.dds.GuidMessage createData() + { + return new ihmc_common_msgs.msg.dds.GuidMessage(); + } + @Override + public int getTypeSize() + { + return us.ihmc.idl.CDR.getTypeSize(getMaxCdrSerializedSize()); + } + + @Override + public java.lang.String getName() + { + return name; + } + + public void serialize(ihmc_common_msgs.msg.dds.GuidMessage data, us.ihmc.idl.CDR cdr) + { + write(data, cdr); + } + + public void deserialize(ihmc_common_msgs.msg.dds.GuidMessage data, us.ihmc.idl.CDR cdr) + { + read(data, cdr); + } + + public void copy(ihmc_common_msgs.msg.dds.GuidMessage src, ihmc_common_msgs.msg.dds.GuidMessage dest) + { + staticCopy(src, dest); + } + + @Override + public GuidMessagePubSubType newInstance() + { + return new GuidMessagePubSubType(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/ihmc_common_msgs/msg/dds/PeerClockOffsetEstimatorPingMessage.java b/ihmc-interfaces/src/main/generated-java/ihmc_common_msgs/msg/dds/PeerClockOffsetEstimatorPingMessage.java new file mode 100644 index 00000000000..2f28591b13a --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/ihmc_common_msgs/msg/dds/PeerClockOffsetEstimatorPingMessage.java @@ -0,0 +1,177 @@ +package ihmc_common_msgs.msg.dds; + +import us.ihmc.communication.packets.Packet; +import us.ihmc.euclid.interfaces.Settable; +import us.ihmc.euclid.interfaces.EpsilonComparable; +import java.util.function.Supplier; +import us.ihmc.pubsub.TopicDataType; + +/** + * This message is a request-reply ping used to estimate the clock offset + * of another node to be used by data synchronization algorithms. + */ +public class PeerClockOffsetEstimatorPingMessage extends Packet implements Settable, EpsilonComparable +{ + /** + * If this is a request message, else it's a reply message + */ + public boolean is_request_; + /** + * The guid of the publisher that's expected to send back a reply + */ + public ihmc_common_msgs.msg.dds.GuidMessage request_target_; + /** + * The guid of the publisher associated with the requester + */ + public ihmc_common_msgs.msg.dds.GuidMessage reply_target_; + /** + * The time at which the request is sent + */ + public ihmc_common_msgs.msg.dds.InstantMessage request_send_time_; + /** + * The time at which the reply is sent + */ + public ihmc_common_msgs.msg.dds.InstantMessage reply_send_time_; + + public PeerClockOffsetEstimatorPingMessage() + { + request_target_ = new ihmc_common_msgs.msg.dds.GuidMessage(); + reply_target_ = new ihmc_common_msgs.msg.dds.GuidMessage(); + request_send_time_ = new ihmc_common_msgs.msg.dds.InstantMessage(); + reply_send_time_ = new ihmc_common_msgs.msg.dds.InstantMessage(); + } + + public PeerClockOffsetEstimatorPingMessage(PeerClockOffsetEstimatorPingMessage other) + { + this(); + set(other); + } + + public void set(PeerClockOffsetEstimatorPingMessage other) + { + is_request_ = other.is_request_; + + ihmc_common_msgs.msg.dds.GuidMessagePubSubType.staticCopy(other.request_target_, request_target_); + ihmc_common_msgs.msg.dds.GuidMessagePubSubType.staticCopy(other.reply_target_, reply_target_); + ihmc_common_msgs.msg.dds.InstantMessagePubSubType.staticCopy(other.request_send_time_, request_send_time_); + ihmc_common_msgs.msg.dds.InstantMessagePubSubType.staticCopy(other.reply_send_time_, reply_send_time_); + } + + /** + * If this is a request message, else it's a reply message + */ + public void setIsRequest(boolean is_request) + { + is_request_ = is_request; + } + /** + * If this is a request message, else it's a reply message + */ + public boolean getIsRequest() + { + return is_request_; + } + + + /** + * The guid of the publisher that's expected to send back a reply + */ + public ihmc_common_msgs.msg.dds.GuidMessage getRequestTarget() + { + return request_target_; + } + + + /** + * The guid of the publisher associated with the requester + */ + public ihmc_common_msgs.msg.dds.GuidMessage getReplyTarget() + { + return reply_target_; + } + + + /** + * The time at which the request is sent + */ + public ihmc_common_msgs.msg.dds.InstantMessage getRequestSendTime() + { + return request_send_time_; + } + + + /** + * The time at which the reply is sent + */ + public ihmc_common_msgs.msg.dds.InstantMessage getReplySendTime() + { + return reply_send_time_; + } + + + public static Supplier getPubSubType() + { + return PeerClockOffsetEstimatorPingMessagePubSubType::new; + } + + @Override + public Supplier getPubSubTypePacket() + { + return PeerClockOffsetEstimatorPingMessagePubSubType::new; + } + + @Override + public boolean epsilonEquals(PeerClockOffsetEstimatorPingMessage other, double epsilon) + { + if(other == null) return false; + if(other == this) return true; + + if (!us.ihmc.idl.IDLTools.epsilonEqualsBoolean(this.is_request_, other.is_request_, epsilon)) return false; + + if (!this.request_target_.epsilonEquals(other.request_target_, epsilon)) return false; + if (!this.reply_target_.epsilonEquals(other.reply_target_, epsilon)) return false; + if (!this.request_send_time_.epsilonEquals(other.request_send_time_, epsilon)) return false; + if (!this.reply_send_time_.epsilonEquals(other.reply_send_time_, epsilon)) return false; + + return true; + } + + @Override + public boolean equals(Object other) + { + if(other == null) return false; + if(other == this) return true; + if(!(other instanceof PeerClockOffsetEstimatorPingMessage)) return false; + + PeerClockOffsetEstimatorPingMessage otherMyClass = (PeerClockOffsetEstimatorPingMessage) other; + + if(this.is_request_ != otherMyClass.is_request_) return false; + + if (!this.request_target_.equals(otherMyClass.request_target_)) return false; + if (!this.reply_target_.equals(otherMyClass.reply_target_)) return false; + if (!this.request_send_time_.equals(otherMyClass.request_send_time_)) return false; + if (!this.reply_send_time_.equals(otherMyClass.reply_send_time_)) return false; + + return true; + } + + @Override + public java.lang.String toString() + { + StringBuilder builder = new StringBuilder(); + + builder.append("PeerClockOffsetEstimatorPingMessage {"); + builder.append("is_request="); + builder.append(this.is_request_); builder.append(", "); + builder.append("request_target="); + builder.append(this.request_target_); builder.append(", "); + builder.append("reply_target="); + builder.append(this.reply_target_); builder.append(", "); + builder.append("request_send_time="); + builder.append(this.request_send_time_); builder.append(", "); + builder.append("reply_send_time="); + builder.append(this.reply_send_time_); + builder.append("}"); + return builder.toString(); + } +} diff --git a/ihmc-interfaces/src/main/generated-java/ihmc_common_msgs/msg/dds/PeerClockOffsetEstimatorPingMessagePubSubType.java b/ihmc-interfaces/src/main/generated-java/ihmc_common_msgs/msg/dds/PeerClockOffsetEstimatorPingMessagePubSubType.java new file mode 100644 index 00000000000..793f94d10cf --- /dev/null +++ b/ihmc-interfaces/src/main/generated-java/ihmc_common_msgs/msg/dds/PeerClockOffsetEstimatorPingMessagePubSubType.java @@ -0,0 +1,184 @@ +package ihmc_common_msgs.msg.dds; + +/** +* +* Topic data type of the struct "PeerClockOffsetEstimatorPingMessage" defined in "PeerClockOffsetEstimatorPingMessage_.idl". Use this class to provide the TopicDataType to a Participant. +* +* This file was automatically generated from PeerClockOffsetEstimatorPingMessage_.idl by us.ihmc.idl.generator.IDLGenerator. +* Do not update this file directly, edit PeerClockOffsetEstimatorPingMessage_.idl instead. +* +*/ +public class PeerClockOffsetEstimatorPingMessagePubSubType implements us.ihmc.pubsub.TopicDataType +{ + public static final java.lang.String name = "ihmc_common_msgs::msg::dds_::PeerClockOffsetEstimatorPingMessage_"; + + @Override + public final java.lang.String getDefinitionChecksum() + { + return "d21275a34208cc47d453c10c1d2d01990ee182f4a62b2de9c99f880e6b2bf166"; + } + + @Override + public final java.lang.String getDefinitionVersion() + { + return "local"; + } + + private final us.ihmc.idl.CDR serializeCDR = new us.ihmc.idl.CDR(); + private final us.ihmc.idl.CDR deserializeCDR = new us.ihmc.idl.CDR(); + + @Override + public void serialize(ihmc_common_msgs.msg.dds.PeerClockOffsetEstimatorPingMessage data, us.ihmc.pubsub.common.SerializedPayload serializedPayload) throws java.io.IOException + { + serializeCDR.serialize(serializedPayload); + write(data, serializeCDR); + serializeCDR.finishSerialize(); + } + + @Override + public void deserialize(us.ihmc.pubsub.common.SerializedPayload serializedPayload, ihmc_common_msgs.msg.dds.PeerClockOffsetEstimatorPingMessage data) throws java.io.IOException + { + deserializeCDR.deserialize(serializedPayload); + read(data, deserializeCDR); + deserializeCDR.finishDeserialize(); + } + + public static int getMaxCdrSerializedSize() + { + return getMaxCdrSerializedSize(0); + } + + public static int getMaxCdrSerializedSize(int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + current_alignment += ihmc_common_msgs.msg.dds.GuidMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += ihmc_common_msgs.msg.dds.GuidMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += ihmc_common_msgs.msg.dds.InstantMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + + current_alignment += ihmc_common_msgs.msg.dds.InstantMessagePubSubType.getMaxCdrSerializedSize(current_alignment); + + + return current_alignment - initial_alignment; + } + + public final static int getCdrSerializedSize(ihmc_common_msgs.msg.dds.PeerClockOffsetEstimatorPingMessage data) + { + return getCdrSerializedSize(data, 0); + } + + public final static int getCdrSerializedSize(ihmc_common_msgs.msg.dds.PeerClockOffsetEstimatorPingMessage data, int current_alignment) + { + int initial_alignment = current_alignment; + + current_alignment += 1 + us.ihmc.idl.CDR.alignment(current_alignment, 1); + + + current_alignment += ihmc_common_msgs.msg.dds.GuidMessagePubSubType.getCdrSerializedSize(data.getRequestTarget(), current_alignment); + + current_alignment += ihmc_common_msgs.msg.dds.GuidMessagePubSubType.getCdrSerializedSize(data.getReplyTarget(), current_alignment); + + current_alignment += ihmc_common_msgs.msg.dds.InstantMessagePubSubType.getCdrSerializedSize(data.getRequestSendTime(), current_alignment); + + current_alignment += ihmc_common_msgs.msg.dds.InstantMessagePubSubType.getCdrSerializedSize(data.getReplySendTime(), current_alignment); + + + return current_alignment - initial_alignment; + } + + public static void write(ihmc_common_msgs.msg.dds.PeerClockOffsetEstimatorPingMessage data, us.ihmc.idl.CDR cdr) + { + cdr.write_type_7(data.getIsRequest()); + + ihmc_common_msgs.msg.dds.GuidMessagePubSubType.write(data.getRequestTarget(), cdr); + ihmc_common_msgs.msg.dds.GuidMessagePubSubType.write(data.getReplyTarget(), cdr); + ihmc_common_msgs.msg.dds.InstantMessagePubSubType.write(data.getRequestSendTime(), cdr); + ihmc_common_msgs.msg.dds.InstantMessagePubSubType.write(data.getReplySendTime(), cdr); + } + + public static void read(ihmc_common_msgs.msg.dds.PeerClockOffsetEstimatorPingMessage data, us.ihmc.idl.CDR cdr) + { + data.setIsRequest(cdr.read_type_7()); + + ihmc_common_msgs.msg.dds.GuidMessagePubSubType.read(data.getRequestTarget(), cdr); + ihmc_common_msgs.msg.dds.GuidMessagePubSubType.read(data.getReplyTarget(), cdr); + ihmc_common_msgs.msg.dds.InstantMessagePubSubType.read(data.getRequestSendTime(), cdr); + ihmc_common_msgs.msg.dds.InstantMessagePubSubType.read(data.getReplySendTime(), cdr); + + } + + @Override + public final void serialize(ihmc_common_msgs.msg.dds.PeerClockOffsetEstimatorPingMessage data, us.ihmc.idl.InterchangeSerializer ser) + { + ser.write_type_7("is_request", data.getIsRequest()); + ser.write_type_a("request_target", new ihmc_common_msgs.msg.dds.GuidMessagePubSubType(), data.getRequestTarget()); + + ser.write_type_a("reply_target", new ihmc_common_msgs.msg.dds.GuidMessagePubSubType(), data.getReplyTarget()); + + ser.write_type_a("request_send_time", new ihmc_common_msgs.msg.dds.InstantMessagePubSubType(), data.getRequestSendTime()); + + ser.write_type_a("reply_send_time", new ihmc_common_msgs.msg.dds.InstantMessagePubSubType(), data.getReplySendTime()); + + } + + @Override + public final void deserialize(us.ihmc.idl.InterchangeSerializer ser, ihmc_common_msgs.msg.dds.PeerClockOffsetEstimatorPingMessage data) + { + data.setIsRequest(ser.read_type_7("is_request")); + ser.read_type_a("request_target", new ihmc_common_msgs.msg.dds.GuidMessagePubSubType(), data.getRequestTarget()); + + ser.read_type_a("reply_target", new ihmc_common_msgs.msg.dds.GuidMessagePubSubType(), data.getReplyTarget()); + + ser.read_type_a("request_send_time", new ihmc_common_msgs.msg.dds.InstantMessagePubSubType(), data.getRequestSendTime()); + + ser.read_type_a("reply_send_time", new ihmc_common_msgs.msg.dds.InstantMessagePubSubType(), data.getReplySendTime()); + + } + + public static void staticCopy(ihmc_common_msgs.msg.dds.PeerClockOffsetEstimatorPingMessage src, ihmc_common_msgs.msg.dds.PeerClockOffsetEstimatorPingMessage dest) + { + dest.set(src); + } + + @Override + public ihmc_common_msgs.msg.dds.PeerClockOffsetEstimatorPingMessage createData() + { + return new ihmc_common_msgs.msg.dds.PeerClockOffsetEstimatorPingMessage(); + } + @Override + public int getTypeSize() + { + return us.ihmc.idl.CDR.getTypeSize(getMaxCdrSerializedSize()); + } + + @Override + public java.lang.String getName() + { + return name; + } + + public void serialize(ihmc_common_msgs.msg.dds.PeerClockOffsetEstimatorPingMessage data, us.ihmc.idl.CDR cdr) + { + write(data, cdr); + } + + public void deserialize(ihmc_common_msgs.msg.dds.PeerClockOffsetEstimatorPingMessage data, us.ihmc.idl.CDR cdr) + { + read(data, cdr); + } + + public void copy(ihmc_common_msgs.msg.dds.PeerClockOffsetEstimatorPingMessage src, ihmc_common_msgs.msg.dds.PeerClockOffsetEstimatorPingMessage dest) + { + staticCopy(src, dest); + } + + @Override + public PeerClockOffsetEstimatorPingMessagePubSubType newInstance() + { + return new PeerClockOffsetEstimatorPingMessagePubSubType(); + } +} diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/ihmc_common_msgs/msg/GuidMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/ihmc_common_msgs/msg/GuidMessage.msg new file mode 100644 index 00000000000..427739a9e45 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/ihmc_common_msgs/msg/GuidMessage.msg @@ -0,0 +1,7 @@ +# Represents a DDS-RTPS Guid + +# Prefix +byte[12] prefix + +# Entity +byte[4] entity diff --git a/ihmc-interfaces/src/main/messages/ihmc_interfaces/ihmc_common_msgs/msg/PeerClockOffsetEstimatorPingMessage.msg b/ihmc-interfaces/src/main/messages/ihmc_interfaces/ihmc_common_msgs/msg/PeerClockOffsetEstimatorPingMessage.msg new file mode 100644 index 00000000000..4da98d6c8fc --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ihmc_interfaces/ihmc_common_msgs/msg/PeerClockOffsetEstimatorPingMessage.msg @@ -0,0 +1,17 @@ +# This message is a request-reply ping used to estimate the clock offset +# of another node to be used by data synchronization algorithms. + +# If this is a request message, else it's a reply message +bool is_request + +# The guid of the publisher that's expected to send back a reply +ihmc_common_msgs/GuidMessage request_target + +# The guid of the publisher associated with the requester +ihmc_common_msgs/GuidMessage reply_target + +# The time at which the request is sent +ihmc_common_msgs/InstantMessage request_send_time + +# The time at which the reply is sent +ihmc_common_msgs/InstantMessage reply_send_time diff --git a/ihmc-interfaces/src/main/messages/ros1/ihmc_common_msgs/msg/GuidMessage.msg b/ihmc-interfaces/src/main/messages/ros1/ihmc_common_msgs/msg/GuidMessage.msg new file mode 100644 index 00000000000..a367a4b165e --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ros1/ihmc_common_msgs/msg/GuidMessage.msg @@ -0,0 +1,9 @@ +# Represents a DDS-RTPS Guid + +# Prefix +int8[] prefix + +# Entity +int8[] entity + + diff --git a/ihmc-interfaces/src/main/messages/ros1/ihmc_common_msgs/msg/PeerClockOffsetEstimatorPingMessage.msg b/ihmc-interfaces/src/main/messages/ros1/ihmc_common_msgs/msg/PeerClockOffsetEstimatorPingMessage.msg new file mode 100644 index 00000000000..a035fc68c86 --- /dev/null +++ b/ihmc-interfaces/src/main/messages/ros1/ihmc_common_msgs/msg/PeerClockOffsetEstimatorPingMessage.msg @@ -0,0 +1,19 @@ +# This message is a request-reply ping used to estimate the clock offset +# of another node to be used by data synchronization algorithms. + +# If this is a request message, else it's a reply message +bool is_request + +# The guid of the publisher that's expected to send back a reply +ihmc_common_msgs/GuidMessage request_target + +# The guid of the publisher associated with the requester +ihmc_common_msgs/GuidMessage reply_target + +# The time at which the request is sent +ihmc_common_msgs/InstantMessage request_send_time + +# The time at which the reply is sent +ihmc_common_msgs/InstantMessage reply_send_time + + From b3100f832e1c54e9cd5ff4c289bae36d2762b68e Mon Sep 17 00:00:00 2001 From: ds58 <30220598+ds58@users.noreply.github.com> Date: Thu, 19 Dec 2024 11:56:23 -0600 Subject: [PATCH 14/16] :arrow_up: ihmc-ros2-library 1.1.5 (#550) --- ihmc-communication/build.gradle.kts | 4 ++-- ihmc-interfaces/build.gradle.kts | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ihmc-communication/build.gradle.kts b/ihmc-communication/build.gradle.kts index 96cbbf1e895..824e272a05e 100644 --- a/ihmc-communication/build.gradle.kts +++ b/ihmc-communication/build.gradle.kts @@ -13,7 +13,7 @@ ihmc { mainDependencies { api("us.ihmc:ihmc-realtime:1.7.0") api("us.ihmc:ihmc-video-codecs:2.1.6") - api("us.ihmc:ros2-library:1.1.4") + api("us.ihmc:ros2-library:1.1.5") api("commons-net:commons-net:3.6") api("org.lz4:lz4-java:1.8.0") @@ -25,5 +25,5 @@ mainDependencies { testDependencies { api("us.ihmc:ihmc-robotics-toolkit-test:source") api("us.ihmc:ihmc-graphics-libgdx:source") - api("us.ihmc:ros2-library-test:1.1.4") + api("us.ihmc:ros2-library-test:1.1.5") } diff --git a/ihmc-interfaces/build.gradle.kts b/ihmc-interfaces/build.gradle.kts index e5f2fbcb7d9..bffb6aeedb2 100644 --- a/ihmc-interfaces/build.gradle.kts +++ b/ihmc-interfaces/build.gradle.kts @@ -27,8 +27,8 @@ ihmc { mainDependencies { api("us.ihmc:euclid-geometry:0.21.0") - api("us.ihmc:ihmc-pub-sub:1.1.4") - api("us.ihmc:ros2-common-interfaces:1.1.4") { + api("us.ihmc:ihmc-pub-sub:1.1.5") + api("us.ihmc:ros2-common-interfaces:1.1.5") { exclude(group = "org.junit.jupiter", module = "junit-jupiter-api") exclude(group = "org.junit.jupiter", module = "junit-jupiter-engine") exclude(group = "org.junit.platform", module = "junit-platform-commons") @@ -38,13 +38,13 @@ mainDependencies { } testDependencies { - api("us.ihmc:ros2-library:1.1.4") + api("us.ihmc:ros2-library:1.1.5") } generatorDependencies { api("us.ihmc:euclid:0.21.0") api("us.ihmc:ihmc-commons:0.35.0") - api("us.ihmc:ros2-msg-to-pubsub-generator:1.1.4") + api("us.ihmc:ros2-msg-to-pubsub-generator:1.1.5") } val generator = us.ihmc.ros2.rosidl.ROS2InterfaceGenerator() From d460ab675418f4861dd11d9dcbcca0eaeac58c0c Mon Sep 17 00:00:00 2001 From: Nick Kitchel <59104880+PotatoPeeler3000@users.noreply.github.com> Date: Thu, 19 Dec 2024 13:21:50 -0600 Subject: [PATCH 15/16] Support OpenCL and CUDA within the RapidHeightMapManager (#545) * Created a RapidHeightMapExtractorInterface so that the manager can use CUDA or OpenCL depending on the hardware of the machine --- .../MonteCarloFootstepPlanningTest.java | 9 +++- .../perception/HumanoidPerceptionModule.java | 5 +- .../perception/RapidHeightMapManager.java | 54 +++++++++++++++---- .../RDXRapidHeightMapExtractorCUDADemo.java | 3 +- .../gpuHeightMap/RapidHeightMapExtractor.java | 35 ++++++------ .../RapidHeightMapExtractorCUDA.java | 35 ++++++------ .../RapidHeightMapExtractorInterface.java | 24 +++++++++ 7 files changed, 111 insertions(+), 54 deletions(-) create mode 100644 ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorInterface.java diff --git a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepPlanningTest.java b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepPlanningTest.java index d8301481ef9..b51b4769516 100644 --- a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepPlanningTest.java +++ b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepPlanningTest.java @@ -1,5 +1,7 @@ package us.ihmc.footstepPlanning.monteCarloPlanning; +import org.bytedeco.opencl.global.OpenCL; +import org.bytedeco.opencv.global.opencv_core; import org.bytedeco.opencv.opencv_core.Mat; import org.junit.jupiter.api.Assertions; import org.junit.jupiter.api.Disabled; @@ -13,6 +15,7 @@ import us.ihmc.footstepPlanning.tools.HeightMapTerrainGeneratorTools; import us.ihmc.footstepPlanning.tools.PlannerTools; import us.ihmc.log.LogTools; +import us.ihmc.perception.BytedecoImage; import us.ihmc.perception.camera.CameraIntrinsics; import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; import us.ihmc.perception.heightMap.TerrainMapData; @@ -28,12 +31,16 @@ public class MonteCarloFootstepPlanningTest // private TerrainPlanningDebugger debugger = new TerrainPlanningDebugger(null, plannerParameters); private MonteCarloFootstepPlanner planner = new MonteCarloFootstepPlanner(plannerParameters, PlannerTools.createFootPolygons(0.2, 0.1, 0.08)); private CameraIntrinsics cameraIntrinsics = new CameraIntrinsics(); - private RapidHeightMapExtractor heightMapExtractor = new RapidHeightMapExtractor(openCLManager); @Disabled @Test public void testMonteCarloFootstepPlanning() { + CameraIntrinsics depthImageIntrinsics = new CameraIntrinsics(); + BytedecoImage heightMapBytedecoImage = new BytedecoImage(depthImageIntrinsics.getWidth(), depthImageIntrinsics.getHeight(), opencv_core.CV_16UC1); + heightMapBytedecoImage.createOpenCLImage(openCLManager, OpenCL.CL_MEM_READ_WRITE); + RapidHeightMapExtractor heightMapExtractor = new RapidHeightMapExtractor(openCLManager, heightMapBytedecoImage, 1); + LogTools.info("Initializing"); RapidHeightMapExtractor.getHeightMapParameters().setInternalGlobalWidthInMeters(4.0); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/HumanoidPerceptionModule.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/HumanoidPerceptionModule.java index 0f30237c733..b2a3403dd67 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/HumanoidPerceptionModule.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/HumanoidPerceptionModule.java @@ -313,9 +313,10 @@ public void initializeHeightMapExtractor(ROS2Helper ros2Helper, HumanoidReferenc LogTools.info("Rapid Height Map: {}", cameraIntrinsics); rapidHeightMapExtractor = new RapidHeightMapExtractor(openCLManager, referenceFrames.getSoleFrame(RobotSide.LEFT), - referenceFrames.getSoleFrame(RobotSide.RIGHT)); + referenceFrames.getSoleFrame(RobotSide.RIGHT), + realsenseDepthImage, + 1); rapidHeightMapExtractor.setDepthIntrinsics(cameraIntrinsics); - rapidHeightMapExtractor.create(realsenseDepthImage, 1); if (ros2Helper != null) ros2Helper.subscribeViaVolatileCallback(PerceptionAPI.RESET_HEIGHT_MAP, message -> resetHeightMapRequested.set()); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java index 385cb0fe0b1..d1b3f067f2f 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java @@ -2,6 +2,7 @@ import controller_msgs.msg.dds.HighLevelStateChangeStatusMessage; import org.bytedeco.javacpp.BytePointer; +import org.bytedeco.opencl.global.OpenCL; import org.bytedeco.opencv.global.opencv_core; import org.bytedeco.opencv.opencv_core.GpuMat; import org.bytedeco.opencv.opencv_core.Mat; @@ -15,8 +16,10 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.transform.RigidBodyTransform; import us.ihmc.perception.camera.CameraIntrinsics; +import us.ihmc.perception.cuda.CUDATools; import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractorCUDA; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractorInterface; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.perception.opencl.OpenCLManager; import us.ihmc.perception.opencv.OpenCVTools; @@ -30,16 +33,19 @@ */ public class RapidHeightMapManager { - private final RapidHeightMapExtractorCUDA rapidHeightMapExtractor; + private final RapidHeightMapExtractorInterface rapidHeightMapExtractor; private final ImageMessage croppedHeightMapImageMessage = new ImageMessage(); private final FramePose3D cameraPoseForHeightMap = new FramePose3D(); private final RigidBodyTransform sensorToWorldForHeightMap = new RigidBodyTransform(); private final RigidBodyTransform sensorToGroundForHeightMap = new RigidBodyTransform(); private final RigidBodyTransform groundToWorldForHeightMap = new RigidBodyTransform(); - private final GpuMat deviceDepthImage; + private GpuMat deviceDepthImage; private final Mat hostDepthImage = new Mat(); + private BytedecoImage heightMapBytedecoImage; + private final Notification resetHeightMapRequested = new Notification(); private final BytePointer compressedCroppedHeightMapPointer = new BytePointer(); + private final boolean hasCUDAAvailable = CUDATools.hasCUDA(); public RapidHeightMapManager(OpenCLManager openCLManager, DRCRobotModel robotModel, @@ -48,11 +54,19 @@ public RapidHeightMapManager(OpenCLManager openCLManager, CameraIntrinsics depthImageIntrinsics, ROS2PublishSubscribeAPI ros2) { - rapidHeightMapExtractor = new RapidHeightMapExtractorCUDA(leftFootSoleFrame, rightFootSoleFrame); - rapidHeightMapExtractor.setDepthIntrinsics(depthImageIntrinsics); + if (hasCUDAAvailable) + { + deviceDepthImage = new GpuMat(depthImageIntrinsics.getWidth(), depthImageIntrinsics.getHeight(), opencv_core.CV_16UC1); + rapidHeightMapExtractor = new RapidHeightMapExtractorCUDA(leftFootSoleFrame, rightFootSoleFrame, deviceDepthImage, 1); + } + else + { + heightMapBytedecoImage = new BytedecoImage(depthImageIntrinsics.getWidth(), depthImageIntrinsics.getHeight(), opencv_core.CV_16UC1); + heightMapBytedecoImage.createOpenCLImage(openCLManager, OpenCL.CL_MEM_READ_WRITE); + rapidHeightMapExtractor = new RapidHeightMapExtractor(openCLManager, leftFootSoleFrame, rightFootSoleFrame, heightMapBytedecoImage, 1); + } - deviceDepthImage = new GpuMat(depthImageIntrinsics.getWidth(), depthImageIntrinsics.getHeight(), opencv_core.CV_16UC1); - rapidHeightMapExtractor.create(deviceDepthImage, 1); + rapidHeightMapExtractor.setDepthIntrinsics(depthImageIntrinsics); // We use a notification in order to only call resetting the height map in one place ros2.subscribeViaVolatileCallback(PerceptionAPI.RESET_HEIGHT_MAP, message -> resetHeightMapRequested.set()); @@ -72,12 +86,30 @@ public void update(Mat latestDepthImage, ReferenceFrame d455ZUpSensorFrame, ROS2PublishSubscribeAPI ros2) { - if (latestDepthImage.type() == opencv_core.CV_32FC1) // Support our simulated sensors - OpenCVTools.convertFloatToShort(latestDepthImage, hostDepthImage, 1000.0, 0.0); + if (hasCUDAAvailable) + { + if (latestDepthImage.type() == opencv_core.CV_32FC1) // Support our simulated sensors + { + OpenCVTools.convertFloatToShort(latestDepthImage, hostDepthImage, 1000.0, 0.0); + } + else + { + latestDepthImage.convertTo(hostDepthImage, opencv_core.CV_16UC1); + } + + deviceDepthImage.upload(hostDepthImage); + } else - latestDepthImage.convertTo(hostDepthImage, opencv_core.CV_16UC1); - - deviceDepthImage.upload(hostDepthImage); + { + if (latestDepthImage.type() == opencv_core.CV_32FC1) // Support our simulated sensors + { + OpenCVTools.convertFloatToShort(latestDepthImage, heightMapBytedecoImage.getBytedecoOpenCVMat(), 1000.0, 0.0); + } + else + { + latestDepthImage.convertTo(heightMapBytedecoImage.getBytedecoOpenCVMat(), opencv_core.CV_16UC1); + } + } if (resetHeightMapRequested.poll()) { diff --git a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXRapidHeightMapExtractorCUDADemo.java b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXRapidHeightMapExtractorCUDADemo.java index 473fb4f793e..ea08b7aa4bc 100644 --- a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXRapidHeightMapExtractorCUDADemo.java +++ b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXRapidHeightMapExtractorCUDADemo.java @@ -94,9 +94,8 @@ public void create() heightMapImage = new GpuMat(intrinsics.getWidth(), intrinsics.getHeight(), opencv_core.CV_16UC1); - extractor = new RapidHeightMapExtractorCUDA(leftFootSoleFrame, rightFootSoleFrame); + extractor = new RapidHeightMapExtractorCUDA(leftFootSoleFrame, rightFootSoleFrame, heightMapImage, 1); extractor.setDepthIntrinsics(intrinsics); - extractor.create(heightMapImage, 1); baseUI.getPrimaryScene().addRenderableProvider(heightMapGraphic, RDXSceneLevel.MODEL); } diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.java index d49ca09bbdf..9e3c508f185 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.java @@ -39,7 +39,7 @@ * for distance away from boundaries and edges for each cell). For more information on Distance Transform visit: * https://en.wikipedia.org/wiki/Distance_transform * */ -public class RapidHeightMapExtractor +public class RapidHeightMapExtractor implements RapidHeightMapExtractorInterface { private int mode = 1; // 0 -> Ouster, 1 -> Realsense private float gridOffsetX; @@ -119,18 +119,24 @@ public class RapidHeightMapExtractor private Mat denoisedHeightMapImage; private Rect cropWindowRectangle; - public RapidHeightMapExtractor(OpenCLManager openCLManager) + public RapidHeightMapExtractor(OpenCLManager openCLManager, ReferenceFrame leftFootSoleFrame, ReferenceFrame rightFootSoleFrame, BytedecoImage depthImage, int mode) { - this.openCLManager = openCLManager; - // denoiser = new HeightMapAutoencoder(); - rapidHeightMapUpdaterProgram = openCLManager.loadProgram("RapidHeightMapExtractor", "HeightMapUtils.cl"); + this(openCLManager, depthImage, mode); + footSoleFrames.put(RobotSide.LEFT, leftFootSoleFrame); + footSoleFrames.put(RobotSide.RIGHT, rightFootSoleFrame); } - public RapidHeightMapExtractor(OpenCLManager openCLManager, ReferenceFrame leftFootSoleFrame, ReferenceFrame rightFootSoleFrame) + public RapidHeightMapExtractor(OpenCLManager openCLManager, BytedecoImage depthImage, int mode) { - this(openCLManager); - footSoleFrames.put(RobotSide.LEFT, leftFootSoleFrame); - footSoleFrames.put(RobotSide.RIGHT, rightFootSoleFrame); + this.openCLManager = openCLManager; + + rapidHeightMapUpdaterProgram = openCLManager.loadProgram("RapidHeightMapExtractor", "HeightMapUtils.cl"); + + this.inputDepthImage = depthImage; + this.mode = mode; + + initialize(); + reset(); } public void initialize() @@ -187,15 +193,6 @@ public void initialize() } } - public void create(BytedecoImage depthImage, int mode) - { - this.inputDepthImage = depthImage; - this.mode = mode; - - initialize(); - reset(); - } - public void recomputeDerivedParameters() { centerIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getLocalWidthInMeters(), heightMapParameters.getLocalCellSizeInMeters()); @@ -796,7 +793,7 @@ public Mat getCroppedImage(Point3DReadOnly origin, int globalCenterIndex, Mat im return imageToCrop.apply(cropWindowRectangle); } - public static HeightMapData packHeightMapData(RapidHeightMapExtractor heightMapExtractor, HeightMapData heightMapDataToPack) + public static HeightMapData packHeightMapData(RapidHeightMapExtractorInterface heightMapExtractor, HeightMapData heightMapDataToPack) { Mat heightMapMat = heightMapExtractor.getTerrainMapData().getHeightMap(); HeightMapData latestHeightMapData = heightMapDataToPack; 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 071c7e67061..998053bf8a5 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 @@ -33,7 +33,7 @@ import static org.bytedeco.cuda.global.cudart.cudaFree; import static org.bytedeco.cuda.global.cudart.cudaStreamSynchronize; -public class RapidHeightMapExtractorCUDA +public class RapidHeightMapExtractorCUDA implements RapidHeightMapExtractorInterface { private static final int BLOCK_SIZE_XY = 32; private static HeightMapParameters heightMapParameters = new HeightMapParameters("GPU"); @@ -112,10 +112,23 @@ public class RapidHeightMapExtractorCUDA private float[] paramsArray; - public RapidHeightMapExtractorCUDA(ReferenceFrame leftFootSoleFrame, ReferenceFrame rightFootSoleFrame) + public RapidHeightMapExtractorCUDA(ReferenceFrame leftFootSoleFrame, ReferenceFrame rightFootSoleFrame, GpuMat depthImage, int mode) { footSoleFrames.put(RobotSide.LEFT, leftFootSoleFrame); footSoleFrames.put(RobotSide.RIGHT, rightFootSoleFrame); + + inputDepthImage = depthImage; + this.mode = mode; + + try + { + initialize(); + } + catch (URISyntaxException e) + { + throw new RuntimeException(e); + } + reset(); } public void setDepthIntrinsics(CameraIntrinsics cameraIntrinsics) @@ -248,22 +261,6 @@ public void reset() sequenceNumber = 0; } - public void create(GpuMat depthImage, int mode) - { - inputDepthImage = depthImage; - - this.mode = mode; - try - { - initialize(); - } - catch (URISyntaxException e) - { - throw new RuntimeException(e); - } - reset(); - } - public void update(RigidBodyTransform sensorToWorldTransform, RigidBodyTransform sensorToGroundTransform, RigidBodyTransform groundToWorldTransform) { int error; @@ -476,7 +473,7 @@ public void populateParameterArray(HeightMapParameters parameters, CameraIntrins (float) parameters.getFastSearchSize()}; } - public static HeightMapData packHeightMapData(RapidHeightMapExtractorCUDA heightMapExtractor, HeightMapData heightMapDataToPack) + public static HeightMapData packHeightMapData(RapidHeightMapExtractorInterface heightMapExtractor, HeightMapData heightMapDataToPack) { Mat heightMapMat = heightMapExtractor.getTerrainMapData().getHeightMap(); HeightMapData latestHeightMapData = heightMapDataToPack; diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorInterface.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorInterface.java new file mode 100644 index 00000000000..2fd3bb491c6 --- /dev/null +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorInterface.java @@ -0,0 +1,24 @@ +package us.ihmc.perception.gpuHeightMap; + +import us.ihmc.euclid.transform.RigidBodyTransform; +import us.ihmc.euclid.tuple3D.Point3D; +import us.ihmc.perception.camera.CameraIntrinsics; +import us.ihmc.perception.heightMap.TerrainMapData; + +public interface RapidHeightMapExtractorInterface +{ + void setDepthIntrinsics(CameraIntrinsics cameraIntrinsics); + + Point3D getSensorOrigin(); + + int getSequenceNumber(); + + TerrainMapData getTerrainMapData(); + + void update(RigidBodyTransform sensorToWorldTransform, RigidBodyTransform sensorToGroundTransform, RigidBodyTransform groundToWorldTransform); + + void reset(); + + void destroy(); + +} From 9be4064c044b8af69ea444cbc1ba9c518c876eeb Mon Sep 17 00:00:00 2001 From: Nick Kitchel <59104880+PotatoPeeler3000@users.noreply.github.com> Date: Thu, 19 Dec 2024 14:00:33 -0600 Subject: [PATCH 16/16] :arrow_up: ihmc-robot-data-logger 0.30.2 (#552) --- example-simulations/build.gradle.kts | 2 +- ihmc-robot-data-visualizer/build.gradle.kts | 2 +- ihmc-whole-body-controller/build.gradle | 2 +- robot-environment-awareness/build.gradle.kts | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/example-simulations/build.gradle.kts b/example-simulations/build.gradle.kts index cc85058e6fd..2acd3c02d07 100644 --- a/example-simulations/build.gradle.kts +++ b/example-simulations/build.gradle.kts @@ -11,7 +11,7 @@ ihmc { } mainDependencies { - api("us.ihmc:ihmc-robot-data-logger:0.29.9") + api("us.ihmc:ihmc-robot-data-logger:0.30.2") api("us.ihmc:ihmc-common-walking-control-modules:source") api("us.ihmc:ihmc-whole-body-controller:source") diff --git a/ihmc-robot-data-visualizer/build.gradle.kts b/ihmc-robot-data-visualizer/build.gradle.kts index 87fc9922ca2..0449ff3c2f4 100644 --- a/ihmc-robot-data-visualizer/build.gradle.kts +++ b/ihmc-robot-data-visualizer/build.gradle.kts @@ -19,7 +19,7 @@ mainDependencies { api("us.ihmc:simulation-construction-set:0.25.1") api("us.ihmc:ihmc-model-file-loader:source") api("us.ihmc:ihmc-humanoid-robotics:source") - api("us.ihmc:ihmc-robot-data-logger:0.29.9") + api("us.ihmc:ihmc-robot-data-logger:0.30.2") } testDependencies { diff --git a/ihmc-whole-body-controller/build.gradle b/ihmc-whole-body-controller/build.gradle index 75ca3f2a77a..e918f6ee8ec 100644 --- a/ihmc-whole-body-controller/build.gradle +++ b/ihmc-whole-body-controller/build.gradle @@ -16,7 +16,7 @@ ihmc { mainDependencies { api("us.ihmc:ihmc-common-walking-control-modules:source") api("us.ihmc:simulation-construction-set-tools:source") - api("us.ihmc:ihmc-robot-data-logger:0.29.9") + api("us.ihmc:ihmc-robot-data-logger:0.30.2") } benchmarksDependencies { diff --git a/robot-environment-awareness/build.gradle.kts b/robot-environment-awareness/build.gradle.kts index bf011a3c06b..ddd6fa35dd0 100644 --- a/robot-environment-awareness/build.gradle.kts +++ b/robot-environment-awareness/build.gradle.kts @@ -28,7 +28,7 @@ mainDependencies { api("us.ihmc:joctomap:1.12.5") api("us.ihmc:ihmc-graphics-javafx:source") api("us.ihmc:ihmc-messager-javafx:0.2.0") - api("us.ihmc:ihmc-robot-data-logger:0.29.9") + api("us.ihmc:ihmc-robot-data-logger:0.30.2") api("us.ihmc:ihmc-ros-tools:source") val openblasVersion = "0.3.23-1.5.9"