Skip to content

Commit

Permalink
Extract interactable sensors from robot visualizer. (#466)
Browse files Browse the repository at this point in the history
  • Loading branch information
@calvertdw
calvertdw authored Nov 15, 2024
1 parent b9245f4 commit 41cf80c
Show file tree
Hide file tree
Showing 2 changed files with 31 additions and 90 deletions.
5 changes: 0 additions & 5 deletions ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXVisualizer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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());
Expand Down
116 changes: 31 additions & 85 deletions ...-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2RobotVisualizer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -54,20 +49,16 @@ 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<RDXInteractableBlackflyFujinon> interactableBlackflyFujinons = new SideDependentList<>();
private RDXInteractableZED2i interactableZED2i;
private boolean isFading = false;
private final Pose3D lastHistoryPelvisPose = new Pose3D();
private final Pose3D currentHistoryPelvisPose = new Pose3D();
private final RDXTrajectoryGraphic pelvisPoseHistoryGraphic = new RDXTrajectoryGraphic(Color.SKY);
private final ConcurrentLinkedQueue<MinimalFootstep> completedFootstepThreadBarrier = new ConcurrentLinkedQueue<>();
private final List<MinimalFootstep> footstepHistory = new ArrayList<>();
private final RDXFootstepPlanGraphic footstepHistoryGraphic;
private final ArrayList<RDXInteractableFrameModel> interactableFrameModels = new ArrayList<>();

public RDXROS2RobotVisualizer(RDXBaseUI baseUI, ROS2PublishSubscribeAPI ros2, ROS2SyncedRobotModel syncedRobot)
{
Expand All @@ -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();
Expand All @@ -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)
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -284,6 +225,15 @@ public void getRenderables(Array<Renderable> renderables, Pool<Renderable> pool,
}
}

@Override
public void setActive(boolean active)
{
super.setActive(active);

for (RDXInteractableFrameModel interactableFrameModel : interactableFrameModels)
interactableFrameModel.setShowing(active);
}

public void destroy()
{
super.destroy();
Expand All @@ -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())
Expand All @@ -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
{
Expand All @@ -336,4 +277,9 @@ public boolean isFading()
{
return isFading;
}

public void attachInteractableFrameModel(RDXInteractableFrameModel interactableFrameModel)
{
interactableFrameModels.add(interactableFrameModel);
}
}

0 comments on commit 41cf80c

Please sign in to comment.