Skip to content

Multi ZED Support #1122

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

Draft
TomaszTB wants to merge 4 commits into
base: develop
Choose a base branch
from
Draft

Multi ZED Support #1122

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
7e509bf
Cleanup ROS2ImageSensors
TomaszTB Nov 24, 2025
c52f897
Cleanup
TomaszTB Nov 25, 2025
e1d127e
Updated ROS 2 topics
TomaszTB Nov 25, 2025
892575d
UI updates
TomaszTB Nov 25, 2025
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
105 changes: 67 additions & 38 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 @@ -38,6 +38,7 @@ public final class PerceptionAPI
public static final ROS2Topic<?> TERRAIN_MAP_MODULE = IHMC_ROOT.withModule("terrain_map");

public static final ROS2Topic<?> BEST_EFFORT = IHMC_ROOT.withQoS(ROS2QosProfile.BEST_EFFORT());
public static final ROS2Topic<?> RELIABLE = IHMC_ROOT.withQoS(ROS2QosProfile.RELIABLE());

/*
* Scene graph
Expand Down Expand Up @@ -80,54 +81,82 @@ public final class PerceptionAPI
public static final ROS2Topic<ArUcoMarkerPoses> ARUCO_MARKER_POSES = PERCEPTION_MODULE.withType(ArUcoMarkerPoses.class).withSuffix("aruco_marker_poses");

/*
* ZED image topics (IHMC ImageMessage type)
* Camera topic bases
*/
public static final ROS2Topic<Empty> REQUEST_ZED = PERCEPTION_MODULE.withSuffix("request_zed").withType(Empty.class);
public static final ROS2Topic<Empty> REQUEST_ZED_PUBLICATION = PERCEPTION_MODULE.withSuffix("request_zed_publication").withType(Empty.class);
public static final ROS2Topic<ImageMessage> ZED_DEPTH = BEST_EFFORT.withModule("zed").withType(ImageMessage.class).withSuffix("depth");
public static final SideDependentList<ROS2Topic<ImageMessage>> ZED_COLOR_IMAGES = new SideDependentList<>(BEST_EFFORT.withModule("zed")
.withType(ImageMessage.class)
.withSuffix("left_color"),
BEST_EFFORT.withModule("zed")
.withType(ImageMessage.class)
.withSuffix("right_color"));
private static final ROS2Topic<?> EXPERIMENTAL_CAMERA = BEST_EFFORT.withPrefix("experimental_camera");
private static final ROS2Topic<?> ROS2_EXPERIMENTAL_CAMERA = RELIABLE.withPrefix("experimental_camera");
private static final ROS2Topic<?> STEPPING_CAMERA = BEST_EFFORT.withPrefix("stepping_camera");
private static final ROS2Topic<?> ROS2_STEPPING_CAMERA = RELIABLE.withPrefix("stepping_camera");

/*
* ZED image topics (official ROS 2 Image and CameraInfo types)
* Experimental ZED image topics (IHMC ImageMessage type)
*/
private static final ROS2Topic<?> ROS2_ZED = new ROS2Topic<>().withPrefix("zed").withQoS(ROS2QosProfile.RELIABLE());
private static final ROS2Topic<?> ROS2_ZED_COLOR = ROS2_ZED.withModule("color");
public static final SideDependentList<ROS2Topic<Image>> ROS2_ZED_COLOR_IMAGES = new SideDependentList<>(ROS2_ZED_COLOR.withSuffix("left/image")
.withType(Image.class),
ROS2_ZED_COLOR.withSuffix("right/image")
.withType(Image.class));
public static final SideDependentList<ROS2Topic<CameraInfo>> ROS2_ZED_COLOR_CAMERA_INFOS = new SideDependentList<>(ROS2_ZED_COLOR.withSuffix(
"left/camera_info").withType(CameraInfo.class), ROS2_ZED_COLOR.withSuffix("right/camera_info").withType(CameraInfo.class));
private static final ROS2Topic<?> ROS2_ZED_DEPTH = ROS2_ZED.withModule("depth");
public static final ROS2Topic<Image> ROS2_ZED_DEPTH_IMAGE = ROS2_ZED_DEPTH.withSuffix("image").withType(Image.class);
public static final ROS2Topic<CameraInfo> ROS2_ZED_DEPTH_CAMERA_INFO = ROS2_ZED_DEPTH.withSuffix("camera_info").withType(CameraInfo.class);
private static final ROS2Topic<?> EXPERIMENTAL_ZED = EXPERIMENTAL_CAMERA.withModule("zed");
public static final ROS2Topic<Empty> REQUEST_EXPERIMENTAL_ZED = EXPERIMENTAL_ZED.withSuffix("request").withType(Empty.class);
public static final ROS2Topic<Empty> REQUEST_EXPERIMENTAL_ZED_PUBLICATION = EXPERIMENTAL_ZED.withSuffix("request_publication").withType(Empty.class);
public static final ROS2Topic<ImageMessage> EXPERIMENTAL_ZED_DEPTH = EXPERIMENTAL_ZED.withType(ImageMessage.class).withSuffix("depth");
public static final SideDependentList<ROS2Topic<ImageMessage>> EXPERIMENTAL_ZED_COLOR
= new SideDependentList<>(EXPERIMENTAL_ZED.withType(ImageMessage.class).withSuffix("left_color"),
EXPERIMENTAL_ZED.withType(ImageMessage.class).withSuffix("right_color"));

