Skip to content

Commit f56cbe5

Browse files
LuigiPenco93LuigiPenco93
committed
extracted ai2r, foundation pose, scene node
1 parent 63190e2 commit f56cbe5

File tree

137 files changed

+16102
-790
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

137 files changed

+16102
-790
lines changed

‎ihmc-communication/src/main/java/us/ihmc/communication/AutonomyAPI.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@ public final class AutonomyAPI
1212
public static final String BEHAVIOR_TREE_MODULE_NAME = "behavior_tree";
1313

1414
public static final ROS2Topic<?> BEHAVIOR_TREE_MODULE = ROS2Tools.IHMC_ROOT.withModule(BEHAVIOR_TREE_MODULE_NAME)
15-
.withQoS(ROS2QosProfile.RELIABLE());
15+
.withQoS(ROS2QosProfile.BEST_EFFORT());
1616
public static final ROS2IOTopicPair<BehaviorTreeStateMessage> BEHAVIOR_TREE
1717
= new ROS2IOTopicPair<>(BEHAVIOR_TREE_MODULE.withTypeName(BehaviorTreeStateMessage.class));
1818

‎ihmc-high-level-behaviors/src/libgdx/external/environmentObjects/barrier/barrier.fbx

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,3 @@
11
version https://git-lfs.github.com/spec/v1
2-
oid sha256:fbb4ec8cd3573a45dd9bba6b953b838516939ae3bcf2595ffaf56566e878a8ea
3-
size 16364
2+
oid sha256:071c01c1fd7f4a2adbad6ef513256cff51b29c3e8e14b59c329577d7f4cc7f81
3+
size 335452

‎ihmc-high-level-behaviors/src/libgdx/external/environmentObjects/jerseyBarrier/BarrierCentered.fbx

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
version https://git-lfs.github.com/spec/v1
2+
oid sha256:b21a0965eff35c5d69579a15d733bcdb45fa36fc6f7c9115b20243cddffe3e0e
3+
size 355596

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

