diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXVisualizer.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXVisualizer.java index 67daeb078d7..0b89b601a8d 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXVisualizer.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXVisualizer.java @@ -14,7 +14,6 @@ import us.ihmc.rdx.sceneManager.RDXRenderableProvider; import us.ihmc.rdx.sceneManager.RDXSceneLevel; import us.ihmc.rdx.ui.graphics.ros2.RDXROS2MultiTopicVisualizer; -import us.ihmc.rdx.ui.graphics.ros2.RDXROS2RobotVisualizer; import us.ihmc.rdx.ui.graphics.ros2.RDXROS2SingleTopicVisualizer; import us.ihmc.ros2.ROS2Node; import us.ihmc.ros2.ROS2Topic; @@ -98,10 +97,6 @@ else if (this instanceof RDXROS2MultiTopicVisualizer multiTopicVisualizer) if (ImGui.checkbox(labels.get("##Active"), active)) { setActive(active.get()); - if (this instanceof RDXROS2RobotVisualizer robotVisualizer) - { - robotVisualizer.visualizeSensors(active.get()); - } if (getPanel() != null) getPanel().getIsShowing().set(active.get()); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2RobotVisualizer.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2RobotVisualizer.java index 37f84fdad3a..2384d627c59 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2RobotVisualizer.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2RobotVisualizer.java @@ -10,7 +10,6 @@ import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; import us.ihmc.behaviors.tools.MinimalFootstep; import us.ihmc.communication.HumanoidControllerAPI; -import us.ihmc.communication.PerceptionAPI; import us.ihmc.communication.StateEstimatorAPI; import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; import us.ihmc.euclid.geometry.Pose3D; @@ -24,15 +23,11 @@ import us.ihmc.rdx.input.ImGui3DViewInput; import us.ihmc.rdx.sceneManager.RDXSceneLevel; import us.ihmc.rdx.ui.RDXBaseUI; +import us.ihmc.rdx.ui.affordances.RDXInteractableFrameModel; import us.ihmc.rdx.ui.graphics.RDXFootstepPlanGraphic; import us.ihmc.rdx.ui.graphics.RDXTrajectoryGraphic; -import us.ihmc.rdx.ui.interactable.RDXInteractableBlackflyFujinon; -import us.ihmc.rdx.ui.interactable.RDXInteractableOuster; -import us.ihmc.rdx.ui.interactable.RDXInteractableRealsenseD455; -import us.ihmc.rdx.ui.interactable.RDXInteractableZED2i; import us.ihmc.robotics.EuclidCoreMissingTools; import us.ihmc.robotics.robotSide.RobotSide; -import us.ihmc.robotics.robotSide.SideDependentList; import java.util.ArrayList; import java.util.List; @@ -54,13 +49,8 @@ public class RDXROS2RobotVisualizer extends RDXROS2MultiBodyGraphic private final Point3D robotTranslationDifference = new Point3D(); private final String chestName; private final ROS2SyncedRobotModel syncedRobot; - private HumanoidReferenceFrames referenceFramesToUseForSensors; private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass()); private final ImGuiSliderFloat opacitySlider = new ImGuiSliderFloat("Opacity", "%.2f", 1.0f); - private RDXInteractableOuster interactableOuster; - private RDXInteractableRealsenseD455 interactableRealsenseD455; - private SideDependentList interactableBlackflyFujinons = new SideDependentList<>(); - private RDXInteractableZED2i interactableZED2i; private boolean isFading = false; private final Pose3D lastHistoryPelvisPose = new Pose3D(); private final Pose3D currentHistoryPelvisPose = new Pose3D(); @@ -68,6 +58,7 @@ public class RDXROS2RobotVisualizer extends RDXROS2MultiBodyGraphic private final ConcurrentLinkedQueue completedFootstepThreadBarrier = new ConcurrentLinkedQueue<>(); private final List footstepHistory = new ArrayList<>(); private final RDXFootstepPlanGraphic footstepHistoryGraphic; + private final ArrayList interactableFrameModels = new ArrayList<>(); public RDXROS2RobotVisualizer(RDXBaseUI baseUI, ROS2PublishSubscribeAPI ros2, ROS2SyncedRobotModel syncedRobot) { @@ -85,8 +76,6 @@ public RDXROS2RobotVisualizer(RDXBaseUI baseUI, this.syncedRobot = syncedRobot; this.cameraForTrackingSupplier = cameraForTrackingSupplier; - referenceFramesToUseForSensors = syncedRobot.getReferenceFrames(); - syncedRobot.addRobotConfigurationDataReceivedCallback(getFrequency()::ping); previousRobotMidFeetUnderPelvis.setToNaN(); chestName = syncedRobot.getRobotModel().getJointMap().getChestName(); @@ -96,61 +85,17 @@ public RDXROS2RobotVisualizer(RDXBaseUI baseUI, footstepHistoryGraphic.setColor(RobotSide.RIGHT, Color.SKY); } - /** In simulation we want to use the ground truth frames rather than state estimator frames. */ - public void setReferenceFramesToUseForSensors(HumanoidReferenceFrames referenceFramesToUseForSensors) - { - this.referenceFramesToUseForSensors = referenceFramesToUseForSensors; - } - @Override public void create() { super.create(); + + baseUI.getPrimary3DPanel().addImGui3DViewInputProcessor(this::processImGuiInput); + getMultiBodyGraphic().create(); - if (baseUI != null) - baseUI.getPrimary3DPanel().addImGui3DViewInputProcessor(this::processImGuiInput); cameraForTracking = cameraForTrackingSupplier.get(); getMultiBodyGraphic().loadRobotModelAndGraphics(syncedRobot.getRobotModel().getRobotDefinition(), syncedRobot.getFullRobotModel().getElevator()); - interactableOuster = new RDXInteractableOuster(baseUI.getPrimary3DPanel(), - referenceFramesToUseForSensors.getOusterLidarFrame(), - syncedRobot.getRobotModel().getSensorInformation().getOusterLidarTransform()); - interactableOuster.getInteractableFrameModel() - .addRemoteTuning(ros2, - PerceptionAPI.OUSTER_TO_CHEST_TUNING, - syncedRobot.getRobotModel().getSensorInformation().getOusterLidarTransform()); - interactableRealsenseD455 = new RDXInteractableRealsenseD455(baseUI.getPrimary3DPanel(), - referenceFramesToUseForSensors.getSteppingCameraFrame(), - syncedRobot.getRobotModel().getSensorInformation().getSteppingCameraTransform()); - interactableRealsenseD455.getInteractableFrameModel() - .addRemoteTuning(ros2, - PerceptionAPI.STEPPING_CAMERA_TO_PARENT_TUNING, - syncedRobot.getRobotModel().getSensorInformation().getSteppingCameraTransform()); - RDXInteractableBlackflyFujinon interactableBlackflyLeftFujinon = new RDXInteractableBlackflyFujinon(baseUI.getPrimary3DPanel(), - referenceFramesToUseForSensors.getSituationalAwarenessCameraFrame(RobotSide.LEFT), - syncedRobot.getRobotModel().getSensorInformation().getSituationalAwarenessCameraTransform(RobotSide.LEFT)); - interactableBlackflyLeftFujinon.getInteractableFrameModel() - .addRemoteTuning(ros2, - PerceptionAPI.SITUATIONAL_AWARENESS_CAMERA_TO_PARENT_TUNING.get(RobotSide.LEFT), - syncedRobot.getRobotModel().getSensorInformation().getSituationalAwarenessCameraTransform(RobotSide.LEFT)); - interactableBlackflyFujinons.set(RobotSide.LEFT, interactableBlackflyLeftFujinon); - - RDXInteractableBlackflyFujinon interactableBlackflyRightFujinon = new RDXInteractableBlackflyFujinon(baseUI.getPrimary3DPanel(), - referenceFramesToUseForSensors.getSituationalAwarenessCameraFrame(RobotSide.RIGHT), - syncedRobot.getRobotModel().getSensorInformation().getSituationalAwarenessCameraTransform(RobotSide.RIGHT)); - interactableBlackflyRightFujinon.getInteractableFrameModel() - .addRemoteTuning(ros2, - PerceptionAPI.SITUATIONAL_AWARENESS_CAMERA_TO_PARENT_TUNING.get(RobotSide.RIGHT), - syncedRobot.getRobotModel().getSensorInformation().getSituationalAwarenessCameraTransform(RobotSide.RIGHT)); - interactableBlackflyFujinons.set(RobotSide.RIGHT, interactableBlackflyRightFujinon); - - interactableZED2i = new RDXInteractableZED2i(baseUI.getPrimary3DPanel(), - referenceFramesToUseForSensors.getExperimentalCameraFrame(), - syncedRobot.getRobotModel().getSensorInformation().getExperimentalCameraTransform()); - interactableZED2i.getInteractableFrameModel().addRemoteTuning(ros2, - PerceptionAPI.EXPERIMENTAL_CAMERA_TO_PARENT_TUNING, - syncedRobot.getRobotModel().getSensorInformation().getExperimentalCameraTransform()); - ros2.subscribeViaVolatileCallback(HumanoidControllerAPI.getTopic(FootstepStatusMessage.class, syncedRobot.getRobotModel().getSimpleRobotName()), footstepStatusMessage -> { if (footstepStatusMessage.getFootstepStatus() == FootstepStatusMessage.FOOTSTEP_STATUS_COMPLETED) @@ -185,15 +130,12 @@ public void update() { getMultiBodyGraphic().getMultiBody().getRigidBodiesToHide().remove(chestName); } - interactableOuster.getInteractableFrameModel().setShowing(!hideChest.get()); - interactableRealsenseD455.getInteractableFrameModel().setShowing(!hideChest.get()); - interactableBlackflyFujinons.forEach((side, blackflyFujinon) -> blackflyFujinon.getInteractableFrameModel().setShowing(!hideChest.get())); - interactableZED2i.getInteractableFrameModel().setShowing(!hideChest.get()); - - interactableOuster.getInteractableFrameModel().update(); - interactableRealsenseD455.getInteractableFrameModel().update(); - interactableBlackflyFujinons.forEach((side, blackflyFujinon) -> blackflyFujinon.getInteractableFrameModel().update()); - interactableZED2i.getInteractableFrameModel().update(); + + for (RDXInteractableFrameModel interactableFrameModel : interactableFrameModels) + { + interactableFrameModel.setShowing(!hideChest.get()); + interactableFrameModel.update(); + } } syncedRobot.getReferenceFrames().getPelvisFrame().getTransformToDesiredFrame(currentHistoryPelvisPose, ReferenceFrame.getWorldFrame()); @@ -252,10 +194,9 @@ public void renderImGuiWidgets() if (getMultiBodyGraphic().isRobotLoaded() && opacitySlider.render(0.0f, 1.0f)) { getMultiBodyGraphic().setOpacity(opacitySlider.getFloatValue()); - interactableOuster.getInteractableFrameModel().getModelInstance().setOpacity(opacitySlider.getFloatValue()); - interactableRealsenseD455.getInteractableFrameModel().getModelInstance().setOpacity(opacitySlider.getFloatValue()); - interactableBlackflyFujinons.forEach((side, blackflyFujinon) -> blackflyFujinon.getInteractableFrameModel().getModelInstance().setOpacity(opacitySlider.getFloatValue())); - interactableZED2i.getInteractableFrameModel().getModelInstance().setOpacity(opacitySlider.getFloatValue()); + + for (RDXInteractableFrameModel interactableFrameModel : interactableFrameModels) + interactableFrameModel.getModelInstance().setOpacity(opacitySlider.getFloatValue()); } ImGui.checkbox(labels.get("Show History"), showHistory); @@ -284,6 +225,15 @@ public void getRenderables(Array renderables, Pool pool, } } + @Override + public void setActive(boolean active) + { + super.setActive(active); + + for (RDXInteractableFrameModel interactableFrameModel : interactableFrameModels) + interactableFrameModel.setShowing(active); + } + public void destroy() { super.destroy(); @@ -305,14 +255,6 @@ public void teleportCameraToRobotPelvis() cameraForTracking.setCameraFocusPoint(syncedRobot.getFramePoseReadOnly(HumanoidReferenceFrames::getPelvisZUpFrame).getPosition()); } - public void visualizeSensors(boolean visualize) - { - interactableOuster.getInteractableFrameModel().setShowing(visualize); - interactableRealsenseD455.getInteractableFrameModel().setShowing(visualize); - interactableBlackflyFujinons.forEach((side, blackflyFujinon) -> blackflyFujinon.getInteractableFrameModel().setShowing(visualize)); - interactableZED2i.getInteractableFrameModel().setShowing(visualize); - } - public void fadeVisuals(float finalOpacity, float opacityVariation) { if (finalOpacity != opacitySlider.getFloatValue()) @@ -321,10 +263,9 @@ public void fadeVisuals(float finalOpacity, float opacityVariation) float newOpacity = (opacitySlider.getFloatValue() > finalOpacity) ? Math.max(opacitySlider.getFloatValue() - opacityVariation, finalOpacity) : Math.min(opacitySlider.getFloatValue() + opacityVariation, finalOpacity); opacitySlider.setFloatValue(newOpacity); getMultiBodyGraphic().setOpacity(newOpacity); - interactableOuster.getInteractableFrameModel().getModelInstance().setOpacity(newOpacity); - interactableRealsenseD455.getInteractableFrameModel().getModelInstance().setOpacity(newOpacity); - interactableBlackflyFujinons.forEach((side, blackflyFujinon) -> blackflyFujinon.getInteractableFrameModel().getModelInstance().setOpacity(newOpacity)); - interactableZED2i.getInteractableFrameModel().getModelInstance().setOpacity(newOpacity); + + for (RDXInteractableFrameModel interactableFrameModel : interactableFrameModels) + interactableFrameModel.getModelInstance().setOpacity(newOpacity); } else { @@ -336,4 +277,9 @@ public boolean isFading() { return isFading; } + + public void attachInteractableFrameModel(RDXInteractableFrameModel interactableFrameModel) + { + interactableFrameModels.add(interactableFrameModel); + } }