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..d67892aed735 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/PerceptionAPI.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/PerceptionAPI.java @@ -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 @@ -80,54 +81,82 @@ public final class PerceptionAPI public static final ROS2Topic 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 REQUEST_ZED = PERCEPTION_MODULE.withSuffix("request_zed").withType(Empty.class); - public static final ROS2Topic REQUEST_ZED_PUBLICATION = PERCEPTION_MODULE.withSuffix("request_zed_publication").withType(Empty.class); - public static final ROS2Topic ZED_DEPTH = BEST_EFFORT.withModule("zed").withType(ImageMessage.class).withSuffix("depth"); - public static final SideDependentList> 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("ros2_experimental_camera"); + private static final ROS2Topic STEPPING_CAMERA = BEST_EFFORT.withPrefix("stepping_camera"); + private static final ROS2Topic ROS2_STEPPING_CAMERA = RELIABLE.withPrefix("ros2_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> 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> 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 ROS2_ZED_DEPTH_IMAGE = ROS2_ZED_DEPTH.withSuffix("image").withType(Image.class); - public static final ROS2Topic 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 REQUEST_EXPERIMENTAL_ZED = EXPERIMENTAL_ZED.withSuffix("request").withType(Empty.class); + public static final ROS2Topic REQUEST_EXPERIMENTAL_ZED_PUBLICATION = EXPERIMENTAL_ZED.withSuffix("request_publication").withType(Empty.class); + public static final ROS2Topic EXPERIMENTAL_ZED_DEPTH = EXPERIMENTAL_ZED.withType(ImageMessage.class).withSuffix("depth"); + public static final SideDependentList> 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 REQUEST_REALSENSE = PERCEPTION_MODULE.withSuffix("request_realsense").withType(Empty.class); - public static final ROS2Topic REQUEST_REALSENSE_PUBLICATION = PERCEPTION_MODULE.withSuffix("request_realsense_publication").withType(Empty.class); - public static final ROS2Topic D455_DEPTH_IMAGE = BEST_EFFORT.withModule("d455").withTypeName(ImageMessage.class).withSuffix("depth"); - public static final ROS2Topic D455_COLOR_IMAGE = BEST_EFFORT.withModule("d455").withTypeName(ImageMessage.class).withSuffix("color"); - public static final ROS2Topic 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> 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> 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 ROS2_EXPERIMENTAL_ZED_DEPTH = ROS2_EXPERIMENTAL_ZED.withSuffix("depth/image").withType(Image.class); + public static final ROS2Topic 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 ROS2_REALSENSE_COLOR_IMAGE = ROS2_REALSENSE_COLOR.withSuffix("image").withType(Image.class); - public static final ROS2Topic 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 ROS2_REALSENSE_DEPTH_IMAGE = ROS2_REALSENSE_DEPTH.withSuffix("image").withType(Image.class); - public static final ROS2Topic 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 REQUEST_STEPPING_ZED = STEPPING_ZED.withSuffix("request").withType(Empty.class); + public static final ROS2Topic REQUEST_STEPPING_ZED_PUBLICATION = STEPPING_ZED.withSuffix("request_publication").withType(Empty.class); + public static final ROS2Topic STEPPING_ZED_DEPTH = STEPPING_ZED.withType(ImageMessage.class).withSuffix("depth"); + public static final SideDependentList> 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> 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> 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 ROS2_STEPPING_ZED_DEPTH = ROS2_STEPPING_ZED.withSuffix("depth/image").withType(Image.class); + public static final ROS2Topic 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 REQUEST_STEPPING_REALSENSE = STEPPING_REALSENSE.withSuffix("request").withType(Empty.class); + public static final ROS2Topic REQUEST_STEPPING_REALSENSE_PUBLICATION = STEPPING_REALSENSE.withSuffix("request_publication").withType(Empty.class); + public static final ROS2Topic STEPPING_REALSENSE_DEPTH = STEPPING_REALSENSE.withType(ImageMessage.class).withSuffix("depth"); + public static final ROS2Topic STEPPING_REALSENSE_COLOR = STEPPING_REALSENSE.withType(ImageMessage.class).withSuffix("color"); + public static final ROS2Topic 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 ROS2_STEPPING_REALSENSE_COLOR = ROS2_STEPPING_REALSENSE.withSuffix("color/image").withType(Image.class); + public static final ROS2Topic ROS2_STEPPING_REALSENSE_COLOR_CAMERA_INFO = ROS2_STEPPING_REALSENSE.withSuffix("color/camera_info") + .withType(CameraInfo.class); + public static final ROS2Topic ROS2_STEPPING_REALSENSE_DEPTH = ROS2_STEPPING_REALSENSE.withSuffix("depth/image").withType(Image.class); + public static final ROS2Topic ROS2_STEPPING_REALSENSE_DEPTH_CAMERA_INFO = ROS2_STEPPING_REALSENSE.withSuffix("depth/camera_info") + .withType(CameraInfo.class); /* * Planar regions diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXIsaacROSFoundationPoseDemoUI.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXIsaacROSFoundationPoseDemoUI.java index e791e928c8b3..2da0eff6b8bc 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXIsaacROSFoundationPoseDemoUI.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXIsaacROSFoundationPoseDemoUI.java @@ -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); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXZED2DisplayDemo.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXZED2DisplayDemo.java index 9594c9006117..9a73e08866e1 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXZED2DisplayDemo.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXZED2DisplayDemo.java @@ -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); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphDemo.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphDemo.java index 0d74730442be..88d6f3c92039 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphDemo.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphDemo.java @@ -111,23 +111,23 @@ public void create() RDXROS2ImageMessageVisualizer zedColorImageVisualizer = new RDXROS2ImageMessageVisualizer("ZED 2 Color %s".formatted(side.getPascalCaseName()), ros2Node, - PerceptionAPI.ZED_COLOR_IMAGES.get(side)); - zedColorImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION); + PerceptionAPI.EXPERIMENTAL_ZED_COLOR.get(side)); + zedColorImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_EXPERIMENTAL_ZED_PUBLICATION); perceptionVisualizerPanel.addVisualizer(zedColorImageVisualizer); } RDXROS2ImageMessageVisualizer zed2DepthImageVisualizer = new RDXROS2ImageMessageVisualizer("ZED 2 Depth Image", ros2Node, - PerceptionAPI.ZED_DEPTH); - zed2DepthImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION); + PerceptionAPI.EXPERIMENTAL_ZED_DEPTH); + zed2DepthImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_EXPERIMENTAL_ZED_PUBLICATION); perceptionVisualizerPanel.addVisualizer(zed2DepthImageVisualizer); RDXROS2ColoredPointCloudVisualizer zed2ColoredPointCloudVisualizer = new RDXROS2ColoredPointCloudVisualizer("ZED 2 Colored Point Cloud", ros2Node, - PerceptionAPI.ZED_DEPTH, - PerceptionAPI.ZED_COLOR_IMAGES.get(RobotSide.LEFT)); - zed2ColoredPointCloudVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION); + PerceptionAPI.EXPERIMENTAL_ZED_DEPTH, + PerceptionAPI.EXPERIMENTAL_ZED_COLOR.get(RobotSide.LEFT)); + zed2ColoredPointCloudVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_EXPERIMENTAL_ZED_PUBLICATION); zed2ColoredPointCloudVisualizer.setActive(true); perceptionVisualizerPanel.addVisualizer(zed2ColoredPointCloudVisualizer); @@ -158,9 +158,9 @@ public void create() zedSVOPlayer.run(true); zedPublishThread = new ImageSensorPublishThread(ros2Node, zedSVOPlayer); - zedPublishThread.addTopic(PerceptionAPI.ZED_COLOR_IMAGES.get(RobotSide.LEFT), ZEDImageSensor.LEFT_COLOR_IMAGE_KEY); - zedPublishThread.addTopic(PerceptionAPI.ZED_COLOR_IMAGES.get(RobotSide.RIGHT), ZEDImageSensor.RIGHT_COLOR_IMAGE_KEY); - zedPublishThread.addTopic(PerceptionAPI.ZED_DEPTH, ZEDImageSensor.DEPTH_IMAGE_KEY); + zedPublishThread.addTopic(PerceptionAPI.EXPERIMENTAL_ZED_COLOR.get(RobotSide.LEFT), ZEDImageSensor.LEFT_COLOR_IMAGE_KEY); + zedPublishThread.addTopic(PerceptionAPI.EXPERIMENTAL_ZED_COLOR.get(RobotSide.RIGHT), ZEDImageSensor.RIGHT_COLOR_IMAGE_KEY); + zedPublishThread.addTopic(PerceptionAPI.EXPERIMENTAL_ZED_DEPTH, ZEDImageSensor.DEPTH_IMAGE_KEY); zedPublishThread.startRepeating(); zedSVORecorderPanel = new RDXZEDSVORecorderPanel(ros2Helper); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXRobotPerceptionVisualizersPanel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXRobotPerceptionVisualizersPanel.java index e9371deccadb..399caf497205 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXRobotPerceptionVisualizersPanel.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXRobotPerceptionVisualizersPanel.java @@ -26,13 +26,14 @@ public abstract class RDXRobotPerceptionVisualizersPanel extends RDXPerceptionVi protected final RDXROS2KSTRobotVisualizer kinematicsStreamingSolutionVisualizer; // Optional — may be null in some robots - protected RDXROS2ColoredPointCloudVisualizer zedColoredPointCloudVisualizer; - protected RDXROS2ImageMessageVisualizer zedLeftColorImageVisualizer; - protected RDXROS2ImageMessageVisualizer zedRightColorImageVisualizer; - protected RDXROS2ImageMessageVisualizer zedDepthImageVisualizer; - protected RDXROS2ColoredPointCloudVisualizer realsenseColoredPointCloudVisualizer; - protected RDXROS2ImageMessageVisualizer realsenseDepthImageVisualizer; - protected RDXROS2ImageMessageVisualizer realsenseColorImageVisualizer; + protected RDXROS2ColoredPointCloudVisualizer experimentalCameraColoredPointCloudVisualizer; + protected RDXROS2ImageMessageVisualizer experimentalCameraLeftColorImageVisualizer; + protected RDXROS2ImageMessageVisualizer experimentalCameraRightColorImageVisualizer; + protected RDXROS2ImageMessageVisualizer experimentalCameraDepthImageVisualizer; + protected RDXROS2ColoredPointCloudVisualizer steppingCameraColoredPointCloudVisualizer; + protected RDXROS2ImageMessageVisualizer steppingCameraLeftColorImageVisualizer; + protected RDXROS2ImageMessageVisualizer steppingCameraRightColorImageVisualizer; + protected RDXROS2ImageMessageVisualizer steppingCameraDepthImageVisualizer; protected RDXROS2YOLOv8Visualizer yoloVisualizer; protected RDXROS2HeightMapVisualizer heightMapVisualizer; protected RDXDetectionManagerSettings detectionManagerSettings; @@ -97,39 +98,39 @@ public RDXROS2KSTRobotVisualizer getKinematicsStreamingSolutionVisualizer() return kinematicsStreamingSolutionVisualizer; } - public RDXROS2ColoredPointCloudVisualizer getZedColoredPointCloudVisualizer() + public RDXROS2ColoredPointCloudVisualizer getExperimentalCameraColoredPointCloudVisualizer() { - return zedColoredPointCloudVisualizer; + return experimentalCameraColoredPointCloudVisualizer; } - public RDXROS2ImageMessageVisualizer getZedLeftColorImageVisualizer() + public RDXROS2ImageMessageVisualizer getExperimentalCameraLeftColorImageVisualizer() { - return zedLeftColorImageVisualizer; + return experimentalCameraLeftColorImageVisualizer; } - public RDXROS2ImageMessageVisualizer getZedRightColorImageVisualizer() + public RDXROS2ImageMessageVisualizer getExperimentalCameraRightColorImageVisualizer() { - return zedRightColorImageVisualizer; + return experimentalCameraRightColorImageVisualizer; } - public RDXROS2ImageMessageVisualizer getZedDepthImageVisualizer() + public RDXROS2ImageMessageVisualizer getExperimentalCameraDepthImageVisualizer() { - return zedDepthImageVisualizer; + return experimentalCameraDepthImageVisualizer; } - public RDXROS2ColoredPointCloudVisualizer getRealsenseColoredPointCloudVisualizer() + public RDXROS2ColoredPointCloudVisualizer getSteppingCameraColoredPointCloudVisualizer() { - return realsenseColoredPointCloudVisualizer; + return steppingCameraColoredPointCloudVisualizer; } - public RDXROS2ImageMessageVisualizer getRealsenseDepthImageVisualizer() + public RDXROS2ImageMessageVisualizer getSteppingCameraDepthImageVisualizer() { - return realsenseDepthImageVisualizer; + return steppingCameraDepthImageVisualizer; } - public RDXROS2ImageMessageVisualizer getRealsenseColorImageVisualizer() + public RDXROS2ImageMessageVisualizer getSteppingCameraLeftColorImageVisualizer() { - return realsenseColorImageVisualizer; + return steppingCameraLeftColorImageVisualizer; } public RDXROS2YOLOv8Visualizer getYoloVisualizer() diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/vr/RDXVRModeControls.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/vr/RDXVRModeControls.java index 4c8402ec3cdd..73cc69be53aa 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/vr/RDXVRModeControls.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/vr/RDXVRModeControls.java @@ -3,16 +3,12 @@ import imgui.ImGui; import imgui.flag.ImGuiTreeNodeFlags; import imgui.type.ImBoolean; -import us.ihmc.log.LogTools; import us.ihmc.rdx.imgui.ImGuiTools; import us.ihmc.rdx.imgui.RDXPanel; import us.ihmc.rdx.ui.RDXBaseUI; import us.ihmc.rdx.ui.RDXJoystickBasedStepping; import us.ihmc.rdx.ui.graphics.RDXRobotPerceptionVisualizersPanel; import us.ihmc.rdx.ui.graphics.ros2.RDXROS2ImageMessageVisualizer; -import us.ihmc.rdx.ui.teleoperation.RDXTeleoperationManager; - -import java.util.Collection; public class RDXVRModeControls { @@ -104,12 +100,12 @@ public void render() { if (panel instanceof RDXRobotPerceptionVisualizersPanel perceptionVisualizersPanel) { - RDXROS2ImageMessageVisualizer leftColorVisualizer = perceptionVisualizersPanel.getZedLeftColorImageVisualizer(); + RDXROS2ImageMessageVisualizer leftColorVisualizer = perceptionVisualizersPanel.getExperimentalCameraLeftColorImageVisualizer(); if (leftColorVisualizer != null) { activateWithSubscriptionOnly(leftColorVisualizer); } - RDXROS2ImageMessageVisualizer rightColorVisualizer = perceptionVisualizersPanel.getZedRightColorImageVisualizer(); + RDXROS2ImageMessageVisualizer rightColorVisualizer = perceptionVisualizersPanel.getExperimentalCameraRightColorImageVisualizer(); if (rightColorVisualizer != null) { activateWithSubscriptionOnly(rightColorVisualizer); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/vr/RDXVRModeManager.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/vr/RDXVRModeManager.java index 852994270472..3d0eb3891c91 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/vr/RDXVRModeManager.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/vr/RDXVRModeManager.java @@ -249,10 +249,10 @@ public void update() vrModeControls3DPanel.update(); vrModeControls.update(); - if (perceptionVisualizers.getZedLeftColorImageVisualizer() != null && perceptionVisualizers.getZedRightColorImageVisualizer() != null) + if (perceptionVisualizers.getExperimentalCameraLeftColorImageVisualizer() != null && perceptionVisualizers.getExperimentalCameraRightColorImageVisualizer() != null) { - stereoPanel.update(perceptionVisualizers.getZedLeftColorImageVisualizer().getTexture(), - perceptionVisualizers.getZedRightColorImageVisualizer().getTexture(), + stereoPanel.update(perceptionVisualizers.getExperimentalCameraLeftColorImageVisualizer().getTexture(), + perceptionVisualizers.getExperimentalCameraRightColorImageVisualizer().getTexture(), syncedRobot.getReferenceFrames().getStereoCameraFrame(RobotSide.LEFT), syncedRobot.getReferenceFrames().getStereoCameraFrame(RobotSide.RIGHT), perceptionVisualizers.getZEDModelData().getVerticalFOV()); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java index d50541391850..38c394285cd0 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java @@ -72,8 +72,10 @@ public ContinuousHikingProcess(DRCRobotModel robotModel, // This is for the height map, it expects the queue of images that we get from the sensors BlockingQueue rawImageCollectionRealsnese = new LinkedBlockingQueue<>(ImageSensor.DEFAULT_IMAGE_QUEUE_CAPACITY); BlockingQueue rawImageCollectionZED = new LinkedBlockingQueue<>(ImageSensor.DEFAULT_IMAGE_QUEUE_CAPACITY); - ros2ImageSensors.registerImageQueueForRealsense(rawImageCollectionRealsnese, RealSenseImageSensor.DEPTH_IMAGE_KEY); - ros2ImageSensors.registerImageQueueForZED(rawImageCollectionZED, ZEDImageSensor.DEPTH_IMAGE_KEY); + if (ros2ImageSensors.getSensor("Stepping Camera") != null) + ros2ImageSensors.getSensor("Stepping Camera").registerImageQueue(rawImageCollectionRealsnese, RealSenseImageSensor.DEPTH_IMAGE_KEY); + if (ros2ImageSensors.getSensor("Experimental Camera") != null) + ros2ImageSensors.getSensor("Experimental Camera").registerImageQueue(rawImageCollectionZED, ZEDImageSensor.DEPTH_IMAGE_KEY); heightMapDemandNode = new ROS2DemandGraphNode(ros2Node, PerceptionAPI.REQUEST_HEIGHT_MAP); heightMapControllerDemandNode = new ROS2DemandGraphNode(ros2Node, PerceptionAPI.REQUEST_HEIGHT_MAP_FOR_CONTROLLER); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/GpuMappingThread.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/GpuMappingThread.java index 121261faa8c5..e34f83e2d007 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/GpuMappingThread.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/GpuMappingThread.java @@ -74,7 +74,7 @@ public GpuMappingThread(ROS2Node ros2Node, // This will make the height map not appear correct cause the center is wrong ReferenceFrame heightMapCenterFrame = syncedRobotModel.getReferenceFrames().getSteppingCameraFrame(); - filteredDepthPublisher = ros2Node.createPublisher(PerceptionAPI.REALSENSE_DEPTH_FILTERED_IMAGE); + filteredDepthPublisher = ros2Node.createPublisher(PerceptionAPI.STEPPING_REALSENSE_DEPTH_FILTERED); bodyCollisionFilter = new DepthImageBodyCollisionFilter(robotCollisionModel, syncedRobotModel.getFullRobotModel().getRootBody()); flyingPointsFilter = new DepthImageFlyingPointsFilter(depthImageFilteringParameters); 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..271363762823 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 @@ -1,121 +1,124 @@ package us.ihmc.perception; -import us.ihmc.commons.thread.RepeatingTaskThread; +import std_msgs.msg.dds.Empty; import us.ihmc.communication.PerceptionAPI; +import us.ihmc.communication.packets.Packet; import us.ihmc.communication.ros2.ROS2DemandGraphNode; +import us.ihmc.communication.ros2.ROS2DemandGraphTools; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2Topic; import us.ihmc.sensors.ImageSensor; import us.ihmc.sensors.realsense.RealSenseImageSensor; import us.ihmc.sensors.zed.ZEDImageSensor; -import java.util.concurrent.BlockingQueue; +import java.util.HashMap; +import java.util.HashSet; +import java.util.Map; +import java.util.Set; public class ROS2ImageSensors { - private final ROS2Node ros2Node; + public static final Map>, Integer> STEPPING_REALSENSE_TOPIC_MAP = Map.of(PerceptionAPI.STEPPING_REALSENSE_COLOR, + RealSenseImageSensor.COLOR_IMAGE_KEY, + PerceptionAPI.STEPPING_REALSENSE_DEPTH, + RealSenseImageSensor.DEPTH_IMAGE_KEY); + + public static final Map>, Integer> EXPERIMENTAL_ZED_TOPIC_MAP = Map.of(PerceptionAPI.EXPERIMENTAL_ZED_COLOR.get(RobotSide.LEFT), + ZEDImageSensor.LEFT_COLOR_IMAGE_KEY, + PerceptionAPI.EXPERIMENTAL_ZED_COLOR.get(RobotSide.RIGHT), + ZEDImageSensor.RIGHT_COLOR_IMAGE_KEY, + PerceptionAPI.EXPERIMENTAL_ZED_DEPTH, + ZEDImageSensor.DEPTH_IMAGE_KEY); + + public static final Map>, Integer> STEPPING_ZED_TOPIC_MAP = Map.of(PerceptionAPI.STEPPING_ZED_COLOR.get(RobotSide.LEFT), + ZEDImageSensor.LEFT_COLOR_IMAGE_KEY, + PerceptionAPI.STEPPING_ZED_COLOR.get(RobotSide.RIGHT), + ZEDImageSensor.RIGHT_COLOR_IMAGE_KEY, + PerceptionAPI.STEPPING_ZED_DEPTH, + ZEDImageSensor.DEPTH_IMAGE_KEY); - // Realsense - private ImageSensor realsenseSensor; - private ROS2DemandGraphNode realsenseDemandNode; - private ROS2DemandGraphNode realsensePublishDemandNode; - private ImageSensorPublishThread realsensePublishThread; + private final ROS2Node ros2Node; - // ZED - private ImageSensor zedSensor; - private ROS2DemandGraphNode zedPublishDemandNode; - private ImageSensorPublishThread zedPublishThread; + private final Map imageSensors = new HashMap<>(); + private final Map sensorDemandNodes = new HashMap<>(); + private final Map> publishThreads = new HashMap<>(); + private final Map publishDemandNodes = new HashMap<>(); public ROS2ImageSensors(ROS2Node ros2Node) { this.ros2Node = ros2Node; } - public void addRealsenseSensor(ImageSensor realsenseSensor, ReferenceFrame sensorFrame) + public void addImageSensor(String sensorId, ImageSensor imageSensor, ReferenceFrame sensorFrame, ROS2Topic demandTopic) { - this.realsenseSensor = realsenseSensor; - realsenseDemandNode = new ROS2DemandGraphNode(ros2Node, PerceptionAPI.REQUEST_REALSENSE); - realsenseSensor.setSensorFrame(sensorFrame); - realsenseSensor.run(true); - - realsensePublishDemandNode = new ROS2DemandGraphNode(ros2Node, PerceptionAPI.REQUEST_REALSENSE_PUBLICATION); - realsenseDemandNode.addDependents(realsensePublishDemandNode); - - realsensePublishThread = new ImageSensorPublishThread(ros2Node, realsenseSensor); - realsensePublishThread.addTopic(PerceptionAPI.D455_DEPTH_IMAGE, RealSenseImageSensor.DEPTH_IMAGE_KEY, 0.25); - realsensePublishThread.addTopic(PerceptionAPI.D455_COLOR_IMAGE, RealSenseImageSensor.COLOR_IMAGE_KEY, 0.25); - setupCallbackForDemandNode(realsensePublishThread, realsensePublishDemandNode); + imageSensor.setSensorFrame(sensorFrame); + imageSensors.put(sensorId, imageSensor); + + if (demandTopic == null) + { + imageSensor.run(true); + } + else + { + ROS2DemandGraphNode sensorDemandNode = new ROS2DemandGraphNode(ros2Node, demandTopic); + sensorDemandNode.addDemandChangedCallback(imageSensor::run); + sensorDemandNodes.put(sensorId, sensorDemandNode); + } } - public void addZEDSensor(ImageSensor zedSensor, ReferenceFrame sensorFrame) + public void publishSensor(String sensorId, Map>, Integer> topicMap, ROS2Topic demandTopic) { - this.zedSensor = zedSensor; - zedSensor.setSensorFrame(sensorFrame); - zedPublishDemandNode = new ROS2DemandGraphNode(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION); - zedSensor.run(true); // Always start ZED, do not wait for any demand node - - zedPublishThread = new ImageSensorPublishThread(ros2Node, zedSensor); - zedPublishThread.addTopic(PerceptionAPI.ZED_DEPTH, ZEDImageSensor.DEPTH_IMAGE_KEY); - zedPublishThread.addTopic(PerceptionAPI.ZED_COLOR_IMAGES.get(RobotSide.LEFT), ZEDImageSensor.LEFT_COLOR_IMAGE_KEY); - zedPublishThread.addTopic(PerceptionAPI.ZED_COLOR_IMAGES.get(RobotSide.RIGHT), ZEDImageSensor.RIGHT_COLOR_IMAGE_KEY); - setupCallbackForDemandNode(zedPublishThread, zedPublishDemandNode); + publishSensor(sensorId, topicMap, demandTopic, ros2Node); } - private static void setupCallbackForDemandNode(RepeatingTaskThread loopThread, ROS2DemandGraphNode demandNode) + public void publishSensor(String sensorId, Map>, Integer> topicMap, ROS2Topic demandTopic, ROS2Node ros2Node) { - if (!loopThread.isAlive()) - loopThread.start(); - - if (demandNode.isDemanded()) - loopThread.startRepeating(); - - demandNode.addDemandChangedCallback(loopThread::setRepeating); + ImageSensor sensor = imageSensors.get(sensorId); + ImageSensorPublishThread publishThread = new ImageSensorPublishThread(ros2Node, sensor); + + for (Map.Entry>, Integer> entry : topicMap.entrySet()) + publishThread.addTopic(entry.getKey(), entry.getValue()); + + publishThreads.putIfAbsent(sensorId, new HashSet<>()); + publishThreads.get(sensorId).add(publishThread); + + if (demandTopic == null) + { + publishThread.startRepeating(); + } + else + { + ROS2DemandGraphNode publishDemandNode = new ROS2DemandGraphNode(ros2Node, demandTopic); + publishDemandNodes.put(sensorId, publishDemandNode); + + ROS2DemandGraphNode sensorDemandNode = sensorDemandNodes.get(sensorId); + if (sensorDemandNode != null) + sensorDemandNode.addDependents(publishDemandNode); + + ROS2DemandGraphTools.runWhileDemanded(publishThread, publishDemandNode); + } } - /** - * These could all be null because we can't guarantee which sensors will be created. - * This is because we know about the sensors that can exist on the humanoid robots, but we can't guarantee that both sensors are created for each robot. - */ - public void destroy() + public ImageSensor getSensor(String sensorId) { - // Destroy Realsense - if (realsenseSensor != null) - realsenseSensor.close(); - if (realsenseDemandNode != null) - realsenseDemandNode.destroy(); - if (realsensePublishDemandNode != null) - realsensePublishDemandNode.destroy(); - if (realsensePublishThread != null) - realsensePublishThread.blockingKill(); - - // Destroy ZED - if (zedSensor != null) - zedSensor.close(); - if (zedPublishDemandNode != null) - zedPublishDemandNode.destroy(); - if (zedPublishThread != null) - zedPublishThread.blockingKill(); + return imageSensors.get(sensorId); } - /** - * We are attempting to add this queue holder to our image sensor. - * That sensor could be null depending on the robot you are using. - * Having the null check allows for the underlying algorithm to stay the same regardless of what sensors are being used. - */ - public void registerImageQueueForRealsense(BlockingQueue rawImageCollection, int imageKey) - { - if (realsenseSensor != null) // Don't have a Realsense Sensor created for this robot - realsenseSensor.registerImageQueue(rawImageCollection, imageKey); - } - /** - * We are attempting to add this queue holder to our image sensor. - * That sensor could be null depending on the robot you are using. - * Having the null check allows for the underlying algorithm to stay the same regardless of what sensors are being used. - */ - public void registerImageQueueForZED(BlockingQueue rawImageCollection, int imageKey) + public void destroy() { - if (zedSensor != null) // Don't have a ZED Sensor created for this robot - zedSensor.registerImageQueue(rawImageCollection, imageKey); + for (ImageSensor sensor : imageSensors.values()) + sensor.close(); + + for (ROS2DemandGraphNode node : sensorDemandNodes.values()) + node.destroy(); + + for (Set threads : publishThreads.values()) + for (ImageSensorPublishThread thread : threads) + thread.kill(); + + for (ROS2DemandGraphNode node : publishDemandNodes.values()) + node.destroy(); } -} +} \ No newline at end of file diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/demo/IsaacROSFoundationPoseDemo.java b/ihmc-perception/src/main/java/us/ihmc/perception/demo/IsaacROSFoundationPoseDemo.java index bd8c83cb5195..1e467a500710 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/demo/IsaacROSFoundationPoseDemo.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/demo/IsaacROSFoundationPoseDemo.java @@ -80,8 +80,8 @@ private void task() RawImage depth = zedImageSensor.getImage(ZEDImageSensor.DEPTH_IMAGE_KEY); // Publish for the UI - imagePublisher.publishImage(PerceptionAPI.ZED_DEPTH, depth); - imagePublisher.publishImage(PerceptionAPI.ZED_COLOR_IMAGES.get(RobotSide.LEFT), color); + imagePublisher.publishImage(PerceptionAPI.EXPERIMENTAL_ZED_DEPTH, depth); + imagePublisher.publishImage(PerceptionAPI.EXPERIMENTAL_ZED_COLOR.get(RobotSide.LEFT), color); // Run YOLO using the color image yoloExecutor.runNextEnabledModel(color, depth); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/streaming/ROS2ZEDSDKVideoStreamImageMessageRelay.java b/ihmc-perception/src/main/java/us/ihmc/perception/streaming/ROS2ZEDSDKVideoStreamImageMessageRelay.java index fc9c8b9e959c..404dea80a364 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/streaming/ROS2ZEDSDKVideoStreamImageMessageRelay.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/streaming/ROS2ZEDSDKVideoStreamImageMessageRelay.java @@ -4,7 +4,6 @@ import perception_msgs.msg.dds.ImageMessage; import us.ihmc.commons.thread.RepeatingTaskThread; import us.ihmc.commons.thread.ThreadTools; -import us.ihmc.communication.PerceptionAPI; import us.ihmc.communication.packets.MessageTools; import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.perception.RawImage; @@ -12,7 +11,9 @@ import us.ihmc.perception.opencv.OpenCVTools; import us.ihmc.perception.tools.PerceptionMessageTools; import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.robotics.robotSide.SideDependentList; import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2Topic; import us.ihmc.sensors.zed.ZEDImageSensor; import us.ihmc.sensors.zed.ZEDModelData; @@ -35,11 +36,16 @@ public class ROS2ZEDSDKVideoStreamImageMessageRelay extends ZEDImageSensor private final ImageMessage lastLeftColorImageMessage; private final ImageMessage lastRightColorImageMessage; + private final ROS2Topic depthTopic; + private final SideDependentList> colorTopics; + public ROS2ZEDSDKVideoStreamImageMessageRelay(ROS2Node ros2Node, ZEDModelData zedModel, int slDepthMode, String remoteStreamingAddress, - int remoteStreamingPort) + int remoteStreamingPort, + ROS2Topic depthTopic, + SideDependentList> colorTopics) { super(lastCameraId++, zedModel, slDepthMode, remoteStreamingAddress, remoteStreamingPort); @@ -51,10 +57,13 @@ public ROS2ZEDSDKVideoStreamImageMessageRelay(ROS2Node ros2Node, lastLeftColorImageMessage = new ImageMessage(); lastRightColorImageMessage = new ImageMessage(); + this.depthTopic = depthTopic; + this.colorTopics = colorTopics; + publishThread.startRepeating(); } - public void publish() throws InterruptedException + private void publish() throws InterruptedException { if (isSensorRunning()) { @@ -62,22 +71,28 @@ public void publish() throws InterruptedException waitForGrab(timeout); RawImage depthImage = getImage(ZEDImageSensor.DEPTH_IMAGE_KEY); - RawImage leftColorImage = getImage(ZEDImageSensor.LEFT_COLOR_IMAGE_KEY); - RawImage rightColorImage = getImage(ZEDImageSensor.RIGHT_COLOR_IMAGE_KEY); - - // Pack all RawImages into ImageMessages - packImageMessage(depthImage, lastDepthImageMessage); - packImageMessage(leftColorImage, lastLeftColorImageMessage); - packImageMessage(rightColorImage, lastRightColorImageMessage); + if (depthImage != null) + { + packImageMessage(depthImage, lastDepthImageMessage); + publisherExecutor.submit(() -> ros2Helper.publish(depthTopic, lastDepthImageMessage)); + depthImage.release(); + } - // Publish async to not block receiving new images - publisherExecutor.submit(() -> ros2Helper.publish(PerceptionAPI.ZED_DEPTH, lastDepthImageMessage)); - publisherExecutor.submit(() -> ros2Helper.publish(PerceptionAPI.ZED_COLOR_IMAGES.get(RobotSide.LEFT), lastLeftColorImageMessage)); - publisherExecutor.submit(() -> ros2Helper.publish(PerceptionAPI.ZED_COLOR_IMAGES.get(RobotSide.RIGHT), lastRightColorImageMessage)); + RawImage leftColorImage = getImage(ZEDImageSensor.LEFT_COLOR_IMAGE_KEY); + if (leftColorImage != null) + { + packImageMessage(leftColorImage, lastLeftColorImageMessage); + publisherExecutor.submit(() -> ros2Helper.publish(colorTopics.get(RobotSide.LEFT), lastLeftColorImageMessage)); + leftColorImage.release(); + } - rightColorImage.release(); - leftColorImage.release(); - depthImage.release(); + RawImage rightColorImage = getImage(ZEDImageSensor.RIGHT_COLOR_IMAGE_KEY); + if (rightColorImage != null) + { + packImageMessage(rightColorImage, lastRightColorImageMessage); + publisherExecutor.submit(() -> ros2Helper.publish(colorTopics.get(RobotSide.RIGHT), lastRightColorImageMessage)); + rightColorImage.release(); + } } else { @@ -100,9 +115,8 @@ private void packImageMessage(RawImage frame, ImageMessage imageMessage) @Override public void close() { - publisherExecutor.shutdownNow(); publishThread.blockingKill(); - + publisherExecutor.shutdownNow(); super.close(); } } diff --git a/ihmc-perception/src/main/java/us/ihmc/sensors/zed/ZEDImageSensor.java b/ihmc-perception/src/main/java/us/ihmc/sensors/zed/ZEDImageSensor.java index 33f6a59e4fac..2c9ceb5e8d21 100644 --- a/ihmc-perception/src/main/java/us/ihmc/sensors/zed/ZEDImageSensor.java +++ b/ihmc-perception/src/main/java/us/ihmc/sensors/zed/ZEDImageSensor.java @@ -25,6 +25,7 @@ import us.ihmc.zed.library.ZEDJavaAPINativeLibrary; import java.time.Instant; +import java.util.concurrent.atomic.AtomicInteger; import static us.ihmc.zed.global.zed.*; @@ -35,7 +36,7 @@ public class ZEDImageSensor extends ImageSensor ZEDJavaAPINativeLibrary.load(); } - private static int nextStreamingPort = 30000; + private static final AtomicInteger nextStreamingPort = new AtomicInteger(30000); public static final int LEFT_COLOR_IMAGE_KEY = 0; public static final int RIGHT_COLOR_IMAGE_KEY = 1; @@ -48,6 +49,7 @@ public class ZEDImageSensor extends ImageSensor private static final float MILLIMETER_TO_METERS = 0.001f; private final int cameraID; + private final int serialNumber; private final ZEDModelData zedModel; private final int slInputType; private final int slDepthMode; @@ -56,7 +58,7 @@ public class ZEDImageSensor extends ImageSensor private int bitrate; private String remoteStreamingAddress; private int remoteStreamingPort; - private final int localStreamingPort = nextStreamingPort++; + private final int localStreamingPort = nextStreamingPort.getAndAdd(2); private final RawImage[] grabbedImages = new RawImage[OUTPUT_IMAGE_COUNT]; private final Pointer[] slMatPointers = new Pointer[OUTPUT_IMAGE_COUNT]; @@ -87,6 +89,11 @@ public ZEDImageSensor(int cameraID, ZEDModelData zedModel, int slInputType, int this(cameraID, zedModel, slInputType, slDepthMode, DEFAULT_RESOLUTION, DEFAULT_FPS); } + public ZEDImageSensor(int cameraID, ZEDModelData zedModel, int slInputType, int slDepthMode, int resolution, int fps) + { + this(cameraID, 0, zedModel, slInputType, slDepthMode, resolution, fps); + } + /** * See the documentation for the available resolutions and frame rates: *
    @@ -99,11 +106,12 @@ public ZEDImageSensor(int cameraID, ZEDModelData zedModel, int slInputType, int *
  • {@link us.ihmc.zed.global.zed#SL_RESOLUTION_VGA}
    • *
*/ - public ZEDImageSensor(int cameraID, ZEDModelData zedModel, int slInputType, int slDepthMode, int resolution, int fps) + public ZEDImageSensor(int cameraID, int serialNumber, ZEDModelData zedModel, int slInputType, int slDepthMode, int resolution, int fps) { super(zedModel.name()); this.cameraID = cameraID; + this.serialNumber = serialNumber; this.zedModel = zedModel; this.slInputType = slInputType; this.slDepthMode = slDepthMode; @@ -255,9 +263,9 @@ protected void setInitParameters(SL_InitParameters parametersToSet) protected int openCamera() { if (slInputType == SL_INPUT_TYPE_STREAM) - return sl_open_camera(cameraID, zedInitParameters, 0, "", remoteStreamingAddress, remoteStreamingPort, "", "", ""); + return sl_open_camera(cameraID, zedInitParameters, serialNumber, "", remoteStreamingAddress, remoteStreamingPort, "", "", ""); else - return sl_open_camera(cameraID, zedInitParameters, 0, "", "", 0, "", "", ""); + return sl_open_camera(cameraID, zedInitParameters, serialNumber, "", "", 0, "", "", ""); } @Override @@ -452,9 +460,12 @@ public void close() } } - for (RawImage image : grabbedImages) - if (image != null) - image.release(); + synchronized (grabbedImages) + { + for (RawImage image : grabbedImages) + if (image != null) + image.release(); + } sl_close_camera(cameraID); @@ -479,7 +490,7 @@ public ZEDException(int zedErrorCode) @Override public String getMessage() { - return "ZED Error (%d): %s".formatted(zedErrorCode, getZEDErrorName(zedErrorCode)); + return "[%s] ZED Error (%d): %s".formatted(getSensorName(), zedErrorCode, getZEDErrorName(zedErrorCode)); } }