Lines changed: 297 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,297 @@
1+
package us.ihmc.rdx.perception;
2+
3+
import imgui.ImGui;
4+
import org.bytedeco.javacpp.BytePointer;
5+
import org.bytedeco.opencv.global.opencv_imgcodecs;
6+
import org.bytedeco.opencv.opencv_core.Mat;
7+
import perception_msgs.msg.dds.FoundationPoseRequest;
8+
import perception_msgs.msg.dds.FoundationPoseResult;
9+
import perception_msgs.msg.dds.ImageMessage;
10+
import us.ihmc.commons.thread.RepeatingTaskThread;
11+
import us.ihmc.communication.PerceptionAPI;
12+
import us.ihmc.communication.ros2.sync.ROS2PeerClockOffsetEstimator;
13+
import us.ihmc.perception.RawImage;
14+
import us.ihmc.perception.cuda.CUDATools;
15+
import us.ihmc.perception.detections.InstantDetection;
16+
import us.ihmc.perception.detections.yolo.YOLOv8DetectionThread;
17+
import us.ihmc.perception.detections.yolo.YOLOv8InstantDetection;
18+
import us.ihmc.perception.imageMessage.CompressionType;
19+
import us.ihmc.perception.imageMessage.PixelFormat;
20+
import us.ihmc.perception.tools.PerceptionMessageTools;
21+
import us.ihmc.rdx.Lwjgl3ApplicationAdapter;
22+
import us.ihmc.rdx.ui.RDXBaseUI;
23+
import us.ihmc.rdx.ui.gizmo.RDXPose3DGizmo;
24+
import us.ihmc.rdx.ui.graphics.RDXRawImagePointCloudVisualizer;
25+
import us.ihmc.rdx.ui.graphics.RDXReferenceFrameGraphic;
26+
import us.ihmc.rdx.ui.graphics.ros2.yolo.RDXROS2YOLOv8Visualizer;
27+
import us.ihmc.ros2.ROS2Node;
28+
import us.ihmc.ros2.ROS2NodeBuilder;
29+
import us.ihmc.ros2.ROS2Publisher;
30+
import us.ihmc.ros2.ROS2QosProfile;
31+
import us.ihmc.ros2.ROS2Topic;
32+
import us.ihmc.sensors.ImageSensor;
33+
import us.ihmc.sensors.zed.ZEDImageSensor;
34+
import us.ihmc.sensors.zed.ZEDModelData;
35+
36+
import java.util.List;
37+
import java.util.concurrent.atomic.AtomicBoolean;
38+
39+
import static us.ihmc.zed.global.zed.*;
40+
41+
class RDXFoundationPoseDemo
42+
{
43+
private static final String OBJECT_ID = "mustard_bottle";
44+
private static final String OBJECT_NAME = "bottle";
45+
46+
private static final BytePointer JPG = new BytePointer(".jpg");
47+
private static final BytePointer PNG = new BytePointer(".png");
48+
49+
private static final ROS2Topic<?> RELIABLE_TOPIC = new ROS2Topic<>().withQoS(ROS2QosProfile.RELIABLE());
50+
private static final ROS2Topic<ImageMessage> COLOR_TOPIC = RELIABLE_TOPIC.withModule("foundation_pose/color_rgb8").withType(ImageMessage.class);
51+
private static final ROS2Topic<ImageMessage> DEPTH_TOPIC = RELIABLE_TOPIC.withModule("foundation_pose/depth_mono16").withType(ImageMessage.class);
52+
private static final ROS2Topic<FoundationPoseRequest> REQUEST_TOPIC = RELIABLE_TOPIC.withModule("foundation_pose/request").withType(FoundationPoseRequest.class);
53+
private static final ROS2Topic<std_msgs.msg.dds.String> REMOVE_TOPIC = RELIABLE_TOPIC.withModule("foundation_pose/remove").withType(std_msgs.msg.dds.String.class);
54+
private static final ROS2Topic<FoundationPoseResult> RESULT_TOPIC = new ROS2Topic<>().withModule("foundation_pose/result").withType(FoundationPoseResult.class);
55+
56+
private final ROS2Node ros2Node;
57+
private final ROS2PeerClockOffsetEstimator robotClockOffsetEstimator;
58+
private final ROS2PeerClockOffsetEstimator uiClockOffsetEstimator;
59+
60+
private final ROS2Publisher<FoundationPoseRequest> requestPublisher;
61+
private final FoundationPoseRequest requestMessage;
62+
private boolean sendRequest;
63+
64+
private final ROS2Publisher<std_msgs.msg.dds.String> removePublisher;
65+
private final std_msgs.msg.dds.String removeMessage;
66+
67+
private final ROS2Publisher<ImageMessage> colorPublisher;
68+
private final ROS2Publisher<ImageMessage> depthPublisher;
69+
private final ImageMessage colorMessage;
70+
private final ImageMessage depthMessage;
71+
72+
private final ImageSensor zed;
73+
private final YOLOv8DetectionThread yoloThread;
74+
private final RepeatingTaskThread zedImageConsumerThread;
75+
76+
77+
// UI Stuff
78+
private final RDXBaseUI baseUI;
79+
private final RDXRawImagePointCloudVisualizer pointCloudVisualizer;
80+
private final RDXROS2YOLOv8Visualizer yoloSettings;
81+
private RDXPose3DGizmo zedPoseGizmo;
82+
private RDXReferenceFrameGraphic objectPoseGraphic;
83+
84+
private final AtomicBoolean destroyed;
85+
86+
private RDXFoundationPoseDemo()
87+
{
88+
Runtime.getRuntime().addShutdownHook(new Thread(this::destroy));
89+
90+
ros2Node = new ROS2NodeBuilder().build(getClass().getSimpleName());
91+
robotClockOffsetEstimator = new ROS2PeerClockOffsetEstimator(ros2Node);
92+
uiClockOffsetEstimator = new ROS2PeerClockOffsetEstimator(ros2Node);
93+
requestPublisher = ros2Node.createPublisher(REQUEST_TOPIC);
94+
requestMessage = new FoundationPoseRequest();
95+
sendRequest = false;
96+
removePublisher = ros2Node.createPublisher(REMOVE_TOPIC);
97+
removeMessage = new std_msgs.msg.dds.String();
98+
99+
ros2Node.createSubscription2(RESULT_TOPIC, this::receivePose);
100+
101+
boolean enableNeuralMode = CUDATools.hasCUDADeviceOfAtLeast(CUDATools.getDeviceName(0), "RTX 3080");
102+
zed = new ZEDImageSensor(0, ZEDModelData.ZED_2, SL_INPUT_TYPE_USB, enableNeuralMode ? SL_DEPTH_MODE_NEURAL : SL_DEPTH_MODE_PERFORMANCE);
103+
104+
colorPublisher = ros2Node.createPublisher(COLOR_TOPIC);
105+
depthPublisher = ros2Node.createPublisher(DEPTH_TOPIC);
106+
colorMessage = new ImageMessage();
107+
depthMessage = new ImageMessage();
108+
109+
yoloThread = new YOLOv8DetectionThread(robotClockOffsetEstimator, () -> true);
110+
yoloThread.setImageSensor(zed, ZEDImageSensor.LEFT_COLOR_IMAGE_KEY, ZEDImageSensor.DEPTH_IMAGE_KEY);
111+
yoloThread.addDetectionConsumerCallback(this::publishRequest);
112+
113+
zedImageConsumerThread = new RepeatingTaskThread("ZEDImageConsumer", this::consumeZEDImage);
114+
115+
baseUI = new RDXBaseUI();
116+
pointCloudVisualizer = new RDXRawImagePointCloudVisualizer("ZED Point Cloud");
117+
yoloSettings = new RDXROS2YOLOv8Visualizer("YOLO Results", ros2Node, uiClockOffsetEstimator, PerceptionAPI.YOLO_ANNOTATED_IMAGE);
118+
119+
destroyed = new AtomicBoolean(false);
120+
121+
baseUI.launchRDXApplication(new Lwjgl3ApplicationAdapter()
122+
{
123+
@Override
124+
public void create()
125+
{
126+
yoloSettings.create();
127+
yoloSettings.setActive(true);
128+
zedPoseGizmo = new RDXPose3DGizmo();
129+
objectPoseGraphic = new RDXReferenceFrameGraphic(0.2);
130+
131+
baseUI.getImGuiPanelManager().addPanel(yoloSettings.getPanel());
132+
baseUI.getImGuiPanelManager().addPanel("YOLO Settings", yoloSettings::renderImGuiWidgets);
133+
baseUI.getImGuiPanelManager().addPanel("Options", this::renderOptions);
134+
baseUI.getPrimaryScene().addRenderableProvider(yoloSettings);
135+
baseUI.getPrimaryScene().addRenderableProvider(pointCloudVisualizer);
136+
baseUI.getPrimaryScene().addRenderableProvider(zedPoseGizmo);
137+
baseUI.getPrimaryScene().addRenderableProvider(objectPoseGraphic);
138+
baseUI.create();
139+
140+
zedPoseGizmo.createAndSetupDefault(baseUI.getPrimary3DPanel());
141+
142+
zed.setSensorFrame(zedPoseGizmo.getGizmoFrame());
143+
zed.run(true);
144+
yoloThread.startRepeating();
145+
zedImageConsumerThread.startRepeating();
146+
}
147+
148+
private void renderOptions()
149+
{
150+
if (ImGui.button("Send request"))
151+
sendRequest = true;
152+
153+
if (ImGui.button("Stop tracking"))
154+
{
155+
removeMessage.setData(OBJECT_ID);
156+
removePublisher.publish(removeMessage);
157+
}
158+
}
159+
160+
@Override
161+
public void render()
162+
{
163+
pointCloudVisualizer.update();
164+
yoloSettings.update();
165+
yoloSettings.updateHeartbeat();
166+
167+
baseUI.renderBeforeOnScreenUI();
168+
baseUI.renderEnd();
169+
}
170+
171+
@Override
172+
public void dispose()
173+
{
174+
pointCloudVisualizer.destroy();
175+
yoloSettings.destroy();
176+
baseUI.dispose();
177+
178+
destroy();
179+
}
180+
});
181+
182+
}
183+
184+
private void receivePose(FoundationPoseResult result)
185+
{
186+
objectPoseGraphic.setPoseInWorldFrame(result.getObjectPose());
187+
}
188+
189+
private void consumeZEDImage()
190+
{
191+
try
192+
{
193+
zed.waitForGrab();
194+
195+
RawImage color = zed.getImage(ZEDImageSensor.LEFT_COLOR_IMAGE_KEY);
196+
RawImage depth = zed.getImage(ZEDImageSensor.DEPTH_IMAGE_KEY);
197+
198+
// Get color in RGB
199+
Mat rgbColor = new Mat();
200+
color.getPixelFormat().convertToPixelFormat(color.getCpuImageMat(), rgbColor, PixelFormat.RGB8);
201+
202+
// Compress images
203+
BytePointer encodedColor = new BytePointer();
204+
BytePointer encodedDepth = new BytePointer();
205+
206+
opencv_imgcodecs.imencode(JPG, rgbColor, encodedColor);
207+
opencv_imgcodecs.imencode(PNG, depth.getCpuImageMat(), encodedDepth);
208+
209+
// Publish compressed images
210+
PerceptionMessageTools.packImageMessage(color, encodedColor, CompressionType.JPEG, colorMessage);
211+
PerceptionMessageTools.packImageMessage(depth, encodedDepth, CompressionType.PNG, depthMessage);
212+
213+
colorPublisher.publish(colorMessage);
214+
depthPublisher.publish(depthMessage);
215+
216+
// Update visualizer
217+
pointCloudVisualizer.setColorImage(color);
218+
pointCloudVisualizer.setDepthImage(depth);
219+
220+
rgbColor.close();
221+
encodedColor.close();
222+
encodedDepth.close();
223+
color.release();
224+
depth.release();
225+
}
226+
catch (InterruptedException ignored) {}
227+
}
228+
229+
private void publishRequest(List<InstantDetection> yoloDetections)
230+
{
231+
if (!sendRequest)
232+
return;
233+
234+
for (InstantDetection detection : yoloDetections)
235+
{
236+
if (!detection.getDetectedObjectClass().equals(OBJECT_NAME))
237+
continue;
238+
239+
if (detection instanceof YOLOv8InstantDetection yoloDetection)
240+
{
241+
System.out.println("Sending request");
242+
243+
RawImage color = yoloDetection.getColorImage();
244+
RawImage depth = yoloDetection.getDepthImage();
245+
RawImage mask = yoloDetection.getObjectMask();
246+
247+
// Get color in rgb
248+
Mat rgbColor = new Mat();
249+
color.getPixelFormat().convertToPixelFormat(color.getCpuImageMat(), rgbColor, PixelFormat.RGB8);
250+
251+
// Compress images
252+
BytePointer encodedColor = new BytePointer();
253+
BytePointer encodedDepth = new BytePointer();
254+
BytePointer encodedMask = new BytePointer();
255+
256+
opencv_imgcodecs.imencode(JPG, rgbColor, encodedColor);
257+
opencv_imgcodecs.imencode(PNG, depth.getCpuImageMat(), encodedDepth);
258+
opencv_imgcodecs.imencode(PNG, mask.getCpuImageMat(), encodedMask);
259+
260+
// Pack and publish request
261+
requestMessage.setObjectId(OBJECT_ID);
262+
requestMessage.setMeshFile("mustard0.obj");
263+
PerceptionMessageTools.packImageMessage(color, encodedColor, CompressionType.JPEG, requestMessage.getColor());
264+
PerceptionMessageTools.packImageMessage(depth, encodedDepth, CompressionType.PNG, requestMessage.getDepth());
265+
PerceptionMessageTools.packImageMessage(mask, encodedMask, CompressionType.PNG, requestMessage.getObjectMask());
266+
requestPublisher.publish(requestMessage);
267+
268+
// Release pointers
269+
rgbColor.close();
270+
encodedColor.close();
271+
encodedDepth.close();
272+
encodedMask.close();
273+
274+
sendRequest = false;
275+
}
276+
}
277+
}
278+
279+
private void destroy()
280+
{
281+
if (destroyed.getAndSet(true))
282+
return;
283+
284+
yoloThread.blockingKill();
285+
zedImageConsumerThread.blockingKill();
286+
zed.close();
287+
288+
robotClockOffsetEstimator.destroy();
289+
uiClockOffsetEstimator.destroy();
290+
ros2Node.destroy();
291+
}
292+
293+
public static void main(String[] args)
294+
{
295+
new RDXFoundationPoseDemo();
296+
}
297+
}

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

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
package us.ihmc.rdx.perception.sceneGraph;
2+
3+
import com.badlogic.gdx.graphics.g3d.Renderable;
4+
import com.badlogic.gdx.utils.Array;
5+
import com.badlogic.gdx.utils.Pool;
6+
import us.ihmc.perception.sceneGraph.SceneGraph;
7+
import us.ihmc.perception.sceneGraph.foundationPose.FoundationPoseNode;
8+
import us.ihmc.rdx.sceneManager.RDXSceneLevel;
9+
import us.ihmc.rdx.ui.graphics.RDXReferenceFrameGraphic;
10+
11+
import java.util.Set;
12+
13+
public class RDXFoundationPoseNode extends RDXDetectableSceneNode
14+
{
15+
private final FoundationPoseNode foundationPoseNode;
16+
private final RDXReferenceFrameGraphic poseGraphic;
17+
18+
public RDXFoundationPoseNode(FoundationPoseNode foundationPoseNode)
19+
{
20+
super(foundationPoseNode);
21+
22+
this.foundationPoseNode = foundationPoseNode;
23+
24+
poseGraphic = new RDXReferenceFrameGraphic(0.75);
25+
poseGraphic.setToReferenceFrame(foundationPoseNode.getNodeFrame());
26+
}
27+
28+
@Override
29+
public void update(SceneGraph sceneGraph)
30+
{
31+
super.update(sceneGraph);
32+
poseGraphic.setToReferenceFrame(foundationPoseNode.getNodeFrame());
33+
}
34+
35+
@Override
36+
public void getRenderables(Array<Renderable> renderables, Pool<Renderable> pool, Set<RDXSceneLevel> sceneLevels)
37+
{
38+
if (sceneLevels.contains(RDXSceneLevel.VIRTUAL) && !super.isGraphicsHidden())
39+
{
40+
poseGraphic.getRenderables(renderables, pool);
41+
}
42+
}
43+
44+
@Override
45+
public void destroy()
46+
{
47+
super.destroy();
48+
poseGraphic.dispose();
49+
}
50+
}

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -91,7 +91,7 @@ public void update(SceneGraph sceneGraph)
9191
{
9292
sceneGraph.modifyTree(modificationQueue ->
9393
{
94-
rigidBodySceneNode.setTrackInitialParent(trackDetectedPoseChanged.read(), modificationQueue);
94+
rigidBodySceneNode.setTrackInitialParent(trackDetectedPoseChanged.read(), sceneGraph, modificationQueue);
9595
// This modification is to get queued after a basic node one
9696
// in order to subsequently update the UI node after the node
9797
// has been moved.

0 commit comments

Comments
 (0)