Skip to content

Commit af1aa95

Browse files
calvertdwcalvertdw
committed
Setup testbed for occupancy map.
1 parent f90716c commit af1aa95

File tree

3 files changed

+171
-0
lines changed

3 files changed

+171
-0
lines changed

‎ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXZEDOccupancyMapDemo.java‎

Lines changed: 110 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
package us.ihmc.rdx.perception;
2+
3+
import imgui.ImGui;
4+
import imgui.type.ImInt;
5+
import us.ihmc.euclid.tuple3D.Point3D;
6+
import us.ihmc.perception.RawImage;
7+
import us.ihmc.rdx.Lwjgl3ApplicationAdapter;
8+
import us.ihmc.rdx.imgui.ImGuiTools;
9+
import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap;
10+
import us.ihmc.rdx.ui.RDXBaseUI;
11+
import us.ihmc.rdx.ui.graphics.RDXImageVisualizer;
12+
import us.ihmc.rdx.ui.graphics.RDXPerceptionVisualizersPanel;
13+
import us.ihmc.robotics.robotSide.RobotSide;
14+
import us.ihmc.robotics.robotSide.SideDependentList;
15+
import us.ihmc.sensors.zed.ZEDModelData;
16+
import us.ihmc.sensors.zed.ZEDSVOPlaybackSensor;
17+
import us.ihmc.zed.global.zed;
18+
19+
/**
20+
* Demo class for ZED occupancy map.
21+
*/
22+
public class RDXZEDOccupancyMapDemo extends Lwjgl3ApplicationAdapter
23+
{
24+
private static final String SVO_FILE = "/home/duncan/Downloads/20251110_162146_H1ZEDXMiniFirstMustardGrab.svo2";
25+
private static final RDXBaseUI baseUI = new RDXBaseUI();
26+
private final ZEDSVOPlaybackSensor zedSensor = new ZEDSVOPlaybackSensor(0, ZEDModelData.ZED_2I, zed.SL_DEPTH_MODE_PERFORMANCE, SVO_FILE);
27+
private RDXPerceptionVisualizersPanel visualizers;
28+
private final SideDependentList<RDXImageVisualizer> zedPanels = new SideDependentList<>();
29+
private long lastZEDTimestamp = 1;
30+
private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass());
31+
private final ImInt requestedPosition = new ImInt();
32+
33+
@Override
34+
public void create()
35+
{
36+
baseUI.create();
37+
baseUI.getPrimary3DPanel().getCamera3D().setCameraFocusPoint(new Point3D(0.7, 0.0, 0.4));
38+
baseUI.getPrimary3DPanel().getCamera3D().changeCameraPosition(-3.0, -4.0, 4.0);
39+
40+
visualizers = new RDXPerceptionVisualizersPanel();
41+
for (RobotSide side : RobotSide.values)
42+
{
43+
RDXImageVisualizer zedPanel = new RDXImageVisualizer("%s ZED".formatted(side.getPascalCaseName()), "%s ZED".formatted(side.getPascalCaseName()), false);
44+
zedPanels.put(side, zedPanel);
45+
zedPanel.setActive(true);
46+
visualizers.addVisualizer(zedPanel);
47+
}
48+
visualizers.create(baseUI);
49+
50+
baseUI.getImGuiPanelManager().addPanel("Demo", this::renderImGuiWidgets);
51+
52+
zedSensor.startSensor();
53+
zedSensor.getGrabThread().setFrequencyLimit(15.0);
54+
}
55+
56+
private void renderImGuiWidgets()
57+
{
58+
ImGui.text("Current SVO:");
59+
ImGui.sameLine();
60+
ImGui.textWrapped(zedSensor.getSVOFileName());
61+
62+
ImGui.text("Current Position: %d".formatted(zedSensor.getCurrentPosition()));
63+
ImGui.text("Length: %d".formatted(zedSensor.getLength()));
64+
65+
requestedPosition.set(zedSensor.getCurrentPosition());
66+
67+
if (ImGui.button(labels.get("Play")))
68+
zedSensor.run(true);
69+
ImGui.sameLine();
70+
if (ImGui.button(labels.get("Pause")))
71+
zedSensor.run(false);
72+
73+
if (ImGuiTools.sliderInt(labels.get("Position"), requestedPosition, 0, Math.max(zedSensor.getLength(), 0)))
74+
zedSensor.setCurrentPosition(requestedPosition.get());
75+
}
76+
77+
@Override
78+
public void render()
79+
{
80+
visualizers.update();
81+
82+
long lastGrabTimestamp = zedSensor.getLastGrabTimestamp();
83+
if (lastGrabTimestamp > lastZEDTimestamp)
84+
{
85+
lastZEDTimestamp = lastGrabTimestamp;
86+
87+
for (RobotSide side : RobotSide.values)
88+
{
89+
RawImage image = zedSensor.getImage(zedSensor.getImageKeys()[side.ordinal()]);
90+
zedPanels.get(side).setImage(image);
91+
image.release();
92+
}
93+
}
94+
95+
baseUI.renderBeforeOnScreenUI();
96+
baseUI.renderEnd();
97+
}
98+
99+
@Override
100+
public void dispose()
101+
{
102+
visualizers.destroy();
103+
baseUI.dispose();
104+
}
105+
106+
public static void main(String[] args)
107+
{
108+
baseUI.launchRDXApplication(new RDXZEDOccupancyMapDemo());
109+
}
110+
}

