Publishing Camera’s Data

Learning Objectives

In this tutorial, we demonstrate how to programmatically set up publishers for Isaac Sim Cameras at an approximate frequency.

Getting Started

Prerequisite

Note

In Windows 10 or 11, depending on your machine’s configuration, RViz2 may not open properly.

Setup a camera in a scene

To begin this tutorial, we first set up an environment with a omni.isaac.sensor Camera object. Running the below will result in a simple warehouse environment loaded with a camera in the scene.

 1import carb
 2from isaacsim import SimulationApp
 3import sys
 4
 5BACKGROUND_STAGE_PATH = "/background"
 6BACKGROUND_USD_PATH = "/Isaac/Environments/Simple_Warehouse/warehouse_with_forklifts.usd"
 7
 8CONFIG = {"renderer": "RayTracedLighting", "headless": False}
 9
10# Example ROS2 bridge sample demonstrating the manual loading of stages and manual publishing of images
11simulation_app = SimulationApp(CONFIG)
12import omni
13import numpy as np
14from omni.isaac.core import SimulationContext
15from omni.isaac.core.utils import stage, extensions, nucleus
16import omni.graph.core as og
17import omni.replicator.core as rep
18import omni.syntheticdata._syntheticdata as sd
19
20from omni.isaac.core.utils.prims import set_targets
21from omni.isaac.sensor import Camera
22import omni.isaac.core.utils.numpy.rotations as rot_utils
23from omni.isaac.core.utils.prims import is_prim_path_valid
24from omni.isaac.core_nodes.scripts.utils import set_target_prims
25
26# Enable ROS2 bridge extension
27extensions.enable_extension("omni.isaac.ros2_bridge")
28
29simulation_app.update()
30
31simulation_context = SimulationContext(stage_units_in_meters=1.0)
32
33# Locate Isaac Sim assets folder to load environment and robot stages
34assets_root_path = nucleus.get_assets_root_path()
35if assets_root_path is None:
36    carb.log_error("Could not find Isaac Sim assets folder")
37    simulation_app.close()
38    sys.exit()
39
40# Loading the environment
41stage.add_reference_to_stage(assets_root_path + BACKGROUND_USD_PATH, BACKGROUND_STAGE_PATH)
42
43
44###### Camera helper functions for setting up publishers. ########
45
46# Paste functions from the tutorial here
47# def publish_camera_tf(camera: Camera): ...
48# def publish_camera_info(camera: Camera, freq): ...
49# def publish_pointcloud_from_depth(camera: Camera, freq): ...
50# def publish_depth(camera: Camera, freq): ...
51# def publish_rgb(camera: Camera, freq): ...
52
53###################################################################
54
55# Create a Camera prim. Note that the Camera class takes the position and orientation in the world axes convention.
56camera = Camera(
57    prim_path="/World/floating_camera",
58    position=np.array([-3.11, -1.87, 1.0]),
59    frequency=20,
60    resolution=(256, 256),
61    orientation=rot_utils.euler_angles_to_quats(np.array([0, 0, 0]), degrees=True),
62)
63camera.initialize()
64
65simulation_app.update()
66camera.initialize()
67
68############### Calling Camera publishing functions ###############
69
70# Call the publishers.
71# Make sure you pasted in the helper functions above, and uncomment out the following lines before running.
72
73approx_freq = 30
74#publish_camera_tf(camera)
75#publish_camera_info(camera, approx_freq)
76#publish_rgb(camera, approx_freq)
77#publish_depth(camera, approx_freq)
78#publish_pointcloud_from_depth(camera, approx_freq)
79
80####################################################################
81
82# Initialize physics
83simulation_context.initialize_physics()
84simulation_context.play()
85
86while simulation_app.is_running():
87    simulation_context.step(render=True)
88
89simulation_context.stop()
90simulation_app.close()

Publish camera intrinsics to CameraInfo topic

The following snippet will publish camera intrinsics associated with an omni.isaac.sensor Camera to a sensor_msgs/CameraInfo topic.

 1def publish_camera_info(camera: Camera, freq):
 2    from omni.isaac.ros2_bridge import read_camera_info
 3    # The following code will link the camera's render product and publish the data to the specified topic name.
 4    render_product = camera._render_product_path
 5    step_size = int(60/freq)
 6    topic_name = camera.name+"_camera_info"
 7    queue_size = 1
 8    node_namespace = ""
 9    frame_id = camera.prim_path.split("/")[-1] # This matches what the TF tree is publishing.