/*
* RealSense image topics (IHMC ImageMessage type)
* Experimental ZED image topics (official ROS 2 Image and CameraInfo types)
*/
public static final ROS2Topic<Empty> REQUEST_REALSENSE = PERCEPTION_MODULE.withSuffix("request_realsense").withType(Empty.class);
public static final ROS2Topic<Empty> REQUEST_REALSENSE_PUBLICATION = PERCEPTION_MODULE.withSuffix("request_realsense_publication").withType(Empty.class);
public static final ROS2Topic<ImageMessage> D455_DEPTH_IMAGE = BEST_EFFORT.withModule("d455").withTypeName(ImageMessage.class).withSuffix("depth");
public static final ROS2Topic<ImageMessage> D455_COLOR_IMAGE = BEST_EFFORT.withModule("d455").withTypeName(ImageMessage.class).withSuffix("color");
public static final ROS2Topic<ImageMessage> REALSENSE_DEPTH_FILTERED_IMAGE = BEST_EFFORT.withModule("realsense")
.withTypeName(ImageMessage.class)
.withSuffix("depth_filtered");
private static final ROS2Topic<?> ROS2_EXPERIMENTAL_ZED = ROS2_EXPERIMENTAL_CAMERA.withModule("zed");
public static final SideDependentList<ROS2Topic<Image>> ROS2_EXPERIMENTAL_ZED_COLOR
= new SideDependentList<>(ROS2_EXPERIMENTAL_ZED.withSuffix("color/left/image").withType(Image.class),
ROS2_EXPERIMENTAL_ZED.withSuffix("color/right/image").withType(Image.class));
public static final SideDependentList<ROS2Topic<CameraInfo>> ROS2_EXPERIMENTAL_ZED_COLOR_CAMERA_INFO
= new SideDependentList<>(ROS2_EXPERIMENTAL_ZED.withSuffix("color/left/camera_info").withType(CameraInfo.class),
ROS2_EXPERIMENTAL_ZED.withSuffix("color/right/camera_info").withType(CameraInfo.class));
public static final ROS2Topic<Image> ROS2_EXPERIMENTAL_ZED_DEPTH = ROS2_EXPERIMENTAL_ZED.withSuffix("depth/image").withType(Image.class);
public static final ROS2Topic<CameraInfo> ROS2_EXPERIMENTAL_ZED_DEPTH_CAMERA_INFO = ROS2_EXPERIMENTAL_ZED.withSuffix("depth/camera_info")
.withType(CameraInfo.class);

