Skip to content

Added an option to log a full resolution heartbeat for the depth image #1120

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

Merged
merged 1 commit into from
Nov 20, 2025
Merged

Added an option to log a full resolution heartbeat for the depth image #1120

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
9 changes: 9 additions & 0 deletions ihmc-communication/src/main/java/us/ihmc/communication/PerceptionAPI.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -118,6 +118,15 @@ public final class PerceptionAPI
.withTypeName(ImageMessage.class)
.withSuffix("depth_filtered");

public static final ROS2Topic<Empty> REQUEST_FULL_RESOLUTION_HEARTBEAT = PERCEPTION_MODULE.withSuffix("request_realsense_full_resolution")
.withType(Empty.class);
public static final ROS2Topic<ImageMessage> D455_DEPTH_IMAGE_FULL_RESOLUTION = BEST_EFFORT.withModule("d455")
.withTypeName(ImageMessage.class)
.withSuffix("depth_full_resolution");
public static final ROS2Topic<ImageMessage> 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)
*/
Expand Down
16 changes: 16 additions & 0 deletions ...bgdx/java/us/ihmc/rdx/ui/graphics/ros2/pointCloud/RDXROS2ColoredPointCloudVisualizer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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<ImageMessage> depthTopic, ROS2Topic<ImageMessage> colorTopic)
{
Expand Down Expand Up @@ -84,6 +91,9 @@ public List<ROS2Topic<?>> getTopics()
@Override
public void renderImGuiWidgets()
{
if (ImGui.checkbox(labels.get("Full Resolution Hearbeat"), requestFullResolution) && fullResolutionHeartbeat != null)
fullResolutionHeartbeat.setAlive(requestFullResolution.get());

depthMessageSizeReadout.renderImGuiWidgets();
colorMessageSizeReadout.renderImGuiWidgets();

Expand All @@ -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);
Expand Down
14 changes: 14 additions & 0 deletions ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/ROS2ImageSensors.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand All @@ -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;
Expand Down
Loading