10
11    writer = rep.writers.get("ROS2PublishCameraInfo")
12    camera_info = read_camera_info(render_product_path=render_product)
13    writer.initialize(
14        frameId=frame_id,
15        nodeNamespace=node_namespace,
16        queueSize=queue_size,
17        topicName=topic_name,
18        width=camera_info["width"],
19        height=camera_info["height"],
20        projectionType=camera_info["projectionType"],
21        k=camera_info["k"].reshape([1, 9]),
22        r=camera_info["r"].reshape([1, 9]),
23        p=camera_info["p"].reshape([1, 12]),
24        physicalDistortionModel=camera_info["physicalDistortionModel"],
25        physicalDistortionCoefficients=camera_info["physicalDistortionCoefficients"],
26    )
27    writer.attach([render_product])
28
29    gate_path = omni.syntheticdata.SyntheticData._get_node_path(
30        "PostProcessDispatch" + "IsaacSimulationGate", render_product
31    )
32
33    # Set step input of the Isaac Simulation Gate nodes upstream of ROS publishers to control their execution rate
34    og.Controller.attribute(gate_path + ".inputs:step").set(step_size)
35    return

Publish pointcloud from depth images

In the following snippet, we publish a pointcloud to a sensor_msgs/PointCloud2 message. This pointcloud is reconstructed from the depth image using the intrinsics of the camera.

 1def publish_pointcloud_from_depth(camera: Camera, freq):
 2    # The following code will link the camera's render product and publish the data to the specified topic name.
 3    render_product = camera._render_product_path
 4    step_size = int(60/freq)
 5    topic_name = camera.name+"_pointcloud" # Set topic name to the camera's name
 6    queue_size = 1
 7    node_namespace = ""
 8    frame_id = camera.prim_path.split("/")[-1] # This matches what the TF tree is publishing.
 9
10    # Note, this pointcloud publisher will simply convert the Depth image to a pointcloud using the Camera intrinsics.
11    # This pointcloud generation method does not support semantic labelled objects.
12    rv = omni.syntheticdata.SyntheticData.convert_sensor_type_to_rendervar(
13        sd.SensorType.DistanceToImagePlane.name
14    )
15
16    writer = rep.writers.get(rv + "ROS2PublishPointCloud")
17    writer.initialize(
18        frameId=frame_id,
19        nodeNamespace=node_namespace,
20        queueSize=queue_size,
21        topicName=topic_name
22    )
23    writer.attach([render_product])
24
25    # Set step input of the Isaac Simulation Gate nodes upstream of ROS publishers to control their execution rate
26    gate_path = omni.syntheticdata.SyntheticData._get_node_path(
27        rv + "IsaacSimulationGate", render_product
28    )
29    og.Controller.attribute(gate_path + ".inputs:step").set(step_size)
30
31    return

Publish RGB images

 1def publish_rgb(camera: Camera, freq):
 2    # The following code will link the camera's render product and publish the data to the specified topic name.
 3    render_product = camera._render_product_path
 4    step_size = int(60/freq)
 5    topic_name = camera.name+"_rgb"
 6    queue_size = 1
 7    node_namespace = ""
 8    frame_id = camera.prim_path.split("/")[-1] # This matches what the TF tree is publishing.
 9
10    rv = omni.syntheticdata.SyntheticData.convert_sensor_type_to_rendervar(sd.SensorType.Rgb.name)
11    writer = rep.writers.get(rv + "ROS2PublishImage")
12    writer.initialize(
13        frameId=frame_id,
14        nodeNamespace=node_namespace,
15        queueSize=queue_size,
16        topicName=topic_name
17    )
18    writer.attach([render_product])
19
20    # Set step input of the Isaac Simulation Gate nodes upstream of ROS publishers to control their execution rate
21    gate_path = omni.syntheticdata.SyntheticData._get_node_path(
22        rv + "IsaacSimulationGate", render_product
23    )
24    og.Controller.attribute(gate_path + ".inputs:step").set(step_size)
25
26    return

