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 5ae047b78ad..ec32f05f2bc 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,6 +4,7 @@ import behavior_msgs.msg.dds.ContinuousWalkingStatusMessage; import controller_msgs.msg.dds.FootstepDataListMessage; import ihmc_common_msgs.msg.dds.PoseListMessage; +import std_msgs.msg.dds.Empty; import us.ihmc.communication.property.StoredPropertySetROS2TopicPair; import us.ihmc.ros2.ROS2Topic; @@ -16,8 +17,10 @@ public class ContinuousHikingAPI // Commands supported for the Continuous Hiking Process public static final ROS2Topic CONTINUOUS_HIKING_COMMAND = IHMC_ROOT.withModule(moduleName).withType(ContinuousHikingCommandMessage.class).withSuffix("command"); - 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 CLEAR_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(Empty.class).withSuffix("clear_goal_footsteps"); public static final ROS2Topic PLACED_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(PoseListMessage.class).withSuffix("placed_goal_footsteps"); + public static final ROS2Topic ROTATE_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(PoseListMessage.class).withSuffix("rotate_goal_footsteps"); + public static final ROS2Topic SQUARE_UP_STEP = IHMC_ROOT.withModule(moduleName).withType(Empty.class).withSuffix("rotate_goal_footsteps"); // Statuses supported from the Continuous Hiking Process public static final ROS2Topic CONTINUOUS_WALKING_STATUS = IHMC_ROOT.withModule(moduleName).withType(ContinuousWalkingStatusMessage.class).withSuffix("status"); 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 cd779a7e29f..0fbd5b0d918 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 @@ -18,6 +18,7 @@ import us.ihmc.perception.BytedecoImage; import us.ihmc.perception.camera.CameraIntrinsics; import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.perception.opencl.OpenCLManager; import us.ihmc.robotics.robotSide.RobotSide; @@ -39,12 +40,17 @@ public void testMonteCarloFootstepPlanning() CameraIntrinsics depthImageIntrinsics = new CameraIntrinsics(); BytedecoImage heightMapBytedecoImage = new BytedecoImage(depthImageIntrinsics.getWidth(), depthImageIntrinsics.getHeight(), opencv_core.CV_16UC1); heightMapBytedecoImage.createOpenCLImage(openCLManager, OpenCL.CL_MEM_READ_WRITE); - RapidHeightMapExtractor heightMapExtractor = new RapidHeightMapExtractor(openCLManager, heightMapBytedecoImage, cameraIntrinsics, 1); + + RapidHeightMapExtractor heightMapExtractor = new RapidHeightMapExtractor(openCLManager, + heightMapBytedecoImage, + cameraIntrinsics, + 1, + RapidHeightMapManager.getHeightMapParameters()); LogTools.info("Initializing"); - RapidHeightMapExtractor.getHeightMapParameters().setInternalGlobalWidthInMeters(4.0); - RapidHeightMapExtractor.getHeightMapParameters().setInternalGlobalCellSizeInMeters(0.02); + RapidHeightMapManager.getHeightMapParameters().setInternalGlobalWidthInMeters(4.0); + RapidHeightMapManager.getHeightMapParameters().setInternalGlobalCellSizeInMeters(0.02); heightMapExtractor.initialize(); heightMapExtractor.reset(); @@ -56,7 +62,7 @@ public void testMonteCarloFootstepPlanning() HeightMapTerrainGeneratorTools.fillWithSteppingStones(heightMap, 0.4f, 0.4f, 0.3f, 0.25f, 3); heightMapExtractor.getInternalGlobalHeightMapImage().writeOpenCLImage(openCLManager); - heightMapExtractor.populateParameterBuffers(RapidHeightMapExtractor.getHeightMapParameters(), cameraIntrinsics, new Point3D()); + heightMapExtractor.populateParameterBuffers(RapidHeightMapManager.getHeightMapParameters(), cameraIntrinsics, new Point3D()); heightMapExtractor.computeContactMap(); heightMapExtractor.readContactMapImage(); 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 b13fffca9df..b338a4b2fc5 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 @@ -14,12 +14,13 @@ import ihmc_common_msgs.msg.dds.PoseListMessage; import imgui.ImGui; import imgui.type.ImBoolean; +import std_msgs.msg.dds.Empty; import us.ihmc.avatar.drcRobot.DRCRobotModel; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; import us.ihmc.behaviors.activeMapping.ContinuousHikingLogger; import us.ihmc.behaviors.activeMapping.ContinuousHikingParameters; import us.ihmc.behaviors.activeMapping.ContinuousPlannerSchedulingTask; -import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; import us.ihmc.behaviors.activeMapping.StancePoseCalculator; import us.ihmc.commonWalkingControlModules.configurations.SwingTrajectoryParameters; import us.ihmc.commonWalkingControlModules.trajectories.PositionOptimizedTrajectoryGenerator; @@ -27,7 +28,6 @@ import us.ihmc.communication.packets.MessageTools; import us.ihmc.communication.property.ROS2StoredPropertySetGroup; import us.ihmc.communication.property.StoredPropertySetROS2TopicPair; -import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.euclid.Axis3D; import us.ihmc.euclid.geometry.ConvexPolygon2D; import us.ihmc.euclid.geometry.Pose3D; @@ -42,11 +42,12 @@ import us.ihmc.footstepPlanning.swing.SwingPlannerParametersBasics; import us.ihmc.footstepPlanning.tools.SwingPlannerTools; import us.ihmc.log.LogTools; +import us.ihmc.mecano.frames.MovingReferenceFrame; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.comms.PerceptionComms; -import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; -import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractorCUDA; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.rdx.imgui.ImGuiSliderDouble; +import us.ihmc.rdx.imgui.ImGuiTools; import us.ihmc.rdx.imgui.RDXPanel; import us.ihmc.rdx.input.ImGui3DViewInput; import us.ihmc.rdx.ui.ImGuiRemoteROS2StoredPropertySetGroup; @@ -72,10 +73,10 @@ 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 ROS2Publisher squareUpPublisher; private final ContinuousHikingCommandMessage commandMessage = new ContinuousHikingCommandMessage(); private final RDXStancePoseSelectionPanel stancePoseSelectionPanel; private final PositionOptimizedTrajectoryGenerator positionTrajectoryGenerator = new PositionOptimizedTrajectoryGenerator(numberOfKnotPoints, @@ -86,11 +87,16 @@ public class RDXContinuousHikingPanel extends RDXPanel implements RenderableProv private final ImGuiRemoteROS2StoredPropertySetGroup hostStoredPropertySets; private final ImGuiSliderDouble stepsBeforeSafetyStop = new ImGuiSliderDouble("Steps Before Safety Stop", "%.2f"); private final ImBoolean squareUpToGoal = new ImBoolean(false); + private final ImBoolean continuousHiking = 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 final ControllerFootstepQueueMonitor controllerFootstepQueueMonitor; private final ContinuousHikingLogger continuousHikingLogger; + private final ROS2Publisher planOffsetStatusPublisher; + private final ROS2Publisher footstepStatusMessagePublisher; + private final ROS2Publisher clearGoalFootstepsPublisher; + private final ContinuousHikingParameters continuousHikingParameters; private SideDependentList startStancePose = new SideDependentList<>(new FramePose3D(), new FramePose3D()); private FootstepPlan latestFootstepPlan; private List>> swingTrajectories; @@ -101,21 +107,27 @@ public class RDXContinuousHikingPanel extends RDXPanel implements RenderableProv private boolean publishAndSubscribe; private double simulatedDriftInMeters = -0.1; - public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, ROS2Helper ros2Helper, DRCRobotModel robotModel, ROS2SyncedRobotModel syncedRobotModel) + private final ROS2Publisher turningPublisher; + + public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, DRCRobotModel robotModel, ROS2SyncedRobotModel syncedRobotModel) { super("Continuous Hiking"); - this.ros2Helper = ros2Helper; setRenderMethod(this::renderImGuiWidgets); this.ros2Node = ros2Node; this.robotModel = robotModel; this.syncedRobotModel = syncedRobotModel; + footstepStatusMessagePublisher = ros2Node.createPublisher(getTopic(FootstepStatusMessage.class, robotModel.getSimpleRobotName())); + planOffsetStatusPublisher = ros2Node.createPublisher(getTopic(PlanOffsetStatus.class, robotModel.getSimpleRobotName())); + clearGoalFootstepsPublisher = ros2Node.createPublisher(ContinuousHikingAPI.CLEAR_GOAL_FOOTSTEPS); + ros2Node.createSubscription2(ContinuousHikingAPI.START_AND_GOAL_FOOTSTEPS, this::onStartAndGoalPosesReceived); ros2Node.createSubscription2(ContinuousHikingAPI.PLANNED_FOOTSTEPS, this::onPlannedFootstepsReceived); ros2Node.createSubscription2(ContinuousHikingAPI.MONTE_CARLO_FOOTSTEP_PLAN, this::onMonteCarloPlanReceived); commandPublisher = ros2Node.createPublisher(ContinuousHikingAPI.CONTINUOUS_HIKING_COMMAND); + squareUpPublisher = ros2Node.createPublisher(ContinuousHikingAPI.SQUARE_UP_STEP); SegmentDependentList> groundContactPoints = robotModel.getContactPointParameters().getControllerFootGroundContactPoints(); SideDependentList defaultContactPoints = new SideDependentList<>(); @@ -127,6 +139,8 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, ROS2Helper defaultContactPoints.put(robotSide, defaultFoothold); } + turningPublisher = ros2Node.createPublisher(ContinuousHikingAPI.ROTATE_GOAL_FOOTSTEPS); + StancePoseCalculator stancePoseCalculator = new StancePoseCalculator(defaultContactPoints); stancePoseSelectionPanel = new RDXStancePoseSelectionPanel(baseUI, ros2Node, stancePoseCalculator); addChild(stancePoseSelectionPanel); @@ -140,11 +154,11 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, ROS2Helper monteCarloPlannerParameters, robotModel.getContactPointParameters().getControllerFootGroundContactPoints()); - ros2Helper.subscribeViaCallback(HumanoidControllerAPI.getTopic(WalkingControllerFailureStatusMessage.class, robotModel.getSimpleRobotName()), - message -> terrainPlanningDebugger.reset()); + ros2Node.createSubscription(HumanoidControllerAPI.getTopic(WalkingControllerFailureStatusMessage.class, robotModel.getSimpleRobotName()), + (s) -> terrainPlanningDebugger.reset()); hostStoredPropertySets = new ImGuiRemoteROS2StoredPropertySetGroup(ros2Node); - ContinuousHikingParameters continuousHikingParameters = new ContinuousHikingParameters(); + continuousHikingParameters = new ContinuousHikingParameters(); createParametersPanel(continuousHikingParameters, continuousHikingParametersPanel, hostStoredPropertySets, @@ -162,21 +176,13 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, ROS2Helper RDXStoredPropertySetTuner swingPlannerParametersPanel = new RDXStoredPropertySetTuner("Swing Planner Parameters (CH)"); createParametersPanel(swingPlannerParameters, swingPlannerParametersPanel, hostStoredPropertySets, ContinuousHikingAPI.SWING_PLANNING_PARAMETERS); RDXStoredPropertySetTuner heightMapParametersPanel = new RDXStoredPropertySetTuner("Height Map Parameters (CH)"); - createParametersPanel(RapidHeightMapExtractor.getHeightMapParameters(), + createParametersPanel(RapidHeightMapManager.getHeightMapParameters(), heightMapParametersPanel, hostStoredPropertySets, PerceptionComms.HEIGHT_MAP_PARAMETERS); - RDXStoredPropertySetTuner heightMapParametersPanelCUDA = new RDXStoredPropertySetTuner("CUDA Height Map Parameters (CH)"); - createParametersPanel(RapidHeightMapExtractorCUDA.getHeightMapParameters(), - heightMapParametersPanelCUDA, - hostStoredPropertySets, - PerceptionComms.HEIGHT_MAP_PARAMETERS); continuousHikingLogger = new ContinuousHikingLogger(); - controllerFootstepQueueMonitor = new ControllerFootstepQueueMonitor(ros2Node, - robotModel.getSimpleRobotName(), - syncedRobotModel.getReferenceFrames(), - continuousHikingLogger); + controllerFootstepQueueMonitor = new ControllerFootstepQueueMonitor(ros2Node, robotModel.getSimpleRobotName()); } /** @@ -201,7 +207,6 @@ public void startContinuousPlannerSchedulingTask(boolean publishAndSubscribe) { this.publishAndSubscribe = publishAndSubscribe; runSubscriberOnly = true; - ROS2Helper ros2Helper = new ROS2Helper(ros2Node); clientStoredPropertySets = new ROS2StoredPropertySetGroup(ros2Node); // Add Continuous Hiking Parameters to be between the UI and this process @@ -222,6 +227,7 @@ public void startContinuousPlannerSchedulingTask(boolean publishAndSubscribe) continuousPlannerSchedulingTask = new ContinuousPlannerSchedulingTask(robotModel, ros2Node, + syncedRobotModel, syncedRobotModel.getReferenceFrames(), controllerFootstepQueueMonitor, continuousHikingLogger, @@ -279,16 +285,35 @@ else if (publishAndSubscribe) // Case 2 public void renderImGuiWidgets() { - ImGui.text("The ContinuousHikingProcess must be running"); - ImGui.text("And the enabled checkbox must be checked"); - ImGui.text("By holding CTRL the robot will walk forward"); - ImGui.separator(); - continuousHikingParametersPanel.renderImGuiWidgets(); + ImGui.checkbox("Enable Continuous Hiking", continuousHiking); + continuousHikingParameters.setStepPublisherEnabled(continuousHiking.get()); + + ImGui.checkbox("Square Up To Goal", squareUpToGoal); + + if (ImGui.button("Turn Left 90°")) + { + turnRobot(Math.PI / 2.0); + } + ImGui.sameLine(); + + if (ImGui.button("Turn Right 90°")) + { + turnRobot(-Math.PI / 2.0); + } + + if (ImGui.collapsingHeader("Continuous Hiking Parameters")) + { + continuousHikingParametersPanel.renderImGuiWidgets(); + } ImGui.separator(); ImGui.text("Options for Continuous Hiking Message"); ImGui.indent(); - ImGui.checkbox("Square Up To Goal", squareUpToGoal); + if (ImGui.button("Square Up")) + { + squareUpPublisher.publish(new Empty()); + } + ImGui.sameLine(); if (ImGui.button("Clear Planned footsteps")) { clearPlannedFootsteps(); @@ -305,14 +330,14 @@ public void renderImGuiWidgets() // Simulate that the controller started a step, this part triggers the drift offset kernel FootstepStatusMessage footstepStatusMessage = new FootstepStatusMessage(); footstepStatusMessage.setFootstepStatus(FootstepStatusMessage.FOOTSTEP_STATUS_STARTED); - ros2Helper.publish(getTopic(FootstepStatusMessage.class, "Nadia"), footstepStatusMessage); + footstepStatusMessagePublisher.publish(footstepStatusMessage); // Simulate that the controller has drifted by some z value PlanOffsetStatus planOffsetStatus = new PlanOffsetStatus(); Vector3D planOffset = new Vector3D(0, 0, simulatedDriftInMeters); planOffsetStatus.getOffsetVector().set(planOffset); LogTools.info("Plan Offset Status: " + planOffsetStatus.getOffsetVector()); - ros2Helper.publish(getTopic(PlanOffsetStatus.class, "Nadia"), planOffsetStatus); + planOffsetStatusPublisher.publish(planOffsetStatus); // The amount of drift that we want to simulation and adjust for if we do this over and over if (simulatedDriftInMeters > -1.0) @@ -352,7 +377,12 @@ else if (controllerConnected) } // Pressing this key will stop Continuous Hiking + // We allow for options for both stopping on the next step, and stopping when the queue expires, so a few more steps if (ImGui.getIO().getKeyAlt()) + { + publishStopContinuousHikingGracefully(); + } + else if (ImGui.isKeyPressed(ImGuiTools.getEscapeKey())) { publishStopContinuousHiking(); } @@ -372,7 +402,33 @@ public void getRenderables(Array renderables, Pool pool) public void clearPlannedFootsteps() { - ros2Helper.publish(ContinuousHikingAPI.CLEAR_GOAL_FOOTSTEPS); + clearGoalFootstepsPublisher.publish(new Empty()); + } + + public void turnRobot(double rotationRadians) + { + MovingReferenceFrame midFeetZUpFrame = syncedRobotModel.getReferenceFrames().getMidFeetZUpFrame(); + FramePose3D midFeetZUpPose = new FramePose3D(midFeetZUpFrame, midFeetZUpFrame.getTransformToWorldFrame()); + SideDependentList goalPoses = new SideDependentList<>(new FramePose3D(syncedRobotModel.getReferenceFrames().getMidFeetZUpFrame()), + new FramePose3D(syncedRobotModel.getReferenceFrames().getMidFeetZUpFrame())); + + midFeetZUpPose.appendYawRotation(rotationRadians); + goalPoses.get(RobotSide.RIGHT).set(midFeetZUpPose); + goalPoses.get(RobotSide.RIGHT).changeFrame(syncedRobotModel.getReferenceFrames().getMidFeetZUpFrame()); + goalPoses.get(RobotSide.RIGHT).appendTranslation(0, -0.15, 0); + + goalPoses.get(RobotSide.LEFT).set(midFeetZUpPose); + goalPoses.get(RobotSide.LEFT).changeFrame(syncedRobotModel.getReferenceFrames().getMidFeetZUpFrame()); + goalPoses.get(RobotSide.LEFT).appendTranslation(0, 0.15, 0); + + List poses = new ArrayList<>(); + poses.add(new Pose3D(goalPoses.get(RobotSide.LEFT))); + poses.add(new Pose3D(goalPoses.get(RobotSide.RIGHT))); + + PoseListMessage poseListMessage = new PoseListMessage(); + MessageTools.packPoseListMessage(poses, poseListMessage); + + turningPublisher.publish(poseListMessage); } /** @@ -432,6 +488,13 @@ private void publishStopContinuousHiking() commandPublisher.publish(commandMessage); } + private void publishStopContinuousHikingGracefully() + { + commandMessage.setEnableContinuousHiking(false); + commandMessage.setSquareUpToGoal(true); + commandPublisher.publish(commandMessage); + } + /** * Publish the status of the joystick controller. We define different buttons to perform different actions which get sent with the message. */ diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXHumanoidPerceptionUI.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXHumanoidPerceptionUI.java index 333b85896fc..2afb80b3a87 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXHumanoidPerceptionUI.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXHumanoidPerceptionUI.java @@ -9,8 +9,8 @@ import imgui.type.ImFloat; import org.bytedeco.opencv.opencv_core.Mat; import us.ihmc.communication.ros2.ROS2Helper; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.gpuHeightMap.HeatMapGenerator; -import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; import us.ihmc.perception.HumanoidPerceptionModule; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.rdx.imgui.RDXPanel; @@ -219,16 +219,16 @@ public void initializeHeightMapUI(ROS2Helper ros2Helper) // RDXImagePanel.DO_NOT_FLIP_Y); depthImagePanel = new RDXBytedecoImagePanel("Depth Image", - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), RDXImagePanel.DO_NOT_FLIP_Y); localHeightMapPanel = new RDXBytedecoImagePanel("Local Height Map", - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), RDXImagePanel.FLIP_Y); croppedHeightMapPanel = new RDXBytedecoImagePanel("Cropped Height Map", - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), RDXImagePanel.FLIP_Y); snapNormalZMapPanel = new RDXBytedecoImagePanel("Normal Z", humanoidPerception.getRapidHeightMapExtractor().getSnapNormalZImage().getBytedecoOpenCVMat().cols(), @@ -251,12 +251,12 @@ public void initializeHeightMapUI(ROS2Helper ros2Helper) // humanoidPerception.getRapidHeightMapExtractor().getSensorCroppedHeightMapImage().rows(), // RDXImagePanel.DO_NOT_FLIP_Y); terrainCostImagePanel = new RDXBytedecoImagePanel("Terrain Cost Image", - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), RDXImagePanel.FLIP_Y); contactMapImagePanel = new RDXBytedecoImagePanel("Contact Map Image", - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), RDXImagePanel.FLIP_Y); addChild(localHeightMapPanel.getImagePanel()); 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 8cc23d736e2..a5498af9508 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 @@ -46,6 +46,7 @@ public class RDXStancePoseSelectionPanel extends RDXPanel implements RenderableP private final RDXBaseUI baseUI; private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass()); private final ROS2Publisher publisher; + private final ROS2Publisher turningPublisher; private ModelInstance pickPointSphere; @@ -73,6 +74,7 @@ public RDXStancePoseSelectionPanel(RDXBaseUI baseUI, ROS2Node ros2Node, StancePo stancePoseCalculatorParametersTuner.create(stancePoseCalculator.getStancePoseParameters()); publisher = ros2Node.createPublisher(ContinuousHikingAPI.PLACED_GOAL_FOOTSTEPS); + turningPublisher = ros2Node.createPublisher(ContinuousHikingAPI.ROTATE_GOAL_FOOTSTEPS); SegmentDependentList> contactPoints = new SideDependentList<>(); contactPoints.set(RobotSide.LEFT, PlannerTools.createFootContactPoints(0.2, 0.1, 0.08)); @@ -210,6 +212,11 @@ else if (dScroll < 0.0) latestPickPoint.getOrientation().setYawPitchRoll(latestFootstepYaw + deltaYaw, 0.0, 0.0); } } + if (input.isWindowHovered() && input.mouseReleasedWithoutDrag(ImGuiMouseButton.Middle) && calculateStancePose.get() && selectionActive) + { + setRotateGoalFootsteps(); + selectionActive = false; + } if (input.isWindowHovered() & input.mouseReleasedWithoutDrag(ImGuiMouseButton.Left) && calculateStancePose.get() && selectionActive) { @@ -254,6 +261,18 @@ public void destroy() rightSpheres.clear(); } + private void setRotateGoalFootsteps() + { + List poses = new ArrayList<>(); + poses.add(new Pose3D(stancePoses.get(RobotSide.LEFT))); + poses.add(new Pose3D(stancePoses.get(RobotSide.RIGHT))); + + PoseListMessage poseListMessage = new PoseListMessage(); + MessageTools.packPoseListMessage(poses, poseListMessage); + + turningPublisher.publish(poseListMessage); + } + private void setGoalFootsteps() { List poses = new ArrayList<>(); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java index 7faeaaaddcc..60cf189d907 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java @@ -161,6 +161,7 @@ public class RDXHighLevelDepthSensorSimulator extends RDXPanel private Thread publishImagesThread; private volatile boolean publishImagesRunning; + private ROS2Publisher depthImagePublisher; public RDXHighLevelDepthSensorSimulator(String sensorName, ReferenceFrame sensorFrame, @@ -233,6 +234,9 @@ public void setupForROS2ImageMessages(ROS2Node ros2Node, ROS2Topic { this.ros2Node = ros2Node; this.ros2Helper = new ROS2Helper(ros2Node); + + depthImagePublisher = ros2Node.createPublisher(ros2DepthTopic); + this.ros2DepthTopic = ros2DepthTopic; this.ros2ColorTopic = ros2ColorTopic; @@ -572,7 +576,8 @@ public void publishROS2DepthImageMessage() sensorPose.setToZero(sensorFrame); sensorPose.changeFrame(ReferenceFrame.getWorldFrame()); OpenCVTools.compressImagePNG(depthImageMat, compressedDepthPointer); - PerceptionMessageTools.publishCompressedDepthImage(compressedDepthPointer, ros2DepthTopic, depthImageMessage, ros2Helper, sensorPose, now, depthSequenceNumber++, + + PerceptionMessageTools.publishCompressedDepthImage(compressedDepthPointer, depthImageMessage, depthImagePublisher, sensorPose, now, depthSequenceNumber++, depthSensorSimulator.getImageHeight(), depthSensorSimulator.getImageWidth(), 0.001f); }); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXHeightMapRenderer.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXHeightMapRenderer.java index 9ce9441ef59..9cea1bed6eb 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXHeightMapRenderer.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXHeightMapRenderer.java @@ -20,7 +20,6 @@ import us.ihmc.rdx.shader.RDXShader; import us.ihmc.rdx.shader.RDXUniform; -import java.nio.FloatBuffer; /** * Renders a height map as a point cloud. The height map is stored as a 16-bit grayscale image. @@ -29,13 +28,6 @@ * 0 is the metric -3.2768f height, and 65536 is the 3.2768f height. * The height is scaled up by 10,000 for storage as 16-bit value (short) */ - -/** - * This has been updated to use {@link us.ihmc.rdx.ui.graphics.RDXHeightMapGraphicNew}, please use that going forward, this implementation has bugs with - * interacting with collisions - * from the mouse - */ -@Deprecated public class RDXHeightMapRenderer implements RenderableProvider { private Renderable renderable; @@ -142,11 +134,6 @@ public void update(RigidBodyTransform zUpFrameToWorld, renderable.meshPart.mesh.setVertices(intermediateVertexBuffer, 0, totalCells * FLOATS_PER_CELL); } - public FloatBuffer getVertexBuffer() - { - return renderable.meshPart.mesh.getVerticesBuffer(); - } - @Override public void getRenderables(Array renderables, Pool pool) { diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2HeightMapVisualizer.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2HeightMapVisualizer.java index 182aa0e4db4..9ff7f37c035 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2HeightMapVisualizer.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2HeightMapVisualizer.java @@ -16,7 +16,7 @@ import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; import us.ihmc.euclid.transform.RigidBodyTransform; import us.ihmc.log.LogTools; -import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.perception.tools.NativeMemoryTools; import us.ihmc.perception.tools.PerceptionMessageTools; @@ -45,14 +45,14 @@ public class RDXROS2HeightMapVisualizer extends RDXROS2MultiTopicVisualizer private final RDXGlobalHeightMapGraphic globalHeightMapGraphic = new RDXGlobalHeightMapGraphic(); private final ResettableExceptionHandlingExecutorService executorService; - private final ImBoolean enableHeightMapVisualizer = new ImBoolean(true); + private final ImBoolean enableHeightMapVisualizer = new ImBoolean(false); private final ImBoolean enableGlobalHeightMapVisualizer = new ImBoolean(false); - private final ImBoolean enableHeightMapRenderer = new ImBoolean(false); + private final ImBoolean enableHeightMapRenderer = new ImBoolean(true); private final ImBoolean displayGlobalHeightMapImage = new ImBoolean(false); private final RigidBodyTransform zUpToWorldTransform = new RigidBodyTransform(); - private final TerrainMapData terrainMapData = new TerrainMapData(RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize()); + private final TerrainMapData terrainMapData = new TerrainMapData(RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize()); private ROS2PublishSubscribeAPI ros2; private HeightMapMessage latestHeightMapMessage = new HeightMapMessage(); @@ -62,7 +62,7 @@ public class RDXROS2HeightMapVisualizer extends RDXROS2MultiTopicVisualizer private ByteBuffer incomingCompressedImageBuffer; private BytePointer incomingCompressedImageBytePointer; - private int compressedBufferDefaultSize = 100000; + private final int compressedBufferDefaultSize = 1000000; private float pixelScalingFactor = 10000.0f; private boolean heightMapMessageGenerated = false; @@ -102,7 +102,7 @@ public void setupForGlobalHeightMapTileMessage(ROS2PublishSubscribeAPI ros2) public void create() { super.create(); - int cellsPerAxis = RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(); + int cellsPerAxis = RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(); heightMapRenderer.create(cellsPerAxis * cellsPerAxis); } @@ -167,10 +167,10 @@ public void acceptImageMessage(ImageMessage imageMessage) if (latestHeightMapData == null) { - latestHeightMapData = new HeightMapData(RapidHeightMapExtractor.getHeightMapParameters() - .getGlobalCellSizeInMeters(), - RapidHeightMapExtractor.getHeightMapParameters() - .getGlobalWidthInMeters(), + latestHeightMapData = new HeightMapData(RapidHeightMapManager.getHeightMapParameters() + .getGlobalCellSizeInMeters(), + RapidHeightMapManager.getHeightMapParameters() + .getGlobalWidthInMeters(), imageMessage.getPosition().getX(), imageMessage.getPosition().getY()); } @@ -186,14 +186,13 @@ public void acceptImageMessage(ImageMessage imageMessage) PerceptionMessageTools.convertToHeightMapData(heightMapImage, latestHeightMapData, imageMessage.getPosition(), - (float) RapidHeightMapExtractor.getHeightMapParameters() - .getGlobalWidthInMeters(), - (float) RapidHeightMapExtractor.getHeightMapParameters() - .getGlobalCellSizeInMeters()); + (float) RapidHeightMapManager.getHeightMapParameters() + .getGlobalWidthInMeters(), + (float) RapidHeightMapManager.getHeightMapParameters() + .getGlobalCellSizeInMeters()); latestHeightMapMessage = HeightMapMessageTools.toMessage(latestHeightMapData); heightMapMessageGenerated = true; } - }); } @@ -251,14 +250,13 @@ public void update() { if (heightMapImage.ptr(0) != null) { - //PerceptionDebugTools.printMat("Height Map Image", heightMapImage, 10); heightMapRenderer.update(zUpToWorldTransform, heightMapImage.ptr(0), - (float) RapidHeightMapExtractor.getHeightMapParameters().getHeightOffset(), + (float) RapidHeightMapManager.getHeightMapParameters().getHeightOffset(), zUpToWorldTransform.getTranslation().getX32(), zUpToWorldTransform.getTranslation().getY32(), heightMapImage.rows() / 2, - (float) RapidHeightMapExtractor.getHeightMapParameters().getGlobalCellSizeInMeters(), + (float) RapidHeightMapManager.getHeightMapParameters().getGlobalCellSizeInMeters(), pixelScalingFactor); } } diff --git a/ihmc-high-level-behaviors/src/libgdx/resources/environments/CinderBlockPallet.json b/ihmc-high-level-behaviors/src/libgdx/resources/environments/CinderBlockPallet.json index 52fcb505cb8..f1b6fe027bf 100644 --- a/ihmc-high-level-behaviors/src/libgdx/resources/environments/CinderBlockPallet.json +++ b/ihmc-high-level-behaviors/src/libgdx/resources/environments/CinderBlockPallet.json @@ -102,56 +102,56 @@ }, { "type" : "RDXPalletObject", "x" : 2.4930577278137207, - "y" : 0.06989191472530365, - "z" : 0.1110124127939346, - "qx" : 0.00856238873690151, + "y" : 0.06989191472530099, + "z" : 0.08732389168723786, + "qx" : 0.008562388736901518, "qy" : -0.005114153010419687, - "qz" : 0.7018977765776746, - "qs" : 0.7122078644422724 + "qz" : 0.7018977765776755, + "qs" : 0.7122078644422707 }, { "type" : "RDXPalletObject", "x" : 3.513807535171509, - "y" : 0.04207326173782348, - "z" : 0.1237301762215778, - "qx" : 0.7094953522455109, - "qy" : 0.7046140939793347, - "qz" : 0.0030468615931168426, - "qs" : -0.011226768997909059 + "y" : 0.04207326173781967, + "z" : 0.08234225453700866, + "qx" : 0.7094654357241527, + "qy" : 0.7046729978293471, + "qz" : 0.007336564946010361, + "qs" : -0.006398160495962346 }, { "type" : "RDXMediumCinderBlockRoughed", - "x" : 2.8034467697143555, - "y" : -0.26781320571899414, - "z" : 0.3262125167995693, - "qx" : -0.10285762375856672, - "qy" : 0.09713503866937312, - "qz" : 0.7006268231279272, - "qs" : 0.6993619579381686 + "x" : 2.803446769714356, + "y" : -0.26781320571898903, + "z" : 0.2791487080525257, + "qx" : -0.10285762375856766, + "qy" : 0.097135038669374, + "qz" : 0.7006268231279288, + "qs" : 0.6993619579381668 }, { "type" : "RDXMediumCinderBlockRoughed", - "x" : 2.6078357696533203, - "y" : -0.2656903862953186, - "z" : 0.32625531796365964, - "qx" : -0.10647845066123686, - "qy" : 0.10638572963805844, - "qz" : 0.6993553654921224, - "qs" : 0.6987463694585758 + "x" : 2.61168138422286, + "y" : -0.2656870360892501, + "z" : 0.2783433061531242, + "qx" : -0.10647845066123718, + "qy" : 0.10638572963805866, + "qz" : 0.6993553654921232, + "qs" : 0.6987463694585748 }, { "type" : "RDXMediumCinderBlockRoughed", "x" : 2.6733238697052, - "y" : 0.23532751202583313, - "z" : 0.30945813106372944, + "y" : 0.2353275120258309, + "z" : 0.23830242759757592, "qx" : 0.0, "qy" : 0.0, - "qz" : 0.2976377221605589, + "qz" : 0.29763772216055895, "qs" : 0.954678891746892 }, { "type" : "RDXMediumCinderBlockRoughed", - "x" : 2.5680363178253174, - "y" : 0.41301260069012646, - "z" : 0.3042421745136381, - "qx" : -4.259043794782986E-5, - "qy" : 1.3876773247399921E-4, - "qz" : 0.2934102624957087, + "x" : 2.5662500681208926, + "y" : 0.389346157233353, + "z" : 0.2386235305666895, + "qx" : -4.259043794782999E-5, + "qy" : 1.387677324739995E-4, + "qz" : 0.2934102624957088, "qs" : 0.9559866091069321 }, { "type" : "RDXMediumCinderBlockRoughed", @@ -264,47 +264,47 @@ }, { "type" : "RDXMediumCinderBlockRoughed", "x" : 3.216216802597046, - "y" : 0.4195704162120819, - "z" : 0.3607892571017146, - "qx" : 0.09720753880522989, - "qy" : 0.09797572562251415, - "qz" : -0.6975779785814231, - "qs" : 0.7030906167674457 + "y" : 0.41957041621207125, + "z" : 0.2886322178312223, + "qx" : 0.09720753880522992, + "qy" : 0.09797572562251418, + "qz" : -0.6975779785814232, + "qs" : 0.7030906167674456 }, { "type" : "RDXMediumCinderBlockRoughed", "x" : 3.4069573879241943, - "y" : 0.41641926765441895, - "z" : 0.3601300859823824, - "qx" : 0.10611619192654026, - "qy" : 0.09930293387417374, - "qz" : -0.6917497248948593, - "qs" : 0.7073617173998588 + "y" : 0.41641926765440784, + "z" : 0.288369582091613, + "qx" : 0.10611619192654027, + "qy" : 0.09930293387417376, + "qz" : -0.6917497248948595, + "qs" : 0.7073617173998586 }, { "type" : "RDXMediumCinderBlockRoughed", - "x" : 3.5369553565979004, - "y" : -0.34754684567451477, - "z" : 0.3093220075592398, - "qx" : 0.0, - "qy" : 0.0, - "qz" : -0.3582957156911485, - "qs" : 0.9336081512697754 + "x" : 3.5393702227098975, + "y" : -0.34539638462598843, + "z" : 0.22806068928635884, + "qx" : -0.0028667441390000164, + "qy" : 0.0019384903262589207, + "qz" : -0.3583054928379458, + "qs" : 0.9335979851282099 }, { "type" : "RDXMediumCinderBlockRoughed", - "x" : 3.6725926399230957, - "y" : -0.20286306738853455, - "z" : 0.3029684722423553, + "x" : 3.672592639923095, + "y" : -0.20286306738852744, + "z" : 0.22763386235801902, "qx" : 0.0, "qy" : 0.0, - "qz" : -0.372481463577173, + "qz" : -0.37248146357717304, "qs" : 0.9280396323926081 }, { "type" : "RDXMediumCinderBlockRoughed", - "x" : 3.8084611892700195, - "y" : -0.056925807148218155, - "z" : 0.30668363943696014, + "x" : 3.8095721431824052, + "y" : -0.06435738580480432, + "z" : 0.22950602759147598, "qx" : 0.0, "qy" : 0.0, - "qz" : -0.3759256660029729, + "qz" : -0.37592566600297295, "qs" : 0.9266498225544648 }, { "type" : "RDXPointLightObject", diff --git a/ihmc-high-level-behaviors/src/libgdx/resources/environments/Simple Rough Terrain b/ihmc-high-level-behaviors/src/libgdx/resources/environments/Simple Rough Terrain new file mode 100644 index 00000000000..f89bb16c72f --- /dev/null +++ b/ihmc-high-level-behaviors/src/libgdx/resources/environments/Simple Rough Terrain @@ -0,0 +1,220 @@ +{ + "ambientLight" : 0.01, + "objects" : [ { + "type" : "RDXLabFloorObject", + "x" : 0.0, + "y" : 0.0, + "z" : 0.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : 10.0, + "y" : 10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : -10.0, + "y" : 10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : -10.0, + "y" : -10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : 10.0, + "y" : -10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.0261326209306856, + "y" : -0.1312642229974892, + "z" : 0.04168685751514941, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.0278165108023485, + "y" : 0.058344485126049284, + "z" : 0.041986173402213625, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.0254283744607902, + "y" : 0.24169205928048654, + "z" : 0.04057212192335316, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.0266319218745057, + "y" : -0.32255398445042305, + "z" : 0.041615879371108555, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.0255996928843913, + "y" : 0.4299430060193219, + "z" : 0.039385070980956435, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.424579962380316, + "y" : 0.43113670704297236, + "z" : 0.09254610128793772, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.4259674753234353, + "y" : 0.24562213822075551, + "z" : 0.09061781008937408, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.4273566781817704, + "y" : 0.0589708940443876, + "z" : 0.08995922266082751, + "qx" : 0.003149536163718456, + "qy" : -1.0842021724855044E-19, + "qz" : 0.0, + "qs" : 0.9999950401986769 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.428751325767549, + "y" : -0.1308297033892502, + "z" : 0.08970254494993041, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.429117948670486, + "y" : -0.319230985173794, + "z" : 0.09054522513823815, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.889698199172627, + "y" : -0.31362771389739263, + "z" : 0.12029186241267142, + "qx" : -2.3889634179018873E-4, + "qy" : 0.09261829908698715, + "qz" : -1.3468733359722775E-6, + "qs" : 0.9957016589325124 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.8925994110200004, + "y" : -0.12334371333174532, + "z" : 0.12113892318644881, + "qx" : -2.61922767217173E-4, + "qy" : 0.09538769007356435, + "qz" : 1.6653387936186146E-16, + "qs" : 0.9954401639369861 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.892382190045252, + "y" : 0.06702701050002413, + "z" : 0.1209962797071884, + "qx" : -4.073356737959721E-4, + "qy" : 0.09598398209739224, + "qz" : -1.6912157996040733E-5, + "qs" : 0.995382795196077 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.8968050937558998, + "y" : 0.25091822068453606, + "z" : 0.1183742422242321, + "qx" : -7.310595024396378E-4, + "qy" : 0.09724279035586246, + "qz" : -5.628849670202829E-5, + "qs" : 0.995260419240821 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.8967171650763754, + "y" : 0.4417683185753126, + "z" : 0.11887800146539662, + "qx" : -7.061977789265678E-4, + "qy" : 0.09361012578193359, + "qz" : -4.8221701554163054E-5, + "qs" : 0.9956086798087164 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.4854011650828687, + "y" : 0.337916486126029, + "z" : 0.030854774019734132, + "qx" : 0.07768412436974872, + "qy" : 0.07593874593882895, + "qz" : -0.7005910590819361, + "qs" : 0.7052451003875139 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.298407879878164, + "y" : 0.3385634336033867, + "z" : 0.030262727840291048, + "qx" : 0.07817291979346568, + "qy" : 0.07754413448983202, + "qz" : -0.7056224031799893, + "qs" : 0.6999806611240655 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.4801712617778033, + "y" : -0.17550711531681293, + "z" : 0.01956208485422517, + "qx" : -0.06847305311043973, + "qy" : 0.06771917439063006, + "qz" : 0.6945107854365579, + "qs" : 0.713007940579824 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.2962901687151547, + "y" : -0.17636865286320602, + "z" : 0.022432163496095836, + "qx" : -0.06365197911790865, + "qy" : 0.06844083238574804, + "qz" : 0.6971263806041734, + "qs" : 0.7108298583222602 + } ] +} \ No newline at end of file diff --git a/ihmc-high-level-behaviors/src/libgdx/resources/environments/Uneven Terrain 3 Levels b/ihmc-high-level-behaviors/src/libgdx/resources/environments/Uneven Terrain 3 Levels new file mode 100644 index 00000000000..4745350e88f --- /dev/null +++ b/ihmc-high-level-behaviors/src/libgdx/resources/environments/Uneven Terrain 3 Levels @@ -0,0 +1,256 @@ +{ + "ambientLight" : 0.01, + "objects" : [ { + "type" : "RDXLabFloorObject", + "x" : 0.0, + "y" : 0.0, + "z" : 0.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : 10.0, + "y" : 10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : -10.0, + "y" : 10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : -10.0, + "y" : -10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : 10.0, + "y" : -10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPalletObject", + "x" : 3.0607671409344284, + "y" : -0.06310664378602587, + "z" : 0.07506210377440695, + "qx" : -1.0842021724855047E-19, + "qy" : 0.0028445913904392786, + "qz" : 6.93889390390723E-17, + "qs" : 0.9999959541417263 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.856897603762217, + "y" : -0.06462814957506413, + "z" : 0.04301444223836254, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.7725192909374095, + "y" : -0.3573444838618175, + "z" : 0.04315923678089523, + "qx" : 0.0, + "qy" : 0.0, + "qz" : -0.7033277782232468, + "qs" : 0.7108656950363771 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.9683418781865694, + "y" : -0.35782223569816507, + "z" : 0.04510366318073977, + "qx" : 0.0024008229853511027, + "qy" : -0.002327662684819567, + "qz" : -0.6960790454542465, + "qs" : 0.7179573667808722 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.7692433473042397, + "y" : 0.22736090391018615, + "z" : 0.04463407766635785, + "qx" : 0.0, + "qy" : 0.0, + "qz" : -0.6991832031499666, + "qs" : 0.7149425490435946 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.958366039907915, + "y" : 0.22510670375295616, + "z" : 0.045219122127525546, + "qx" : 0.6955503250719798, + "qy" : 0.7184704667441876, + "qz" : -0.0029953814049817817, + "qs" : 9.80509505919548E-4 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.5778746753913864, + "y" : -0.35140658275500114, + "z" : 0.04094344602728395, + "qx" : 0.0, + "qy" : 0.0, + "qz" : -0.7108421321498666, + "qs" : 0.7033515928471561 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.5814870340806877, + "y" : 0.039696925710773545, + "z" : 0.04093598509427332, + "qx" : 0.0, + "qy" : 0.0, + "qz" : -0.7003398091610843, + "qs" : 0.7138096046595451 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.254567570271429, + "y" : -0.26246316058216784, + "z" : 0.08398364191738705, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.2543383177500473, + "y" : -0.45188103127684, + "z" : 0.08437391630526325, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.2567335113487488, + "y" : -0.07563787043052153, + "z" : 0.08331043805643118, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.256972731643839, + "y" : 0.1089200658313772, + "z" : 0.0831901145643251, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.2568512020237517, + "y" : 0.2998872055301931, + "z" : 0.083989292309432, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 2.6524621715840238, + "y" : 0.11525193387083688, + "z" : 0.2115532611096348, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 2.6512512829296737, + "y" : 0.3088006898672824, + "z" : 0.2127774719692071, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 2.6524316022722947, + "y" : -0.07200348851432764, + "z" : 0.2113775293633806, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 2.6534353464148004, + "y" : -0.2624579488098271, + "z" : 0.20899677238844694, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 2.6523451753964458, + "y" : -0.45492220028035296, + "z" : 0.20508528682018912, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 3.853173395299581, + "y" : -0.07850545679836562, + "z" : 0.046833812609385905, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 3.7660761367552613, + "y" : -0.3677813682161868, + "z" : 0.04590330560432699, + "qx" : 0.0, + "qy" : 0.0, + "qz" : -0.7068686309521961, + "qs" : 0.7073448512400214 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 3.9563662963951316, + "y" : -0.37261381461621124, + "z" : 0.04585163099189009, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.7079959342146718, + "qs" : 0.7062165086823545 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 3.761101071744873, + "y" : 0.2114722527067107, + "z" : 0.04291788963533616, + "qx" : 0.0, + "qy" : 0.0, + "qz" : -0.7036791662089575, + "qs" : 0.7105178611713194 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 3.953645008963975, + "y" : 0.21726026395490247, + "z" : 0.04430687538108835, + "qx" : -6.867242264466872E-4, + "qy" : 7.306747962616092E-4, + "qz" : -0.7066230485210534, + "qs" : 0.7075894726626394 + } ] +} \ No newline at end of file diff --git a/ihmc-high-level-behaviors/src/libgdx/resources/environments/ZZZRoughTerrainOne.json b/ihmc-high-level-behaviors/src/libgdx/resources/environments/ZZZRoughTerrainOne.json index fdb813bf62b..f89bb16c72f 100644 --- a/ihmc-high-level-behaviors/src/libgdx/resources/environments/ZZZRoughTerrainOne.json +++ b/ihmc-high-level-behaviors/src/libgdx/resources/environments/ZZZRoughTerrainOne.json @@ -47,18 +47,18 @@ "qs" : 1.0 }, { "type" : "RDXSmallCinderBlockRoughed", - "x" : 1.0261326209306865, - "y" : -0.13126422299748575, - "z" : 0.0, + "x" : 1.0261326209306856, + "y" : -0.1312642229974892, + "z" : 0.04168685751514941, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXSmallCinderBlockRoughed", - "x" : 1.027816510802349, - "y" : 0.0583444851260503, - "z" : 0.0, + "x" : 1.0278165108023485, + "y" : 0.058344485126049284, + "z" : 0.041986173402213625, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, @@ -66,71 +66,71 @@ }, { "type" : "RDXSmallCinderBlockRoughed", "x" : 1.0254283744607902, - "y" : 0.24169205928048704, - "z" : 1.1102230246251565E-16, + "y" : 0.24169205928048654, + "z" : 0.04057212192335316, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXSmallCinderBlockRoughed", - "x" : 1.0266319218744941, - "y" : -0.32255398445039835, - "z" : 1.1102230246251565E-16, + "x" : 1.0266319218745057, + "y" : -0.32255398445042305, + "z" : 0.041615879371108555, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXSmallCinderBlockRoughed", - "x" : 1.0255996928843942, - "y" : 0.4299430060193301, - "z" : 0.0, + "x" : 1.0255996928843913, + "y" : 0.4299430060193219, + "z" : 0.039385070980956435, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.4245799623803117, - "y" : 0.4311367070429927, - "z" : 0.0, + "x" : 1.424579962380316, + "y" : 0.43113670704297236, + "z" : 0.09254610128793772, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.4259674753234355, - "y" : 0.2456221382207675, - "z" : 0.0, + "x" : 1.4259674753234353, + "y" : 0.24562213822075551, + "z" : 0.09061781008937408, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.4273566781817708, - "y" : 0.05897089404440137, - "z" : 0.0, - "qx" : 0.0, - "qy" : 0.0, + "x" : 1.4273566781817704, + "y" : 0.0589708940443876, + "z" : 0.08995922266082751, + "qx" : 0.003149536163718456, + "qy" : -1.0842021724855044E-19, "qz" : 0.0, - "qs" : 1.0 + "qs" : 0.9999950401986769 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.428751325767556, - "y" : -0.13082970338925246, - "z" : 0.0, + "x" : 1.428751325767549, + "y" : -0.1308297033892502, + "z" : 0.08970254494993041, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.4291179486704777, - "y" : -0.3192309851737916, - "z" : 0.0, + "x" : 1.429117948670486, + "y" : -0.319230985173794, + "z" : 0.09054522513823815, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, @@ -138,83 +138,83 @@ }, { "type" : "RDXLargeCinderBlockRoughed", "x" : 1.889698199172627, - "y" : -0.3136277138974171, - "z" : 0.0, - "qx" : -2.3890013852761622E-4, - "qy" : 0.08700325143367432, - "qz" : -1.4571706576721966E-16, - "qs" : 0.9962079989473547 + "y" : -0.31362771389739263, + "z" : 0.12029186241267142, + "qx" : -2.3889634179018873E-4, + "qy" : 0.09261829908698715, + "qz" : -1.3468733359722775E-6, + "qs" : 0.9957016589325124 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.8925994110199995, - "y" : -0.12334371333177716, - "z" : 0.0, - "qx" : -2.619227672171765E-4, - "qy" : 0.09538769007356443, - "qz" : 2.2204403793185408E-16, + "x" : 1.8925994110200004, + "y" : -0.12334371333174532, + "z" : 0.12113892318644881, + "qx" : -2.61922767217173E-4, + "qy" : 0.09538769007356435, + "qz" : 1.6653387936186146E-16, "qs" : 0.9954401639369861 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.8923821900452733, - "y" : 0.06702701050002052, - "z" : 0.0, - "qx" : -4.074078878578608E-4, - "qy" : 0.09148835465765207, - "qz" : -1.5072662212893554E-5, - "qs" : 0.9958060628223065 + "x" : 1.892382190045252, + "y" : 0.06702701050002413, + "z" : 0.1209962797071884, + "qx" : -4.073356737959721E-4, + "qy" : 0.09598398209739224, + "qz" : -1.6912157996040733E-5, + "qs" : 0.995382795196077 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.896805093755894, - "y" : 0.25091822068453806, - "z" : 0.0, - "qx" : -7.314712274236847E-4, - "qy" : 0.08957687143892619, - "qz" : -5.0658014366053786E-5, - "qs" : 0.9959796416025897 + "x" : 1.8968050937558998, + "y" : 0.25091822068453606, + "z" : 0.1183742422242321, + "qx" : -7.310595024396378E-4, + "qy" : 0.09724279035586246, + "qz" : -5.628849670202829E-5, + "qs" : 0.995260419240821 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.896717165076377, - "y" : 0.4417683185753008, - "z" : 0.0, - "qx" : -7.061977789265809E-4, - "qy" : 0.09361012578193358, - "qz" : -4.822170155417867E-5, + "x" : 1.8967171650763754, + "y" : 0.4417683185753126, + "z" : 0.11887800146539662, + "qx" : -7.061977789265678E-4, + "qy" : 0.09361012578193359, + "qz" : -4.8221701554163054E-5, "qs" : 0.9956086798087164 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 2.4854011650828647, - "y" : 0.33791648612603054, - "z" : 0.0, + "x" : 2.4854011650828687, + "y" : 0.337916486126029, + "z" : 0.030854774019734132, "qx" : 0.07768412436974872, "qy" : 0.07593874593882895, "qz" : -0.7005910590819361, "qs" : 0.7052451003875139 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 2.2984078798781646, - "y" : 0.33856343360339525, - "z" : -0.004837357989063484, - "qx" : 0.08236230486456504, - "qy" : 0.08170002376624297, - "qz" : -0.7051456813991942, - "qs" : 0.6995077732649281 + "x" : 2.298407879878164, + "y" : 0.3385634336033867, + "z" : 0.030262727840291048, + "qx" : 0.07817291979346568, + "qy" : 0.07754413448983202, + "qz" : -0.7056224031799893, + "qs" : 0.6999806611240655 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 2.480171261777798, - "y" : -0.17550711531681523, - "z" : 0.0, - "qx" : -0.06847305311043997, - "qy" : 0.06771917439063073, - "qz" : 0.6945107854365578, + "x" : 2.4801712617778033, + "y" : -0.17550711531681293, + "z" : 0.01956208485422517, + "qx" : -0.06847305311043973, + "qy" : 0.06771917439063006, + "qz" : 0.6945107854365579, "qs" : 0.713007940579824 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 2.296290168715155, - "y" : -0.1763686528631838, - "z" : 0.0, - "qx" : -0.0710345669393736, - "qy" : 0.07596834185010112, - "qz" : 0.6964128483016595, - "qs" : 0.7100648182079236 + "x" : 2.2962901687151547, + "y" : -0.17636865286320602, + "z" : 0.022432163496095836, + "qx" : -0.06365197911790865, + "qy" : 0.06844083238574804, + "qz" : 0.6971263806041734, + "qs" : 0.7108298583222602 } ] } \ No newline at end of file diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java index fb94f36f441..bba7dcf5277 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java @@ -9,9 +9,10 @@ import us.ihmc.footstepPlanning.communication.ContinuousHikingAPI; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersBasics; import us.ihmc.footstepPlanning.swing.SwingPlannerParametersBasics; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; import us.ihmc.perception.StandAloneRealsenseProcess; import us.ihmc.perception.comms.PerceptionComms; -import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractorCUDA; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.ros2.ROS2Node; import us.ihmc.ros2.ROS2NodeBuilder; import us.ihmc.sensorProcessing.heightMap.HeightMapParameters; @@ -57,19 +58,17 @@ public ContinuousHikingProcess(DRCRobotModel robotModel) SwingPlannerParametersBasics swingPlannerParameters = robotModel.getSwingPlannerParameters(); ros2PropertySetGroup.registerStoredPropertySet(ContinuousHikingAPI.SWING_PLANNING_PARAMETERS, swingPlannerParameters); - HeightMapParameters heightMapParameters = RapidHeightMapExtractorCUDA.getHeightMapParameters(); + HeightMapParameters heightMapParameters = RapidHeightMapManager.getHeightMapParameters(); ros2PropertySetGroup.registerStoredPropertySet(PerceptionComms.HEIGHT_MAP_PARAMETERS, heightMapParameters); ContinuousHikingLogger continuousHikingLogger = new ContinuousHikingLogger(); - ControllerFootstepQueueMonitor controllerFootstepQueueMonitor = new ControllerFootstepQueueMonitor(ros2Node, - robotModel.getSimpleRobotName(), - syncedRobot.getReferenceFrames(), - continuousHikingLogger); + ControllerFootstepQueueMonitor controllerFootstepQueueMonitor = new ControllerFootstepQueueMonitor(ros2Node, robotModel.getSimpleRobotName()); standAloneRealsenseProcess = new StandAloneRealsenseProcess(ros2Node, ros2Helper, syncedRobot, controllerFootstepQueueMonitor); continuousPlannerSchedulingTask = new ContinuousPlannerSchedulingTask(robotModel, ros2Node, + syncedRobot, syncedRobot.getReferenceFrames(), controllerFootstepQueueMonitor, continuousHikingLogger, diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/DoNothingState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/DoNothingState.java index f865afd87d1..ccf5a0165e2 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/DoNothingState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/DoNothingState.java @@ -56,6 +56,7 @@ public void doAction(double timeInState) message.setClearRemainingFootstepQueue(true); continuousPlanner.setLatestFootstepPlan(null); pauseWalkingPublisher.publish(message); + debugger.resetVisualizationForUIPublisher(); } robotFeet.get(RobotSide.LEFT).set(referenceFrames.getSoleFrame(RobotSide.LEFT).getTransformToWorldFrame()); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java index d1351e433e6..fed6d7dfa77 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java @@ -1,21 +1,114 @@ package us.ihmc.behaviors.activeMapping.ContinuousHikingStateMachine; -import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; +import behavior_msgs.msg.dds.ContinuousHikingCommandMessage; +import controller_msgs.msg.dds.FootstepDataListMessage; +import controller_msgs.msg.dds.QueuedFootstepStatusMessage; +import ihmc_common_msgs.msg.dds.PoseListMessage; +import ihmc_common_msgs.msg.dds.QueueableMessage; +import org.jetbrains.annotations.NotNull; +import us.ihmc.avatar.drcRobot.DRCRobotModel; +import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; +import us.ihmc.avatar.networkProcessor.footstepPlanningModule.FootstepPlanningModuleLauncher; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; +import us.ihmc.commons.thread.ThreadTools; +import us.ihmc.communication.HumanoidControllerAPI; +import us.ihmc.communication.packets.MessageTools; +import us.ihmc.communication.ros2.ROS2Helper; +import us.ihmc.euclid.geometry.Pose3D; +import us.ihmc.euclid.geometry.interfaces.Pose3DReadOnly; +import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; +import us.ihmc.footstepPlanning.FootstepPlan; +import us.ihmc.footstepPlanning.FootstepPlannerOutput; +import us.ihmc.footstepPlanning.FootstepPlannerRequest; +import us.ihmc.footstepPlanning.FootstepPlanningModule; +import us.ihmc.footstepPlanning.PlannedFootstep; +import us.ihmc.footstepPlanning.communication.ContinuousHikingAPI; +import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapAndWiggler; +import us.ihmc.footstepPlanning.graphSearch.graph.DiscreteFootstep; +import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersBasics; +import us.ihmc.footstepPlanning.swing.SwingPlannerParametersBasics; +import us.ihmc.footstepPlanning.swing.SwingPlannerType; +import us.ihmc.footstepPlanning.tools.PlannerTools; +import us.ihmc.log.LogTools; +import us.ihmc.mecano.frames.MovingReferenceFrame; +import us.ihmc.perception.heightMap.TerrainMapData; +import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.stateMachine.core.State; +import us.ihmc.ros2.ROS2Topic; +import us.ihmc.sensorProcessing.heightMap.HeightMapData; + +import java.util.List; +import java.util.concurrent.atomic.AtomicReference; +import java.util.function.Supplier; public class JustWaitState implements State { + @NotNull + private final ROS2Helper ros2Helper; + @NotNull + private final ROS2SyncedRobotModel syncedRobot; + private final AtomicReference commandMessage; private final ControllerFootstepQueueMonitor controllerQueueMonitor; + private final FootstepPlannerEnvironmentHandler environmentHandler; private boolean isDone; + private final FootstepPlanningModule footstepPlanner; + private final DefaultFootstepPlannerParametersBasics footstepPlannerParameters; + private final SwingPlannerParametersBasics swingPlannerParameters; + private final Supplier heightMapData; + private final Supplier terrainMapData; + + private final ROS2Topic controllerFootstepDataTopic; + private final FramePose3D midFeetZUpPose = new FramePose3D(); + private final MovingReferenceFrame midFeetZUpFrame; + private final FramePose3D startPose = new FramePose3D(); + private final FootstepSnapAndWiggler footstepSnapAndWiggler; - public JustWaitState(ControllerFootstepQueueMonitor controllerQueueMonitor) + public JustWaitState(DRCRobotModel robotModel, + ROS2Helper ros2Helper, + ROS2SyncedRobotModel syncedRobot, + AtomicReference commandMessage, + ControllerFootstepQueueMonitor controllerQueueMonitor, + DefaultFootstepPlannerParametersBasics footstepPlannerParameters, + SwingPlannerParametersBasics swingPlannerParameters, + Supplier heightMapData, + Supplier terrainMapData) { + this.ros2Helper = ros2Helper; + this.syncedRobot = syncedRobot; + this.commandMessage = commandMessage; this.controllerQueueMonitor = controllerQueueMonitor; + + footstepPlanner = FootstepPlanningModuleLauncher.createModule(robotModel); + this.footstepPlannerParameters = footstepPlannerParameters; + this.swingPlannerParameters = swingPlannerParameters; + this.heightMapData = heightMapData; + this.terrainMapData = terrainMapData; + + environmentHandler = new FootstepPlannerEnvironmentHandler(); + footstepSnapAndWiggler = new FootstepSnapAndWiggler(PlannerTools.createDefaultFootPolygons(), footstepPlannerParameters, environmentHandler); + + midFeetZUpFrame = syncedRobot.getReferenceFrames().getMidFeetZUpFrame(); + + controllerFootstepDataTopic = HumanoidControllerAPI.getTopic(FootstepDataListMessage.class, "Nadia"); + ros2Helper.createPublisher(controllerFootstepDataTopic); + + ros2Helper.subscribeViaCallback(ContinuousHikingAPI.ROTATE_GOAL_FOOTSTEPS, this::planToGoal); + ros2Helper.subscribeViaCallback(ContinuousHikingAPI.SQUARE_UP_STEP, this::squareUpStep); } @Override public void onEntry() { + ContinuousHikingCommandMessage continuousHikingCommandMessage = commandMessage.get(); + + if (continuousHikingCommandMessage.getSquareUpToGoal() && !controllerQueueMonitor.getControllerFootstepQueue().isEmpty()) + { + squareUpStep(); + } + isDone = false; } @@ -39,4 +132,157 @@ public boolean isDone(double timeInState) { return isDone; } + + public void planToGoal(PoseListMessage poseListMessage) + { + ThreadTools.startAThread(() -> + { + List poses = MessageTools.unpackPoseListMessage(poseListMessage); + FramePose3D leftFootPose = new FramePose3D(); + FramePose3D rightFootPose = new FramePose3D(); + + leftFootPose.set(poses.get(0)); + rightFootPose.set(poses.get(1)); + + footstepPlanner.getFootstepPlannerParameters().set(footstepPlannerParameters); + footstepPlanner.getSwingPlannerParameters().set(swingPlannerParameters); + + FootstepPlannerRequest footstepPlannerRequest = new FootstepPlannerRequest(); + footstepPlannerRequest.setGoalFootPoses(leftFootPose, rightFootPose); + footstepPlannerRequest.setSwingPlannerType(SwingPlannerType.NONE); + + footstepPlannerRequest.getStartFootPoses().forEach((side, pose3D) -> + { + FramePose3DReadOnly soleFramePose; + if (controllerQueueMonitor.getNumberOfIncompleteFootsteps() > 0) + { + LogTools.info("Yes this queue is not empty"); + // We pass in the opposite side because the method returns the footstep on the opposite side + soleFramePose = controllerQueueMonitor.getLastFootstepQueuedOnOppositeSide( + side.getOppositeSide()); + } + else + { + soleFramePose = syncedRobot.getFramePoseReadOnly(referenceFrames -> referenceFrames.getSoleFrame( + side)); + } + soleFramePose.get(pose3D); + }); + + footstepPlannerRequest.setRequestedInitialStanceSide(RobotSide.LEFT); + footstepPlannerRequest.setHeightMapData(heightMapData.get()); + + footstepPlannerRequest.setSnapGoalSteps(true); + footstepPlannerRequest.setPlanBodyPath(true); + + FramePose3D goalFramePose = new FramePose3D(); + goalFramePose.interpolate(leftFootPose, rightFootPose, 0.5); + + Pose3DReadOnly goalPose = new Pose3D(goalFramePose.getPosition(), goalFramePose.getOrientation()); + + midFeetZUpPose.setToZero(midFeetZUpFrame); + midFeetZUpPose.changeFrame(ReferenceFrame.getWorldFrame()); + startPose.setToZero(midFeetZUpFrame); + startPose.changeFrame(ReferenceFrame.getWorldFrame()); + startPose.getOrientation().set(goalPose.getOrientation()); + footstepPlannerRequest.getBodyPathWaypoints().add(midFeetZUpPose); + footstepPlannerRequest.getBodyPathWaypoints().add(startPose); + footstepPlannerRequest.getBodyPathWaypoints().add(goalPose); + + FootstepPlannerOutput plannerOutput = footstepPlanner.handleRequest(footstepPlannerRequest); + + FootstepPlan newestFootstepPlan = null; + + if (plannerOutput != null) + { + newestFootstepPlan = plannerOutput.getFootstepPlan(); + } + + FootstepDataListMessage footstepDataListMessage = new FootstepDataListMessage(); + footstepDataListMessage.setDefaultSwingDuration(0.8); + footstepDataListMessage.setDefaultTransferDuration(0.4); + footstepDataListMessage.getQueueingProperties().setExecutionMode(QueueableMessage.EXECUTION_MODE_QUEUE); + + for (int i = 0; i < footstepPlanner.getOutput().getFootstepPlan().getNumberOfSteps(); i++) + { + assert newestFootstepPlan != null; + PlannedFootstep footstep = newestFootstepPlan.getFootstep(i); + footstep.limitFootholdVertices(); + footstepDataListMessage.getFootstepDataList().add().set(footstep.getAsMessage()); + } + + ros2Helper.publish(controllerFootstepDataTopic, footstepDataListMessage); + }, "PlanToGoalThread"); + } + + private final FramePose3D tempFramePose = new FramePose3D(); + + public void squareUpStep() + { + environmentHandler.setHeightMap(heightMapData.get()); + environmentHandler.setTerrainMapData(terrainMapData.get()); + + FramePose3DReadOnly lastStepInQueue; + PlannedFootstep squareUpStep = null; + ReferenceFrame leftFootFrame = syncedRobot.getReferenceFrames().getFootFrame(RobotSide.LEFT); + ReferenceFrame rightFootFrame = syncedRobot.getReferenceFrames().getFootFrame(RobotSide.RIGHT); + + if (controllerQueueMonitor.getNumberOfIncompleteFootsteps() > 0) + { + lastStepInQueue = controllerQueueMonitor.getLastFootstepInQueue(); + List controllerFootstepQueue = controllerQueueMonitor.getControllerFootstepQueue(); + + RobotSide robotSide = RobotSide.fromByte(controllerFootstepQueue.get(controllerFootstepQueue.size() - 1).getRobotSide()); + + if (robotSide == RobotSide.LEFT) + { + tempFramePose.set(lastStepInQueue); + tempFramePose.changeFrame(leftFootFrame); + tempFramePose.getTranslation().addY(-footstepPlannerParameters.getIdealFootstepWidth()); + tempFramePose.changeFrame(ReferenceFrame.getWorldFrame()); + + squareUpStep = new PlannedFootstep(robotSide.getOppositeSide(), tempFramePose); + } + else if (robotSide == RobotSide.RIGHT) + { + tempFramePose.set(lastStepInQueue); + tempFramePose.changeFrame(rightFootFrame); + tempFramePose.getTranslation().addY(footstepPlannerParameters.getIdealFootstepWidth()); + tempFramePose.changeFrame(ReferenceFrame.getWorldFrame()); + + squareUpStep = new PlannedFootstep(robotSide.getOppositeSide(), tempFramePose); + } + } + else + { + FramePose3D rightFootPose = new FramePose3D(ReferenceFrame.getWorldFrame(), + syncedRobot.getReferenceFrames().getSoleFrame(RobotSide.RIGHT).getTransformToWorldFrame()); + rightFootPose.changeFrame(leftFootFrame); + RobotSide furthestForwardFootstep = rightFootPose.getTranslationX() > 0 ? RobotSide.RIGHT : RobotSide.LEFT; + MovingReferenceFrame furthestForwardSoleFrame = syncedRobot.getReferenceFrames().getSoleFrame(furthestForwardFootstep); + + tempFramePose.setToZero(furthestForwardSoleFrame); + tempFramePose.getTranslation().addY(furthestForwardFootstep.negateIfLeftSide(footstepPlannerParameters.getIdealFootstepWidth())); + tempFramePose.changeFrame(ReferenceFrame.getWorldFrame()); + + squareUpStep = new PlannedFootstep(furthestForwardFootstep.getOppositeSide(), tempFramePose); + } + + if (squareUpStep == null) + return; + + DiscreteFootstep footstep = new DiscreteFootstep(squareUpStep.getFootstepPose().getX(), squareUpStep.getFootstepPose().getY()); + footstepSnapAndWiggler.snapFootstep(footstep); + + squareUpStep.getFootstepPose().setZ(0.0); + squareUpStep.getFootstepPose().applyTransform(footstep.getSnapData().getSnapTransform()); + + FootstepDataListMessage footstepDataListMessage = new FootstepDataListMessage(); + footstepDataListMessage.setDefaultSwingDuration(0.8); + footstepDataListMessage.setDefaultTransferDuration(0.4); + footstepDataListMessage.getQueueingProperties().setExecutionMode(QueueableMessage.EXECUTION_MODE_QUEUE); + footstepDataListMessage.getFootstepDataList().add().set(squareUpStep.getAsMessage()); + + ros2Helper.publish(controllerFootstepDataTopic, footstepDataListMessage); + } } 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 a201c67be51..088fa375d02 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 @@ -8,7 +8,7 @@ import us.ihmc.behaviors.activeMapping.ContinuousHikingParameters; import us.ihmc.behaviors.activeMapping.ContinuousPlanner; import us.ihmc.behaviors.activeMapping.ContinuousPlannerTools; -import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; import us.ihmc.communication.packets.MessageTools; import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.euclid.geometry.Pose3D; diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/WaitingToLandState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/WaitingToLandState.java index 0d2ab1a1d74..50dc960d600 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/WaitingToLandState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/WaitingToLandState.java @@ -5,7 +5,7 @@ import us.ihmc.behaviors.activeMapping.ContinuousHikingLogger; import us.ihmc.behaviors.activeMapping.ContinuousHikingParameters; import us.ihmc.behaviors.activeMapping.ContinuousPlanner; -import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; import us.ihmc.communication.HumanoidControllerAPI; import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.log.LogTools; 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 6d0d8b27e0e..a4568b1a9f1 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 @@ -2,6 +2,7 @@ import behavior_msgs.msg.dds.ContinuousHikingCommandMessage; import us.ihmc.avatar.drcRobot.DRCRobotModel; +import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; import us.ihmc.behaviors.activeMapping.ContinuousHikingStateMachine.*; import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.euclid.referenceFrame.FramePose3D; @@ -9,6 +10,7 @@ import us.ihmc.footstepPlanning.communication.ContinuousHikingAPI; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersBasics; import us.ihmc.footstepPlanning.swing.SwingPlannerParametersBasics; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames; import us.ihmc.log.LogTools; import us.ihmc.perception.heightMap.TerrainMapData; @@ -45,6 +47,7 @@ public class ContinuousPlannerSchedulingTask public ContinuousPlannerSchedulingTask(DRCRobotModel robotModel, ROS2Node ros2Node, + ROS2SyncedRobotModel syncedRobotModel, HumanoidReferenceFrames referenceFrames, ControllerFootstepQueueMonitor controllerFootstepQueueMonitor, ContinuousHikingLogger continuousHikingLogger, @@ -94,7 +97,15 @@ public ContinuousPlannerSchedulingTask(DRCRobotModel robotModel, controllerFootstepQueueMonitor, continuousHikingParameters, continuousHikingLogger); - State justWaitState = new JustWaitState(controllerFootstepQueueMonitor); + State justWaitState = new JustWaitState(robotModel, + ros2Helper, + syncedRobotModel, + commandMessage, + controllerFootstepQueueMonitor, + footstepPlannerParameters, + swingPlannerParameters, + this::getHeightMapData, + this::getTerrainMap); // Adding the different states stateMachineFactory.addState(ContinuousHikingState.DO_NOTHING, notStartedState); @@ -137,9 +148,12 @@ public ContinuousPlannerSchedulingTask(DRCRobotModel robotModel, if (from == null) { // This means the state machine has just started up, for the visuals put them under the feet till we start walking - SideDependentList robotFeet = new SideDependentList<>(new FramePose3D(), new FramePose3D()); - robotFeet.get(RobotSide.LEFT).set(referenceFrames.getSoleFrame(RobotSide.LEFT).getTransformToWorldFrame()); - robotFeet.get(RobotSide.RIGHT).set(referenceFrames.getSoleFrame(RobotSide.RIGHT).getTransformToWorldFrame()); + SideDependentList robotFeet = new SideDependentList<>(new FramePose3D(), + new FramePose3D()); + robotFeet.get(RobotSide.LEFT) + .set(referenceFrames.getSoleFrame(RobotSide.LEFT).getTransformToWorldFrame()); + robotFeet.get(RobotSide.RIGHT) + .set(referenceFrames.getSoleFrame(RobotSide.RIGHT).getTransformToWorldFrame()); debugger.publishStartAndGoalForVisualization(robotFeet, robotFeet); } 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 13055597ef1..eff0611a7c5 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 @@ -241,6 +241,16 @@ public void printScoreStats(MonteCarloFootstepNode root, MonteCarloFootstepPlann } } + public void resetVisualizationForUIPublisher() + { + PoseListMessage poseListMessage = new PoseListMessage(); + FootstepDataListMessage footstepDataListMessage = new FootstepDataListMessage(); + + startAndGoalPublisherForUI.publish(poseListMessage); + monteCarloNodesPublisherForUI.publish(poseListMessage); + plannedFootstesPublisherForUI.publish(footstepDataListMessage); + } + public void publishStartAndGoalForVisualization(SideDependentList startPoses, SideDependentList goalPoses) { List poses = new ArrayList<>(); 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 fdf1b68d4f0..d7e2abf8c35 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 @@ -21,6 +21,7 @@ import us.ihmc.perception.depthData.CollisionBoxProvider; import us.ihmc.perception.filters.CollidingScanRegionFilter; import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.heightMap.RemoteHeightMapUpdater; import us.ihmc.perception.opencl.OpenCLManager; import us.ihmc.perception.opencv.OpenCVTools; @@ -34,6 +35,7 @@ import us.ihmc.robotics.geometry.PlanarRegionsList; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2Publisher; import us.ihmc.ros2.ROS2Topic; import us.ihmc.ros2.RealtimeROS2Node; import us.ihmc.sensorProcessing.globalHeightMap.GlobalHeightMap; @@ -87,6 +89,8 @@ public class HumanoidPerceptionModule private boolean mappingEnabled = false; private boolean occupancyGridEnabled = false; public boolean heightMapDataBeingProcessed = false; + private ROS2Publisher heightMapPublisher; + private ROS2Publisher heightMapImagePublisher; public HumanoidPerceptionModule(OpenCLManager openCLManager) { @@ -115,11 +119,7 @@ public boolean isHeightMapDataBeingProcessed() return heightMapDataBeingProcessed; } - public void updateTerrain(ROS2Helper ros2Helper, - Mat incomingDepth, - ReferenceFrame cameraFrame, - ReferenceFrame cameraZUpFrame, - boolean metricDepth) + public void updateTerrain(ROS2Helper ros2Helper, Mat incomingDepth, ReferenceFrame cameraFrame, ReferenceFrame cameraZUpFrame, boolean metricDepth) { if (localizationAndMappingTask != null) localizationAndMappingTask.setEnableLiveMode(mappingEnabled); @@ -152,7 +152,7 @@ public void updateTerrain(ROS2Helper ros2Helper, { if (!heightMapDataBeingProcessed) { - if (rapidHeightMapExtractor.getHeightMapParameters().getResetHeightMap()) + if (RapidHeightMapManager.getHeightMapParameters().getResetHeightMap()) { rapidHeightMapExtractor.reset(); } @@ -168,7 +168,7 @@ public void updateTerrain(ROS2Helper ros2Helper, if (ros2Helper != null) { - publishHeightMapImage(ros2Helper, + publishHeightMapImage(ros2Helper.getROS2Node(), croppedHeightMapImage, compressedCroppedHeightMapPointer, PerceptionAPI.HEIGHT_MAP_CROPPED, @@ -181,27 +181,34 @@ public void updateTerrain(ROS2Helper ros2Helper, } } - public void publishHeightMapImage(ROS2Helper ros2Helper, + public void publishHeightMapImage(ROS2Node ros2Node, Mat image, BytePointer pointer, ROS2Topic topic, ImageMessage message, Instant acquisitionTime) { + if (heightMapImagePublisher == null) + { + heightMapImagePublisher = ros2Node.createPublisher(topic); + } + OpenCVTools.compressImagePNG(image, pointer); PerceptionMessageTools.publishCompressedDepthImage(pointer, - topic, message, - ros2Helper, + heightMapImagePublisher, cameraPose, acquisitionTime, rapidHeightMapExtractor.getSequenceNumber(), image.rows(), image.cols(), - (float) RapidHeightMapExtractor.getHeightMapParameters().getHeightScaleFactor()); + (float) RapidHeightMapManager.getHeightMapParameters().getHeightScaleFactor()); } - private static void publishGlobalHeightMapTile(ROS2Helper ros2Helper, GlobalHeightMap globalHeightMap, Instant acquisitionTime, ROS2Topic topic) + private static void publishGlobalHeightMapTile(ROS2Helper ros2Helper, + GlobalHeightMap globalHeightMap, + Instant acquisitionTime, + ROS2Topic topic) { // Get tiles (made out of modified cells) from the global height map class and publish them in a for loop Collection modifiedCells = globalHeightMap.getModifiedMapTiles(); @@ -221,29 +228,33 @@ private static void packGlobalMapTileMessage(GlobalMapTileMessage messageToPack, messageToPack.getHeightMap().set(HeightMapMessageTools.toMessage(tile)); } - - public void publishExternalHeightMapImage(ROS2Helper ros2Helper) + public void publishExternalHeightMapImage(ROS2Node ros2Node) { + if (heightMapPublisher == null) + { + heightMapPublisher = ros2Node.createPublisher(PerceptionAPI.HEIGHT_MAP_CROPPED); + } + executorService.clearTaskQueue(); executorService.submit(() -> - { - Instant acquisitionTime = Instant.now(); - Mat heightMapImage = rapidHeightMapExtractor.getInternalGlobalHeightMapImage().getBytedecoOpenCVMat(); - OpenCVTools.compressImagePNG(heightMapImage, compressedInternalHeightMapPointer); - //PerceptionDebugTools.displayDepth("Published Global Height Map", heightMapImage, 1); - PerceptionMessageTools.publishCompressedDepthImage(compressedInternalHeightMapPointer, - PerceptionAPI.HEIGHT_MAP_CROPPED, - croppedHeightMapImageMessage, - ros2Helper, - new FramePose3D(ReferenceFrame.getWorldFrame(), - rapidHeightMapExtractor.getSensorOrigin(), - new Quaternion()), - acquisitionTime, - rapidHeightMapExtractor.getSequenceNumber(), - heightMapImage.rows(), - heightMapImage.cols(), - (float) RapidHeightMapExtractor.getHeightMapParameters().getHeightScaleFactor()); - }); + { + Instant acquisitionTime = Instant.now(); + Mat heightMapImage = rapidHeightMapExtractor.getInternalGlobalHeightMapImage().getBytedecoOpenCVMat(); + OpenCVTools.compressImagePNG(heightMapImage, compressedInternalHeightMapPointer); + //PerceptionDebugTools.displayDepth("Published Global Height Map", heightMapImage, 1); + PerceptionMessageTools.publishCompressedDepthImage(compressedInternalHeightMapPointer, + croppedHeightMapImageMessage, + heightMapPublisher, + new FramePose3D(ReferenceFrame.getWorldFrame(), + rapidHeightMapExtractor.getSensorOrigin(), + new Quaternion()), + acquisitionTime, + rapidHeightMapExtractor.getSequenceNumber(), + heightMapImage.rows(), + heightMapImage.cols(), + (float) RapidHeightMapManager.getHeightMapParameters() + .getHeightScaleFactor()); + }); } private void updatePlanarRegions(ROS2Helper ros2Helper, ReferenceFrame cameraFrame) @@ -316,7 +327,8 @@ public void initializeHeightMapExtractor(ROS2Helper ros2Helper, HumanoidReferenc referenceFrames.getSoleFrame(RobotSide.RIGHT), realsenseDepthImage, cameraIntrinsics, - 1); + 1, + RapidHeightMapManager.getHeightMapParameters()); if (ros2Helper != null) ros2Helper.subscribeViaVolatileCallback(PerceptionAPI.RESET_HEIGHT_MAP, message -> resetHeightMapRequested.set()); @@ -443,16 +455,16 @@ public HeightMapMessage getGlobalHeightMapMessage() Mat heightMapMat = heightMapImage.getBytedecoOpenCVMat().clone(); if (latestHeightMapData == null) { - latestHeightMapData = new HeightMapData((float) RapidHeightMapExtractor.getHeightMapParameters().getGlobalCellSizeInMeters(), - (float) RapidHeightMapExtractor.getHeightMapParameters().getGlobalWidthInMeters(), + latestHeightMapData = new HeightMapData((float) RapidHeightMapManager.getHeightMapParameters().getGlobalCellSizeInMeters(), + (float) RapidHeightMapManager.getHeightMapParameters().getGlobalWidthInMeters(), rapidHeightMapExtractor.getSensorOrigin().getX(), rapidHeightMapExtractor.getSensorOrigin().getY()); } PerceptionMessageTools.convertToHeightMapData(heightMapMat, latestHeightMapData, rapidHeightMapExtractor.getSensorOrigin(), - (float) RapidHeightMapExtractor.getHeightMapParameters().getGlobalWidthInMeters(), - (float) RapidHeightMapExtractor.getHeightMapParameters().getGlobalCellSizeInMeters()); + (float) RapidHeightMapManager.getHeightMapParameters().getGlobalWidthInMeters(), + (float) RapidHeightMapManager.getHeightMapParameters().getGlobalCellSizeInMeters()); return HeightMapMessageTools.toMessage(latestHeightMapData); } 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 index cf1bbea7c50..baaf42479a3 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java @@ -1,18 +1,19 @@ package us.ihmc.perception; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; -import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; import us.ihmc.commons.thread.RepeatingTaskThread; -import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.log.LogTools; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.heightMap.TerrainMapData; +import us.ihmc.ros2.ROS2Node; import us.ihmc.sensorProcessing.heightMap.HeightMapData; import us.ihmc.sensors.ImageSensor; public class RapidHeightMapUpdateThread extends RepeatingTaskThread { - private final ROS2PublishSubscribeAPI ros2; + private final ROS2Node ros2Node; private final ROS2SyncedRobotModel syncedRobotModel; private final ReferenceFrame leftFootFrame; private final ReferenceFrame rightFootFrame; @@ -27,7 +28,7 @@ public class RapidHeightMapUpdateThread extends RepeatingTaskThread private final boolean runWithCUDA; private final int depthImageKey; - public RapidHeightMapUpdateThread(ROS2PublishSubscribeAPI ros2, + public RapidHeightMapUpdateThread(ROS2Node ros2Node, ROS2SyncedRobotModel syncedRobotModel, ReferenceFrame leftFootFrame, ReferenceFrame rightFootFrame, @@ -38,7 +39,7 @@ public RapidHeightMapUpdateThread(ROS2PublishSubscribeAPI ros2, { super(imageSensor.getSensorName() + RapidHeightMapUpdateThread.class.getSimpleName()); - this.ros2 = ros2; + this.ros2Node = ros2Node; this.syncedRobotModel = syncedRobotModel; this.leftFootFrame = leftFootFrame; this.rightFootFrame = rightFootFrame; @@ -62,8 +63,9 @@ protected void runTask() // Initialize if (heightMapManager == null) { - heightMapManager = new RapidHeightMapManager(ros2, - syncedRobotModel.getRobotModel(), + heightMapManager = new RapidHeightMapManager(ros2Node, + syncedRobotModel.getFullRobotModel(), + syncedRobotModel.getRobotModel().getSimpleRobotName(), leftFootFrame, rightFootFrame, controllerFootstepQueueMonitor, 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 a6ae7000b49..2dfd0a0e876 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 @@ -1,12 +1,13 @@ package us.ihmc.perception; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; -import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; import us.ihmc.commons.thread.RepeatingTaskThread; import us.ihmc.communication.PerceptionAPI; import us.ihmc.communication.packets.Packet; import us.ihmc.communication.ros2.ROS2DemandGraphNode; import us.ihmc.communication.ros2.ROS2Helper; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.perception.opencl.OpenCLManager; import us.ihmc.sensors.realsense.RealSenseConfiguration; @@ -78,7 +79,7 @@ public StandAloneRealsenseProcess(ROS2Node ros2Node, private void initializeHeightMap(ControllerFootstepQueueMonitor controllerFootstepQueueMonitor) { boolean runWithCUDA = true; - heightMapUpdateThread = new RapidHeightMapUpdateThread(ros2Helper, + heightMapUpdateThread = new RapidHeightMapUpdateThread(ros2Helper.getROS2Node(), syncedRobot, syncedRobot.getReferenceFrames().getSoleFrame(RobotSide.LEFT), syncedRobot.getReferenceFrames().getSoleFrame(RobotSide.RIGHT), diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/TerrainPerceptionProcessWithDriver.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/TerrainPerceptionProcessWithDriver.java index 011a1bdfe2f..8905b64aebb 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/TerrainPerceptionProcessWithDriver.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/TerrainPerceptionProcessWithDriver.java @@ -17,9 +17,11 @@ import us.ihmc.log.LogTools; import us.ihmc.perception.comms.PerceptionComms; import us.ihmc.perception.depthData.CollisionBoxProvider; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.opencl.OpenCLManager; import us.ihmc.perception.opencv.OpenCVTools; import us.ihmc.perception.parameters.PerceptionConfigurationParameters; +import us.ihmc.ros2.ROS2Publisher; import us.ihmc.sensors.realsense.RealSenseConfiguration; import us.ihmc.sensors.realsense.RealSenseDevice; import us.ihmc.sensors.realsense.RealSenseDeviceManager; @@ -94,13 +96,13 @@ public class TerrainPerceptionProcessWithDriver private final double outputPeriod = UnitConversions.hertzToSeconds(30.0f); private volatile boolean running = true; - private final int depthWidth; private final int depthHeight; private final int colorWidth; private final int colorHeight; private long depthSequenceNumber = 0; private long colorSequenceNumber = 0; + private final ROS2Publisher depthPublisher; public TerrainPerceptionProcessWithDriver(String robotName, CollisionBoxProvider collisionBoxProvider, @@ -129,6 +131,8 @@ public TerrainPerceptionProcessWithDriver(String robotName, realtimeROS2Node.spin(); realsenseDeviceManager = new RealSenseDeviceManager(); + depthPublisher = realtimeROS2Node.createPublisher(depthTopic); + LogTools.info("Creating Bytedeco Realsense"); realsense = realsenseDeviceManager.createBytedecoRealsenseDevice(realsenseConfiguration); if (realsense.getDevice() == null) @@ -171,8 +175,7 @@ public TerrainPerceptionProcessWithDriver(String robotName, humanoidPerception.getRapidRegionsExtractor().setEnabled(true); ros2PropertySetGroup.registerStoredPropertySet(PerceptionComms.PERCEPTION_CONFIGURATION_PARAMETERS, parameters); - ros2PropertySetGroup.registerStoredPropertySet(PerceptionComms.HEIGHT_MAP_PARAMETERS, - humanoidPerception.getRapidHeightMapExtractor().getHeightMapParameters()); + ros2PropertySetGroup.registerStoredPropertySet(PerceptionComms.HEIGHT_MAP_PARAMETERS, RapidHeightMapManager.getHeightMapParameters()); ros2PropertySetGroup.registerStoredPropertySet(PerceptionComms.PERSPECTIVE_RAPID_REGION_PARAMETERS, humanoidPerception.getRapidRegionsExtractor().getParameters()); ros2PropertySetGroup.registerStoredPropertySet(PerceptionComms.PERSPECTIVE_POLYGONIZER_PARAMETERS, @@ -251,9 +254,8 @@ private void update() PerceptionMessageTools.setDepthIntrinsicsFromRealsense(realsense, depthImageMessage); CameraModel.PINHOLE.packMessageFormat(depthImageMessage); PerceptionMessageTools.publishCompressedDepthImage(compressedDepthPointer, - depthTopic, depthImageMessage, - ros2Helper, + depthPublisher, cameraPose, acquisitionTime, depthSequenceNumber++, diff --git a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXRapidHeightMapExtractionDemo.java b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXRapidHeightMapExtractionDemo.java index c8a2d807a84..c4d5b7e1aee 100644 --- a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXRapidHeightMapExtractionDemo.java +++ b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXRapidHeightMapExtractionDemo.java @@ -13,6 +13,7 @@ import us.ihmc.log.LogTools; import us.ihmc.perception.camera.CameraIntrinsics; import us.ihmc.perception.HumanoidPerceptionModule; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.logging.PerceptionDataLoader; import us.ihmc.perception.logging.PerceptionLoggerConstants; import us.ihmc.perception.opencl.OpenCLManager; @@ -100,7 +101,7 @@ public void create() humanoidPerceptionUI = new RDXHumanoidPerceptionUI(humanoidPerception, ros2Helper); humanoidPerceptionUI.initializeHeightMapUI(ros2Helper); - HeightMapParameters heightMapParameters = humanoidPerception.getRapidHeightMapExtractor().getHeightMapParameters(); + HeightMapParameters heightMapParameters = RapidHeightMapManager.getHeightMapParameters(); LogTools.info("Height Map Parameters Save File " + heightMapParameters.findSaveFileDirectory().getFileName().toString()); heightMapParametersTuner.create(heightMapParameters, false); humanoidPerceptionUI.addChild(heightMapParametersTuner); 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 0497fa8fc0a..9532d552dfe 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 @@ -14,6 +14,7 @@ import us.ihmc.perception.camera.CameraIntrinsics; import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractorCUDA; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.tools.PerceptionMessageTools; import us.ihmc.rdx.Lwjgl3ApplicationAdapter; import us.ihmc.rdx.sceneManager.RDXSceneLevel; @@ -94,7 +95,12 @@ public void create() heightMapImage = new GpuMat(intrinsics.getWidth(), intrinsics.getHeight(), opencv_core.CV_16UC1); - extractor = new RapidHeightMapExtractorCUDA(leftFootSoleFrame, rightFootSoleFrame, heightMapImage, intrinsics, 1); + extractor = new RapidHeightMapExtractorCUDA(leftFootSoleFrame, + rightFootSoleFrame, + heightMapImage, + intrinsics, + 1, + RapidHeightMapManager.getHeightMapParameters()); baseUI.getPrimaryScene().addRenderableProvider(heightMapGraphic, RDXSceneLevel.MODEL); } @@ -144,8 +150,8 @@ public void render() PerceptionMessageTools.convertToHeightMapData(sensorSimulator.getDepthImage().getCpuImageMat(), heightMapData, croppedHeightMapImageMessage.getPosition(), - (float) RapidHeightMapExtractor.getHeightMapParameters().getGlobalWidthInMeters(), - (float) RapidHeightMapExtractor.getHeightMapParameters().getGlobalCellSizeInMeters()); + (float) RapidHeightMapManager.getHeightMapParameters().getGlobalWidthInMeters(), + (float) RapidHeightMapManager.getHeightMapParameters().getGlobalCellSizeInMeters()); HeightMapMessageTools.toMessage(heightMapData, heightMapMessage); // List multiColorMeshBuilders = RDXHeightMapGraphicNew.generateHeightCells(heightsProvided, diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/ControllerFootstepQueueMonitor.java similarity index 71% rename from ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java rename to ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/ControllerFootstepQueueMonitor.java index 51c827c5be7..208c77454b3 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java +++ b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/ControllerFootstepQueueMonitor.java @@ -1,4 +1,4 @@ -package us.ihmc.behaviors.activeMapping; +package us.ihmc.humanoidRobotics.communication; import controller_msgs.msg.dds.FootstepQueueStatusMessage; import controller_msgs.msg.dds.FootstepStatusMessage; @@ -6,8 +6,9 @@ import controller_msgs.msg.dds.QueuedFootstepStatusMessage; import controller_msgs.msg.dds.WalkingStatusMessage; import us.ihmc.communication.HumanoidControllerAPI; +import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; import us.ihmc.humanoidRobotics.communication.packets.walking.WalkingStatus; -import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames; import us.ihmc.log.LogTools; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.ros2.ROS2Node; @@ -25,19 +26,11 @@ public class ControllerFootstepQueueMonitor private final AtomicReference footstepStatusMessage = new AtomicReference<>(new FootstepStatusMessage()); private final AtomicReference planOffsetMessage = new AtomicReference<>(new PlanOffsetStatus()); - private final HumanoidReferenceFrames referenceFrames; - private final ContinuousHikingLogger continuousHikingLogger; private boolean footstepStarted; private final AtomicBoolean isWalking = new AtomicBoolean(false); - public ControllerFootstepQueueMonitor(ROS2Node ros2Node, - String simpleRobotName, - HumanoidReferenceFrames referenceFrames, - ContinuousHikingLogger continuousHikingLogger) + public ControllerFootstepQueueMonitor(ROS2Node ros2Node, String simpleRobotName) { - this.referenceFrames = referenceFrames; - this.continuousHikingLogger = continuousHikingLogger; - ros2Node.createSubscription2(HumanoidControllerAPI.getTopic(FootstepQueueStatusMessage.class, simpleRobotName), this::footstepQueueStatusReceived); ros2Node.createSubscription2(HumanoidControllerAPI.getTopic(FootstepStatusMessage.class, simpleRobotName), this::footstepStatusReceived); ros2Node.createSubscription2(getTopic(PlanOffsetStatus.class, simpleRobotName), this::acceptPlanOffsetStatus); @@ -51,7 +44,6 @@ private void footstepQueueStatusReceived(FootstepQueueStatusMessage footstepQueu { String message = String.format("Latest Controller Queue Footstep Size: " + footstepQueueStatusMessage.getQueuedFootstepList().size()); LogTools.info(message); - continuousHikingLogger.appendString(message); } // For the statistics set the that controller queue size before getting the new one @@ -60,14 +52,6 @@ private void footstepQueueStatusReceived(FootstepQueueStatusMessage footstepQueu private void footstepStatusReceived(FootstepStatusMessage footstepStatusMessage) { - if (footstepStatusMessage.getFootstepStatus() == FootstepStatusMessage.FOOTSTEP_STATUS_COMPLETED) - { - double distance = referenceFrames.getSoleFrame(RobotSide.LEFT) - .getTransformToDesiredFrame(referenceFrames.getSoleFrame(RobotSide.RIGHT)) - .getTranslation() - .norm(); - } - footstepStarted = footstepStatusMessage.getFootstepStatus() == FootstepStatusMessage.FOOTSTEP_STATUS_STARTED; this.footstepStatusMessage.set(footstepStatusMessage); @@ -100,6 +84,42 @@ public AtomicReference getFootstepStatusMessage() return footstepStatusMessage; } + public int getNumberOfIncompleteFootsteps() + { + return controllerQueueSize; + } + + /** + * This method assumes the list is not empty; you need to check outside this method that the list has at least one in it + */ + public FramePose3DReadOnly getLastFootstepInQueue() + { + FramePose3D previousFootstepPose = new FramePose3D(); + + previousFootstepPose.getPosition().set(controllerQueue.get(controllerQueueSize - 1).getLocation()); + previousFootstepPose.getRotation().setToYawOrientation(controllerQueue.get(controllerQueueSize - 1).getOrientation().getYaw()); + + return previousFootstepPose; + } + + /** + * This method assumes the list is not empty; you need to check outside this method that the list has at least one in it + */ + public FramePose3DReadOnly getLastFootstepQueuedOnOppositeSide(RobotSide candidateFootstepSide) + { + FramePose3D previousFootstepPose = new FramePose3D(); + + int i = controllerQueue.size() - 1; + // Moved the index of the list to the last step on the other side + while (i >= 1 && controllerQueue.get(i).getRobotSide() == candidateFootstepSide.toByte()) + --i; + + previousFootstepPose.getPosition().set(controllerQueue.get(i).getLocation()); + previousFootstepPose.getRotation().setToYawOrientation(controllerQueue.get(i).getOrientation().getYaw()); + + return previousFootstepPose; + } + // TODO Polling this in multiple threads may cause issues as the second time its pulled the value will be null. // TODO If needed this should change to handle that case correctly. public PlanOffsetStatus pollPlanOffsetMessage() diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java new file mode 100644 index 00000000000..1c3fe4b32e3 --- /dev/null +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java @@ -0,0 +1,134 @@ +package us.ihmc.perception.gpuHeightMap; + +import org.bytedeco.cuda.cudart.CUstream_st; +import org.bytedeco.cuda.cudart.cudaExtent; +import org.bytedeco.cuda.cudart.cudaPitchedPtr; +import org.bytedeco.cuda.cudart.dim3; +import org.bytedeco.opencv.global.opencv_core; +import org.bytedeco.opencv.opencv_core.GpuMat; +import us.ihmc.perception.cuda.CUDAKernel; +import us.ihmc.perception.cuda.CUDAProgram; +import us.ihmc.perception.cuda.CUDATools; + +import java.net.URL; + +import static org.bytedeco.cuda.global.cudart.*; + +public class FilteredRapidHeightMapExtractor +{ + private static final int BLOCK_SIZE_XY = 32; + private static final float ALPHA = 0.2F; + + private final cudaPitchedPtr pointerTo3DArray; + private int currentIndex; + private final int layers; + + private final CUstream_st stream; + private final int rows; + private final int cols; + private final CUDAKernel kernel; + private final CUDAProgram program; + private int loopTracker = 0; + + public FilteredRapidHeightMapExtractor(CUstream_st stream, int rows, int cols, int layers) + { + this.stream = stream; + this.rows = rows; + this.cols = cols; + this.layers = layers; + + // Load header and main file + URL kernelPath = getClass().getResource("FilteredRapidHeightMapExtractor.cu"); + try + { + program = new CUDAProgram(kernelPath); + kernel = program.loadKernel("filterRapidHeightMap"); + } + catch (Exception e) + { + throw new RuntimeException(e); + } + + pointerTo3DArray = new cudaPitchedPtr(); + cudaExtent extent = make_cudaExtent((long) cols * Short.BYTES, rows, layers); + int error = cudaMalloc3D(pointerTo3DArray, extent); + CUDATools.checkCUDAError(error); + + currentIndex = 0; + } + + public GpuMat update(GpuMat latestGlobalHeightMap) + { + int error; + + // Only want to compute the average if we have the past values to use + if (loopTracker < layers) + { + loopTracker++; + + error = cudaMemcpy2D(pointerTo3DArray.ptr().position(currentIndex * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), + pointerTo3DArray.pitch(), + latestGlobalHeightMap.data(), + latestGlobalHeightMap.step(), + pointerTo3DArray.xsize(), + pointerTo3DArray.ysize(), + cudaMemcpyDefault); + + CUDATools.checkCUDAError(error); + + currentIndex = (currentIndex + 1) % layers; + return latestGlobalHeightMap; + } + + GpuMat result = new GpuMat(rows, cols, opencv_core.CV_16UC1); + + kernel.withPointer(pointerTo3DArray.ptr()).withLong(pointerTo3DArray.pitch()); + kernel.withPointer(result.data()).withLong(result.step()); + kernel.withPointer(latestGlobalHeightMap.data()).withLong(latestGlobalHeightMap.step()); + kernel.withInt(layers); + kernel.withLong(pointerTo3DArray.pitch() * rows); + kernel.withInt(rows); + kernel.withInt(cols); + kernel.withFloat(ALPHA); + + //Execute the CUDA kernels with the provided stream + //Each kernel performs a specific task related to the height map update, registration, and cropping + int registerKernelGridSizeXY = (rows + BLOCK_SIZE_XY - 1) / BLOCK_SIZE_XY; + + dim3 blockSize = new dim3(BLOCK_SIZE_XY, BLOCK_SIZE_XY, 1); + dim3 registerKernelGridDim = new dim3(registerKernelGridSizeXY, registerKernelGridSizeXY, 1); + + kernel.run(stream, registerKernelGridDim, blockSize, 0); + + error = cudaStreamSynchronize(stream); + CUDATools.checkCUDAError(error); + + // Upload the latest height map to the GPU for the next iteration + error = cudaMemcpy2D(pointerTo3DArray.ptr().position(currentIndex * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), + pointerTo3DArray.pitch(), + latestGlobalHeightMap.data(), + latestGlobalHeightMap.step(), + pointerTo3DArray.xsize(), + pointerTo3DArray.ysize(), + cudaMemcpyDefault); + + CUDATools.checkCUDAError(error); + + currentIndex = (currentIndex + 1) % layers; + + return result; + } + + public void reset() + { + loopTracker = 0; + } + + public void destroy() + { + program.close(); + kernel.close(); + int error = cudaFree(pointerTo3DArray); + CUDATools.checkCUDAError(error); + } +} diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapDriftOffset.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapDriftOffset.java similarity index 95% rename from ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapDriftOffset.java rename to ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapDriftOffset.java index 04419b02d6b..d1c58799a55 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapDriftOffset.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapDriftOffset.java @@ -1,8 +1,8 @@ -package us.ihmc.perception; +package us.ihmc.perception.gpuHeightMap; import controller_msgs.msg.dds.PlanOffsetStatus; -import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; /** * This class is meant to keep track of the drift in Z that is being calculated in the controller on the robot. 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 da3bcd87ad2..60a23f082a5 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 @@ -60,7 +60,6 @@ public class RapidHeightMapExtractor implements RapidHeightMapExtractorInterface private final SteppableRegionCalculatorParameters steppableRegionParameters = new SteppableRegionCalculatorParameters(); - private static HeightMapParameters heightMapParameters = new HeightMapParameters("GPU"); private final RigidBodyTransform currentSensorToWorldTransform = new RigidBodyTransform(); private final RigidBodyTransform currentGroundToWorldTransform = new RigidBodyTransform(); private final Point3D sensorOrigin = new Point3D(); @@ -119,21 +118,28 @@ public class RapidHeightMapExtractor implements RapidHeightMapExtractorInterface private TerrainMapData terrainMapData; private Mat denoisedHeightMapImage; private Rect cropWindowRectangle; + private HeightMapParameters heightMapParameters; public RapidHeightMapExtractor(OpenCLManager openCLManager, ReferenceFrame leftFootSoleFrame, ReferenceFrame rightFootSoleFrame, BytedecoImage depthImage, CameraIntrinsics depthCameraIntrinsics, - int mode) + int mode, + HeightMapParameters heightMapParameters) { - this(openCLManager, depthImage, depthCameraIntrinsics, mode); + this(openCLManager, depthImage, depthCameraIntrinsics, mode, heightMapParameters); footSoleFrames.put(RobotSide.LEFT, leftFootSoleFrame); footSoleFrames.put(RobotSide.RIGHT, rightFootSoleFrame); } - public RapidHeightMapExtractor(OpenCLManager openCLManager, BytedecoImage depthImage, CameraIntrinsics depthCameraIntrinsics, int mode) + public RapidHeightMapExtractor(OpenCLManager openCLManager, + BytedecoImage depthImage, + CameraIntrinsics depthCameraIntrinsics, + int mode, + HeightMapParameters heightMapParameters) { + this.heightMapParameters = heightMapParameters; this.openCLManager = openCLManager; this.inputDepthImage = depthImage; this.depthCameraIntrinsics = depthCameraIntrinsics; @@ -787,14 +793,8 @@ else if (steppableCell.isBorderCell()) public Mat getCroppedImage(Point3DReadOnly origin, int globalCenterIndex, Mat imageToCrop) { - int xIndex = HeightMapTools.coordinateToIndex(origin.getX(), - 0, - RapidHeightMapExtractor.getHeightMapParameters().getGlobalCellSizeInMeters(), - globalCenterIndex); - int yIndex = HeightMapTools.coordinateToIndex(origin.getY(), - 0, - RapidHeightMapExtractor.getHeightMapParameters().getGlobalCellSizeInMeters(), - globalCenterIndex); + int xIndex = HeightMapTools.coordinateToIndex(origin.getX(), 0, heightMapParameters.getGlobalCellSizeInMeters(), globalCenterIndex); + int yIndex = HeightMapTools.coordinateToIndex(origin.getY(), 0, heightMapParameters.getGlobalCellSizeInMeters(), globalCenterIndex); cropWindowRectangle = new Rect((yIndex - heightMapParameters.getCropWindowSize() / 2), (xIndex - heightMapParameters.getCropWindowSize() / 2), heightMapParameters.getCropWindowSize(), @@ -804,8 +804,8 @@ public Mat getCroppedImage(Point3DReadOnly origin, int globalCenterIndex, Mat im public HeightMapData getHeightMapData() { - HeightMapData latestHeightMapData = new HeightMapData((float) getHeightMapParameters().getGlobalCellSizeInMeters(), - (float) getHeightMapParameters().getGlobalWidthInMeters(), + HeightMapData latestHeightMapData = new HeightMapData((float) heightMapParameters.getGlobalCellSizeInMeters(), + (float) heightMapParameters.getGlobalWidthInMeters(), getSensorOrigin().getX(), getSensorOrigin().getY()); @@ -813,8 +813,8 @@ public HeightMapData getHeightMapData() PerceptionMessageTools.convertToHeightMapData(heightMapMat, latestHeightMapData, getSensorOrigin(), - (float) getHeightMapParameters().getGlobalWidthInMeters(), - (float) getHeightMapParameters().getGlobalCellSizeInMeters()); + (float) heightMapParameters.getGlobalWidthInMeters(), + (float) heightMapParameters.getGlobalCellSizeInMeters()); return latestHeightMapData; } @@ -864,11 +864,6 @@ public boolean isHeightMapDataAvailable() return heightMapDataAvailable; } - public static HeightMapParameters getHeightMapParameters() - { - return heightMapParameters; - } - public boolean isInitialized() { return initialized; 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 3b9ad9b593f..864aaeaa150 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,17 +33,17 @@ public class RapidHeightMapExtractorCUDA implements RapidHeightMapExtractorInter { private static final boolean PRINT_TIMING_FOR_KERNELS = false; static final int BLOCK_SIZE_XY = 32; - static final HeightMapParameters heightMapParameters = new HeightMapParameters("GPU"); private final SideDependentList footSoleFrames = new SideDependentList<>(); private final TerrainMapData terrainMapData; private final CameraIntrinsics cameraIntrinsics; private final Point3D sensorOrigin = new Point3D(); private final int mode; // 0 -> Ouster, 1 -> Realsense + private final HeightMapParameters heightMapParameters; private final GpuMat inputDepthImage; private final GpuMat localHeightMapImage; - private final GpuMat globalHeightMapImage; + private GpuMat globalHeightMapImage; private final GpuMat terrainCostImage; private final GpuMat contactMapImage; private final GpuMat sensorCroppedHeightMapImage; @@ -70,6 +70,7 @@ public class RapidHeightMapExtractorCUDA implements RapidHeightMapExtractorInter private final FloatPointer worldToGroundTransformDevicePointer; private final FloatPointer parametersHostPointer; private final FloatPointer parametersDevicePointer; + private final FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor; public int sequenceNumber = 0; private float gridOffsetX; @@ -92,11 +93,13 @@ public RapidHeightMapExtractorCUDA(ReferenceFrame leftFootSoleFrame, ReferenceFrame rightFootSoleFrame, GpuMat depthImage, CameraIntrinsics depthImageIntrinsics, - int mode) + int mode, + HeightMapParameters heightMapParameters) { inputDepthImage = depthImage; this.cameraIntrinsics = depthImageIntrinsics; this.mode = mode; + this.heightMapParameters = heightMapParameters; footSoleFrames.put(RobotSide.LEFT, leftFootSoleFrame); footSoleFrames.put(RobotSide.RIGHT, rightFootSoleFrame); @@ -111,6 +114,8 @@ public RapidHeightMapExtractorCUDA(ReferenceFrame leftFootSoleFrame, terrainMapData = new TerrainMapData(heightMapParameters.getCropWindowSize(), heightMapParameters.getCropWindowSize()); recomputeDerivedParameters(); + // Need to initialize this after the parameters have been computed to get the right size + filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, globalCellsPerAxis, globalCellsPerAxis, 6); try { @@ -150,7 +155,7 @@ public RapidHeightMapExtractorCUDA(ReferenceFrame leftFootSoleFrame, parametersHostPointer = new FloatPointer(37); parametersDevicePointer = new FloatPointer(); - snappedFootstepsExtractor = new SnappingHeightMapExtractor(terrainMapData); + snappedFootstepsExtractor = new SnappingHeightMapExtractor(heightMapParameters, terrainMapData); } catch (Exception e) { @@ -160,11 +165,6 @@ public RapidHeightMapExtractorCUDA(ReferenceFrame leftFootSoleFrame, reset(); } - public static HeightMapParameters getHeightMapParameters() - { - return heightMapParameters; - } - private void recomputeDerivedParameters() { centerIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getLocalWidthInMeters(), heightMapParameters.getLocalCellSizeInMeters()); @@ -196,6 +196,7 @@ public void reset() globalHeightMapImage.setTo(new Scalar(resetOffset)); emptyGlobalHeightMapImage.setTo(new Scalar(resetOffset)); + filteredRapidHeightMapExtractor.reset(); snappedFootstepsExtractor.reset(resetOffset); sequenceNumber = 0; @@ -286,6 +287,9 @@ public void update(RigidBodyTransform sensorToWorldTransform, RigidBodyTransform error = cudaStreamSynchronize(stream); CUDATools.checkCUDAError(error); + if (heightMapParameters.getEnableAlphaFilter()) + globalHeightMapImage = filteredRapidHeightMapExtractor.update(globalHeightMapImage); + // Run the cropping kernel croppingKernel.withPointer(globalHeightMapImage.data()).withLong(globalHeightMapImage.step()); croppingKernel.withPointer(sensorCroppedHeightMapImage.data()).withLong(sensorCroppedHeightMapImage.step()); @@ -432,6 +436,7 @@ public void destroy() sensorCroppedHeightMapImage.close(); snappedFootstepsExtractor.destroy(); + filteredRapidHeightMapExtractor.destroy(); // At the end we have to destroy the stream to release the memory CUDAStreamManager.releaseStream(stream); @@ -460,8 +465,8 @@ public int getSequenceNumber() public HeightMapData getHeightMapData() { - HeightMapData latestHeightMapData = new HeightMapData((float) getHeightMapParameters().getGlobalCellSizeInMeters(), - (float) getHeightMapParameters().getGlobalWidthInMeters(), + HeightMapData latestHeightMapData = new HeightMapData((float) heightMapParameters.getGlobalCellSizeInMeters(), + (float) heightMapParameters.getGlobalWidthInMeters(), getSensorOrigin().getX(), getSensorOrigin().getY()); @@ -469,8 +474,8 @@ public HeightMapData getHeightMapData() PerceptionMessageTools.convertToHeightMapData(heightMapMat, latestHeightMapData, getSensorOrigin(), - (float) getHeightMapParameters().getGlobalWidthInMeters(), - (float) getHeightMapParameters().getGlobalCellSizeInMeters()); + (float) heightMapParameters.getGlobalWidthInMeters(), + (float) heightMapParameters.getGlobalCellSizeInMeters()); return latestHeightMapData; } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java similarity index 77% rename from ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java rename to ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java index 901139e15ba..a0f1b560938 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java @@ -1,4 +1,4 @@ -package us.ihmc.perception; +package us.ihmc.perception.gpuHeightMap; import controller_msgs.msg.dds.HighLevelStateChangeStatusMessage; import org.bytedeco.javacpp.BytePointer; @@ -7,25 +7,25 @@ import org.bytedeco.opencv.opencv_core.GpuMat; import org.bytedeco.opencv.opencv_core.Mat; import perception_msgs.msg.dds.ImageMessage; -import us.ihmc.avatar.drcRobot.DRCRobotModel; -import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; import us.ihmc.commons.thread.Notification; import us.ihmc.communication.HumanoidControllerAPI; import us.ihmc.communication.PerceptionAPI; -import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; import us.ihmc.euclid.referenceFrame.FramePose3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.transform.RigidBodyTransform; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; +import us.ihmc.perception.BytedecoImage; import us.ihmc.perception.camera.CameraIntrinsics; import us.ihmc.perception.filters.CUDAFlyingPointsFilter; -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; import us.ihmc.perception.tools.PerceptionMessageTools; +import us.ihmc.robotModels.FullHumanoidRobotModel; +import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2Publisher; import us.ihmc.sensorProcessing.heightMap.HeightMapData; +import us.ihmc.sensorProcessing.heightMap.HeightMapParameters; import java.time.Instant; @@ -34,10 +34,11 @@ */ public class RapidHeightMapManager { + private static final HeightMapParameters heightMapParameters = new HeightMapParameters("GPU"); + private final RapidHeightMapExtractorInterface rapidHeightMapExtractor; private final ImageMessage croppedHeightMapImageMessage = new ImageMessage(); private final FramePose3D cameraPose = new FramePose3D(); - private final ROS2PublishSubscribeAPI ros2; private final boolean runWithCUDA; private final Mat hostDepthImage = new Mat(); private final Notification resetHeightMapRequested = new Notification(); @@ -48,22 +49,28 @@ public class RapidHeightMapManager private GpuMat deviceDepthImage; private BytedecoImage heightMapBytedecoImage; + private final ROS2Publisher heightMapPublisher; - public RapidHeightMapManager(ROS2PublishSubscribeAPI ros2, - DRCRobotModel robotModel, + public RapidHeightMapManager(ROS2Node ros2Node, + FullHumanoidRobotModel robotModel, + String robotName, ReferenceFrame leftFootSoleFrame, ReferenceFrame rightFootSoleFrame, ControllerFootstepQueueMonitor controllerFootstepQueueMonitor, CameraIntrinsics depthImageIntrinsics, boolean runWithCUDA) throws Exception { - this.ros2 = ros2; this.runWithCUDA = runWithCUDA; if (runWithCUDA) { deviceDepthImage = new GpuMat(depthImageIntrinsics.getHeight(), depthImageIntrinsics.getWidth(), opencv_core.CV_16UC1); - rapidHeightMapExtractor = new RapidHeightMapExtractorCUDA(leftFootSoleFrame, rightFootSoleFrame, deviceDepthImage, depthImageIntrinsics, 1); + rapidHeightMapExtractor = new RapidHeightMapExtractorCUDA(leftFootSoleFrame, + rightFootSoleFrame, + deviceDepthImage, + depthImageIntrinsics, + 1, + heightMapParameters); rapidHeightMapDriftOffset = new RapidHeightMapDriftOffset(controllerFootstepQueueMonitor); flyingPointsFilter = new CUDAFlyingPointsFilter(); } @@ -77,17 +84,22 @@ public RapidHeightMapManager(ROS2PublishSubscribeAPI ros2, rightFootSoleFrame, heightMapBytedecoImage, depthImageIntrinsics, - 1); + 1, + heightMapParameters); } + heightMapPublisher = ros2Node.createPublisher(PerceptionAPI.HEIGHT_MAP_CROPPED); + // 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()); - if (robotModel != null) // Will be null on test bench + ros2Node.createSubscription2(PerceptionAPI.RESET_HEIGHT_MAP, message -> resetHeightMapRequested.set()); + if (robotModel != null) { - ros2.subscribeViaVolatileCallback(HumanoidControllerAPI.getTopic(HighLevelStateChangeStatusMessage.class, robotModel.getSimpleRobotName()), message -> - { // Automatically reset the height map when the robot goes into the walking state - if (message.getEndHighLevelControllerName() == HighLevelStateChangeStatusMessage.WALKING) + ros2Node.createSubscription(HumanoidControllerAPI.getTopic(HighLevelStateChangeStatusMessage.class, robotName), message -> + { + if (message.takeNextData().getEndHighLevelControllerName() == HighLevelStateChangeStatusMessage.WALKING) + { resetHeightMapRequested.set(); + } }); } } @@ -146,7 +158,7 @@ public void update(Mat latestDepthImage, Instant imageAcquisitionTime, Reference Mat croppedHeightMapImage = rapidHeightMapExtractor.getTerrainMapData().getHeightMap(); - if (runWithCUDA && RapidHeightMapExtractorCUDA.getHeightMapParameters().getFlyingPointsFilter()) + if (runWithCUDA && getHeightMapParameters().getFlyingPointsFilter()) { GpuMat deviceCroppedHeightMapImage = new GpuMat(); deviceCroppedHeightMapImage.upload(croppedHeightMapImage); @@ -156,27 +168,16 @@ public void update(Mat latestDepthImage, Instant imageAcquisitionTime, Reference deviceCroppedHeightMapImage.close(); } - float heightScaleFactor; - if (runWithCUDA) - { - heightScaleFactor = (float) RapidHeightMapExtractorCUDA.getHeightMapParameters().getHeightScaleFactor(); - } - else - { - heightScaleFactor = (float) RapidHeightMapExtractor.getHeightMapParameters().getHeightScaleFactor(); - } - OpenCVTools.compressImagePNG(croppedHeightMapImage, compressedCroppedHeightMapPointer); PerceptionMessageTools.publishCompressedDepthImage(compressedCroppedHeightMapPointer, - PerceptionAPI.HEIGHT_MAP_CROPPED, croppedHeightMapImageMessage, - ros2, + heightMapPublisher, cameraPose, imageAcquisitionTime, rapidHeightMapExtractor.getSequenceNumber(), croppedHeightMapImage.rows(), croppedHeightMapImage.cols(), - heightScaleFactor); + (float) getHeightMapParameters().getHeightScaleFactor()); } public HeightMapData getLatestHeightMapData() @@ -189,6 +190,11 @@ public TerrainMapData getTerrainMapData() return rapidHeightMapExtractor.getTerrainMapData(); } + public static HeightMapParameters getHeightMapParameters() + { + return heightMapParameters; + } + public void destroy() { rapidHeightMapExtractor.destroy(); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingHeightMapExtractor.java index 199da1f2f59..952dbfbabd0 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingHeightMapExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingHeightMapExtractor.java @@ -15,13 +15,13 @@ import us.ihmc.perception.cuda.CUDATools; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.perception.steppableRegions.SteppableRegionCalculatorParameters; +import us.ihmc.sensorProcessing.heightMap.HeightMapParameters; import java.net.URL; import static org.bytedeco.cuda.global.cudart.cudaFree; import static org.bytedeco.cuda.global.cudart.cudaStreamSynchronize; import static us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractorCUDA.BLOCK_SIZE_XY; -import static us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractorCUDA.heightMapParameters; public class SnappingHeightMapExtractor { @@ -29,6 +29,7 @@ public class SnappingHeightMapExtractor private final SteppableRegionCalculatorParameters steppableRegionParameters = new SteppableRegionCalculatorParameters(); + private final HeightMapParameters heightMapParameters; private final TerrainMapData terrainMapData; private final CUstream_st stream; @@ -48,8 +49,9 @@ public class SnappingHeightMapExtractor private final GpuMat snapNormalZImage; private final GpuMat snappedAreaFractionImage; - public SnappingHeightMapExtractor(TerrainMapData terrainMapData) + public SnappingHeightMapExtractor(HeightMapParameters heightMapParameters, TerrainMapData terrainMapData) { + this.heightMapParameters = heightMapParameters; this.terrainMapData = terrainMapData; try diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/heightMap/TerrainMapData.java b/ihmc-perception/src/main/java/us/ihmc/perception/heightMap/TerrainMapData.java index a8f1e6f2217..aa490119757 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/heightMap/TerrainMapData.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/heightMap/TerrainMapData.java @@ -8,6 +8,7 @@ import us.ihmc.euclid.tuple3D.UnitVector3D; import us.ihmc.euclid.tuple3D.interfaces.UnitVector3DReadOnly; import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.steppableRegions.SnapResult; import us.ihmc.robotics.geometry.LeastSquaresZPlaneFitter; import us.ihmc.sensorProcessing.heightMap.HeightMapData; @@ -25,7 +26,7 @@ public class TerrainMapData private final LeastSquaresZPlaneFitter planeFitter = new LeastSquaresZPlaneFitter(); - private int localGridSize = 201; + private final int localGridSize; private int cellsPerMeter = 50; private Mat heightMap; @@ -35,12 +36,12 @@ public class TerrainMapData private Mat steppableRegionAssignmentMat; private Mat steppableRegionRingMat; private Mat steppabilityImage; + private Mat steppabilityConnectionsImage; private Mat snapHeightImage; private Mat snappedAreaFractionImage; private Mat snapNormalXImage; private Mat snapNormalYImage; private Mat snapNormalZImage; - private Mat steppabilityConnectionsImage; public TerrainMapData(Mat heightMap, Mat contactMap) { @@ -62,20 +63,15 @@ public TerrainMapData(Mat heightMap, setContactMap(contactMap); setTerrainCostMap(terrainCostMap); setSteppabilityImage(steppability); + setSnappedAreaFractionImage(snappedAreaFractionImage); setSnapNormalXImage(snapNormalXImage); setSnapNormalYImage(snapNormalYImage); setSnapNormalZImage(snapNormalZImage); - setSnappedAreaFractionImage(snappedAreaFractionImage); this.localGridSize = heightMap.rows(); // TODO need to add cells per meter } - public TerrainMapData(TerrainMapData other) - { - set(other); - } - public TerrainMapData(int height, int width) { heightMap = new Mat(height, width, opencv_core.CV_16UC1); @@ -84,27 +80,31 @@ public TerrainMapData(int height, int width) localGridSize = height; } - public void set(TerrainMapData other) + public TerrainMapData(TerrainMapData other) { this.localGridSize = other.localGridSize; this.cellsPerMeter = other.cellsPerMeter; heightMapCenter.set(other.heightMapCenter); setHeightMap(other.heightMap); - setSnapHeightImage(other.snapHeightImage); setContactMap(other.contactMap); setTerrainCostMap(other.terrainCostMap); + + setSteppableRegionAssignmentMat(other.steppableRegionAssignmentMat); + setSteppableRegionRingMat(other.steppableRegionRingMat); setSteppabilityImage(other.steppabilityImage); + setSteppabilityConnectionsImage(other.steppabilityConnectionsImage); + setSnapHeightImage(other.snapHeightImage); + setSnappedAreaFractionImage(other.snappedAreaFractionImage); setSnapNormalXImage(other.snapNormalXImage); setSnapNormalYImage(other.snapNormalYImage); setSnapNormalZImage(other.snapNormalZImage); - setSnappedAreaFractionImage(other.snappedAreaFractionImage); } private int getLocalIndex(double coordinate, double center) { // TODO probably a height map tools method for this. - return (int) ((coordinate - center) * cellsPerMeter + localGridSize / 2); + return (int) ((coordinate - center) * cellsPerMeter + (double) localGridSize / 2); } public float getSnappedAreaFractionInWorld(double x, double y) @@ -173,8 +173,8 @@ private boolean isOutOfBounds(int rIndex, int cIndex) private static float convertScaledAndOffsetValue(float value) { - return (float) (value / RapidHeightMapExtractor.getHeightMapParameters().getHeightScaleFactor()) - - (float) RapidHeightMapExtractor.getHeightMapParameters().getHeightOffset(); + return (float) (value / RapidHeightMapManager.getHeightMapParameters().getHeightScaleFactor()) - (float) RapidHeightMapManager.getHeightMapParameters() + .getHeightOffset(); } private float getHeightLocal(int rIndex, int cIndex) @@ -246,8 +246,8 @@ private int getSteppabilityLocal(int rIndex, int cIndex) public void setHeightLocal(float height, int rIndex, int cIndex) { - float offsetHeight = height + (float) RapidHeightMapExtractor.getHeightMapParameters().getHeightOffset(); - int finalHeight = (int) (offsetHeight * RapidHeightMapExtractor.getHeightMapParameters().getHeightScaleFactor()); + float offsetHeight = height + (float) RapidHeightMapManager.getHeightMapParameters().getHeightOffset(); + int finalHeight = (int) (offsetHeight * RapidHeightMapManager.getHeightMapParameters().getHeightScaleFactor()); heightMap.ptr(rIndex, cIndex).putShort((short) finalHeight); } 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 5c4fb551a83..381dc796f2f 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 @@ -15,18 +15,16 @@ import us.ihmc.communication.packets.PlanarRegionMessageConverter; import us.ihmc.communication.producers.VideoSource; import us.ihmc.communication.ros2.ROS2Helper; -import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; import us.ihmc.euclid.geometry.interfaces.Pose3DReadOnly; import us.ihmc.euclid.tuple3D.Point3D; import us.ihmc.euclid.tuple4D.Quaternion; import us.ihmc.idl.IDLSequence; import us.ihmc.perception.RawImage; -import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.perception.imageMessage.CompressionType; import us.ihmc.perception.imageMessage.PixelFormat; import us.ihmc.perception.opencv.OpenCVTools; -import us.ihmc.ros2.ROS2Node; import us.ihmc.ros2.ROS2Publisher; import us.ihmc.sensors.realsense.RealSenseDevice; import us.ihmc.robotics.geometry.FramePlanarRegionsList; @@ -76,9 +74,8 @@ public static void toBoofCV(ImageMessage imageMessage, Object cameraPinholeToPac } public static void publishCompressedDepthImage(BytePointer compressedDepthPointer, - ROS2Topic topic, ImageMessage depthImageMessage, - ROS2PublishSubscribeAPI helper, + ROS2Publisher publisher, Pose3DReadOnly cameraPose, Instant acquisitionTime, long sequenceNumber, @@ -87,7 +84,7 @@ public static void publishCompressedDepthImage(BytePointer compressedDepthPointe float depthToMetersRatio) { packCompressedDepthImage(compressedDepthPointer, depthImageMessage, cameraPose, acquisitionTime, sequenceNumber, height, width, depthToMetersRatio); - helper.publish(topic, depthImageMessage); + publisher.publish(depthImageMessage); } public static void publishJPGCompressedColorImage(BytePointer compressedColorPointer, @@ -107,7 +104,7 @@ public static void publishJPGCompressedColorImage(BytePointer compressedColorPoi public static void publishFramePlanarRegionsList(FramePlanarRegionsList framePlanarRegionsList, ROS2Topic topic, - ROS2PublishSubscribeAPI ros2) + ROS2Helper ros2) { ros2.publish(topic, PlanarRegionMessageConverter.convertToFramePlanarRegionsListMessage(framePlanarRegionsList)); } @@ -276,8 +273,8 @@ public static void convertToHeightMapData(Mat heightMapPointer, HeightMapData he for (int yIndex = 0; yIndex < cellsPerAxis; yIndex++) { int height = ((int) heightMapPointer.ptr(xIndex, yIndex).getShort() & 0xFFFF); - float cellHeight = (float) ((float) (height) / RapidHeightMapExtractor.getHeightMapParameters().getHeightScaleFactor()) - - (float) RapidHeightMapExtractor.getHeightMapParameters().getHeightOffset(); + float cellHeight = (float) ((float) (height) / RapidHeightMapManager.getHeightMapParameters().getHeightScaleFactor()) + - (float) RapidHeightMapManager.getHeightMapParameters().getHeightOffset(); int key = HeightMapTools.indicesToKey(xIndex, yIndex, centerIndex); heightMapDataToPack.setHeightAt(key, cellHeight); @@ -301,8 +298,8 @@ public static Mat convertHeightMapDataToMat(HeightMapData heightMapData, float w double cellHeight = heightMapData.getHeightAt(key); // Reverse the height calculation to get the raw height value - int height = (int) ((cellHeight + (float) RapidHeightMapExtractor.getHeightMapParameters().getHeightOffset()) - * RapidHeightMapExtractor.getHeightMapParameters().getHeightScaleFactor()); + int height = (int) ((cellHeight + (float) RapidHeightMapManager.getHeightMapParameters().getHeightOffset()) + * RapidHeightMapManager.getHeightMapParameters().getHeightScaleFactor()); // Store the height value in the Mat object heightMapMat.ptr(xIndex, yIndex).putShort((short) height); diff --git a/ihmc-perception/src/main/java/us/ihmc/sensors/deprecated/RealsenseColorDepthPublisher.java b/ihmc-perception/src/main/java/us/ihmc/sensors/deprecated/RealsenseColorDepthPublisher.java index a7939a670f4..fbae05ff085 100644 --- a/ihmc-perception/src/main/java/us/ihmc/sensors/deprecated/RealsenseColorDepthPublisher.java +++ b/ihmc-perception/src/main/java/us/ihmc/sensors/deprecated/RealsenseColorDepthPublisher.java @@ -26,6 +26,7 @@ import us.ihmc.perception.logging.PerceptionLoggerConstants; import us.ihmc.perception.opencv.OpenCVTools; import us.ihmc.perception.parameters.PerceptionConfigurationParameters; +import us.ihmc.ros2.ROS2Publisher; import us.ihmc.sensors.realsense.RealSenseConfiguration; import us.ihmc.sensors.realsense.RealSenseDevice; import us.ihmc.sensors.realsense.RealSenseDeviceManager; @@ -57,7 +58,6 @@ public class RealsenseColorDepthPublisher private final ROS2StoredPropertySetGroup ros2PropertySetGroup; private final Supplier sensorFrameUpdater; private final ROS2Topic colorTopic; - private final ROS2Topic depthTopic; private final ROS2Node ros2Node; private final ROS2Helper ros2Helper; private final PerceptionConfigurationParameters parameters = new PerceptionConfigurationParameters(); @@ -82,6 +82,7 @@ public class RealsenseColorDepthPublisher private boolean loggerInitialized = false; private volatile boolean running = true; private final Notification destroyedNotification = new Notification(); + private final ROS2Publisher depthPublisher; public RealsenseColorDepthPublisher(RealSenseConfiguration realsenseConfiguration, ROS2Topic depthTopic, @@ -89,7 +90,6 @@ public RealsenseColorDepthPublisher(RealSenseConfiguration realsenseConfiguratio Supplier sensorFrameUpdater) { this.colorTopic = colorTopic; - this.depthTopic = depthTopic; this.sensorFrameUpdater = sensorFrameUpdater; realsenseDeviceManager = new RealSenseDeviceManager(); @@ -105,6 +105,8 @@ public RealsenseColorDepthPublisher(RealSenseConfiguration realsenseConfiguratio ros2Node = new ROS2NodeBuilder().build("realsense_color_and_depth_publisher"); ros2Helper = new ROS2Helper(ros2Node); + depthPublisher = ros2Node.createPublisher(depthTopic); + LogTools.info("Setting up ROS2StoredPropertySetGroup"); ros2PropertySetGroup = new ROS2StoredPropertySetGroup(ros2Node); ros2PropertySetGroup.registerStoredPropertySet(PerceptionComms.PERCEPTION_CONFIGURATION_PARAMETERS, parameters); @@ -175,9 +177,8 @@ private void update() PerceptionMessageTools.setDepthIntrinsicsFromRealsense(realsense, depthImageMessage); CameraModel.PINHOLE.packMessageFormat(depthImageMessage); PerceptionMessageTools.publishCompressedDepthImage(compressedDepthPointer, - depthTopic, depthImageMessage, - ros2Helper, + depthPublisher, depthPose, acquisitionTime, depthSequenceNumber++, diff --git a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu new file mode 100644 index 00000000000..9da95db011f --- /dev/null +++ b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu @@ -0,0 +1,40 @@ +extern "C" + +__global__ +void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, + unsigned short * resultPointer, size_t pitchResult, + unsigned short * newestHeightMap, size_t pitchNewestHeightMap, + int layers, size_t layerSize, int rows, int cols, float alpha) +{ + int indexX = blockIdx.x * blockDim.x + threadIdx.x; + int indexY = blockIdx.y * blockDim.y + threadIdx.y; + + if (indexX >= cols || indexY >= rows) + return; + + unsigned int sum = 0; + + // Compute the average height of the history in order to get the variance at each layer + for (int layer = 0; layer < layers; layer++) + { + unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); + unsigned short * matrixCell = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; + +// printf("Layer: %d, Value: %d, %d, and %d\n", layer, indexY, indexX, (int) *matrixCell); + sum += (int) *matrixCell; + } + + unsigned short avg = sum / layers; + + unsigned short * heightValue = (unsigned short*)((char*) newestHeightMap + indexY * pitchNewestHeightMap) + indexX; + + int heightValueInt = (int) *heightValue; + float diff = (float) heightValueInt - avg; + float newVariance = abs(diff); + +// printf("Equation parameters (heightValue %hu) (alpha %f) (avg %hu)\n", *heightValue, alpha, avg); + float heightEstimate = (float) *heightValue * alpha + (avg * (1.0 - alpha)); // (newHeight * newVariance) / newVariance; + + unsigned short *outputPointer = (unsigned short *)((char*) resultPointer + indexY * pitchResult) + indexX; + *outputPointer = (unsigned short) heightEstimate; +} diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java new file mode 100644 index 00000000000..0d8c29bcc8d --- /dev/null +++ b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java @@ -0,0 +1,100 @@ +package us.ihmc.perception.cuda.examples; + +import org.bytedeco.cuda.cudart.CUstream_st; +import org.bytedeco.cuda.cudart.cudaExtent; +import org.bytedeco.cuda.cudart.cudaPitchedPtr; +import org.bytedeco.cuda.cudart.dim3; +import org.bytedeco.opencv.global.opencv_core; +import org.bytedeco.opencv.opencv_core.GpuMat; +import org.bytedeco.opencv.opencv_core.Mat; +import org.bytedeco.opencv.opencv_core.Scalar; +import us.ihmc.perception.cuda.CUDAKernel; +import us.ihmc.perception.cuda.CUDAProgram; +import us.ihmc.perception.cuda.CUDAStreamManager; +import us.ihmc.perception.cuda.CUDATools; +import us.ihmc.perception.tools.PerceptionDebugTools; + +import java.net.URL; + +import static org.bytedeco.cuda.global.cudart.*; + +public class CUDAExampleKernel3D +{ + /** + * This is an example of how to pass in a 3d array into the CUDA kernel. + * Each index will have values in each layer. + * This averages each index's layer and returns the result in a {@link GpuMat}. + */ + public CUDAExampleKernel3D() throws Exception + { + // Create a 2x2 with some amount of layers + int rows = 2; + int cols = 2; + int layers = 4; + + CUstream_st stream = CUDAStreamManager.getStream(); + + URL programPath = getClass().getResource("matrix_3d.cu"); + String kernelName = "matrix_3d_example"; + CUDAProgram program = new CUDAProgram(programPath); + CUDAKernel kernel = program.loadKernel(kernelName); + + // Allocate enough memory for all layers + cudaPitchedPtr pointerTo3DArray = new cudaPitchedPtr(); + cudaExtent extent = make_cudaExtent(cols * Short.BYTES, rows, layers); + int error = cudaMalloc3D(pointerTo3DArray, extent); + CUDATools.checkCUDAError(error); + + for (int i = 0; i < layers; i++) + { + // To get the average to be a whole number do (i * 2 + 2) + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(i * 2 + 2)); + + // Allocate memory for each layer, creating a 3d array + cudaMemcpy2D(pointerTo3DArray.ptr().position(i * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), + pointerTo3DArray.pitch(), + cpuData.data(), + cpuData.step(), + pointerTo3DArray.xsize(), + pointerTo3DArray.ysize(), + cudaMemcpyDefault); + + // Note we can't close the GpuMat here cause we need to access the data later in the program, so add it to a list, and close the list at the end + cpuData.close(); + } + + GpuMat result = new GpuMat(rows, cols, opencv_core.CV_16UC1); + + kernel.withPointer(pointerTo3DArray.ptr()).withLong(pointerTo3DArray.pitch()); + kernel.withPointer(result.data()).withLong(result.step()); + kernel.withLong(pointerTo3DArray.pitch() * rows); + kernel.withInt(rows); + kernel.withInt(cols); + kernel.withInt(layers); + + dim3 gridDim = new dim3(); + dim3 blockDim = new dim3(cols, rows, 1); + kernel.run(stream, gridDim, blockDim, 0); + + error = cudaStreamSynchronize(stream); + CUDATools.checkCUDAError(error); + + // Print the result to make sure we get what we expect + // In this example we go through 2,4,6,8 so we expect the average to be 5 + Mat cpuResult = new Mat(); + result.download(cpuResult); + PerceptionDebugTools.printMat("Result", cpuResult, 1); + + kernel.close(); + program.close(); + gridDim.close(); + blockDim.close(); + + CUDAStreamManager.releaseStream(stream); + } + + public static void main(String[] args) throws Exception + { + new CUDAExampleKernel3D(); + } +} diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java b/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java new file mode 100644 index 00000000000..c93e3e3b1f6 --- /dev/null +++ b/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java @@ -0,0 +1,130 @@ +package us.ihmc.perception.gpuHeightMap; + +import org.bytedeco.cuda.cudart.CUstream_st; +import org.bytedeco.opencv.global.opencv_core; +import org.bytedeco.opencv.opencv_core.GpuMat; +import org.bytedeco.opencv.opencv_core.Mat; +import org.bytedeco.opencv.opencv_core.Scalar; +import org.junit.jupiter.api.Test; +import us.ihmc.perception.cuda.CUDAStreamManager; +import us.ihmc.perception.tools.PerceptionDebugTools; + +import static org.junit.jupiter.api.Assertions.*; + +/** + * Test that we can compute the average and keep a history. + * Also ensures that things are printed correctly. + */ +public class FilteredRapidHeightMapExtractorTest +{ + @Test + public void testGettingAverage() + { + int rows = 10; + int cols = 10; + int layers = 2; + + CUstream_st stream = CUDAStreamManager.getStream(); + FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, rows, cols, layers); + + for (int i = 0; i < 8; i++) + { + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(i * 2 + 2)); + GpuMat latestDepthMat = new GpuMat(); + latestDepthMat.upload(cpuData); + + GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat); + Mat temp = new Mat(); + currentAverage.download(temp); + PerceptionDebugTools.printMat("Current", temp, 1); + } + + filteredRapidHeightMapExtractor.destroy(); + } + + @Test + public void testChangeAfterSteadyState() + { + int rows = 2; + int cols = 2; + int layers = 2; + + CUstream_st stream = CUDAStreamManager.getStream(); + FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, rows, cols, layers); + + // This example fills the history with the same value, all 8's + // Lets see how this behaves when we later than introduce a change (noise) + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(800)); + for (int i = 0; i < 8; i++) + { + cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(800)); + GpuMat latestDepthMat = new GpuMat(); + latestDepthMat.upload(cpuData); + + filteredRapidHeightMapExtractor.update(latestDepthMat); + } + + Mat cpuDataAdjusted = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(1000)); + GpuMat latestDepthMat = new GpuMat(); + latestDepthMat.upload(cpuDataAdjusted); + + GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat); + Mat temp = new Mat(); + currentAverage.download(temp); + PerceptionDebugTools.printMat("Current", temp, 1); + + for (int i = 0; i < temp.rows(); i++) + { + for (int j = 0; j < temp.cols(); j++) + { + // This data should be more than the latest data because it's weighted towards the previous average + assertTrue(temp.col(i).row(j).data().getShort() > cpuData.col(j).data().getShort()); + } + } + + filteredRapidHeightMapExtractor.destroy(); + } + + @Test + public void testChangedAfterSteadyStateRealDepthValuesUnsignedShort() + { + int rows = 2; + int cols = 2; + int layers = 2; + + CUstream_st stream = CUDAStreamManager.getStream(); + FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, rows, cols, layers); + + // This example fills the history with the same value, all 8's + // Lets see how this behaves when we later than introduce a change (noise) + for (int i = 0; i < 8; i++) + { + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(32768)); + GpuMat latestDepthMat = new GpuMat(); + latestDepthMat.upload(cpuData); + + filteredRapidHeightMapExtractor.update(latestDepthMat); + } + + // 400 is about 20 centimeters? Ish depending on the parameters, this is hard coded could change it to be based on the parameters + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(33100)); + GpuMat latestDepthMat = new GpuMat(); + latestDepthMat.upload(cpuData); + + GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat); + Mat temp = new Mat(); + currentAverage.download(temp); + PerceptionDebugTools.printMat("Current", temp, 1); + + for (int i = 0; i < temp.rows(); i++) + { + for (int j = 0; j < temp.cols(); j++) + { + // This data should be less than the latest data because it's weighted towards the previous average + assertTrue(temp.col(i).row(j).data().getShort() < cpuData.col(j).data().getShort()); + } + } + + filteredRapidHeightMapExtractor.destroy(); + } +} diff --git a/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu b/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu new file mode 100644 index 00000000000..cf8a5bb23bb --- /dev/null +++ b/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu @@ -0,0 +1,33 @@ +extern "C" +__global__ +void matrix_3d_example(unsigned short * matrixPointer, size_t pitchA, + unsigned short * resultPointer, size_t pitchResult, + size_t layerSize, int rows, int cols, int layers) +{ + int indexX = blockIdx.x * blockDim.x + threadIdx.x; + int indexY = blockIdx.y * blockDim.y + threadIdx.y; + + if (indexX >= cols || indexY >= rows) + return; + + int sum = 0; + + for (int layer = 0; layer < layers; layer++) + { + // Compute the base address of the current layer + unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); + + // Compute row offset using pitchA + unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; + + printf("Layer: %d, Value: %d, %d, and %d\n", layer, indexY, indexX, (int) *matrixPointerRow); + sum += (int) *matrixPointerRow; + } + + unsigned short avg = sum / layers; + + unsigned short *outputPointer = (unsigned short *)((char*) resultPointer + indexY * pitchResult) + indexX; + *outputPointer = avg; + printf("GPU Average: %d, ", avg); +} + diff --git a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java index fe8e42169c6..218b162923e 100644 --- a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java +++ b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java @@ -20,6 +20,7 @@ public class HeightMapParameters extends StoredPropertySet implements HeightMapP public static final BooleanStoredPropertyKey flyingPointsFilter = keys.addBooleanKey("Flying points filter"); public static final BooleanStoredPropertyKey resetHeightMap = keys.addBooleanKey("Reset Height Map"); + public static final BooleanStoredPropertyKey enableAlphaFilter = keys.addBooleanKey("Enable alpha filter"); public static final IntegerStoredPropertyKey searchWindowHeight = keys.addIntegerKey("Search window height"); public static final IntegerStoredPropertyKey searchWindowWidth = keys.addIntegerKey("Search window width"); public static final DoubleStoredPropertyKey minHeightRegistration = keys.addDoubleKey("Min height registration"); diff --git a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java index fe0f7c4347c..134f6e02ae0 100644 --- a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java +++ b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java @@ -18,6 +18,11 @@ default void setResetHeightMap(boolean resetHeightMap) set(HeightMapParameters.resetHeightMap, resetHeightMap); } + default void setEnableAlphaFilter(boolean enableAlphaFilter) + { + set(HeightMapParameters.enableAlphaFilter, enableAlphaFilter); + } + default void setSearchWindowHeight(int searchWindowHeight) { set(HeightMapParameters.searchWindowHeight, searchWindowHeight); diff --git a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java index 641c126f17c..ec1ac8d67cb 100644 --- a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java +++ b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java @@ -20,6 +20,11 @@ default boolean getResetHeightMap() return get(resetHeightMap); } + default boolean getEnableAlphaFilter() + { + return get(enableAlphaFilter); + } + default int getSearchWindowHeight() { return get(searchWindowHeight); diff --git a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json index c20f5cabae0..03c0ee8e323 100644 --- a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json +++ b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json @@ -2,6 +2,7 @@ "title" : "HeightMapParameters", "Flying points filter" : true, "Reset Height Map" : false, + "Enable alpha filter" : true, "Search window height": { "value": 20, "lowerBound": 10, diff --git a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json index 535647918b9..6b91c14cb6b 100644 --- a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json +++ b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json @@ -2,6 +2,7 @@ "title" : "GPU Height Map Parameters", "Flying points filter" : true, "Reset Height Map" : false, + "Enable alpha filter" : true, "Search window height" : { "value" : 350, "lowerBound" : 10,