isaac_ros_foundationpose on zed camera

[kc645]

Hi, I am trying to run foundationpose on zed camera.

In the documentation, I don't see a column for zed camera, but there is for rosbag, realsense camera, and hawk camera. I managed to look at centerpose and compared the launch arguments. So I ran this launch command

ros2 launch isaac_ros_examples isaac_ros_examples.launch.py launch_fragments:=zed_mono_rect_depth,foundationpose mesh_file_path:=${ISAAC_ROS_WS}/isaac_ros_assets/isaac_ros_foundationpose/Mac_and_cheese_0_1/Mac_and_cheese_0_1.obj texture_path:=${ISAAC_ROS_WS}/isaac_ros_assets/isaac_ros_foundationpose/Mac_and_cheese_0_1/materials/textures/baked_mesh_tex0.png score_engine_file_path:=${ISAAC_ROS_WS}/isaac_ros_assets/models/foundationpose/score_trt_engine.plan refine_engine_file_path:=${ISAAC_ROS_WS}/isaac_ros_assets/models/foundationpose/refine_trt_engine.plan rt_detr_engine_file_path:=${ISAAC_ROS_WS}/isaac_ros_assets/models/synthetica_detr/sdetr_grasp.plan interface_specs_file:=${ISAAC_ROS_WS}/isaac_ros_assets/isaac_ros_centerpose/zed2_quickstart_interface_specs.json

to run zed mono rect depth. To visualize, I ran rviz2 -d $(ros2 pkg prefix isaac_ros_foundationpose --share)/rviz/foundationpose_zed.rviz, to visualize

The correct feed shows up but somehow when it detects my tumblr (even though I am running Mac and cheese?) the program shuts down by saying below

What would be the solution to this problem?

Also, do we have to only use DETR object detection? What if we want to use yolo model or other object detection models, how would I go about this?

Thank you so much for your help!

[kc645]

so by changing the ZED setting of pub resolution from CUSTOM to NATIVE, I was able to let the program not quit. However I get an error from zed_wrapper-2 Error as following

[zed_wrapper-2] Error: TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id "fp_object" from authority "Authority undetectable" because of an invalid quaternion in the transform (-nan nan nan -nan)

What would be the solution to this?

[tomasz-lewicki]

Hey, I encountered a similar problem working with ZED & FoundationPose. Here's a solution to make it work with ZED:

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
import numpy as np

class DepthCleanerNode(Node):
    def __init__(self):
        super().__init__("depth_cleaner_node")
        self.bridge = CvBridge()
        self.subscription = self.create_subscription(Image, DEPTH_TOPIC, self.depth_callback, 10)
        self.publisher = self.create_publisher(Image, DEPTH_TOPIC_CLEAN, 10)

    def depth_callback(self, msg):
        depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough")
        depth_image_cleaned = np.nan_to_num(depth_image, nan=0)
        cleaned_msg = self.bridge.cv2_to_imgmsg(depth_image_cleaned, encoding="passthrough")
        cleaned_msg.header = msg.header  # Preserve original message header
        self.publisher.publish(cleaned_msg)


def main(args=None):
    rclpy.init(args=args)
    node = DepthCleanerNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info("Shutting down DepthCleanerNode.")
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == "__main__":
    main()