Publish depth images

 1def publish_depth(camera: Camera, freq):
 2    # The following code will link the camera's render product and publish the data to the specified topic name.
 3    render_product = camera._render_product_path
 4    step_size = int(60/freq)
 5    topic_name = camera.name+"_depth"
 6    queue_size = 1
 7    node_namespace = ""
 8    frame_id = camera.prim_path.split("/")[-1] # This matches what the TF tree is publishing.
 9
10    rv = omni.syntheticdata.SyntheticData.convert_sensor_type_to_rendervar(
11                            sd.SensorType.DistanceToImagePlane.name
12                        )
13    writer = rep.writers.get(rv + "ROS2PublishImage")
14    writer.initialize(
15        frameId=frame_id,
16        nodeNamespace=node_namespace,
17        queueSize=queue_size,
18        topicName=topic_name
19    )
20    writer.attach([render_product])
21
22    # Set step input of the Isaac Simulation Gate nodes upstream of ROS publishers to control their execution rate
23    gate_path = omni.syntheticdata.SyntheticData._get_node_path(
24        rv + "IsaacSimulationGate", render_product
25    )
26    og.Controller.attribute(gate_path + ".inputs:step").set(step_size)
27
28    return

Publish a TF Tree for the camera pose

The pointcloud, published using the above function, will publish the pointcloud in the ROS camera axes convention (-Y up, +Z forward). To make visualizing this pointcloud easy in ROS via RViz, the following snippet will publish a TF Tree to the /tf, containing two frames.

The two frames are:

  1. {camera_frame_id}: This is the camera’s pose in the ROS camera convention (-Y up, +Z forward). Pointclouds are published in this frame.

    ../_images/camera_frames_v2.005.png

  2. {camera_frame_id}_world: This is the camera’s pose in the World axes convention (+Z up, +X forward). This will reflect the true pose of the camera.

    ../_images/camera_frames_v2.004.png

The TF Tree looks like this:

../_images/transformation.png

  • world -> {camera_frame_id} is a dynamic transform from origin to the camera in the ROS camera convention, following any movement of the camera.

  • {camera_frame_id} -> {camera_frame_id}_world is a static transform consisting of only a rotation and zero translation. This static transform can be represented by the quaternion [0.5, -0.5, 0.5, 0.5] in [w, x, y, z] convention.

Since the pointcloud is published in {camera_frame_id}, it is encouraged to set the frame_id of the pointcloud topic to {camera_frame_id}. The resulting visualization of the pointclouds can be viewed in the world frame in RViz.

 1def publish_camera_tf(camera: Camera):
 2    camera_prim = camera.prim_path
 3
 4    if not is_prim_path_valid(camera_prim):
 5        raise ValueError(f"Camera path '{camera_prim}' is invalid.")
 6
 7    try:
 8        # Generate the camera_frame_id. OmniActionGraph will use the last part of
 9        # the full camera prim path as the frame name, so we will extract it here