/*
* RealSense image topics (official ROS 2 Image and CameraInfo types)
* Stepping ZED image topics (IHMC ImageMessage type)
*/
private static final ROS2Topic<?> ROS2_REALSENSE = new ROS2Topic<>().withPrefix("realsense").withQoS(ROS2QosProfile.RELIABLE());
private static final ROS2Topic<?> ROS2_REALSENSE_COLOR = ROS2_REALSENSE.withModule("color");
public static final ROS2Topic<Image> ROS2_REALSENSE_COLOR_IMAGE = ROS2_REALSENSE_COLOR.withSuffix("image").withType(Image.class);
public static final ROS2Topic<CameraInfo> ROS2_REALSENSE_COLOR_CAMERA_INFO = ROS2_REALSENSE_COLOR.withSuffix("camera_info").withType(CameraInfo.class);
private static final ROS2Topic<?> ROS2_REALSENSE_DEPTH = ROS2_REALSENSE.withModule("depth");
public static final ROS2Topic<Image> ROS2_REALSENSE_DEPTH_IMAGE = ROS2_REALSENSE_DEPTH.withSuffix("image").withType(Image.class);
public static final ROS2Topic<CameraInfo> ROS2_REALSENSE_DEPTH_CAMERA_INFO = ROS2_REALSENSE_DEPTH.withSuffix("camera_info").withType(CameraInfo.class);
private static final ROS2Topic<?> STEPPING_ZED = STEPPING_CAMERA.withModule("zed");
public static final ROS2Topic<Empty> REQUEST_STEPPING_ZED = STEPPING_ZED.withSuffix("request").withType(Empty.class);
public static final ROS2Topic<Empty> REQUEST_STEPPING_ZED_PUBLICATION = STEPPING_ZED.withSuffix("request_publication").withType(Empty.class);
public static final ROS2Topic<ImageMessage> STEPPING_ZED_DEPTH = STEPPING_ZED.withType(ImageMessage.class).withSuffix("depth");
public static final SideDependentList<ROS2Topic<ImageMessage>> STEPPING_ZED_COLOR
= new SideDependentList<>(STEPPING_ZED.withType(ImageMessage.class).withSuffix("left_color"),
STEPPING_ZED.withType(ImageMessage.class).withSuffix("right_color"));

/*
* Stepping ZED image topics (official ROS 2 Image and CameraInfo types)
*/
private static final ROS2Topic<?> ROS2_STEPPING_ZED = ROS2_STEPPING_CAMERA.withModule("zed");
public static final SideDependentList<ROS2Topic<Image>> ROS2_STEPPING_ZED_COLOR
= new SideDependentList<>(ROS2_STEPPING_ZED.withSuffix("color/left/image").withType(Image.class),
ROS2_STEPPING_ZED.withSuffix("color/right/image").withType(Image.class));
public static final SideDependentList<ROS2Topic<CameraInfo>> ROS2_STEPPING_ZED_COLOR_CAMERA_INFO
= new SideDependentList<>(ROS2_STEPPING_ZED.withSuffix("color/left/camera_info").withType(CameraInfo.class),
ROS2_STEPPING_ZED.withSuffix("color/right/camera_info").withType(CameraInfo.class));
public static final ROS2Topic<Image> ROS2_STEPPING_ZED_DEPTH = ROS2_STEPPING_ZED.withSuffix("depth/image").withType(Image.class);
public static final ROS2Topic<CameraInfo> ROS2_STEPPING_ZED_DEPTH_CAMERA_INFO = ROS2_STEPPING_ZED.withSuffix("depth/camera_info").withType(CameraInfo.class);

/*
* Stepping RealSense image topics (IHMC ImageMessage type)
*/
private static final ROS2Topic<?> STEPPING_REALSENSE = STEPPING_CAMERA.withModule("realsense");
public static final ROS2Topic<Empty> REQUEST_STEPPING_REALSENSE = STEPPING_REALSENSE.withSuffix("request").withType(Empty.class);
public static final ROS2Topic<Empty> REQUEST_STEPPING_REALSENSE_PUBLICATION = STEPPING_REALSENSE.withSuffix("request_publication").withType(Empty.class);
public static final ROS2Topic<ImageMessage> STEPPING_REALSENSE_DEPTH = STEPPING_REALSENSE.withType(ImageMessage.class).withSuffix("depth");
public static final ROS2Topic<ImageMessage> STEPPING_REALSENSE_COLOR = STEPPING_REALSENSE.withType(ImageMessage.class).withSuffix("color");
public static final ROS2Topic<ImageMessage> STEPPING_REALSENSE_DEPTH_FILTERED = STEPPING_REALSENSE.withType(ImageMessage.class).withSuffix("depth_filtered");

/*
* Stepping RealSense image topics (official ROS 2 Image and CameraInfo types)
*/
private static final ROS2Topic<?> ROS2_STEPPING_REALSENSE = new ROS2Topic<>().withPrefix("realsense").withQoS(ROS2QosProfile.RELIABLE());
public static final ROS2Topic<Image> ROS2_STEPPING_REALSENSE_COLOR = ROS2_STEPPING_REALSENSE.withSuffix("color/image").withType(Image.class);
public static final ROS2Topic<CameraInfo> ROS2_STEPPING_REALSENSE_COLOR_CAMERA_INFO = ROS2_STEPPING_REALSENSE.withSuffix("color/camera_info")
.withType(CameraInfo.class);
public static final ROS2Topic<Image> ROS2_STEPPING_REALSENSE_DEPTH = ROS2_STEPPING_REALSENSE.withSuffix("depth/image").withType(Image.class);
public static final ROS2Topic<CameraInfo> ROS2_STEPPING_REALSENSE_DEPTH_CAMERA_INFO = ROS2_STEPPING_REALSENSE.withSuffix("depth/camera_info")
.withType(CameraInfo.class);