‎ihmc-high-level-behaviors/src/libgdx/resources/configurations/RDXZEDOccupancyMapDemo/ImGuiPanels.json‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
{
2+
"dockspacePanels" : { },
3+
"windows" : {
4+
"3D View" : true,
5+
"Demo" : true,
6+
"Perception Visualizers" : true,
7+
"Left ZED###RDXImageVisualizer1:Left ZED" : true,
8+
"Right ZED###RDXImageVisualizer3:Right ZED" : true
9+
}
10+
}

‎ihmc-high-level-behaviors/src/libgdx/resources/configurations/RDXZEDOccupancyMapDemo/ImGuiSettings.ini‎

Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
[Window][DockSpaceViewport_11111111]
2+
Pos=0,22
3+
Size=1734,979
4+
Collapsed=0
5+
6+
[Window][3D View]
7+
Pos=409,22
8+
Size=1325,979
9+
Collapsed=0
10+
DockId=0x00000002,0
11+
12+
[Window][Debug##Default]
13+
Pos=60,60
14+
Size=400,400
15+
Collapsed=0
16+
17+
[Window][Demo]
18+
Pos=0,162
19+
Size=407,358
20+
Collapsed=0
21+
DockId=0x00000007,0
22+
23+
[Window][Perception Visualizers]
24+
Pos=0,22
25+
Size=407,138
26+
Collapsed=0
27+
DockId=0x00000003,0
28+
29+
[Window][###RDXImageVisualizer1:Left ZED]
30+
Pos=0,522
31+
Size=407,237
32+
Collapsed=0
33+
DockId=0x00000008,0
34+
35+
[Window][###RDXImageVisualizer3:Right ZED]
36+
Pos=0,761
37+
Size=407,240
38+
Collapsed=0
39+
DockId=0x00000006,0
40+
41+
[Docking][Data]
42+
DockSpace ID=0x8B93E3BD Window=0xA787BDB4 Pos=191,162 Size=1734,979 Split=X Selected=0x2E64DC63
43+
DockNode ID=0x00000001 Parent=0x8B93E3BD SizeRef=407,979 Split=Y Selected=0x7670709E
44+
DockNode ID=0x00000003 Parent=0x00000001 SizeRef=299,138 Selected=0x4E964DE0
45+
DockNode ID=0x00000004 Parent=0x00000001 SizeRef=299,839 Split=Y Selected=0x7670709E
46+
DockNode ID=0x00000005 Parent=0x00000004 SizeRef=485,597 Split=Y Selected=0x7670709E
47+
DockNode ID=0x00000007 Parent=0x00000005 SizeRef=485,358 Selected=0x7670709E
48+
DockNode ID=0x00000008 Parent=0x00000005 SizeRef=485,237 Selected=0xA473CDA8
49+
DockNode ID=0x00000006 Parent=0x00000004 SizeRef=485,240 Selected=0x05C832D5
50+
DockNode ID=0x00000002 Parent=0x8B93E3BD SizeRef=1325,979 CentralNode=1 Selected=0x2E64DC63
51+

0 commit comments

Comments
 (0)