10        # and use it for the pointcloud frame_id.
11        camera_frame_id=camera_prim.split("/")[-1]
12
13        # Generate an action graph associated with camera TF publishing.
14        ros_camera_graph_path = "/CameraTFActionGraph"
15
16        # If a camera graph is not found, create a new one.
17        if not is_prim_path_valid(ros_camera_graph_path):
18            (ros_camera_graph, _, _, _) = og.Controller.edit(
19                {
20                    "graph_path": ros_camera_graph_path,
21                    "evaluator_name": "execution",
22                    "pipeline_stage": og.GraphPipelineStage.GRAPH_PIPELINE_STAGE_SIMULATION,
23                },
24                {
25                    og.Controller.Keys.CREATE_NODES: [
26                        ("OnTick", "omni.graph.action.OnTick"),
27                        ("IsaacClock", "omni.isaac.core_nodes.IsaacReadSimulationTime"),
28                        ("RosPublisher", "omni.isaac.ros2_bridge.ROS2PublishClock"),
29                    ],
30                    og.Controller.Keys.CONNECT: [
31                        ("OnTick.outputs:tick", "RosPublisher.inputs:execIn"),
32                        ("IsaacClock.outputs:simulationTime", "RosPublisher.inputs:timeStamp"),
33                    ]
34                }
35            )
36
37        # Generate 2 nodes associated with each camera: TF from world to ROS camera convention, and world frame.
38        og.Controller.edit(
39            ros_camera_graph_path,
40            {
41                og.Controller.Keys.CREATE_NODES: [
42                    ("PublishTF_"+camera_frame_id, "omni.isaac.ros2_bridge.ROS2PublishTransformTree"),
43                    ("PublishRawTF_"+camera_frame_id+"_world", "omni.isaac.ros2_bridge.ROS2PublishRawTransformTree"),
44                ],
45                og.Controller.Keys.SET_VALUES: [
46                    ("PublishTF_"+camera_frame_id+".inputs:topicName", "/tf"),
47                    # Note if topic_name is changed to something else besides "/tf",
48                    # it will not be captured by the ROS tf broadcaster.
49                    ("PublishRawTF_"+camera_frame_id+"_world.inputs:topicName", "/tf"),
50                    ("PublishRawTF_"+camera_frame_id+"_world.inputs:parentFrameId", camera_frame_id),
51                    ("PublishRawTF_"+camera_frame_id+"_world.inputs:childFrameId", camera_frame_id+"_world"),
52                    # Static transform from ROS camera convention to world (+Z up, +X forward) convention:
53                    ("PublishRawTF_"+camera_frame_id+"_world.inputs:rotation", [0.5, -0.5, 0.5, 0.5]),
54                ],
55                og.Controller.Keys.CONNECT: [
56                    (ros_camera_graph_path+"/OnTick.outputs:tick",
57                        "PublishTF_"+camera_frame_id+".inputs:execIn"),
58                    (ros_camera_graph_path+"/OnTick.outputs:tick",
59                        "PublishRawTF_"+camera_frame_id+"_world.inputs:execIn"),
60                    (ros_camera_graph_path+"/IsaacClock.outputs:simulationTime",
61                        "PublishTF_"+camera_frame_id+".inputs:timeStamp"),
62                    (ros_camera_graph_path+"/IsaacClock.outputs:simulationTime",
63                        "PublishRawTF_"+camera_frame_id+"_world.inputs:timeStamp"),
64                ],
65            },
66        )
67    except Exception as e:
68        print(e)
69
70    # Add target prims for the USD pose. All other frames are static.
71    set_target_prims(
72        primPath=ros_camera_graph_path+"/PublishTF_"+camera_frame_id,
73        inputName="inputs:targetPrims",
74        targetPrimPaths=[camera_prim],
75    )
76    return

Running the Example

Enable ros2_bridge and set up ROS2 environment variables following this workflow tutorial. Save the above script and run it using python.sh in the Isaac Sim folder. In our example, {camera_frame_id} is the prim name of the camera, which is floating_camera.

You will see a floating camera with prim path /World/floating_camera in the scene, and the camera should see a forklift:

You should expect to see the following:

../_images/isaac_tutorial_ros_camera_publishing_simview.png

If you open a terminal and type ros2 topic list, you should expect to see the following:

ros2 topic list
/camera_camera_info
/camera_depth
/camera_pointcloud
/camera_rgb
/clock
/parameter_events
/rosout
/tf

The frames published by TF will look like the following:

../_images/frames.png

Now, we can visualize the pointcloud and depth images via RViz2. Open RViz2, and set the “Fixed Frame” field to world.

../_images/rviz.png

Then, enable viewing /camera_depth, /camera_rgb, /camera_pointcloud, and /tf topics.

The depth image /camera_depth and RGB image /camera_rgb should look like this respectively:

../_images/isaac_tutorial_ros_camera_publishing_rgbd.png

The pointcloud will look like so. Note that the camera frames published by the TF publisher shows the two frames. The image on the left shows the {camera_frame_id}_world frame, and the image on the right shows the {camera_frame_id} frame.

../_images/isaac_tutorial_ros_camera_publishing_pc_frontview.png

From the side view:

../_images/isaac_tutorial_ros_camera_publishing_pc_sideview.png

Summary

This tutorial demonstrated how to programmatically set up ROS2 publishers for Isaac Sim Cameras at an approximate frequency.