Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Extract interactable sensors from robot visualizer. #466

Merged
merged 3 commits into from
Nov 15, 2024
Merged

Extract interactable sensors from robot visualizer. #466

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
7775366
Extract interactable sensors from robot visualizer.
calvertdw Nov 15, 2024
04f45d8
Cleanup.
calvertdw Nov 15, 2024
a50a7de
Move interactable sensors to Nadia.
calvertdw Nov 15, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 0 additions & 5 deletions ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXVisualizer.java
View file
Open in desktop
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
Open in desktop
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);
}
}
Loading