diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/PerceptionAPI.java b/ihmc-communication/src/main/java/us/ihmc/communication/PerceptionAPI.java index 7aa9ee5fdb3e..86652283d3d5 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/PerceptionAPI.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/PerceptionAPI.java @@ -118,6 +118,15 @@ public final class PerceptionAPI .withTypeName(ImageMessage.class) .withSuffix("depth_filtered"); + public static final ROS2Topic REQUEST_FULL_RESOLUTION_HEARTBEAT = PERCEPTION_MODULE.withSuffix("request_realsense_full_resolution") + .withType(Empty.class); + public static final ROS2Topic D455_DEPTH_IMAGE_FULL_RESOLUTION = BEST_EFFORT.withModule("d455") + .withTypeName(ImageMessage.class) + .withSuffix("depth_full_resolution"); + public static final ROS2Topic D455_COLOR_IMAGE_FULL_RESOLUTION = BEST_EFFORT.withModule("d455") + .withTypeName(ImageMessage.class) + .withSuffix("color_full_resolution"); + /* * RealSense image topics (official ROS 2 Image and CameraInfo types) */ diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizer.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizer.java index f8162293df9a..194fcb4001ee 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizer.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizer.java @@ -3,7 +3,11 @@ import com.badlogic.gdx.graphics.g3d.Renderable; import com.badlogic.gdx.utils.Array; import com.badlogic.gdx.utils.Pool; +import imgui.ImGui; +import imgui.type.ImBoolean; import perception_msgs.msg.dds.ImageMessage; +import us.ihmc.communication.PerceptionAPI; +import us.ihmc.communication.ros2.ROS2Heartbeat; import us.ihmc.perception.RawImage; import us.ihmc.perception.imageMessage.ImageMessageDecoder; import us.ihmc.rdx.sceneManager.RDXSceneLevel; @@ -36,6 +40,9 @@ public class RDXROS2ColoredPointCloudVisualizer extends RDXROS2MultiTopicVisuali private final ImageMessageDecoder colorMessageDecoder = new ImageMessageDecoder(); private final RDXRawImagePointCloudVisualizer visualizer; + private ROS2Heartbeat fullResolutionHeartbeat; + private final ImBoolean requestFullResolution = new ImBoolean(false); + public RDXROS2ColoredPointCloudVisualizer(String title, ROS2Node ros2Node, ROS2Topic depthTopic, ROS2Topic colorTopic) { @@ -84,6 +91,9 @@ public List> getTopics() @Override public void renderImGuiWidgets() { + if (ImGui.checkbox(labels.get("Full Resolution Hearbeat"), requestFullResolution) && fullResolutionHeartbeat != null) + fullResolutionHeartbeat.setAlive(requestFullResolution.get()); + depthMessageSizeReadout.renderImGuiWidgets(); colorMessageSizeReadout.renderImGuiWidgets(); @@ -110,6 +120,12 @@ public void destroy() colorMessageDecoder.destroy(); } + public void setupFullResolutionRequestHeartbeat(ROS2Node ros2Node) + { + fullResolutionHeartbeat = new ROS2Heartbeat(ros2Node, PerceptionAPI.REQUEST_FULL_RESOLUTION_HEARTBEAT); + fullResolutionHeartbeat.setAlive(requestFullResolution.get()); + } + private void subscribe() { depthSubscription = ros2Node.createSubscription2(depthTopic, this::onDepthMessageReceived); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/ROS2ImageSensors.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/ROS2ImageSensors.java index 7f7f7ee4bc98..aeb9bf366892 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/ROS2ImageSensors.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/ROS2ImageSensors.java @@ -26,6 +26,8 @@ public class ROS2ImageSensors private ImageSensor zedSensor; private ROS2DemandGraphNode zedPublishDemandNode; private ImageSensorPublishThread zedPublishThread; + private ROS2DemandGraphNode ros2DemandGraphNode; + private ImageSensorPublishThread imageSensorPublishThread; public ROS2ImageSensors(ROS2Node ros2Node) { @@ -48,6 +50,18 @@ public void addRealsenseSensor(ImageSensor realsenseSensor, ReferenceFrame senso setupCallbackForDemandNode(realsensePublishThread, realsensePublishDemandNode); } + // Must be called after the realsense has been added, no safety checks + public void addFullResolutionRealSensePublisher(ImageSensor realsenseSensor) + { + ros2DemandGraphNode = new ROS2DemandGraphNode(ros2Node, PerceptionAPI.REQUEST_FULL_RESOLUTION_HEARTBEAT); + realsenseDemandNode.addDependents(ros2DemandGraphNode); + + imageSensorPublishThread = new ImageSensorPublishThread(ros2Node, realsenseSensor); + realsensePublishThread.addTopic(PerceptionAPI.D455_DEPTH_IMAGE_FULL_RESOLUTION, RealSenseImageSensor.DEPTH_IMAGE_KEY); + realsensePublishThread.addTopic(PerceptionAPI.D455_COLOR_IMAGE_FULL_RESOLUTION, RealSenseImageSensor.COLOR_IMAGE_KEY); + setupCallbackForDemandNode(imageSensorPublishThread, ros2DemandGraphNode); + } + public void addZEDSensor(ImageSensor zedSensor, ReferenceFrame sensorFrame) { this.zedSensor = zedSensor;