You will have to set topic names DEPTH_TOPIC and ``DEPTH_TOPIC_CLEAN`.

The problem was that ZED publishes NaN values where we don't get a valid depth, such as in the stereo occlusion shadow behind objects. The solution replaces NaN values with zeros.

(I looked at sample data from https://github.com/NVlabs/FoundationPose, and it also has zeros in occlusion shadows, so I'm 99% sure that that's what paper authors used for training, and that's what the neural net expects).

[ArghyaChatterjee]

This seems to be a good topic to start from the first place. I can see inside the isaac_ros_zed_mono_rect_depth_core.launch.py, here is the current situation:

import os
from typing import Any, Dict

from ament_index_python import get_package_share_directory
from isaac_ros_examples import IsaacROSLaunchFragment
import launch
from launch.substitutions import Command
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode


class IsaacROSZedMonoRectDepthLaunchFragment(IsaacROSLaunchFragment):

    @staticmethod
    def get_interface_specs() -> Dict[str, Any]:
        return {
            'camera_resolution': {'width': 1200, 'height': 720},
            'camera_frame': 'zed2_base_link',
            'camera_model': 'zed2',
            'focal_length': {
                # Approximation - most zed cameras should be close to this value
                'f_x': 380.0,
                # Approximation - most zed cameras should be close to this value
                'f_y': 380.0
            }
        }

    @staticmethod
    def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, ComposableNode]:

        camera_model = interface_specs['camera_model']

        # ZED Configurations to be loaded by ZED Node
        config_common = os.path.join(
            get_package_share_directory('isaac_ros_zed'),
            'config',
            'zed_mono_depth.yaml'
        )

        config_camera = os.path.join(
            get_package_share_directory('zed_wrapper'),
            'config',
            camera_model + '.yaml'
        )
        return {
            'image_format_converter_node_left': ComposableNode(
                package='isaac_ros_image_proc',
                plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode',
                name='image_format_node_left',
                parameters=[{
                    'encoding_desired': 'rgb8',
                    'image_width': interface_specs['camera_resolution']['width'],
                    'image_height': interface_specs['camera_resolution']['height'],
                }],
                remappings=[
                    ('image_raw', 'zed_node/left/image_rect_color'),
                    ('image', 'image_rect')]
            ),
            'zed_wrapper_component': ComposableNode(
                package='zed_components',
                plugin='stereolabs::ZedCamera',
                name='zed_node',
                parameters=[
                    config_common,  # Common parameters
                    config_camera,  # Camera related parameters
                ],
                remappings=[
                    ('zed_node/left/camera_info', '/camera_info'),
                ],
                extra_arguments=[{'use_intra_process_comms': True}]
            )
        }

    @staticmethod
    def get_launch_actions(interface_specs: Dict[str, Any]) -> Dict[str, Any]:
        # The zed camera mode name. zed, zed2, zed2i, zedm, zedx or zedxm
        camera_model = interface_specs['camera_model']

        # URDF/xacro file to be loaded by the Robot State Publisher node
        xacro_path = os.path.join(
            get_package_share_directory('zed_wrapper'),
            'urdf', 'zed_descr.urdf.xacro'
        )

        return {
            # Robot State Publisher node
            'rsp_node': Node(
                package='robot_state_publisher',
                executable='robot_state_publisher',
                name='zed_state_publisher',
                output='screen',
                parameters=[{
                    'robot_description': Command(
                        [
                            'xacro', ' ', xacro_path, ' ',
                            'camera_name:=', camera_model, ' ',
                            'camera_model:=', camera_model
                        ])
                }]
            )
        }


def generate_launch_description():
    zed_container = ComposableNodeContainer(
        package='rclcpp_components',
        name='zed_container',
        namespace='',
        executable='component_container_mt',
        composable_node_descriptions=(
            IsaacROSZedMonoRectDepthLaunchFragment.get_composable_nodes()
        ),
        output='screen'
    )

    return launch.LaunchDescription(
        [zed_container] + IsaacROSZedMonoRectDepthLaunchFragment.get_launch_actions())

The topics needed for foundation pose are:

/camera_info_rect
/depth
/image_rect

In this launch script above that I posted, I only see the remapping for the /image_rect and a wrong remapping for camera_info_rect (it's camera_info in the file) and no remapping for /depth. Then a drop node is subscribing to these topics to drop some of the messages.

If you launch it this way:

ros2 launch isaac_ros_examples isaac_ros_examples.launch.py launch_fragments:=zed_mono_rect_depth,foundationpose interface_specs_file:=$(pwd)/isaac_ros_assets/isaac_ros_foundationpose/zed2_quickstart_interface_specs.json mesh_file_path:=$(pwd)/isaac_ros_assets/isaac_ros_foundationpose/Mustard/textured_simple.obj texture_path:=$(pwd)/isaac_ros_assets/isaac_ros_foundationpose/Mustard/texture_map.png score_engine_file_path:=$(pwd)/isaac_ros_assets/models/foundationpose/score_trt_engine.plan refine_engine_file_path:=$(pwd)/isaac_ros_assets/models/foundationpose/refine_trt_engine.plan rt_detr_engine_file_path:=$(pwd)/isaac_ros_assets/models/synthetica_detr/sdetr_grasp.plan

This is remapping only /camera_info not camera_info_rect, /image_rect and not at all depth. Have you seen this problem before ??

[ArghyaChatterjee]

Okay, I encountered the same problem now. I had to add a depth processing node to remap the zed depth to /depth topic and changed the camera_info remapping topic to camera_info_rect and then encountered this problem. So, I tried to write a NaN checker code:

test_depth.py

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
import numpy as np
import cv2
from cv_bridge import CvBridge


class DepthNaNChecker(Node):

    def __init__(self):
        super().__init__('depth_nan_checker')
        self.bridge = CvBridge()
        self.subscription = self.create_subscription(
            Image,
            '/depth',  # Adjust this if your topic name is different
            self.listener_callback,
            10
        )

    def listener_callback(self, msg):
        try:
            # Convert depth image to CV format
            depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough')

            # Convert to float32 if it's not already
            if depth_image.dtype != np.float32:
                depth_image = depth_image.astype(np.float32)

            # Convert 0 or out-of-range to NaN (optional but common for depth cleaning)
            depth_image[depth_image == 0] = np.nan

            # Count NaNs
            nan_count = np.isnan(depth_image).sum()
            self.get_logger().info(f'NaNs in frame: {nan_count}')
        except Exception as e:
            self.get_logger().error(f'Error processing depth image: {e}')


def main(args=None):
    rclpy.init(args=args)
    node = DepthNaNChecker()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

here is what I get:

admin@arghya:/workspaces/isaac_ros-dev/src/isaac_ros_pose_estimation/isaac_ros_foundationpose/scripts$ python3 test_depth.py
[INFO] [1745432030.196195464] [depth_nan_checker]: NaNs in frame: 188756
[INFO] [1745432030.198241681] [depth_nan_checker]: NaNs in frame: 190627
[INFO] [1745432030.199940640] [depth_nan_checker]: NaNs in frame: 190172
[INFO] [1745432030.201340740] [depth_nan_checker]: NaNs in frame: 190634
[INFO] [1745432030.228598965] [depth_nan_checker]: NaNs in frame: 189784
[INFO] [1745432030.543517595] [depth_nan_checker]: NaNs in frame: 188811
[INFO] [1745432030.545614093] [depth_nan_checker]: NaNs in frame: 190649
[INFO] [1745432030.547458218] [depth_nan_checker]: NaNs in frame: 190911
[INFO] [1745432030.549257069] [depth_nan_checker]: NaNs in frame: 189690
[INFO] [1745432030.550999455] [depth_nan_checker]: NaNs in frame: 190199
....

I have also changed the zed_mono_depth.yaml file to produce mono16 depth instead of 32FC1 which helped me write the depth type converted node that I mentioned in the beginning. It looks like the zed is actively publishing NaN values. One thing that I also noticed that you can only publish ULTRA depth but can't publish NEURAL and NEURAL+ depth inside the docker container.

[ArghyaChatterjee]

@kc645 , were you able to run it changing the resolution from pub_resolution: 'CUSTOM' to pub_resolution: 'NATIVE'. Camera resolution becomes 640x360 from 1280x720 when you change it from custom to native. Does it have any impact on this issue ?