/*
* Planar regions
Expand Down
8 changes: 4 additions & 4 deletions ...vel-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXIsaacROSFoundationPoseDemoUI.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,10 +30,10 @@ public void create()
{
visualizers.addVisualizer(new RDXROS2ColoredPointCloudVisualizer("ZED Point Cloud",
ros2Node,
PerceptionAPI.ZED_DEPTH,
PerceptionAPI.ZED_COLOR_IMAGES.get(RobotSide.LEFT)));
visualizers.addVisualizer(new RDXROS2ImageMessageVisualizer("ZED Color", ros2Node, PerceptionAPI.ZED_COLOR_IMAGES.get(RobotSide.LEFT)));
visualizers.addVisualizer(new RDXROS2ImageMessageVisualizer("ZED Depth", ros2Node, PerceptionAPI.ZED_DEPTH));
PerceptionAPI.EXPERIMENTAL_ZED_DEPTH,
PerceptionAPI.EXPERIMENTAL_ZED_COLOR.get(RobotSide.LEFT)));
visualizers.addVisualizer(new RDXROS2ImageMessageVisualizer("ZED Color", ros2Node, PerceptionAPI.EXPERIMENTAL_ZED_COLOR.get(RobotSide.LEFT)));
visualizers.addVisualizer(new RDXROS2ImageMessageVisualizer("ZED Depth", ros2Node, PerceptionAPI.EXPERIMENTAL_ZED_DEPTH));
visualizers.addVisualizer(new RDXROS2YOLOv8Visualizer("YOLO", ros2Node, peerClockOffsetEstimator, PerceptionAPI.YOLO_ANNOTATED_IMAGE));
visualizers.addVisualizer(new RDXIsaacROSFoundationPoseVisualizer("FoundationPose", ros2Node, peerClockOffsetEstimator));
visualizers.create(baseUI);
Expand Down
10 changes: 5 additions & 5 deletions ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXZED2DisplayDemo.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,26 +26,26 @@ public void create()
{
RDXROS2ImageMessageVisualizer zed2LeftColorImageVisualizer = new RDXROS2ImageMessageVisualizer("ZED 2 Color Left",
ros2Node,
PerceptionAPI.ZED_COLOR_IMAGES.get(RobotSide.LEFT));
PerceptionAPI.EXPERIMENTAL_ZED_COLOR.get(RobotSide.LEFT));
zed2LeftColorImageVisualizer.setActive(true);
perceptionVisualizerPanel.addVisualizer(zed2LeftColorImageVisualizer);

RDXROS2ImageMessageVisualizer zed2RightColorImageVisualizer = new RDXROS2ImageMessageVisualizer("ZED 2 Color Right",
ros2Node,
PerceptionAPI.ZED_COLOR_IMAGES.get(RobotSide.RIGHT));
PerceptionAPI.EXPERIMENTAL_ZED_COLOR.get(RobotSide.RIGHT));
zed2RightColorImageVisualizer.setActive(true);
perceptionVisualizerPanel.addVisualizer(zed2RightColorImageVisualizer);

RDXROS2ImageMessageVisualizer zed2DepthImageVisualizer = new RDXROS2ImageMessageVisualizer("ZED 2 Depth",
ros2Node,
PerceptionAPI.ZED_DEPTH);
PerceptionAPI.EXPERIMENTAL_ZED_DEPTH);
zed2DepthImageVisualizer.setActive(true);
perceptionVisualizerPanel.addVisualizer(zed2DepthImageVisualizer);

RDXROS2ColoredPointCloudVisualizer zed2ColoredPointCloudVisualizer = new RDXROS2ColoredPointCloudVisualizer("ZED 2 Colored Point Cloud",
ros2Node,
PerceptionAPI.ZED_DEPTH,
PerceptionAPI.ZED_COLOR_IMAGES.get(
PerceptionAPI.EXPERIMENTAL_ZED_DEPTH,
PerceptionAPI.EXPERIMENTAL_ZED_COLOR.get(
RobotSide.LEFT));
zed2ColoredPointCloudVisualizer.setActive(true);
perceptionVisualizerPanel.addVisualizer(zed2ColoredPointCloudVisualizer);
Expand Down
Loading
Loading